# Iterated Orthogonal Simplex Cubature Kalman Filter and Its Applications in Target Tracking
## 5 Simulations and Results
### Spacecraft Target Tracking Simulation
#### 5.2.2 Ground Radar Measurement Model  
The radar measurement model is built in the radar horizon coordinate system \((O-X_{h}Y_{h}Z_{h})\), and the orbit model is built in the radar horizon coordinate system \((O-XYZ)\), so it is necessary to use the WGS84 Earth fixed coordinate system \((O-X_{e}Y_{e}Z_{e})\) to achieve the conversion from \(O-XYZ\) to \(O-X_{h}Y_{h}Z_{h}\). Assuming that the orbital state of the spacecraft in \(O-XYZ\) is \( \begin{pmatrix}\mathbf{X}&\dot{\mathbf{X}}\end{pmatrix} ^{\text{T}}\), the orbital state in \(O-X_{e}Y_{e}Z_{e}\) is \( \begin{pmatrix}\mathbf{X}_{f}&\dot{\mathbf{X}}_{f}\end{pmatrix} ^{\text{T}}\), and the orbital state in \(O-X_{h}Y_{h}Z_{h}\) is \( \begin{pmatrix}\mathbf{X}_{h}&\dot{\mathbf{X}}_{h}\end{pmatrix} ^{\text{T}}\), the conversion of the orbital state is completed in two steps. Step 1: Transition from \(O-XYZ\) to \(O-X_{e}Y_{e}Z_{e}\)  
The conversion matrix is \(\mathbf{M}_{I}^{W}=M_{Pw}\mathbf{M}_{Ro}\mathbf{M}_{Nu}\mathbf{M}_{Pr}\), where \(\mathbf{M}_{Pr}\) is the precession matrix, \(\mathbf{M}_{Nu}\) is the nutation matrix, \(\mathbf{M}_{Ro}\) is the Earth rotation matrix, and \(\mathbf{M}_{Pw}\) is the pole shift matrix; then, we can obtain:  
\[\mathbf{X}_{f}=\mathbf{M}_{I}^{W}\mathbf{X} \tag{41}\]  
\[\dot{\mathbf{X}}_{f}=\mathbf{M}_{I}^{W}\dot{\mathbf{X}}+\dot{\mathbf{M}}_{I}^{W}\mathbf{X} \tag{42}\]  
In these formulas, the derivative of the transformation matrix \(\dot{\mathbf{M}}_{I}^{W}\) is shown as follows:  
\[\dot{\mathbf{M}}_{I}^{W}=\frac{d(\mathbf{M}_{Pw}\mathbf{M}_{Ro}\mathbf{M}_{Nu}\mathbf{M}_{Pr})}{ dt}\approx\mathbf{M}_{Pw}\frac{d(\mathbf{M}_{Ro})}{dt}\mathbf{M}_{Nu}\mathbf{M}_{Pr} \tag{43}\]  
where \(d(\mathbf{M}_{Ro})/dt\) is the derivative matrix of the Earth rotation matrix as follows:  
\[\frac{d(\mathbf{M}_{Ro})}{dt}= \begin{bmatrix}0&\omega_{e}&0\\ -\omega_{e}&0&0\\ 0&0&0\end{bmatrix} \cdot\mathbf{M}_{Ro} \tag{44}\]  
Equations (36) and (37) are written in matrix form as:  
\[ \begin{bmatrix}\mathbf{X}_{f}\\ \dot{\mathbf{X}}_{f}\end{bmatrix} = \begin{bmatrix}\mathbf{M}_{I}^{W}&\mathbf{0}\\ \dot{\mathbf{M}}_{I}^{W}&\mathbf{M}_{I}^{W}\end{bmatrix}\begin{bmatrix}\mathbf{X}\\ \dot{\mathbf{X}}\end{bmatrix}  \tag{45}\]  
Step 2: Transition from \(O-X_{e}Y_{e}Z_{e}\) to \(O-X_{h}Y_{h}Z_{h}\)  
The conversion matrix \(\mathbf{M}_{W}^{H}\) is:  
\[\mathbf{M}_{W}^{H}= \begin{bmatrix}-\sin\lambda&\cos\lambda&0\\ -\sin\varphi\cos\lambda&-\sin\varphi\sin\lambda&\cos\varphi\\ \cos\varphi\cos\lambda&\cos\varphi\sin\lambda&\sin\varphi\end{bmatrix}  \tag{46}\]where \(\lambda\) is the radar geocentric longitude and \(\varphi\) is the radar geocentric latitude, which can be converted to the geocentric coordinates of the radar \(\mathbf{X}_{c}=\left(x_{c}\quad y_{c}\quad z_{c}\right)^{\text{T}}\) in \(O-X_{c}Y_{c}Z_{c}\); so, we obtain:  
\[\mathbf{X}_{h}=\mathbf{M}_{W}^{H}\Big{(}\mathbf{X}_{f}-\mathbf{X}_{c}\Big{)} \tag{47}\]  
\[\dot{\mathbf{X}}_{h}=\mathbf{M}_{W}^{H}\dot{\mathbf{X}}_{f} \tag{48}\]  
This is written in matrix form as below:  
\[ \begin{bmatrix}\mathbf{X}_{h}\\ \dot{\mathbf{X}}_{h}\end{bmatrix} = \begin{bmatrix}\mathbf{M}_{W}^{H}&\mathbf{0}\\ \mathbf{0}&\mathbf{M}_{W}^{H}\end{bmatrix}\begin{bmatrix}\mathbf{X}_{f}\\ \dot{\mathbf{X}}_{f}\end{bmatrix} - \begin{bmatrix}\mathbf{M}_{W}^{H}\mathbf{X}_{c}\\ \mathbf{0}\end{bmatrix}  \tag{49}\]  
Substituting Equation (40) into Equation (44) yields:  
\[ \begin{bmatrix}\mathbf{X}_{h}\\ \dot{\mathbf{X}}_{h}\end{bmatrix} = \begin{bmatrix}\mathbf{M}_{W}^{H}&\mathbf{0}\\ \mathbf{0}&\mathbf{M}_{W}^{H}\end{bmatrix}\begin{bmatrix}\mathbf{M}_{f}^{W}&\mathbf{0}\\ \dot{\mathbf{M}}_{f}^{W}&\mathbf{M}_{f}^{W}\end{bmatrix}\begin{bmatrix}\mathbf{X}\\ \dot{\mathbf{X}}\end{bmatrix} - \begin{bmatrix}\mathbf{M}_{W}^{H}\mathbf{X}_{c}\\ \mathbf{0}\end{bmatrix}  \tag{50}\]  
Thus, the transformation of the orbital state from \(O-XYZ\) to \(O-X_{h}Y_{h}Z_{h}\) is achieved, as shown in Equation (45).