# Iterated Orthogonal Simplex Cubature Kalman Filter and Its Applications in Target Tracking
## 4 Iterated Orthogonal Simplex Cubature Kalman Filter
### The Iterated Orthogonal Simplex Cubature Kalman Filter  
The enhanced Gauss-Newton iteration and the aforementioned altered cubature points are combined to create the IOSCKF method within the nonlinear Kalman filter architecture, and the specific calculation steps are as follows.  
Step 1: Filter Initialization  
\[\left\{\begin{array}{l}\hat{\mathbf{x}}_{0}^{+}=E(\mathbf{x}_{0})\\ \mathbf{P}_{0}^{+}=E\Bigl{[}\bigl{(}\mathbf{x}_{0}-\hat{\mathbf{x}}_{0}^{+}\bigr{)} \bigl{(}\mathbf{x}_{0}-\hat{\mathbf{x}}_{0}^{+}\bigr{)}^{\mathsf{T}}\Bigr{]}\end{array}\right. \tag{30}\]  
Step 2: Time Update  
\(\hat{\mathbf{x}}_{k-1}^{+}\) and \(\mathbf{P}_{k-1}^{+}\) take the place of \(\mathbf{\overline{x}}\) and \(\mathbf{P}_{x}\) in Equation (24) to calculate the cubature points \(\hat{\mathbf{x}}_{k-1}^{(i)}\), which are disseminated nonlinearly using \(\mathbf{f}(\cdot)\), and the prior state \(\hat{\mathbf{x}}_{k}^{-}\) and covariance \(\mathbf{P}_{k}^{-}\) are calculated using the weights in Equation (23) on the basis of Equations (4) and (5), respectively.  
Step 3: Measurement Update  
Take the prior state \(\hat{\mathbf{x}}_{k}^{j}=\hat{\mathbf{x}}_{k}^{-}\) and covariance matrix \(\mathbf{P}_{k}^{0}=\mathbf{P}_{k}^{-}\) as the initial values of the iterated measurement update, and the estimated state and covariance matrix in the \(j\)th iteration express \(\hat{\mathbf{x}}_{k}^{j}\) and \(\mathbf{P}_{k}^{j}\) (\(j=1,2,\cdots,N_{\text{max}}\)), respectively. The cubature points \(\hat{\mathbf{x}}_{k}^{(i),j-1}\) are calculated using \(\hat{\mathbf{x}}_{k}^{j-1}\) and \(\mathbf{P}_{k}^{j-1}\) instead of \(\mathbf{\bar{x}}\) and \(\mathbf{P}_{x}\) in Equation (24), and propagated nonlinearly using \(\mathbf{h}(\cdot)\). The predicted measurement \(\hat{\mathbf{x}}_{k}^{j}\), the measurement covariance matrix \(\mathbf{P}_{z}^{j}\), and the cross covariance matrix \(\mathbf{P}_{xz}^{j}\) are calculated using the weights in Equation (23) on the basis of the measurement update process of the Gaussian approximation filter, respectively. The Kalman filtering \(\mathbf{K}_{k}^{j}\) and covariance matrix \(\mathbf{P}_{k}^{j}\) are estimated to be equivalent to the Gaussian approximation filter, respectively, while the posterior state \(\hat{\mathbf{x}}_{k}^{j}\) is calculated using the following iterated form.  
\[\hat{\mathbf{x}}_{k}^{j}=\hat{\mathbf{x}}_{k}^{-}+\mathbf{K}_{k}^{j}\left[\mathbf{z}_{k}-\mathbf{ h}\left(\hat{\mathbf{x}}_{k}^{j-1}\right)-\left(\mathbf{P}_{xz}^{j}\right)^{\text{T}} \left(\mathbf{P}_{k}^{-}\right)^{-1}\left(\hat{\mathbf{x}}_{k}^{-}-\hat{\mathbf{x}}_{k}^ {j-1}\right)\right] \tag{31}\]  
The above iteration format obtains a posterior estimate higher than the last one after each iteration step, and the system model \(\mathbf{h}(\cdot)\) is known and unchanged, so the whole iterative calculation process is convergent and stable.  
The iteration termination condition is set as shown below.  
\[\left\|\hat{\mathbf{x}}_{k}^{j}-\hat{\mathbf{x}}_{k}^{j-1}\right\|\leq\text{cor}\ j=N_{ \text{max}} \tag{32}\]  
where \(\varepsilon\) and \(N_{\text{max}}\) are the pre-set threshold and maximum iteration times.  
The number of iterations in the end is presumed to be \(N\), whereupon the estimated state and covariance matrix at time \(k\) are \(\hat{\mathbf{x}}_{k}^{+}=\hat{\mathbf{x}}_{k}^{N}\) and \(\mathbf{P}_{k}^{+}=\mathbf{P}_{k}^{N}\), respectively. Note that the calculational burden of the ICKF and CKF is within the same order of magnitude if \(N_{\text{max}}\) is not too large, and in general, \(N_{\text{max}}\) is no more than three.  
Considering that all parameters in the algorithm are definite values, the algorithm has low sensitivity and strong robustness to parameters.