# Iterated Orthogonal Simplex Cubature Kalman Filter and Its Applications in Target Tracking
## 5 Simulations and Results
### CV Target Tracking Simulation  
The target is assumed to be in constant velocity (CV) motion; the state equation of the CV model in a two-dimensional case is described as follows:  
\[\mathbf{X}_{k}=\mathbf{F}_{CV}\mathbf{X}_{k-1}+\mathbf{G}\mathbf{w}_{k-1} \tag{33}\]  
where \(\mathbf{X}_{k}=\left(x_{k}\quad\dot{x}_{k}\quad y_{k}\quad\dot{y}_{k}\right)^{ \text{T}}\) represents the position and velocity of the target at time index \(k\), \(w_{k}\) denotes the process noise, and \(\mathbf{F}_{CV}\) and \(\mathbf{G}\) denote the state transformation matrix and the noise-driven matrix, which are defined, respectively as follows:  
\[\mathbf{F}_{CV}= \begin{bmatrix}1&T&0&0\\ 0&1&0&0\\ 0&0&1&T\\ 0&0&0&1\end{bmatrix} ,\ \mathbf{G}=\left[ \begin{array}{cc}T^{2}/2&0\\ T&0\\ 0&T^{2}/2\\ 0&T\end{array} \right] \tag{34}\]  
where \(T\) is the sampling interval. In the target tracking system, the bearings-only measurement equation is written as follows. \[Z_{k}=\text{arctan}\bigg{(}\frac{y_{k}-y_{r}}{x_{k}-x_{r}}\bigg{)}+v_{k} \tag{35}\]where \(Z_{k}\) is the radar measurement at time \(k\), \(\big{(}x_{r},\quad y_{r}\big{)}\) is the location of the radar, and \(v_{k}\) is the measurement noise. The proposed algorithm has the following two degraded forms: the cubature Kalman filter with the improved spherical simplex-radial rule (SCKF), and the SCKF with orthogonal transformation (OSCKF). In order to evaluate the performance of the proposed algorithm, it is compared to the general CKF, SSRCKF, SCKF, and OSCKF. This simulation shows the radar location \(\big{(}x_{r},\quad y_{r}\big{)}=\big{(}200\text{ m}\quad 300\text{ m}\big{)}\), the simulation time is 40 s, \(T=1\), the initial position of the target is \(\big{(}x_{0}\quad y_{0}\big{)}=\big{(}100\text{ m}\quad 200\text{ m}\big{)}\) and the initial velocity is \(\big{(}\dot{x}_{0}\quad\dot{y}_{0}\big{)}=\big{(}2\text{ m/s}\quad 20\text{ m/s}\big{)}\). The initial state and covariance matrix in the simulation are set to \(\hat{\mathbf{x}}_{0}^{+}=\big{(}100\text{ m}\quad 2\text{ m/s}\quad 200\text{ m}\big{)}^{ \text{T}}\) and \(\mathbf{P}_{0}^{+}=diag\big{[}0.01\quad 0.01\quad 0.01\big{]}\), respectively. The standard deviation of the measurement noise is 0.1 deg. The maximum iteration time \(N_{\text{max}}\) is set to 3. The Monte Carlo simulation is implemented 500 times, and the position accuracy is evaluated using the root mean square error (RMSE), defined as follows:  
\[RMSE_{\text{pos}}=\sqrt{\frac{1}{N}\sum_{i=1}^{N}\left[\left(x_{k}-\hat{x}_{i,k}^{+}\right)^{2}+\left(y_{k}-\hat{y}_{i,k}^{+}\right)^{2}\right]} \tag{36}\]  
where \(N\) is the execution number of Monte Carlo simulations, \(\big{(}x_{k}\quad y_{k}\big{)}\) is the real position of the target, and \(\left(\hat{x}_{i,k}^{+}\quad\hat{y}_{i,k}^{+}\right)\) is the estimated position at time \(k\) in the \(i\)th Monte Carlo simulation. The velocity RMSE is defined similarly. The simulation results are shown in Figure 1. The position RMSE and velocity RMSE of various filters are shown in Figure 1a,b, respectively, and the partial magnification is given to show the details of the curve. It can be seen from the figure that all the four filters have the ability to track the target trajectory, the tracking accuracy of the conventional CKF is significantly lower than that of the other filters, and the proposed IOSCKF achieves higher target tracking accuracy than the other filters. The average RMSE of the various filters is shown in Table 1; compared with the CKF, the estimated position accuracy of the SCKF is improved by 20.77%, indicating that the second-order Gauss-Laguerre rule used in the proposed modified spherical simplex-radial cubature rule has higher accuracy than the spherical-radial cubature rule. The accuracy of the OSCKF is 1% greater than that of the SCKF due to the orthogonal transformation of the cubature points reducing the high-order error components.