Spherical Simplex-Radial Cubature Quadrature Kalman Filter

A spherical simplex-radial cubature quadrature Kalman filter (SSRCQKF) is proposed in order to further improve the nonlinear filtering accuracy. The Gaussian probability weighted integral of the nonlinear function is decomposed into spherical integral and radial integral, which are approximated by spherical simplex cubature rule and arbitrary order Gauss-Laguerre quadrature rule, respectively, and the novel spherical simplex-radial cubature quadrature rule is obtained. Combined with the Bayesian filtering framework, the general form and the specific form of SSRCQKF are put forward, and the numerical simulation results indicate that the proposed algorithm can achieve a higher filtering accuracy than CKF and SSRCKF.


Introduction
The nonlinear state estimation problem widely exists in signal processing, target tracking, intelligent sensing, and other fields, which is a subject undergoing intense study [1][2][3][4].In suboptimal nonlinear filtering under Bayesian theory framework, the posterior probability density function (pdf) is assumed to be Gaussian distribution, and the core issue is to calculate the intractable integral as "nonlinear function × Gaussian pdf."Since to achieve the analytical solution is difficult for the integral, the focus of the research is seeking a highprecision integral rule for its numerical approximation [5].
The most widely used nonlinear Kalman filtering algorithms are extended Kalman filter (EKF) [6,7] and unscented Kalman filter (UKF) [8,9], respectively.EKF uses the firstorder Taylor formula to linearize the nonlinear function directly, thereby has only first-order filtering accuracy, and needs to calculate the Jacobian matrix, which limits its further application.UKF adopts a set of sigma points to approximate the intractable integral and achieves a third-order accuracy.However, the selection of the sigma points and corresponding weights lacks rigorous mathematical basis, and the stability of numerical calculation is reduced for the high-dimensional system.Cubature Kalman filter (CKF) proposed by Arasaratnam and Haykin [10,11] decomposes the intractable integral into spherical integral and radial integral, which are approximated by the third-order cubature rule.CKF not only has a rigorous mathematical basis in selecting the cubature points but also has a complete stability in numerical calculation [12][13][14].Moreover, Wang et al. [15] proposed the spherical simplex-radial cubature Kalman filter (SSRCKF), in which the spherical simplex rule instead of spherical rule is used in calculating the spherical integral [16][17][18].However, radial integral is calculated by moment matching method in both CKF and SSRCKF, which cannot guarantee the optimal solution.To solve this problem, Shovan and Swati [19] proposed cubature quadrature Kalman filter (CQKF): the algorithm adopts the same method for solving the spherical integral as CKF, but, for radial integral, arbitrary order Gaussian-Laguerre quadrature formula is used to achieve a higher radial integral accuracy, so as to improve the filtering accuracy further.It is also pointed out that CKF is a simplified form of CQKF with the first-order Gaussian-Laguerre quadrature in radial integral.
And   denote the corresponding weights, which can be solved as follows: It can be seen that the approximation accuracy of the above rule depends on the number of quadrature points.

Spherical Simplex-Radial
Cubature Quadrature Rule.Equation ( 4) is substituted into rule , and we obtain The spherical simplex rule (2) and () = (√) are plugged into (7) to obtain x, the Gaussian probability weighted integral of arbitrary nonlinear function is obtained as follows: Equation ( 9) is the novel spherical simplex-radial cubature quadrature rule that proposed in this paper, which requires the calculation of 2(+1) points and corresponding weights.In particular, it can be solved that  1 = /2,  1 = Γ(/2) when  = 1, and results in the spherical simplexradial cubature rule.Thus, the spherical simplex-radial cubature rule is the degenerate form of the proposed rule, and the proposed rule can achieve a higher approximation accuracy when  ≥ 2.

SSRCQKF Algorithm
3.1.The General Form of SSRCQKF.Consider the following discrete nonlinear system with additive noise: where x  ∈ R  denotes the state vector, z  ∈ R   denotes the measurement vector, and the noises w  , k  are uncorrelated Gaussian white noise.With the system dimension  and the order  of Chebyshev-Laguerre polynomial being known,   ,  = 1, . . .,  can be solved by (5), and the corresponding weights can be calculated as follows: 2 ,  = 2 + 3, . . ., 4 ( + 1) . . .!Γ ( +  + 1) The matrix a = [a 1 a 2 ⋅ ⋅ ⋅ a +1 ] consists of a  that is used to construct the following expansion matrix denotes the th column of matrix; based on the rule (9) and Bayesian filtering framework, the primary calculation process of the general form of SSRCQKF algorithm is listed as follows: Calculate the following points: Calculate the nonlinear propagation of the points: Calculate the prior state estimation and prior error covariance matrix: Calculate the following points: Calculate the nonlinear propagation of the points: Calculate the predicted measurement value: Calculate the predicted measurement covariance matrix: Calculate the cross covariance matrix: Calculate the Kalman filtering gain: Calculate the a posteriori state estimation: Calculate the a posteriori error covariance matrix: It can be seen from the algorithm process that the filtering accuracy depends on the order of the Gaussian-Laguerre quadrature rule; the higher the order is, the higher that estimation accuracy is achieved, but the more the points and weights are required.However, for the identified  and , the points can be calculated in advance and stored offline and called directly from the memory in the process of implementation, that is necessary to improve the real-time performance of the algorithm.

The Specific Form of SSRCQKF
When  = 2.The general form of SSRCQKF is presented in Section 3.1, and, in this section, the specific form of SSRCQKF algorithm when  = 2 is given.When  = 2, (7) is simplified as follows: The values of  1 and  2 are needed to be calculated.Plug  = 2 into (5), we obtain The item ( 2 / 2 )( +2  − ) is expanded to achieve   2 () and its derivative L  2 () as follows: Let   2 () = 0, the solutions are  =  + 2 ± √ + 2, combined with  = ( − 2)/2, and we obtain Then,  1 and  2 are solved from (6) as follows: Furthermore, the weights   are solved as follows: The spherical simplex-radial cubature quadrature rule with  = 2 is obtained by plugging  1 ,  2 ,  1 , and  2 into rule (9) as follows: Based on (29), the calculation steps of the specific form of SSRCQKF when  = 2 are given as follows.

Simulation Results and Analysis
4.1.Simulation 1.The effectiveness of the proposed SSRC-QKF algorithm is verified by the following three-dimensional strong nonlinear system, which includes trigonometric function operation, power operation, and exponential operation. ] where  = 0.1,  = 1, the theoretical initial value of the nonlinear system is x 0 = [−0.7 1 1] Τ , the filtering initial value is x0 = [0 0 0] Τ , the initial covariance matrix is P 0 = I 3×3 .The SSRCQKF-2 (when  = 2) and SSRCQKF-3 (when  = 3) are compared with CKF and SSRCKF, the simulation step is 1, and the total step size is 100.The root mean square error (RMSE) is used to describe the filtering accuracy and run 500 times Monte-Carlo simulation, and the results are shown in Figures 1-3.In order to show the details of the curve more clearly, the data only between 30 and 80 are captured in figures.It can be seen from the figures that the RMSE of the proposed SSRCQKF is significantly smaller than the other two algorithms.In order to compare the four filters quantitatively, the average value and variance of RMSE of the four filters in 100 steps are counted and listed in Tables 1 and 2, respectively.From Table 1, we see that CKF achieves a higher filtering accuracy than SSRCKF in this simulation.Compared with CKF, the SSRCQKF-2 proposed in this paper improves the estimation accuracy of state 1 by 1.4%, improves the estimation accuracy of state 2 by 6.26%, and improves the estimation accuracy of state 3 by 10.17%.Compared with SSRCQKF-2, the SSRCQKF-3 improves the estimation accuracy of the three states by 0.59%, 0.56%, and 2.76%, respectively.All these indicate that the proposed filter has the optimal performance in terms of estimation accuracy.Moreover, the RMSE variance of the two SSRCQKF are smaller than that of the other two filters, which indicates that the estimated fluctuation is more stable.

Simulation 2.
The proposed SSRCQKF is applied in target tracking in this section.The target is assumed to be in constant velocity (CV) motion; the state equation of CV model in two-dimensional case is described as follows: where X  = (  , ẋ  ,   , ẏ  ) Τ represents the target states (position and velocity) at time index , w  denotes the process noise, F CV and G denote the state transformation matrix and the noise driven matrix, which are defined, respectively, as follows: where  is the sampling interval.
In target tracking system, the bearings-only measurement equation is written as follows: where   is the radar measurement at time , (  ,   ) is the location of radar, and V  is the measurement noise.
The simulation results, including the position RMSE and velocity RMSE of various filters, are shown in Figures 4 and 5     respectively, and the partial magnification is given to show the details of the curve.It can be seen that the tracking accuracy of CKF is significantly lower than that of other filters, and the proposed SSRCQKF-2 and SSRCQKF-3 have achieved higher target tracking accuracy than that of SSRCKF.The average RMSE of the various filters is shown in Table 3, and compared with CKF, the estimated position accuracy of SSRCKF is improved by 20.85%, indicating that the spherical simplexradial cubature rule has higher accuracy than spherical-radial cubature rule in this simulation.Since the second-order Gauss-Laguerre rule is used in the SSRCQKF-2, the accuracy is improved by 1.86% compared to SSRCKF.Due to the more quadrature points used in SSRCQKF-3, its accuracy is higher than that of SSRCQKF-2 by 1.50%.Through the analysis of 2, . . ., 2 + 2 x−  + √( + 2 − √ 2 + 4) P −  [a −a] −2−2 ,  = 2 + 3, . . ., 4 + 4

Figure 4 :
Figure 4: The position RMSE of various filters.

Figure 5 :
Figure 5: The velocity RMSE of various filters.

Table 1 :
Average RMSE of state.