Iterated Orthogonal Simplex Cubature Kalman Filter and Its Applications in Target Tracking

Abstract

In order to increase a nonlinear system’s state estimate precision, an iterated orthogonal simplex cubature Kalman filter (IOSCKF) is presented in this study for target tracking. The Gaussian-weighted integral is decomposed into a spherical integral and a radial integral, which are approximated using the spherical simplex-radial rule and second-order Gauss–Laguerre quadrature rule, respectively, and result in the novel simplex cubature rule. To decrease the high-order error terms, cubature points with appropriate weights are taken from the cubature rule and processed using the provided orthogonal matrix. The structure supporting the nonlinear Kalman filter incorporates the altered points and weights and the calculation steps; from this, the updated time and measurement can be inferred. The Gauss–Newton iteration is employed repeatedly to adjust the measurement update until the termination condition is met and the IOSCKF is attained. The proposed algorithms are applied in target tracking, including CV target tracking and spacecraft orbit tracking, and the simulation results reveal that the IOSCKF can achieve higher accuracy compared to the CKF, SCKF, and OSCKF. In spacecraft orbit tracking simulation, compared with the SCKF, the position tracking accuracy and velocity tracking accuracy of the OSCKF are increased by 2.21% and 1.94%, respectively, which indicates that the orthogonal transformation can improve the tracking accuracy. Furthermore, compared with the OSCKF, the position tracking accuracy and velocity tracking accuracy of the IOSCKF are increased by 2.71% and 2.97%, respectively, which indicates that the tracking accuracy can be effectively improved by introducing iterative calculation into the measurement equation, thus verifying the effectiveness of the method presented in this paper.

1. Introduction

For several decades, target tracking has had substantial applications in many fields, such as navigation guidance, military application, and sensor networks [1,2]. Target tracking is a process whereby the position and velocity of a target’s motion are estimated using to discrete measurements with random noise output by sensors. Mathematical models of target tracking are generally described in different coordinate systems, where the process model is established in the Cartesian coordinate system and the measurements are generally implemented in the spherical coordinate system [3,4,5]. Thus, target tracking can be considered to be a typical nonlinear filtering problem [6,7]. The posterior probability density function (pdf) is very important in nonlinear filtering theory. Because pdf includes every piece of information on the state vector, it provides an integral filtering solution. Therefore, nonlinear filtering should focus on the calculation of posterior pdf, as the posterior mean and covariance of the state cannot be propagated using the nonlinear function directly.

In nonlinear systems, there contain two types of approaches to estimate the posterior pdf; one is optimal estimation, while the other is suboptimal estimation [8]. The former mainly includes a particle filter [9] and makes no assumptions on the pdf, which leads to a large amount of computation and is not suitable for applications in engineering. For the latter method, the posterior pdf is assumed to have a Gaussian distribution, and the typical representation, the nonlinear Kalman filter, is primarily studied in practical applications [10].

In a nonlinear Kalman filter, the recursive estimation of the state vector is divided into two processes consisting of a time update and a measurement update, and a unified filtering calculation framework is given. The key problem in the filtering framework is in the approximation of the Gaussian-weighted integral of multi-dimensional nonlinear function [8], and two approximate approaches, including the approximation of the nonlinear function and that of the Gaussian pdf, are mainly taken. The extended Kalman filter (EKF) [11] belongs to the former, and the basic idea of EKF is to use the first-order Taylor expansion to linearize the nonlinear system equations, and then, the conventional Kalman filter is used to estimate the state [12]. It is required to calculate the Jacobian matrix of the system, which demands that the model be smooth and differentiable. The EKF filtering accuracy is reduced or even divergent for a highly nonlinear system, and an improved algorithm, named iterated EKF (IEKF) [13], is proposed. The latest measurements are fully utilized through iterative calculation in the measurement update of IEKF to improve the estimation accuracy; however, the inherent disadvantage of EKF is still present in IEKF, that is, the differential operation of the Jacobian matrix. In general, the approximation of pdf is easier than that of the nonlinear function, so the approximation is performed using the deterministic sampling points in the latter method, and results in the unscented Kalman filter (UKF) [14], Gauss–Hermite–Kalman filter (GHKF) [15], sparse-grid Kalman filter (SGKF) [16], and cubature Kalman filter (CKF) [17,18]. These filters are all derivative-free filters, that is, there is no need to calculate the Jacobian matrix to linearize the nonlinear equation [19]. Specifically, a set of sigma points is used to approximate the posterior mean and covariance in UKF, and Chang [20] proposes using the orthogonal transformation of the sigma points for improving the approximation accuracy without increasing the computational complexity, thus reducing the high-order error term. However, there exist three adjustable parameters in UKF, and the inappropriate choice of parameters may reduce the filtering accuracy and the numerical stability. The computation amount of GHKF increases exponentially with the system state, which may cause the curse of dimensionality [16]. In order to reduce the computational complexity, the sparse-grid integral rule can be utilized to derive the SGKF, and the number of integral points is increased polynomially [17]. However, the CKF is widely used in engineering [21,22,23,24]. The CKF helps the intractable Gaussian-weighted integral to decompose and is composed of a spherical integral and a radial integral, and a set of cubature points with equal weights are deduced [25]. It can achieve more filtering accuracy and numerical stability compared with UKF. Similar to the IEKF, the principle of iteration can also be expanded to these filters’ measurement update processes [26,27,28,29].

Based on the CKF, Jia [30] puts forward the high-degree cubature Kalman filter (HDCKF) by using arbitrary degree symmetric spherical rule and moment matching method, which achieve higher accuracy than the CKF. And then, Wang [31] employs a transformation group of regular simplexs to approximate the spherical integral, and proposes the spherical simplex-radial cubature Kalman filter (SSRCKF). Compared with the HDCKF, the SSRCKF has more accurate performance, whereas the moment matching method cannot guarantee optimality when it is used in the abovementioned approaches’ radial integrals. Furthermore, Singh [32] uses the high-order Gauss–Laguerre quadrature rule instead of the moment matching method to calculate the radial integral, and proposes the high-degree cubature quadrature Kalman filter (HDCQKF), which could improve the radial integral accuracy effectively, so as to enhance the filtering accuracy. The advantage of the high-degree cubature Kalman filter is that it can increase the filtering accuracy, although it will increase the computational complexity and reduce the numerical stability for the high-dimensional system, and thus limit the application of filters.

To further improve the nonlinear filtering accuracy without using the high-degree cubature rule, a new iterated orthogonal simplex cubature Kalman filter (IOSCKF) is proposed in this paper. The way to calculate the intractable integral is to combine the spherical simplex rule with the second-order Gauss–Laguerre quadrature rule. By introducing the orthogonal matrix to reduce the higher-order moments of the Taylor series expansion which appear during the approximation of the mean and covariance, the results in the novel cubature points with corresponding weights are improved. The time update is the method used to reduce the computational complexity for the linear process equation. It is equivalent to a simplification of the time update of the standard linear Kalman filter. The posterior state and covariance matrix, in the measurement update, perform Gaussian Newton iteration until the result is optimal. On target tracking problems, the IOSCKF’s performance was tested, and the simulation results verify the effectiveness of the proposed algorithm.

The remainder of this paper is organized as follows: a review of Gaussian approximation filter is described in Section 2, the improved spherical simplex-radial cubature rule is deduced in Section 3, the iterated orthogonal simplex cubature Kalman filter is derived in Section 4, the target tracking simulations and results are presented in Section 5, and the conclusions are drawn in Section 6.

2. Brief Review of Gaussian Approximation Filter

The following discrete nonlinear system model is defined:

$$\left\{\begin{array}{l}{\mathit{x}}_k=\mathit{f}\left({\mathit{x}}_{k-1}\right)+{\mathit{w}}_{k-1}\\ {\mathit{z}}_k=\mathit{h}\left({\mathit{x}}_k\right)+{\mathit{v}}_k\end{array}\right.$$

(1)

where k is the time step, ${\mathit{x}}_k\in {\mathbf{R}}^n$ denotes the state vector at time step k, ${\mathit{z}}_k\in {\mathbf{R}}^p$ represents the measurement vector at time step k, and $\mathit{f}\left(\cdot \right)$ and $\mathit{h}\left(\cdot \right)$ are the known nonlinear state function and measurement function, respectively. The process noise ${\mathit{w}}_k$ and measurement noise ${\mathit{v}}_k$ are uncorrelated zero-mean Gaussian white noise, with the covariance matrices being ${\mathit{Q}}_k$ and ${\mathit{R}}_k$, respectively.

The key problem in the nonlinear Kalman filter is calculating the multi-dimensional vector integral of the form “nonlinear function × Gaussian pdf” [8], that is, calculating the integral $I_N={\displaystyle {\int }_{{\mathbf{R}}^n}\mathit{g}\left(\mathit{x}\right)N\left(\mathit{x};\overline{\mathit{x}},{\mathit{P}}_x\right)d\mathit{x}}$ in the Cartesian coordinate system, where $\mathit{g}\left(\mathit{x}\right)$ denotes the arbitrary nonlinear function, and $N\left(\mathit{x};\overline{\mathit{x}},{\mathit{P}}_x\right)$ represents the Gaussian pdf, with the mean and covariance being $\overline{\mathit{x}}$ and ${\mathit{P}}_x$, respectively.

This Gaussian-weighted integral $I_N$ can be approximated using various quadrature rules, and the integral with respect to $N(\mathit{x};\mathbf{0},\mathit{I})$ can be approximated by the following quadrature rule:

$$I_N={\displaystyle {\int }_{{\mathbf{R}}^n}\mathit{g}\left(\mathit{x}\right)N\left(\mathit{x};\mathbf{0},\mathit{I}\right)d\mathit{x}}\approx {\displaystyle \sum _{i=1}^{N_p}{\omega }_i\mathit{g}\left({\sigma }_i\right)}$$

(2)

where I is the identity matrix, $N_p$ denotes the total number of points, and ${\sigma }_i$ and ${\omega }_i$ represent the quadrature points and weights, respectively.

The integral with respect to the more general Gaussian distribution $N\left(\mathit{x};\overline{\mathit{x}},{\mathit{P}}_x\right)$ can be obtained using the equation below:

$$\begin{array}{ll}{I}_N& ={\displaystyle {\int }_{{\mathbf{R}}^n}\mathit{g}\left(\mathit{x}\right)N\left(\mathit{x};\overline{\mathit{x}},{\mathit{P}}_x\right)d\mathit{x}}={\displaystyle {\int }_{{\mathbf{R}}^n}\mathit{g}\left(\overline{\mathit{x}}+\sqrt{{\mathit{P}}_x}\mathit{x}\right)N\left(\mathit{x};\mathbf{0},\mathit{I}\right)d\mathit{x}}\\ & \approx {\displaystyle \sum _{i=1}^{N_p}{\omega }_i\mathit{g}\left(\overline{\mathit{x}}+\sqrt{{\mathit{P}}_x}{\sigma }_i\right)}\end{array}$$

(3)

where $\sqrt{{\mathit{P}}_x}$ denotes the square root of ${\mathit{P}}_x$, which can be solved by the Cholesky decomposition, and ${\overline{\mathit{x}}}^{\left(i\right)}=\overline{\mathit{x}}+\sqrt{{\mathit{P}}_x}{\sigma }_i$ represents the integral point.

The Gaussian approximation filter, which is deduced using the numerical integration method in Equation (3), is summarized below [30].

Time Update:

The predicted state ${\widehat{\mathit{x}}}_k^{-}$ and error covariance ${\mathit{P}}_k^{-}$ are calculated, respectively, as follows:

$${\widehat{\mathit{x}}}_k^{-}={\displaystyle \sum _{i=1}^{N_p}{\omega }_i\mathit{f}\left({\widehat{\mathit{x}}}_{k-1}^{\left(i\right)}\right)}$$

(4)

$${\mathit{P}}_k^{-}={\displaystyle \sum _{i=1}^{N_p}{\omega }_i\left(\mathit{f}\left({\widehat{\mathit{x}}}_{k-1}^{\left(i\right)}\right)-{\widehat{\mathit{x}}}_k^{-}\right){\left(\mathit{f}\left({\widehat{\mathit{x}}}_{k-1}^{\left(i\right)}\right)-{\widehat{\mathit{x}}}_k^{-}\right)}^{\mathrm{T}}}+{\mathit{Q}}_{k-1}$$

(5)

where ${\widehat{\mathit{x}}}_{k-1}^{\left(i\right)}={\widehat{\mathit{x}}}_{k-1}^{+}+\sqrt{{\mathit{P}}_{k-1}^{+}}{\mathit{\sigma }}_i$ stands for the integral points generated using the numerical rule.

Measurement Update:

The updated state ${\widehat{\mathit{x}}}_k^{+}$ and corresponding error covariance ${\mathit{P}}_k^{+}$ are estimated, respectively, as follows:

$${\widehat{\mathit{x}}}_k^{+}={\widehat{\mathit{x}}}_k^{-}+{\mathit{K}}_k\left({\mathit{z}}_k-{\widehat{\mathit{z}}}_k\right)$$

(6)

$${\mathit{P}}_k^{+}={\mathit{P}}_k^{-}+{\mathit{K}}_k{\mathit{P}}_z{\mathit{K}}_k^{\mathrm{T}}$$

(7)

 where ${\mathit{K}}_k={\mathit{P}}_{xz}{\mathit{P}}_z^{-1}$ denotes the Kalman filtering gain, ${\widehat{\mathit{z}}}_k={\displaystyle \sum _{i=1}^{N_p}{\omega }_i\mathit{h}\left({\widehat{\mathit{x}}}_k^{\left(i\right)}\right)}$ represents the predicted measurement, ${\widehat{\mathit{x}}}_k^{\left(i\right)}={\widehat{\mathit{x}}}_k^{-}+\sqrt{{\mathit{P}}_k^{-}}{\mathit{\sigma }}_i$ denotes the integral points generated by the numerical rule, ${\mathit{P}}_z={\displaystyle \sum _{i=1}^{N_p}{\omega }_i\left(\mathit{h}\left({\widehat{\mathit{x}}}_k^{\left(i\right)}\right)-{\widehat{\mathit{z}}}_k\right){\left(\mathit{h}\left({\widehat{\mathit{x}}}_k^{\left(i\right)}\right)-{\widehat{\mathit{z}}}_k\right)}^{\mathrm{T}}}+{\mathit{R}}_k$ denotes the innovation covariance matrix, and ${\mathit{P}}_{xz}={\displaystyle \sum _{i=1}^{N_p}{\omega }_i\left({\widehat{\mathit{x}}}_k^{\left(i\right)}-{\widehat{\mathit{x}}}_k^{-}\right){\left(\mathit{h}\left({\widehat{\mathit{x}}}_k^{\left(i\right)}\right)-{\widehat{\mathit{z}}}_k\right)}^{\mathrm{T}}}$ represents the cross-covariance matrix.

3. The Improved Spherical Simplex-Radial Cubature Rule

In this section, an improved spherical simplex-radial cubature rule is proven to be more accurate than the traditional conventional spherical-radial rule. Before introducing the improved cubature rule, the definition of the dth-degree rule is given first for the integral

Definition 1

([30]).

$${\displaystyle {\int }_{{\mathbf{R}}^n}\mathit{g}\left(\mathit{x}\right){\omega }_g\left(\mathit{x}\right)d\mathit{x}}\approx {\displaystyle \sum _i{\omega }_i\mathit{g}\left({\gamma }_i\right)}$$

(8)

where $\mathit{x}={\left(\begin{array}{cccc}{x}_1& x_2& \cdots & x_n\end{array}\right)}^{\mathrm{T}}\in {\mathbf{R}}^n$ and ${\omega }_g\left(\mathit{x}\right)$ is a given weighing function. Equation (8) is a dth-degree rule if it is accurate for $\mathit{g}\left(\mathit{x}\right)$, whose components are linear combinations of monomials $x_1^{{\alpha }_1}{x}_2^{{\alpha }_2}\cdots x_n^{{\alpha }_n}$  with a total degree up to d. ${\alpha }_1$  , ${\alpha }_2$  , $\cdots$  , ${\alpha }_n$ are nonnegative integers and ${\alpha }_1+{\alpha }_2+\cdots {\alpha }_n\le d$, and there is at least one monomial of degree $d+1$  for which Equation (8) is not exact.

Specifically, an intractable integral of the form $I_g={\displaystyle {\int }_{{\mathbf{R}}^n}\mathit{g}\left(\mathit{x}\right)e^{-x^{\mathrm{T}}x}d\mathit{x}}$ is considered first in the cubature rule. Let $\mathit{x}=r\mathit{y}$ with ${\mathit{y}}^{\mathrm{T}}\mathit{y}=1$, where $\mathit{y}$ means the direction vector and the radius $r\ge 0$. As a consequence, ${\mathit{x}}^{\mathrm{T}}\mathit{x}=r^2$; then, the integral $I_g$ can be written anew as in a spherical-radial coordinate system as follows [12]:

$$I_g={\displaystyle {\int }_0^{\infty }{\displaystyle {\int }_{U_n}\mathit{g}\left(r\mathit{y}\right)r^{n-1}{e}^{-r^2}d\sigma \left(\mathit{y}\right)dr}}$$

(9)

where $U_n$ is the sphere surface defined by $U_n=\left\{\mathit{y}\in {\mathbf{R}}^n:{\mathit{y}}^{\mathrm{T}}\mathit{y}=1\right\}$, and $\sigma \left(\cdot \right)$ is the area element on $U_n$. The integral $I_g$ is decomposed into the spherical integral $S\left(r\right)={\displaystyle {\int }_{U_n}\mathit{g}\left(r\mathit{y}\right)d\sigma \left(\mathit{y}\right)}$ and radial integral $R={\displaystyle {\int }_0^{\infty }S\left(r\right)r^{n-1}{e}^{-r^2}dr}$, respectively.

Because of the difficulty in achieving the analytical solutions, the approximate solution method of $I_g$ requires numerical integration, which should be approximated using the numerical integration method. From the third-degree spherical-radial cubature rule proposed in [17], we obtain

$$S\left(r\right)\approx {\displaystyle \sum _{i=1}^{N_s}{\omega }_{s,i}\mathit{g}\left(r{\mathit{y}}_i\right)}$$

(10)

$$R\approx {\displaystyle \sum _{j=1}^{N_r}{\omega }_{r,j}S\left(r_j\right)}$$

(11)

and $I_g$ is approximated using the following formula:

$$I_g={\displaystyle {\int }_0^{\infty }{r}^{n-1}{e}^{-r^2}dr}{\displaystyle {\int }_{U_n}\mathit{g}\left(r\mathit{y}\right)d\sigma \left(\mathit{y}\right)}\approx {\displaystyle \sum _{j=1}^{N_r}{\displaystyle \sum _{i=1}^{N_s}{\omega }_{r,j}{\omega }_{s,i}\mathit{g}\left(r_j{\mathit{y}}_i\right)}}$$

(12)

where ${\mathit{y}}_i$ and ${\omega }_{s,i}$ denote the points and weights to calculate the spherical integral, and the number of points is $N_s$. $r_j$ and ${\omega }_{r,j}$ represent the points and weights to calculate the radial integral, and the number of points is $N_r$. The total number of points is $N_r{N}_s$ if $r_j\ne 0$, or $\left(N_r-1\right)N_s+1$ if one of $r_j$ is zero.

3.1. Spherical Simplex Rule

The proposed third-degree spherical rule and spherical simplex rule to calculate Equation (10) are listed below, respectively.

The third-degree spherical rule is

$$S_s\left(r\right)\approx {\displaystyle \sum _{i=1}^{N_s}{\omega }_{s,i}\mathit{g}\left(r{\mathit{y}}_i\right)}=\frac{A_n}{2n}{\displaystyle \sum _{i=1}^n\left[\mathit{g}\left(r{\mathit{e}}_i\right)+\mathit{g}\left(-r{\mathit{e}}_i\right)\right]}$$

(13)

where $A_n=2\sqrt{{\pi }^n}/\Gamma \left(n/2\right)$ denotes the surface area of the unit sphere, and $\Gamma \left(z\right)={\displaystyle {\int }_0^{\infty }{e}^{-t}{t}^{z-1}dt}$ is the Gamma function. The vector ${\mathit{e}}_i$ represents the unit column vector with the ith element being 1. The number of cubature points needed is $N_s=2n$.

The third-degree spherical simplex rule [31] consisting of $N_s=2\left(n+1\right)$ points is

$$S_{ss}\left(r\right)=\frac{A_n}{2\left(n+1\right)}{\displaystyle \sum _{i=1}^{n+1}\left[\mathit{g}\left(r{\mathit{a}}_i\right)+\mathit{g}\left(-r{\mathit{a}}_i\right)\right]}$$

(14)

where ${\mathit{a}}_j={\left(\begin{array}{cccc}{a}_{j,1}& a_{j,2}& \cdots & a_{j,n}\end{array}\right)}^{\mathrm{T}},j=1,2,\cdots ,n+1$ denote the vertices of the n-dimensional simplex, and the elements are defined as follows.

$$a_{j,i}=\left\{\begin{array}{l}-\sqrt{\frac{n+1}{n\left(n-i+2\right)\left(n-i+1\right)}},i<j\\ \sqrt{\frac{\left(n+1\right)\left(n-j+1\right)}{n\left(n-j+2\right)}},i=j\\ 0,i>j\end{array}\right.$$

(15)

Although the aforementioned two spherical rules share the same accuracy of the third degree, through extensive simulations, it has been proven that spherical simplex rules have higher accuracy than spherical rules [31], and that is the reason the spherical simplex rule is chosen to approximate $S\left(r\right)$ in this paper. For more details, please refer to the related references.

3.2. Radial Rule

Then, the calculation of the radial integral $R$ is considered. Two methods, including the moment matching method and Gauss–Laguerre quadrature rule, may be used. To construct the (2m + 1)th-degree radial rule, the two methods lead to identical points and weights when m is odd and $\left(m+1\right)/2$ points are used. However, they may be different when m is even and $\left(m/2+1\right)$ points are used [30]. In the CKF and SSRCKF, the radial integral is calculated using the former one, and results in the following third-degree radial rule with only one point (that is, m = 1, $N_r=1$).

$$R=\frac{1}{2}\Gamma \left(\frac{n}{2}\right)S\left(\sqrt{\frac{n}{2}}\right)$$

(16)

In this paper, the second-order Gauss–Laguerre quadrature rule is adopted to approximate the radial integral. This method improves the accuracy of radial integration.

For radial integral $R={\displaystyle {\int }_0^{\infty }S\left(r\right)r^{n-1}{e}^{-r^2}dr}$, let $r^2=t$ to obtain $r=\sqrt{t}$, which takes the place of the radial integral, and for $R=\frac{1}{2}{\displaystyle {\int }_0^{\infty }S\left(\sqrt{t}\right)t^{\frac{n-2}{2}}{e}^{-t}dt}$, let $q\left(t\right)=S\left(\sqrt{t}\right)$, and $\beta =\frac{n}{2}-1$. Then, $R$ is transformed into $R=\frac{1}{2}{\displaystyle {\int }_0^{\infty }q\left(t\right)t^{\beta }{e}^{-t}dt}$, and this form can be handily approximated using the Gauss–Laguerre quadrature [32] rule as follows:

$$R=\frac{1}{2}{\displaystyle {\int }_0^{\infty }q\left(t\right)t^{\beta }{e}^{-t}dt}\approx \frac{1}{2}{\displaystyle \sum _{j=1}^m{A}_{j}q\left(t_j\right)}$$

(17)

where $t_j$ denotes the quadrature points, with $A_j$ being the corresponding weights.

The approximation accuracy depends on the order m. The m-order Chebyshev–Laguerre polynomial’s solutions are the quadrature points, as shown in the following:

$$L_m^{\beta }\left(t\right)={\left(-1\right)}^m{t}^{-\beta }{e}^t\frac{d^m}{d{t}^m}\left(t^{\beta +m}{e}^{-t}\right)=0$$

(18)

and the weights are calculated using the following equation.

$$A_j=\frac{m!\Gamma \left(\beta +m+1\right)}{t_j{\left[{\dot{L}}_m^{\beta }\left(t_j\right)\right]}^2}$$

(19)

On account of m being set at 2, the solutions are obtained are $t_1=n/2+1+\sqrt{n/2+1}$, $t_2=n/2+1-\sqrt{n/2+1}$, $A_1=n\Gamma \left(n/2\right)/\left(2n+4+2\sqrt{2n+4}\right)$, and $A_2=n\Gamma \left(n/2\right)/\left(2n+4-2\sqrt{2n+4}\right)$, which are substituted into Equation (17) to deduce the following radial rule.

$$R=\frac{n\Gamma \left(\frac{n}{2}\right)}{4n+8+4\sqrt{2n+4}}S\left(\sqrt{\frac{n}{2}+1+\sqrt{\frac{n}{2}+1}}\right)+\frac{n\Gamma \left(\frac{n}{2}\right)}{4n+8-4\sqrt{2n+4}}S\left(\sqrt{\frac{n}{2}+1-\sqrt{\frac{n}{2}+1}}\right)$$

(20)

From Equation (20) we see that the number of points is $N_r=2$, which leads to the radial rule to achieve the fifth-degree accuracy. Compared with third-degree radial rule used in the CKF and SSRCKF Equation (16), the fifth-degree accuracy is more accurate. It is noted that the two points can be solved using the moment matching method as well, with one of the points set to zero. However, it is pointed out that the two points generated by the Gauss–Laguerre quadrature rule have higher radial accuracy than those generated using the moment matching method [30].

3.3. Spherical Simplex-Radial Cubature Rule

By substituting Equation (14) into Equation (20), we obtain the following improved spherical simplex-radial cubature rule which is composed of $N_p=N_r{N}_s=4(n+1)$ points.

$$\begin{array}{ll}{I}_N& ={\displaystyle {\int }_{{\mathbf{R}}^n}\mathit{g}\left(x\right)N\left(\mathit{x};\overline{\mathit{x}},{\mathit{P}}_x\right)d\mathit{x}}\\ & =\frac{n}{4\left(n+1\right)\left(n+2+\sqrt{2n+4}\right)}{\displaystyle \sum _{i=1}^{n+1}\left[\mathit{g}\left(\overline{\mathit{x}}+\sqrt{\left(n+2+\sqrt{2n+4}\right){\mathit{P}}_x}{\mathit{a}}_i\right)+\mathit{g}\left(\overline{\mathit{x}}-\sqrt{\left(n+2+\sqrt{2n+4}\right){\mathit{P}}_x}{\mathit{a}}_i\right)\right]}+\\ & \frac{n}{4\left(n+1\right)\left(n+2-\sqrt{2n+4}\right)}{\displaystyle \sum _{i=1}^{n+1}\left[\mathit{g}\left(\overline{\mathit{x}}+\sqrt{\left(n+2-\sqrt{2n+4}\right){\mathit{P}}_x}{\mathit{a}}_i\right)+\mathit{g}\left(\overline{\mathit{x}}-\sqrt{\left(n+2-\sqrt{2n+4}\right){\mathit{P}}_x}{\mathit{a}}_i\right)\right]}\end{array}$$

(21)

If we define matrix $\mathit{a}=\left[\begin{array}{cccc}{\mathit{a}}_1& {\mathit{a}}_2& \cdots & {\mathit{a}}_{n+1}\end{array}\right]$, and the calculation methods of the cubature points and weights are extracted from cubature rule (21), we obtain the following.

$${\overline{\mathit{x}}}^{\left(i\right)}=\left\{\begin{array}{l}\overline{\mathit{x}}+\sqrt{\left(n+2+\sqrt{2n+4}\right){\mathit{P}}_x}{\left[\begin{array}{cc}\mathit{a}& -\mathit{a}\end{array}\right]}_i,i=1,2,\cdots ,2n+2\\ \overline{\mathit{x}}+\sqrt{\left(n+2-\sqrt{2n+4}\right){\mathit{P}}_x}{\left[\begin{array}{cc}\mathit{a}& -\mathit{a}\end{array}\right]}_i,i=2n+3,\cdots ,4n+4\end{array}\right.$$

(22)

$${\omega }_i=\left\{\begin{array}{l}\frac{n}{4\left(n+1\right)\left(n+2+\sqrt{2n+4}\right)},i=1,2,\cdots ,2n+2\\ \frac{n}{4\left(n+1\right)\left(n+2-\sqrt{2n+4}\right)},i=2n+3,\cdots ,4n+4\end{array}\right.$$

(23)

The order of the quadrature rule determines the accuracy of the cubature rule. The relationship of the number of quadrature points and the accuracy is proportional [8,32]. The radial rule proposed in Section 3.2 uses two points to achieve fifth-degree accuracy. In contrast to the third degree adopted by the CKF and SSRCKF, when only one point is used, fifth-degree is more accurate. And the spherical simplex rule performs better than the spherical rule; therefore, the improved spherical simplex-radial cubature rule, which is shown in Equation (21), can achieve higher accuracy than the conventional CKF and SSRCKF.

4. Iterated Orthogonal Simplex Cubature Kalman Filter

4.1. Orthogonal Transformation of Cubature Points

It is suggested from the invariant theory and the orthogonal transformation property that the transformed points $\mathit{O}\times \left[\begin{array}{cc}\mathit{a}& -\mathit{a}\end{array}\right]$, with weights shown in Equation (23), remain to constitute a numerical cubature rule if $\mathit{O}$ is an $n\times n$ orthogonal matrix. The orthogonally transformed points are able to reduce the higher-order terms of the Taylor series expansion that appeared while approximating the mean and covariance [20], and it is observed that more accurate estimation is obtained with the transformed points compared to the ordinary cubature points, which lead to the approximation accuracy of the cubature rule becoming further enhanced. In other words, the new set of cubature points can be modified as follows:

$${\overline{\mathit{x}}}^{\left(i\right)}=\left\{\begin{array}{l}\overline{\mathit{x}}+\sqrt{\left(n+2+\sqrt{2n+4}\right){\mathit{P}}_x}{\mathit{\gamma }}_i,i=1,2,\cdots ,2n+2\\ \overline{\mathit{x}}+\sqrt{\left(n+2-\sqrt{2n+4}\right){\mathit{P}}_x}{\mathit{\gamma }}_i,i=2n+3,\cdots ,4n+4\end{array}\right.$$

(24)

where $\mathit{\gamma }=\left[\begin{array}{cccc}{\mathit{\gamma }}_1& {\mathit{\gamma }}_2& \cdots & {\mathit{\gamma }}_{2n+2}\end{array}\right]=\mathit{O}\times \left[\begin{array}{cc}\mathit{a}& -\mathit{a}\end{array}\right]$, and the orthogonal matrix $\mathit{O}$ can be designed using different methods. As an effective design method, the $n\times n$ matrix $\mathit{O}$ is constructed in this paper as $\mathit{O}=\left[\begin{array}{cccc}{\mathit{O}}_1& {\mathit{O}}_2& \cdots & {\mathit{O}}_n\end{array}\right]$, and ${\mathit{O}}_i={(O_{i,1}{O}_{i,2}\dots O_{i,n})}^T$, whose elements are defined as follows:

$$\left\{\begin{array}{c}{O}_{i,2r-1}=\sqrt{\frac{2}{n}}\cos \left[\frac{\left(2r-1\right)i\pi }{n}\right]\\ O_{i,2r}=\sqrt{\frac{2}{n}}\sin \left[\frac{\left(2r-1\right)i\pi }{n}\right]\end{array}\right.,r=1,2,\cdots ,\max \left(\left.p\in \mathbf{Z}\right|p\le \frac{n}{2}\right)$$

(25)

where $O_{i,n}={\left(-1\right)}^i/\sqrt{n}$ if $n$ is odd.

The information of high-order terms can be reduced to a low level using the orthogonal matrix, which means the non-local sampling problem can be significantly reduced with a rare computational complexity increase.

4.2. Iterated Measurement Update Process

The prior state ${\widehat{\mathit{x}}}_k^{-}$ and covariance matrix ${\mathit{P}}_k^{-}$ are used in the measurement update process to calculate the posterior state ${\widehat{\mathit{x}}}_k^{+}$ and covariance matrix ${\mathit{P}}_k^{+}$, which are revised using the latest measurement ${\mathit{z}}_k$ via Kalman gain ${\mathit{K}}_k$. This means an approximation accuracy of ${\widehat{\mathit{x}}}_k^{+}$ to the real state is bound to be better than ${\widehat{\mathit{x}}}_k^{-}$, and the filtering accuracy will be improved if we can use ${\widehat{\mathit{x}}}_k^{+}$ and ${\mathit{P}}_k^{+}$ directly in the measurement update. Therefore, to iteratively make use of the measurement information, the Gauss–Newton iteration is introduced into the updating process of the mean and covariance of the state. Under the assumption of Gaussian variable independence, there exist ${\mathit{x}}_k:N\left({\widehat{\mathit{x}}}_k^{-},{\mathit{P}}_k^{-}\right)$ and ${\mathit{z}}_k:N\left(\mathit{h}\left({\mathit{x}}_k\right),{\mathit{R}}_k\right)$, and the likelihood function of ${\mathit{x}}_k$ and ${\mathit{z}}_k$ is defined as follows [33].

$$p\left(\left.{\mathit{x}}_k\right|{\mathit{z}}_k\right)=C\exp \left\{-\frac{1}{2}\left[{\left({\mathit{x}}_k-{\widehat{\mathit{x}}}_k^{-}\right)}^{\mathrm{T}}{\left({\mathit{P}}_k^{-}\right)}^{-1}\left({\mathit{x}}_k-{\widehat{\mathit{x}}}_k^{-}\right)+{\left({\mathit{z}}_k-\mathit{h}\left({\mathit{x}}_k\right)\right)}^{\mathrm{T}}{\mathit{R}}_k^{-1}\left({\mathit{z}}_k-\mathit{h}\left({\mathit{x}}_k\right)\right)\right]\right\}$$

(26)

where $C$ is a constant, and the cost function is defined as follows.

$$J\left({\mathit{x}}_k\right)={\left({\mathit{x}}_k-{\widehat{\mathit{x}}}_k^{-}\right)}^{\mathrm{T}}{\left({\mathit{P}}_k^{-}\right)}^{-1}\left({\mathit{x}}_k-{\widehat{\mathit{x}}}_k^{-}\right)+{\left[{\mathit{z}}_k-\mathit{h}\left({\mathit{x}}_k\right)\right]}^{\mathrm{T}}{\mathit{R}}_k^{-1}\left[{\mathit{z}}_k-\mathit{h}\left({\mathit{x}}_k\right)\right]$$

(27)

The maximum likelihood estimate of Equation (26) is equivalent to the minimum value of the cost function (27), which is usually solved by the nonlinear iteration method. The linearized measurement equation and the Gauss–Newton iteration are used to achieve the iterative formula for solving the minimum value of $J_{x_k}$ as follows:

$${\widehat{\mathit{x}}}_k^{i+1}={\widehat{\mathit{x}}}_k^{-}+{\mathit{P}}_k^{-}{\left({\mathit{H}}_k^i\right)}^{\mathrm{T}}{\left[{\mathit{H}}_k^i{\mathit{P}}_k^{-}{\left({\mathit{H}}_k^i\right)}^{\mathrm{T}}+{\mathit{R}}_k\right]}^{-1}\left[{\mathit{z}}_k-\mathit{h}\left({\widehat{\mathit{x}}}_k^i\right)-{\mathit{H}}_k^i\left({\widehat{\mathit{x}}}_k^{-}-{\widehat{\mathit{x}}}_k^i\right)\right]$$

(28)

where ${\mathit{H}}_k^i={\left.\partial \mathit{h}\left({\mathit{x}}_k\right)/\partial {\mathit{x}}_k\right|}_{{\mathit{x}}_k={\widehat{\mathit{x}}}_k^i}$ denotes the Jacobian matrix, and i represents the iteration index.

The analogous representation that ${\mathit{P}}_{xz}={\mathit{P}}_k^{-}{\mathit{H}}_k^{\mathrm{T}}$ and ${\mathit{P}}_z={\mathit{H}}_k{\mathit{P}}_k^{-}{\mathit{H}}_k^{\mathrm{T}}+{\mathit{R}}_k$ is obtained using the statistical linear error approach, which is used instead of the Jacobian matrix to reduce the computational complexity and avoid the linearization error caused by the Taylor first-order expansion. Furthermore, the enhanced Gauss–Newton iteration is displayed as follows.

$${\widehat{\mathit{x}}}_k^{i+1}={\widehat{\mathit{x}}}_k^{-}+{\mathit{P}}_{xz}^i{\left({\mathit{P}}_z^i\right)}^{-1}\left[{\mathit{z}}_k-\mathit{h}\left({\widehat{\mathit{x}}}_k^i\right)-{\left({\mathit{P}}_{xz}^i\right)}^{\mathrm{T}}{\left({\mathit{P}}_k^{-}\right)}^{-1}\left({\widehat{\mathit{x}}}_k^{-}-{\widehat{\mathit{x}}}_k^i\right)\right]$$

(29)

4.3. 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}{\widehat{\mathit{x}}}_0^{+}=E\left({\mathit{x}}_0\right)\\ {\mathit{P}}_0^{+}=E\left[\left({\mathit{x}}_0-{\widehat{\mathit{x}}}_0^{+}\right){\left({\mathit{x}}_0-{\widehat{\mathit{x}}}_0^{+}\right)}^{\mathrm{T}}\right]\end{array}\right.$$

(30)

Step 2: Time Update

${\widehat{\mathit{x}}}_{k-1}^{+}$ and ${\mathit{P}}_{k-1}^{+}$ take the place of $\overline{\mathit{x}}$ and ${\mathit{P}}_x$ in Equation (24) to calculate the cubature points ${\widehat{\mathit{x}}}_{k-1}^{\left(i\right)}$, which are disseminated nonlinearly using $\mathit{f}\left(\cdot \right)$, and the prior state ${\widehat{\mathit{x}}}_k^{-}$ and covariance ${\mathit{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 ${\widehat{\mathit{x}}}_k^0={\widehat{\mathit{x}}}_k^{-}$ and covariance matrix ${\mathit{P}}_k^0={\mathit{P}}_k^{-}$ as the initial values of the iterated measurement update, and the estimated state and covariance matrix in the jth iteration express ${\widehat{\mathit{x}}}_k^j$ and ${\mathit{P}}_k^j$ ($j=1,2,\cdots ,N_{\max }$), respectively. The cubature points ${\widehat{\mathit{x}}}_k^{\left(i\right),j-1}$ are calculated using ${\widehat{\mathit{x}}}_k^{j-1}$ and ${\mathit{P}}_k^{j-1}$ instead of $\overline{\mathit{x}}$ and ${\mathit{P}}_x$ in Equation (24), and propagated nonlinearly using $\mathit{h}\left(\cdot \right)$. The predicted measurement ${\widehat{\mathit{z}}}_k^j$, the measurement covariance matrix ${\mathit{P}}_z^j$, and the cross covariance matrix ${\mathit{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 ${\mathit{K}}_k^j$ and covariance matrix ${\mathit{P}}_k^j$ are estimated to be equivalent to the Gaussian approximation filter, respectively, while the posterior state ${\widehat{\mathit{x}}}_k^j$ is calculated using the following iterated form.

$${\widehat{\mathit{x}}}_k^j={\widehat{\mathit{x}}}_k^{-}+{\mathit{K}}_k^j\left[{\mathit{z}}_k-\mathit{h}\left({\widehat{\mathit{x}}}_k^{j-1}\right)-{\left({\mathit{P}}_{xz}^j\right)}^{\mathrm{T}}{\left({\mathit{P}}_k^{-}\right)}^{-1}\left({\widehat{\mathit{x}}}_k^{-}-{\widehat{\mathit{x}}}_k^{j-1}\right)\right]$$

(31)

The above iteration format obtains a posterior estimate higher than the last one after each iteration step, and the system model h(·) is known and unchanged, so the whole iterative calculation process is convergent and stable.

The iteration termination condition is set as shown below.

$$‖{\widehat{\mathit{x}}}_k^j-{\widehat{\mathit{x}}}_k^{j-1}‖\le \epsilon \mathrm{or} j=N_{\max }$$

(32)

where $\epsilon$ and $N_{\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 ${\widehat{\mathit{x}}}_k^{+}={\widehat{\mathit{x}}}_k^N$ and ${\mathit{P}}_k^{+}={\mathit{P}}_k^N$, respectively. Note that the calculational burden of the ICKF and CKF is within the same order of magnitude if $N_{\max }$ is not too large, and in general, $N_{\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.

5. Simulations and Results

The proposed IOSCKF algorithm is applied in CV target tracking and spacecraft orbit tracking, respectively, in this section.

5.1. 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:

$${\mathit{X}}_k={\mathit{F}}_{CV}{\mathit{X}}_{k-1}+\mathit{G}{\mathit{w}}_{k-1}$$

(33)

where ${\mathit{X}}_k={\left(\begin{array}{cccc}{x}_k& {\dot{x}}_k& y_k& {\dot{y}}_k\end{array}\right)}^{\mathrm{T}}$ represents the position and velocity of the target at time index $k$, ${\mathit{w}}_k$ denotes the process noise, and ${\mathit{F}}_{CV}$ and $\mathit{G}$ denote the state transformation matrix and the noise-driven matrix, which are defined, respectively as follows:

$${\mathit{F}}_{CV}=\left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right], \mathit{G}=\left[\begin{array}{ll}{T}^2/2& 0\\ T& 0\\ 0& T^2/2\\ 0& T\end{array}\right]$$

(34)

where T is the sampling interval.

In the target tracking system, the bearings-only measurement equation is written as follows.

$$Z_k=\arctan \left(\frac{y_k-y_r}{x_k-x_r}\right)+v_k$$

(35)

where $Z_k$ is the radar measurement at time $k$, $\left(\begin{array}{cc}{x}_r& y_r\end{array}\right)$ 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 $\left(\begin{array}{cc}{x}_r& y_r\end{array}\right)=\left(\begin{array}{cc}200 \mathrm{m}& 300 \mathrm{m}\end{array}\right)$, the simulation time is 40 s, T = 1, the initial position of the target is $\left(\begin{array}{cc}{x}_0& y_0\end{array}\right)=\left(\begin{array}{cc}100 \mathrm{m}& 200 \mathrm{m}\end{array}\right)$ and the initial velocity is $\left(\begin{array}{cc}{\dot{x}}_0& {\dot{y}}_0\end{array}\right)=\left(\begin{array}{cc}2 \mathrm{m}/\mathrm{s}& 20 \mathrm{m}/\mathrm{s}\end{array}\right)$. The initial state and covariance matrix in the simulation are set to ${\widehat{\mathit{x}}}_0^{+}={\left(\begin{array}{cccc}100 \mathrm{m}& 2 \mathrm{m}/\mathrm{s}& 200 \mathrm{m}& 20 \mathrm{m}/\mathrm{s}\end{array}\right)}^{\mathrm{T}}$ and ${\mathit{P}}_0^{+}=diag\left[\begin{array}{cccc}0.01& 0.01& 0.01& 0.01\end{array}\right]$, respectively. The standard deviation of the measurement noise is 0.1 deg. The maximum iteration time $N_{\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:

$$RMS{E}_{pos}=\sqrt{\frac{1}{N}{\displaystyle \sum _{i=1}^N\left[{\left(x_k-{\widehat{x}}_{i,k}^{+}\right)}^2+{\left(y_k-{\widehat{y}}_{i,k}^{+}\right)}^2\right]}}$$

(36)

where N is the execution number of Monte Carlo simulations, $\left(\begin{array}{cc}{x}_k& y_k\end{array}\right)$ is the real position of the target, and $\left(\begin{array}{cc}{\widehat{x}}_{i,k}^{+}& {\widehat{y}}_{i,k}^{+}\end{array}\right)$ is the estimated position at time k in the ith 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. Average RMSE of various filters.

Filters	Position RMSE/m	Velocity RMSE/(m/s)
CKF	2.9379	0.1852
SCKF	2.3277	0.1643
OSCKF	2.3045	0.1622
IOSCKF	2.2929	0.1619

; 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. While the filtering accuracy is further enhanced by the repetitive calculation of the measurement update, the IOSCKF’s accuracy is 0.5% higher than that of the OSCKF. Through the analysis of the simulation results, the validity of the proposed method is verified.

5.2. Spacecraft Target Tracking Simulation

5.2.1. Spacecraft Orbit Dynamics Model

For spacecrafts operating in low Earth orbit, Earth’s J2 term non-spherical perturbation and atmospheric drag perturbation are the most important perturbations. In the J2000 geocentric inertial coordinate system (O-XYZ), considering the above two perturbations, the orbital dynamics model of the spacecraft is as follows:

$$\left\{\begin{array}{l}\dot{x}=v_x\\ \dot{y}=v_y\\ \dot{z}=v_z\\ {\dot{v}}_x=\frac{\mu x}{r^3}\left[J_2\left(\frac{R_e}{r}\right)\left(7.5\frac{z^2}{r^2}-1.5\right)-1\right]+f_{ax}+f_x\\ {\dot{v}}_y=\frac{\mu y}{r^3}\left[J_2\left(\frac{R_e}{r}\right)\left(7.5\frac{z^2}{r^2}-1.5\right)-1\right]+f_{ay}+f_y\\ {\dot{v}}_z=\frac{\mu z}{r^3}\left[J_2\left(\frac{R_e}{r}\right)\left(7.5\frac{z^2}{r^2}-4.5\right)-1\right]+f_{az}+f_z\\ r=\sqrt{x^2+y^2+z^2}\end{array}\right.$$

(37)

where $X={\left(\begin{array}{ccc}x& y& z\end{array}\right)}^{\mathrm{T}}$ is the position vector of the spacecraft in O-XYZ; $\dot{X}={\left(\begin{array}{ccc}{v}_x& v_y& v_z\end{array}\right)}^{\mathrm{T}}$ is the velocity vector of the spacecraft in O-XYZ; J₂ is the harmonic term coefficient; $\mu$ is the gravitational constant of the Earth; $R_e$ is the radius of the Earth; and ${\left(\begin{array}{ccc}{f}_x& f_y& f_z\end{array}\right)}^{\mathrm{T}}$ is the sum of the high-order non-spherical perturbation of the Earth, three-body gravitational perturbation, and solar luminous pressure perturbation on the three coordinate axes, which can be equivalent to Gaussian white noise with a zero mean value in this study. $f_a={\left(\begin{array}{ccc}{f}_{ax}& f_{ay}& f_{az}\end{array}\right)}^{\mathrm{T}}$ is the atmospheric drag perturbation, and its specific expression is as follows:

$${\mathit{f}}_a=-\frac{1}{2}\frac{c_{d}A}{m}{\rho }_d{v}_{rel}{\mathit{v}}_{rel}$$

(38)

where $c_d$ is the atmospheric drag coefficient, $A/m$ is the surface mass ratio of the spacecraft, ${\rho }_d$ is the atmospheric density, and ${\mathit{v}}_{rel}$ is the relative speed between the spacecraft and the atmosphere, assuming that the atmosphere rotates with the Earth, as follows:

$${\mathit{v}}_{rel}=\dot{\mathit{X}}-\mathit{\omega }\times \mathit{X}$$

(39)

where $\mathit{\omega }={\omega }_e\times {\left(\begin{array}{ccc}0& 0& 1\end{array}\right)}^{\mathrm{T}}$ is the angular velocity vector of the Earth rotation and ${\omega }_e$ is the angular velocity of the Earth rotation. Using the fourth-order Runge–Kutta method, Equation (32) is written in the following discrete form:

$${\mathit{X}}_k=\mathit{f}\left({\mathit{X}}_{k-1}\right)+{\mathit{w}}_{k-1}$$

(40)

where ${\mathit{X}}_k={\left[\begin{array}{llllll}{x}_k& y_k& z_k& v_{x,k}& v_{y,k}& v_{z,k}\end{array}\right]}^{\mathrm{T}}$ is the track state at time k, and ${\mathit{w}}_k$ is the system noise.

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 ${\left(\begin{array}{cc}\mathit{X}& \dot{\mathit{X}}\end{array}\right)}^{\mathrm{T}}$, the orbital state in $O-X_e{Y}_e{Z}_e$ is ${\left(\begin{array}{cc}{\mathit{X}}_f& {\dot{\mathit{X}}}_f\end{array}\right)}^{\mathrm{T}}$, and the orbital state in $O-X_h{Y}_h{Z}_h$ is ${\left(\begin{array}{cc}{\mathit{X}}_h& {\dot{\mathit{X}}}_h\end{array}\right)}^{\mathrm{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 ${\mathit{M}}_J^W={\mathit{M}}_{Pw}{\mathit{M}}_{Ro}{\mathit{M}}_{Nu}{\mathit{M}}_{Pr}$, where ${\mathit{M}}_{Pr}$ is the precession matrix, ${\mathit{M}}_{Nu}$ is the nutation matrix, ${\mathit{M}}_{Ro}$ is the Earth rotation matrix, and ${\mathit{M}}_{Pw}$ is the pole shift matrix; then, we can obtain:

$${\mathit{X}}_f={\mathit{M}}_J^W\mathit{X}$$

(41)

$${\dot{\mathit{X}}}_f={\mathit{M}}_J^W\dot{\mathit{X}}+{\dot{\mathit{M}}}_J^W\mathit{X}$$

(42)

In these formulas, the derivative of the transformation matrix ${\dot{\mathit{M}}}_J^W$ is shown as follows:

$${\dot{\mathit{M}}}_J^W=\frac{d\left({\mathit{M}}_{Pw}{\mathit{M}}_{Ro}{\mathit{M}}_{Nu}{\mathit{M}}_{Pr}\right)}{dt}\approx {\mathit{M}}_{Pw}\frac{d\left({\mathit{M}}_{Ro}\right)}{dt}{\mathit{M}}_{Nu}{\mathit{M}}_{Pr}$$

(43)

where $d\left({\mathit{M}}_{Ro}\right)/dt$ is the derivative matrix of the Earth rotation matrix as follows:

$$\frac{d\left({\mathit{M}}_{Ro}\right)}{dt}=\left[\begin{array}{ccc}0& {\omega }_e& 0\\ -{\omega }_e& 0& 0\\ 0& 0& 0\end{array}\right]\cdot {\mathit{M}}_{Ro}$$

(44)

Equations (36) and (37) are written in matrix form as:

$$\left[\begin{array}{c}{\mathit{X}}_f\\ {\dot{\mathit{X}}}_f\end{array}\right]=\left[\begin{array}{cc}{\mathit{M}}_J^W& \mathbf{0}\\ {\dot{\mathit{M}}}_J^W& {\mathit{M}}_J^W\end{array}\right]\left[\begin{array}{c}\mathit{X}\\ \dot{\mathit{X}}\end{array}\right]$$

(45)

Step 2: Transition from $O-X_e{Y}_e{Z}_e$ to $O-X_h{Y}_h{Z}_h$

The conversion matrix ${\mathit{M}}_W^H$ is:

$${\mathit{M}}_W^H=\left[\begin{array}{ccc}-\sin \lambda & \cos \lambda & 0\\ -\sin \phi \cos \lambda & -\sin \phi \sin \lambda & \cos \phi \\ \cos \phi \cos \lambda & \cos \phi \sin \lambda & \sin \phi \end{array}\right]$$

(46)

where $\lambda$ is the radar geocentric longitude and $\phi$ is the radar geocentric latitude, which can be converted to the geocentric coordinates of the radar ${\mathit{X}}_c={\left(\begin{array}{ccc}{x}_c& y_c& z_c\end{array}\right)}^{\mathrm{T}}$ in $O-X_e{Y}_e{Z}_e$; so, we obtain:

$${\mathit{X}}_h={\mathit{M}}_W^H\left({\mathit{X}}_f-{\mathit{X}}_c\right)$$

(47)

$${\dot{\mathit{X}}}_h={\mathit{M}}_W^H{\dot{\mathit{X}}}_f$$

(48)

This is written in matrix form as below:

$$\left[\begin{array}{c}{\mathit{X}}_h\\ {\dot{\mathit{X}}}_h\end{array}\right]=\left[\begin{array}{cc}{\mathit{M}}_W^H& \mathbf{0}\\ \mathbf{0}& {\mathit{M}}_W^H\end{array}\right]\left[\begin{array}{c}{\mathit{X}}_f\\ {\dot{\mathit{X}}}_f\end{array}\right]-\left[\begin{array}{c}{\mathit{M}}_W^H{\mathit{X}}_c\\ \mathbf{0}\end{array}\right]$$

(49)

Substituting Equation (40) into Equation (44) yields:

$$\left[\begin{array}{c}{\mathit{X}}_h\\ {\dot{\mathit{X}}}_h\end{array}\right]=\left[\begin{array}{cc}{\mathit{M}}_W^H& \mathbf{0}\\ \mathbf{0}& {\mathit{M}}_W^H\end{array}\right]\left[\begin{array}{cc}{\mathit{M}}_J^W& \mathbf{0}\\ {\dot{M}}_J^W& {\mathit{M}}_J^W\end{array}\right]\left[\begin{array}{c}\mathit{X}\\ \dot{\mathit{X}}\end{array}\right]-\left[\begin{array}{c}{\mathit{M}}_W^H{\mathit{X}}_c\\ \mathbf{0}\end{array}\right]$$

(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). In order to obtain the relationship between the measured value and the orbit state, we write ${\mathit{X}}_h$ and ${\dot{\mathit{X}}}_h$ in the form of a specific vector as ${\mathit{X}}_h={\left(\begin{array}{ccc}{x}_h& y_h& z_h\end{array}\right)}^{\mathrm{T}}$ and ${\dot{\mathit{X}}}_h={\left(\begin{array}{ccc}{\dot{x}}_h& {\dot{y}}_h& {\dot{z}}_h\end{array}\right)}^{\mathrm{T}}$; then, the geometric relationship between the radar ranging value R, velocity measurement value $\dot{R}$, azimuth angle A, and pitch angle E and the orbit state is as follows:

$$\left\{\begin{array}{c}R=\sqrt{x_h^2+y_h^2+z_h^2}\\ A=\arctan \frac{y_h}{x_h}\\ E=\arctan \frac{z_h}{\sqrt{x_h^2+y_h^2}}\end{array}\right.$$

(51)

Equation (46) can be written as the following discrete measurement equation:

$${\mathit{Z}}_k=\mathit{h}\left({\mathit{X}}_k\right)+{\mathit{v}}_k$$

(52)

where ${\mathit{Z}}_k={\left(\begin{array}{ccc}{R}_k& A_k& E_k\end{array}\right)}^{\mathrm{T}}$ is the measured value at time k, and ${\mathit{v}}_k$ is the measured noise.

Thus, Equations (35) and (47) constitute the state equation and measurement equation corresponding to Equation (11) for real-time filtering orbit determination.

5.2.3. Simulation Result and Analysis

Figure 2a shows the spacecraft ground simulator on which the algorithm of High-Precision Orbit Propagation (HPOP) is run, which takes into account the perturbation effects of the Earth’s high-order non-spherical gravity, atmospheric drag, solar light pressure, three-body gravity, and tides. Among them, the 21-layer Earth model, atmospheric drag coefficient $c_d=2.2$, and spacecraft surface mass ratio $0.02 {\mathrm{m}}^2/\mathrm{kg}$ are considered; the atmospheric density model adopts the Jacchia–Roberts model; and the solar luminous pressure coefficient $c_r=1$. The orbital epoch of the spacecraft considered in the experiment is 16:00:00 (UTC) on 1 September 2016, and the six orbital roots are:

$$a=7028.14 \mathrm{km}, e = 0, i = {80.7}^{\circ }, \mathsf{\Omega } = {63.8}^{\circ }, \omega = 0^{\circ }, f = {107.1}^{\circ }$$

As shown in Figure 2b, the lowest measured elevation angle of the radar is 10°. It is assumed that the radar ranging error is 60 m and the angle error is 0.02°.

The algorithm proposed in this paper is compared with the SCKF and OSCKF, and the root mean square error (RMSE) is used to evaluate the spacecraft tracking accuracy. The Monte-Carlo simulation is run 200 times, and the results are shown in Figure 3. The mean positioning RMSE and velocity RMSE are listed in

Table 2. Spacecraft tracking RMSE of the various filters.

Filters	Position RMSE/m	Velocity RMSE/(m/s)
SCKF	24.86	0.103
OSCKF	24.31	0.101
IOSCKF	23.65	0.098

.

It can be seen that the spacecraft tracking accuracy of the OSCKF is higher than that of the SCKF; specifically, the position tracking accuracy is increased by 2.21%, and the velocity tracking accuracy is increased by 1.94%, which indicates that the orthogonal transformation can improve the tracking accuracy. The spacecraft tracking accuracy of the IOSCKF is higher than that of the OSCKF; specifically, the position tracking accuracy is increased by 2.71%, and the velocity tracking accuracy is increased by 2.97%, which indicates that the tracking accuracy can be effectively improved by introducing iterative calculation into the measurement equation, thus verifying the effectiveness of the method presented in this paper.

6. Conclusions

A new iterated orthogonal simplex cubature Kalman filter (IOSCKF) is proposed to improve the nonlinear system filtering accuracy in target tracking. The new cubature points are determined using the orthogonal matrix after being modified using the spherical simplex-radial rule and the second-order Gauss–Laguerre quadrature rule. This lowers the high-order error terms. The Gauss–Newton iteration is used to maximize the posteriori state and covariance in the measurement update process until the iteration is finished. Target tracking simulations, including CV target tracking and spacecraft orbit tracking, are used to examine the performance of the proposed method, and the results reveal that the IOSCKF can achieve greater accuracy compared to the CKF, SCKF, and OSCKF. In spacecraft orbit tracking simulation, compared with the SCKF, the position tracking accuracy and velocity tracking accuracy of the OSCKF are increased by 2.21% and 1.94%, respectively, which indicate that the orthogonal transformation can improve the tracking accuracy. Furthermore, compared with the OSCKF, the position tracking accuracy and velocity tracking accuracy of the IOSCKF are increased by 2.71% and 2.97%, respectively, which indicate that the tracking accuracy can be effectively improved by introducing iterative calculation into the measurement equation, thus verifying the effectiveness of the method presented in this paper.