Strong Tracking Spherical Simplex-Radial Cubature Kalman Filter for Maneuvering Target Tracking

Conventional spherical simplex-radial cubature Kalman filter (SSRCKF) for maneuvering target tracking may decline in accuracy and even diverge when a target makes abrupt state changes. To overcome this problem, a novel algorithm named strong tracking spherical simplex-radial cubature Kalman filter (STSSRCKF) is proposed in this paper. The proposed algorithm uses the spherical simplex-radial (SSR) rule to obtain a higher accuracy than cubature Kalman filter (CKF) algorithm. Meanwhile, by introducing strong tracking filter (STF) into SSRCKF and modifying the predicted states’ error covariance with a time-varying fading factor, the gain matrix is adjusted on line so that the robustness of the filter and the capability of dealing with uncertainty factors is improved. In this way, the proposed algorithm has the advantages of both STF’s strong robustness and SSRCKF’s high accuracy. Finally, a maneuvering target tracking problem with abrupt state changes is used to test the performance of the proposed filter. Simulation results show that the STSSRCKF algorithm can get better estimation accuracy and greater robustness for maneuvering target tracking.


Introduction
Maneuvering target tracking has drawn increasing attention because of its widespread application in areas such as radar tracking, aircrafts surveillance, and spacecraft orbit control [1,2]. For maneuvering target tracking, many algorithms are developed and grouped into two types. One type is to improve the accuracy of the motion model, such as multiple-model (MM) methods [3], optimization of multiple model neural filter [4], current statistical (CS) model [5,6], and so on. The other type is to detect the target maneuverability and then to cope with it effectively, such as strong tracking filter (STF) [7], tracking algorithm based on maneuvering detection [8], and so on. In these methods, the performance of the filter is an important factor affecting the performance of these methods. Therefore, improving the accuracy of the filter is also a useful method to improve the performance of maneuvering target tracking. Thus, a large number of nonlinear filters have been developed. Among these algorithms, the extended Kalman filter (EKF) [9] is one of the earliest and most widely used nonlinear filters. The EKF uses a linearization technique, based on the first-order Taylor series expansion, and approximates the nonlinear system. However, EKF has some limitations, such as complex Jacobian matrix calculations and poor accuracy in estimating the states of the strongly nonlinear system.
As better alternatives to the EKF, many nonlinear filters based on the idea of Bayesian theory have been proposed. One popular approach for the nonlinear non-Gaussian filtering problem is to use sequential Monte Carlo methods. The most famous method is known as particle filter (PF) [10][11][12][13]. The key idea of PF is to represent the posterior distribution by a set of random samples

A Review of UKF and CKF
The nonlinear discrete-time system is represented by where k ∈ N denotes discrete time, f(·) represents the nonlinear function, h(·) represents the measurement function. x k ∈ R n is the state vector of system, z k ∈ R m is the measurement, w k ∈ R n is the process noise vector, and v k ∈ R m is the measurement noise vector. The process w k and measurement noise v k are uncorrelated zero-mean Gaussian white sequences and have zero cross-correlation with each other, represented as w k ∼ N(0, Q k ) and v k ∼ N(0, R k ), respectively.
Under the Gaussian assumption in the Bayesian filtering framework, the key problem of the nonlinear filtering problem is to calculate the multi-dimensional integrals. However, in most cases, the multi-dimensional integrals cannot be solved analytically. As a result, several approximation methods have been proposed, such as the unscented transformation (UT) and the cubature rule.
The UT with 2n + 1 the sigma points χ i and corresponding weights is chosen as where [P k|k ] i is the ith column of the matrix square root of P k|k , n is the dimension of state. λ = α 2 (n + κ) − n is the scaling parameter; α determines the spread of the sigma points around x k|k . The positive constants β and κ are used as parameters of the method. The third-degree cubature rule with 2n cubature points and weights is given by: As indicated above, the main difference between the UT used in UKF and the third-degree cubature rule used in CKF is that the UT has one more point in the center with a tune parameter κ. If the parameter κ is set to zero, the sigma points set will evolve into the cubature points set and the UKF becomes identical to the CKF. For UKF, the scaling parameter κ is always set to n − 3. Based on this point, for high-dimensional problems (n > 3), it will lead to the negative weight of the center point. The presence of the negative weight may lead the covariance matrix to become non-positively defined. Thus, the cubature rule is more stable than the UT. In summary, the CKF is virtually a special case of UKF and the CKF has better numerical stability than UKF.

Strong Tracking Spherical Simplex-Radial Cubature Kalman Filter
The heart of the spherical simplex cubature Kalman filter is the spherical-radial cubature rule. The spherical-radial cubature rule does not approximate the nonlinear function, but it can approximate the integral of the form (nonlinear function × Gaussian) using weighted quadrature point sets. The integral with the standard Gaussian distribution N(x; 0, I) can be approximated by the quadrature where m is the total number of quadrature points in the state-space R n , {γ i , ω i } m i is a set of quadrature points and corresponding weights. The general Gaussian integral R n f(x)N(x;x, P)dx can be approximated by the following transformation R n f(x)N(x;x, P)dx = R n f( The computational complexity of the numerical integration is proportional to the number of quadrature points, and the accuracy of the numerical integration rule is usually assessed by the polynomial approximation degrees.

Review of the Third-Degree Spherical Simplex-Radial Cubature Rule
The SSRCKF algorithm has the same structure as the general Gaussian approximation filters, such as the CKF, but uses the third-degree spherical simplex-radial cubature rule to calculate the Gaussian weight integral I(f) = R n f(x)N(x; 0, I)dx. By using the spherical simplex-radial cubature rule, the SSRCKF method can get more accurate estimation than CKF. In the third-degree spherical simplex-radial cubature rule, the following integral is considered [19]: where f(·) is arbitrary nonlinear function, R n is the integral domain. To calculate the above integral, let x = rs (s T s = 1, r = √ x T x). Equation (6) can be transformed into the spherical-radial coordinate system where s = [s 1 , s 2 , · · · , s n ] T , U n = {s ∈ R n : s 2 1 + s 2 2 + · · · + s 2 n = 1 is the spherical surface, and σ(·) is the area element on U n . Then, the Equation (7) can be decomposed into the spherical integral S(r) = U n f(rs)dσ(s) and the radial integral I(f) = ∞ 0 S(r)r n−1 exp(−r 2 )dr.

Spherical Simplex Rule
As can be seen from the literature [25], the spherical integral U n f(rs)dσ(s) can be approximated by the transformation group of the regular n-simplex with vertices a j . P 0 The third-degree spherical simplex rule with 2n + 2 quadrature points is given by where A n = 2 √ π n /Γ n (1/2), N s = 2n + 2.

Radial Rule
The radial integral ∞ 0 S(r)r n−1 exp(−r 2 )dr can be calculated by the following moment matching equation where S(r) = r l is a monomial in r, with l an even integer. Using the moment method with the minimum number of points, the third-degree radial rule (N r = 1) can be derived. From Equation (9) we can obtain the moments' equations as By solving Equation (10), the points and weights for the third-degree radial rule are given by By using Equations (7), (8) and (11), the third-degree spherical simplex-cubature rule (N r = 1, where m = 2n + 2, ξ k = √ n[a, −a] k and ω k = 1/(2n + 2) are the corresponding weights. The steps of SSRCKF algorithm for the nonlinear system can be found in the literature [17].

Strong Tracking Filter
To improve the performance of EKF, a concept of STF was proposed by Zhou and Frank [7]. They proved that a filter can obtain the strong tracking estimation of the state can have the strong tracking performance only if the filter satisfies the orthogonal principle [7]. In strong tracking, the time-varying suboptimal fading factor is incorporated, which online adjusts the covariance of the predicted state. In this way, the algorithm has the ability to track abrupt state change and strong robustness against mode uncertainties. The algorithm has the following steps [21]: where F k|k−1 and H k are the process matrix and measure matrix, respectively. The suboptimal time-varying fading factor λ k is given by where tr[·] is the trace operation, v k = z k −ẑ k|k−1 denotes the measurement residual vector; β ≥ 1 is the softening factor, which can improve the smoothness of state estimation; 0 < ρ ≤ 1 is the forgetting factor. In generally, the parameters β and ρ are chosen as 4.5 and 0.95, respectively [26,27].

Equivalent Expression of the Fading Factor
As we know, STF need calculate the linearization of the nonlinear measurement matrix (Hessian matrix). However, SSRCKF is not necessary to compute the Hessian matrix. So we give the equivalent expression of STF, which need not calculate the Hessian matrix. Suppose P l k|k−1 is the state error covariance matrix before introducing fading factor, P l zz,k|k−1 is the measurement covariance matrix and P l xz,k|k−1 is cross-covariance matrix, Equations (15) and (16) have the following equivalent expressions: The new fading factor can be obtained through Equations (14) and (17)- (19). It can be verified from Equations (18) and (19) that the calculation of suboptimal fading factor in the Equation expression does not need to compute any Jacobian matrix.

Steps of the STSSRCKF
Based on the previous sections, the strong tracking spherical-simplex cubature Kalman filtering (STSSRCKF) can adjust the prediction error covariance matrix by introducing a suboptimal factor. Hence, the robustness and real-time tracking ability are provided in the STSSRCKF algorithm. The initial state is assumed to be Gaussian distribution withx 0|0 and P 0|0 . The computation steps of the third-degree strong tracking spherical simplex-radial cubature Kalman filter is summarized as follows: Step 1. Give the state estimatex k−1|k−1 and the error covariance matrix P k−1|k−1 ; Step 2. State estimate prediction: The cubature points are obtained as where chol(·)is the Cholesky factorization. Propagate the cubature points, the predicted state x k|k−1 , and the predicted covariance P l k|k−1 without the fading factor are given as where Q k−1 is the covariance matrix of process noise.
Step 3. Calculation of the fading factor λ k : Using the predicted statex k|k−1 and the predicted covariance P l k|k−1 , the innovation covariance P l zz,k|k−1 and the cross covariance P l xz,k|k−1 can be calculated as The fading factor λ k can be calculated by using Equations (14) and (17)- (19).
Step 4. Measurement updating modified by the fading factor: The modified prediction covariance P k|k−1 can be updated by By utilizing the predicted state estimatex k|k−1 and the modified predicted covariance P k|k−1 with the fading factor λ k , the modified predicted measurementẑ k|k−1 , the modified cross covariance and the modified innovation covariance P zz,k|k−1 can be calculated as follows Step 5. Estimation results: The state estimatex k and the covariance P k at time k are calculate as follows The STSSRCKF combines the advantages of STF and SSRCKF. Then the STSSRCKF has strong robustness against model uncertainties and good real-time state tracking capability [28]. Moreover, the STSSRCKF algorithm eliminates the cumbersome evaluation of Jacobian/Hessian matrices, its numerical stability and estimated accuracy are significantly improved.

Simulation and Results
The effectiveness of the proposed algorithm will be illustrated through two examples of maneuvering target tracking. Taking the root mean square error (RMSE) and accumulative RMSE (ARMSE), the study compared the STSSRCKF algorithm with the EKF algorithm and the SSRCKF algorithm.

Tracking Model and Measurement Model
The constant acceleration (CA) model is a common tool for tracking target modeling. The state Equation of CA model in two-dimensional case is described as follow: .. y k−1 ) represent the target position, velocity and acceleration in the x and y coordinate at time k − 1, respectively; diag[Φ CA , Φ CA ] is the state transition matrix, G k−1 is the state input matrix, V k−1 is the process noise, w k−1 is zero-mean white Gaussian noise and its corresponding covariance matrix is Q ca . Φ CA , G k−1 are described as: where T is the sampling interval. In radar tracking system, the target motion is usually modelled in Cartesian coordinates, whereas the target's position and azimuth are obtained in polar coordinate. The radar is located at the origin, and provides range and bearing measurements. The measurement model can be established as where atan2(·) is the four-quadrant inverse tangent function, v k is the white Gaussian measurement noise with zero mean and covariance R k = diag([σ 2 r , σ 2 θ ]). σ r and σ θ denote the standard deviation of range measurement noise and bearing angle measurement noise, respectively.

Simulation of the STSSRCKF
where M is the number of Monte Carlo runs, (x k , y k ) is the actual value of the target position at time k and (x m,k ,ŷ m,k ) is the estimated position at time k in mth Monte-Carlo. The RMSE and the accumulative RMSE in the velocity and acceleration can be defined in the same way.
This example considers a two-dimensional simulation scenario including one motion mode of high maneuver. The initial location of the target is (x, y) = (100 m, 400 m), its initial velocity is (v x , v y ) = (15 m/s, 20 m/s), and its initial acceleration is (a x , a y ) = (0 m/s 2 , 0 m/s 2 ). The target makes a uniform motion during the first 150 s. Then, it takes a high maneuver with the acceleration (a x , a y ) = (15 m/s 2 , 25 m/s 2 ) up to the end of this simulation at t = 200 s. In this simulation, the initial valuex 0|0  respectively. The standard deviation of range measurement noise σ r is 30 m and the standard deviation of bearing angle measurement noise σ θ is 10 mrad. The example is executed to examine the performance among the SSRCFK, STF, STUKF, STCKF and STSSRCK methods. The RMSEs of the position, velocity and acceleration using the five filters are shown in Figures 1-3. It can be shown that the STF, STUKF, STCKF and STSSRCK methods can converge quickly when the target engages in high maneuvering. The SSRCKF algorithm only has a good performance for uniform motion. However, the performance of SSRCKF decreases seriously when the target engages in high maneuvering. This is because that the prediction covariance cannot be adjusted timely when the target state suddenly changes. The STF algorithm has the fourth speed of convergence, which is due to the fact that the linear approximation in the STF may introduce errors in the state which may lead the state to diverge. As can be seen from Figures 1-3, when the target is making uniform motion within the first 100 s, the five methods have a similar performance. When the maneuver starts at t = 101 s, it obviously shows that STF, STUKF, STCKF and STSSRCKF have the ability to convergence. The main reason is that the fading factor can adjust the prediction covariance and the corresponding filter gain in real time, which makes these algorithms converge in a short time. We can also see that the RMSE of the proposed algorithm is lower than that of STUKF and STCKF. It means that estimate precision of the proposed algorithm is higher than that of the two algorithms. It is demonstrated that the proposed algorithm can effectively track the abrupt motion state of the target. 1 , respectively. The standard deviation of range measurement noise r  is 30 m and the standard deviation of bearing angle measurement noise   is 10 mrad. The example is executed to examine the performance among the SSRCFK, STF, STUKF, STCKF and STSSRCK methods. The RMSEs of the position, velocity and acceleration using the five filters are shown in Figures 1-3. It can be shown that the STF, STUKF, STCKF and STSSRCK methods can converge quickly when the target engages in high maneuvering. The SSRCKF algorithm only has a good performance for uniform motion. However, the performance of SSRCKF decreases seriously when the target engages in high maneuvering. This is because that the prediction covariance cannot be adjusted timely when the target state suddenly changes. The STF algorithm has the fourth speed of convergence, which is due to the fact that the linear approximation in the STF may introduce errors in the state which may lead the state to diverge. As can be seen from Figures 1-3, when the target is making uniform motion within the first 100 s, the five methods have a similar performance. When the maneuver starts at t  101 s, it obviously shows that STF, STUKF, STCKF and STSSRCKF have the ability to convergence. The main reason is that the fading factor can adjust the prediction covariance and the corresponding filter gain in real time, which makes these algorithms converge in a short time. We can also see that the RMSE of the proposed algorithm is lower than that of STUKF and STCKF. It means that estimate precision of the proposed algorithm is higher than that of the two algorithms. It is demonstrated that the proposed algorithm can effectively track the abrupt motion state of the target.     To quantitatively describe the tracking performance, the ARMSEs of the five methods in estimating different target parameters are listed in Table 1. As shown, the STSSRCKF provided the best result in terms of estimation. The STCKF also performed well, followed by the STUKF and STF. The SSRCKF provided the worst estimate. We can also draw the conclusion that the STSSRCKF has the highest tracking accuracy of the position, velocity and acceleration. The program is made on the Intel Core (TM) i5-4430 3.0GHZ CPU with 4.00G RAM. Table 2 shows the computational complexity and the computational time of SSRCFK, STF, STUKF, STCKF and STSSRCK for each run. Apart from STF, the computational complexity of different filters is mainly determined by the number of points they use. The computational complexity of STCKF as well as STUKF differs only by one points. The computational complexity of SSRCKF and STSSRCKF is O{(2n + 2)}, where n denotes the dimension of state. In addition, we can see that the computational complexity of STF is the lowest. Because there is a clear formula in STF to calculate the Jacobian matrix, the computational complexity of STF is much smaller than other four algorithms. It is also shown that the computational time of the SSRCKF is 0.07 s for each run. However, the computational time of the STSSRCKF is 0.15 s, which is greater than that of the SSRCKF. This is because the STSSRCKF needs to calculate the suboptimal fading factor at each time step. At present, the time consumption is acceptable. The STSSRCKF needs more computational time than the SSRCKF, but considering the significant performance improvement gained from the STSSRCKF, this To quantitatively describe the tracking performance, the ARMSEs of the five methods in estimating different target parameters are listed in Table 1. As shown, the STSSRCKF provided the best result in terms of estimation. The STCKF also performed well, followed by the STUKF and STF. The SSRCKF provided the worst estimate. We can also draw the conclusion that the STSSRCKF has the highest tracking accuracy of the position, velocity and acceleration. The program is made on the Intel Core (TM) i5-4430 3.0GHZ CPU with 4.00G RAM. Table 2 shows the computational complexity and the computational time of SSRCFK, STF, STUKF, STCKF and STSSRCK for each run. Apart from STF, the computational complexity of different filters is mainly determined by the number of points they use. The computational complexity of STCKF as well as STUKF differs only by one points. The computational complexity of SSRCKF and STSSRCKF is O{(2n + 2) 3 }, where n denotes the dimension of state. In addition, we can see that the computational complexity of STF is the lowest. Because there is a clear formula in STF to calculate the Jacobian matrix, the computational complexity of STF is much smaller than other four algorithms. It is also shown that the computational time of the SSRCKF is 0.07 s for each run. However, the computational time of the STSSRCKF is 0.15 s, which is greater than that of the SSRCKF. This is because the STSSRCKF needs to calculate the suboptimal fading factor at each time step. At present, the time consumption is acceptable. The STSSRCKF needs more computational time than the SSRCKF, but considering the significant performance improvement gained from the STSSRCKF, this increased computational time is not substantial.
From this simulation, we can conclude that the STSSRCKF can perform the best in terms of the balance between computational complexity and estimation accuracy.

Example 2.
This example evaluates the proposed algorithm in tracking a target with weak maneuver and medium maneuver. Therefore, two simulations are simulated as follows. Assume that there is a target making uniform at first. The initial location of the target is (x, y) = (5000 m, 5000 m), its initial velocity is (v x , v y ) = (150 m/s, 80 m/s), and its initial acceleration is (a x , a y ) = (0 m/s 2 , 0 m/s 2 ).
Case 1: Simulation of medium maneuvering target tacking. The target moves with initial acceleration until t = 150 s. Then, it maneuvers with acceleration of (a x (151), a y (151)) = (5 m/s 2 , 5 m/s 2 ) up to end of this simulation at t = 200 s. Case 2: Simulation of weak maneuvering target tracking. The initial position, velocity and acceleration of the target are the same as those in Case1. The target also moves with initial acceleration until t = 150 s. Then, it maneuvers with acceleration of (a x (151), a y (151)) = (0.5 m/s 2 , 0.5 m/s 2 ) up to end of this simulation at t = 200 s. Table 3 lists the accumulative RMSEs of the five methods in estimation the three target parameters. As can be seen from Table 3, the STSSRCKF algorithm also has a good tracking performance for a weak or medium maneuvering target.

Conclusions
To implement higher tracking accuracy for a maneuvering target, a new method has been proposed based on the STF and SSRCKF algorithms. Firstly, the time-varying suboptimal fading factor is introduced in order to adjust the prediction covariance and the corresponding filter gain in real time. Secondly, in the proposed method, the spherical simplex-cubature rule takes the place of calculating nonlinear function Jacobian matrix. In this way, STSSRCKF can converge rapidly in a short time. Thus, the proposed method has a high tracking accuracy for maneuvering target tracking. Simulation results show that the STSSRCKF can achieve higher accuracy and robustness than STF, STUKF, STCKF and SSRCKF, and indicate that it is suitable for maneuvering target tracking.