Abstract
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.
1. 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–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 high-precision 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 first-order 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–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–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.
In order to further improve the nonlinear Kalman filtering accuracy, this paper proposes a novel spherical simplex-radial cubature quadrature Kalman filter (SSRCQKF). The structure of this paper is as follows. The spherical simplex-radial cubature quadrature rule is proposed in Section 2, and the SSRCQKF algorithm is proposed in Section 3, the numerical simulation results of strong nonlinear system and target tracking are shown in Section 4, and finally the conclusion is given in Section 5.
2. Spherical Simplex-Radial Cubature Quadrature Rule
2.1. Spherical Simplex-Radial Cubature Rule
Consider the integral , let , where denotes the unit sphere surface that satisfies , denotes the sphere radius, and, then, can be decomposed into the following spherical integral and radial integral [10]:
In general, the analytical solutions of above integrals are difficult to obtain, so the numerical approximate method is considered. It is pointed out in [15] that the spherical integral can be approximated using the spherical simplex rule that contains integral points as follows:where denotes the surface area of n-dimensional unit sphere with representing the Gamma function, , and denotes the th vertex of the -dimensional simplex, whose elements are defined as follows:
2.2. Gaussian-Laguerre Quadrature Rule
For the radial integral , let and we get ; furthermore, let and ; then is obtained; the integral term in is approximated using the following Gaussian-Laguerre quadrature rule [20]:where denote the quadrature points, which can be solved from the solutions of the following -order Chebyshev-Laguerre polynomial.
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.
2.3. 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
Due to , 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 points and corresponding weights. In particular, it can be solved that , when , and results in the spherical simplex-radial 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 .
3. SSRCQKF Algorithm
3.1. The General Form of SSRCQKF
Consider the following discrete nonlinear system with additive noise:where denotes the state vector, denotes the measurement vector, and the noises , are uncorrelated Gaussian white noise. With the system dimension and the order of Chebyshev-Laguerre polynomial being known, , can be solved by (5), and the corresponding weights can be calculated as follows:
The matrix consists of that is used to construct the following expansion matrix . The subscript in 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.
3.2. The Specific Form of SSRCQKF When
The general form of SSRCQKF is presented in Section 3.1, and, in this section, the specific form of SSRCQKF algorithm when is given. When , (7) is simplified as follows:
The values of and are needed to be calculated. Plug into (5), we obtain
The item is expanded to achieve and its derivative as follows:
Let , the solutions are , combined with , and we obtain
Then, and are solved from (6) as follows:
Furthermore, the weights are solved as follows:
The spherical simplex-radial cubature quadrature rule with is obtained by plugging , , , and into rule (9) as follows:
Based on (29), the calculation steps of the specific form of SSRCQKF when are given as follows.
Step 1 (filter initialization). One hasCycle , and complete the following steps.
Step 2 (time update). One has
Step 3 (measurement update). One has
Step 4 (state update). One has
4. Simulation Results and Analysis
4.1. Simulation 1
The effectiveness of the proposed SSRCQKF algorithm is verified by the following three-dimensional strong nonlinear system, which includes trigonometric function operation, power operation, and exponential operation.where , , the theoretical initial value of the nonlinear system is , the filtering initial value is , the initial covariance matrix is . The SSRCQKF-2 (when ) and SSRCQKF-3 (when ) 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.
4.2. 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 represents the target states (position and velocity) at time index , denotes the process noise, and 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 is the measurement noise.
In the simulation, the radar location is set to be , the simulation time is 40 s, , and the initial state of the target is . The initial state and covariance matrix are and , respectively. The standard deviation of the measurement noise is 0.1 deg. The Monte-Carlo simulation is performed 500 times, and the tracking accuracy is also evaluated using the RMSE.
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 simplex-radial 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 the simulation results, the effectiveness of the proposed filter is verified.
5. Conclusion
In order to further improve the filtering accuracy of the nonlinear system, this paper proposes a SSRCQKF algorithm by combining the spherical simplex-radial cubature rule with arbitrary order Gaussian-Laguerre quadrature rule. The proposed algorithm has a higher filtering accuracy than CKF and SSRCKF. The results of the two numerical simulations, including the three-dimensional strongly nonlinear system and target tracking, verify the validity of the proposed algorithm.
Conflicts of Interest
The authors declare that there are no conflicts of interest regarding the publication of this paper.