Abstract
Conventional low degree spherical simplex-radial cubature Kalman filters often generate low filtering accuracy or even diverge for handling highly nonlinear systems. The high-degree Kalman filters can improve filtering accuracy at the cost of increasing computational complexity; nevertheless their stability will be influenced by the negative weights existing in the high-dimensional systems. To efficiently improve filtering accuracy and stability, a novel mixed-degree spherical simplex-radial cubature Kalman filter (MSSRCKF) is proposed in this paper. The accuracy analysis shows that the true posterior mean and covariance calculated by the proposed MSSRCKF can agree accurately with the third-order moment and the second-order moment, respectively. Simulation results show that, in comparison with the conventional spherical simplex-radial cubature Kalman filters that are based on the same degrees, the proposed MSSRCKF can perform superior results from the aspects of filtering accuracy and computational complexity.
1. Introduction
Bayes’ rule has been widely applied into solution to state estimation problem for both linear and nonlinear systems [1, 2]. The application of Bayes’ rule to a probabilistic state space model generates the Bayesian filter, which can be generally divided into two categories, that is, the global approach and the local approach [3, 4]. The global approach makes no assumption of probability density function (pdf) and can achieve preferable performance at the cost of enormous computational burden, for example, the adaptive grid point-mass filter [5], the Gaussian mixture filter [6], and the particle filter [7]. The local approach is an approximation solution based on some assumption of pdf.
Under Gaussian assumption, the local approach can be derived using the minimum mean square error (MMSE) criterion [8] and thus be summarized as the calculation of the multidimensional integrals whose integrands bear the form nonlinear function × Gaussian pdf, which is intractable to directly solve [4]. The problem is thus transformed into the approximation of the nonlinear function or the Gaussian pdf [4]. The approximation of the nonlinear function is achieved mainly by utilizing different polynomial expansions, which yields a set of nonlinear filters, for example, the extended Kalman filter (EKF) [9–11], the divided difference filter (DDF) [12], the Chebyshev polynomial Kalman filter (CPKF) [13], and the Fourier-Hermit Kalman filter (FHKF) [14]. As the most commonly used one of this kind of nonlinear Kalman filters, the EKF performs the first-order Taylor series approximation of the nonlinear functions, which is appropriate for a “mild” nonlinear environment [9–11]. However, when the system is highly nonlinear, the truncated higher order terms of the nonlinear system may degrade the estimation accuracy [10]. An efficient approximation of the Gaussian pdf is therefore required. A series of nonlinear Kalman filters based on deterministic sampling strategy are proposed, for example, the unscented Kalman filter (UKF) [10, 15, 16], the cubature Kalman filter (CKF) [4, 17–19], the Gauss-Hermite quadrature filter (GHQF) [3], and the sparse grid quadrature filter (SGQF) [20]. For non-Gaussian noise environment, for example, impulse noise, the maximum correntropy Kalman filter (MCKF) [8] utilizes the maximum correntropy criterion (MCC) to combat heavy-tailed noises.
From the numerical integration perspective, these aforementioned nonlinear Kalman filters based on approximation of the Gaussian pdf are only different in numerical integration methods that are utilized to calculate the Gaussian weighted integrals in the filtering framework. An efficient numerical integral rule is thus required. Based on the spherical simplex-radial cubature rule [21, 22], a new class of nonlinear Kalman filters were proposed, that is, the third-degree SSRCKF and the fifth-degree SSRCKF. The SSRCKF can obtain a better filtering performance in comparison with the conventional CKF [22]. In this paper, a novel mixed-degree spherical simplex-radial cubature Kalman filter (MSSRCKF), which combines the third-degree spherical simplex rule and the fifth-degree radial rule, is proposed. The proposed MSSRCKF can improve filtering accuracy effectively at the cost of slightly increasing computational complexity.
2. Numerical Integration Based on Bayesian Filtering Framework
The general discrete-time nonlinear dynamic systems can be described aswhere denotes the state; denotes the measurement; and are both Gaussian noises with zero means and covariances and , respectively.
Based on (1), under the Gaussian assumption, the Bayesian filter can be divided into the following two steps: prediction and correction.
Predictionwhere represents the Gaussian distribution with the mean and the covariance .
Correctionwhere
From (2) to (4), the Bayesian filter is generalized as the calculation of the following integral:where denotes arbitrary nonlinear function.
Generally, it is intractable for directly calculating the integral of (5). Hence, we can obtain an approximated expression by utilizing the numerical integration theory; that is,where is the total number of points; represents the quadrature point and is the corresponding weight.
2.1. Spherical-Radial Transformation
For simplicity, let denote the numerical integration. can be regarded as a dth-degree rule of , if it is exact for with and is not exact for at least one polynomial of degree [4, 19, 22].
Consider the following integral:
Let with , where . It is difficult to directly find numerical approximation of (7). Therefore, an integral transformation is required and (7) is transformed into the following spherical-radial coordinate integral.where and represent the spherical surface and surface measure of the sphere, respectively [4].
According to the spherical-radial transformation, (8) can be decomposed into the spherical integral and the radial integral . Assume that and can be approximated by the following numerical integration:where and denote the quadrature point and the corresponding weight of ; and denote the quadrature point and the corresponding weight of ; and are the number of the points, respectively.
Therefore, applying (9) and (10) into (8) generates the following approximation:
2.2. Spherical Simplex Rule
The aforementioned spherical integration in (9) can be efficiently calculated using the transformation group of the regular -simplex [21, 22] with the vector , as follows:
The projection from the midpoints of the vector on the spherical sphere leads to the following points [21, 22]:For example, when the dimension , we can get that and .
Employing the central symmetry of the cubature formula and just treating the points and of the cubature points as in (9), we can obtain the third-degree spherical simplex rule with points as [22]and the fifth-degree spherical simplex rule with points as [22]where denotes the surface area of the sphere; the Gamma function is defined as with the properties of and .
2.3. Radial Rule
The radial integral in (10) can be approximated bySubstituting with the th-degree monomial , we obtainwhere represents an even integer.
Since the resultant spherical simplex-radial cubature rule is fully symmetric, we only need to match the even-degree monomials. Matching different even-degree monomials yields different quadrature points and weights. The third-degree radial rule [4, 19, 22] can be expressed as
Similarly, the fifth-degree radial rule [22] with can be obtained by
2.4. Conventional Spherical Simplex-Radial Rule
Based on (11), the spherical simplex-radial rule for the standard Gaussian distribution can be obtained as
For the calculation of the multidimensional Gaussian distribution , a linear transformation of is required [22], where is the final quadrature point combining the spherical simplex rule with the radial rule, and denotes the square root matrix of ; that is, .
Conventional spherical simplex-radial cubature Kalman filters are all based on the same th-degree spherical simplex rule and radial rule, respectively [22]. The degree for the spherical simplex-radial rule is thus up to the th-degree, for example, the third-degree spherical simplex-radial rule (3SSR) and the fifth-degree spherical simplex-radial rule (5SSR). However, it is interesting to note that the spherical simplex rule or radial rule with higher than th-degree can also be used to achieve the th-degree accuracy for integration. Hence, the degrees of the spherical simplex rule and radial rule are not necessarily required to be the same [19].
2.5. Mixed-Degree Spherical Simplex-Radial Cubature Kalman Filter
To improve filtering accuracy and reduce computational complexity, a novel mixed-degree spherical simplex-radial rule (MSSR), namely, the third-degree spherical simplex rule and the fifth-degree radial rule, is proposed in this paper.
Combining (14) and (19), we can get the MSSR as
The quadrature points and weights based on the MSSR are therefore denoted by
Substituting the numerical integration of (21) into the Bayesian filter framework from (2) to (4) generates the mixed-degree spherical simplex-radial cubature Kalman filter (MSSRCKF), which is summarized in Algorithm 1.
Algorithm 1 (mixed-degree spherical simplex-radial cubature Kalman filter).
(I) Initialization. Set the initial state estimate value and the covariance .
(II) Prediction
(1) Evaluate the sampling points and by (22), where , and .
(2) Calculate the points propagating through the state process equation(3) Estimate the predicted state value and the predicted error covariance matrix(III) Correction
(4) Regenerate the points and by (22), where , and .
(5) Calculate the propagated points(6) Estimate the predicted measurement value(7) Estimate the cross covariance matrix and the innovation covariance matrix(8) Calculate the Kalman gain(9) Update the state estimate and the corresponding error covariance matrix
Remark 2. In comparison with the 3SSRCKF [22] with points, the proposed MSSRCKF with points can achieve better filtering accuracy at the cost of almost the same computational complexity, which can be clearly seen in Figure 1. In comparison with the 5SSR, the MSSR with positive weights preferably achieves the “good” integral rule as summarized in [4]. Therefore, the MSSRCKF can obtain better filtering accuracy than the 5SSRCKF, which is shown in the following examples. In addition, the MSSRCKF with requires less computational complexity than the 5SSRCKF with points.
Remark 3. The other combination of the fifth-degree spherical simplex rule and third-degree radial rule, which requires quadrature points, has a relatively higher computational complexity. Similar to the 5SSRCKF, this mixed-order combination may introduce negative weights, which may lead to the filter instability in some high-dimensional systems. Hence, only the MSSRCKF that combines the third-degree spherical simplex rule and fifth-degree radial rule is presented in this paper.
3. Accuracy Analysis
The filtering performance of nonlinear Kalman filter is directly decided by the accuracy of state estimates [1, 18]. In this section, we will give the accuracy analysis about the proposed MSSR from the aspects of the estimated mean and its covariance of nonlinear function, which shows the filtering performance of the MSSRCKF theoretically.
3.1. Accuracy in Estimating the Mean of a Nonlinear Function
Assume that a small disturbance with zero mean and covariance is introduced around the mean . The Taylor series of a nonlinear function can be expressed as [1]where the operator is the differential of regarding the mean , which can also be denoted using the Jacobian matrix of ; that is, .
Thus, the th term in the Taylor series is given as
Taking expectations of (31), we can get that
Owing to the symmetry of the Gaussian distribution, all the odd terms in the above Taylor series are zero. Therefore, (33) can be rewritten bywhere the second term can be transformed as
Further, since , we have
Substituting (36) into (34), the true mean of nonlinear function can be expressed by
In (22), for simplicity, let and . The propagation of the th point through the nonlinear transformation as a Taylor expansion about can be written as
According to (24), the mean of the estimated nonlinear transformation can be expressed by
Since the odd moments for the points symmetrically distributed around are zero, we can obtain
After denoting (12) by the matrix form of with some mathematical operation, we have . The second-order term in (39) can be written as
Therefore, (40) can be rewritten as
Comparing (42) with (37), we can see that the mean estimated by the MSSRCKF agrees exactly with the true mean up to the third order with errors introduced in the fourth-order and higher order moments. Note that the lower moments are significant in practical applications, which have direct influences on filtering accuracy.
3.2. Accuracy in Estimating the Covariance of a Nonlinear Function
Given the true mean , the true covariance matrix can be calculated by
Combining (31) and (34), we can obtain that
Substituting (44) into (43) and employing the symmetry of the vector , the expected values of all odd ordered terms will be zero. Hence, the true covariance matrix can be expressed as
Since , (45) can be rewritten as
Given the mean of the estimated nonlinear transformation in (39), the estimated covariance matrix of the proposed MSSR is calculated bywhere
Sincethe estimated covariance matrix (47) can be simplified as follows:
Comparing (50) with the true covariance matrix (46), we can find that the estimated covariance matrix obtained by the MSSRCKF agrees exactly with the true covariance matrix up to the second order.
4. Simulations
In this section, two examples are presented to demonstrate the superiority of the proposed MSSRCKF. The simulation results were obtained in MATLAB R2011b using a computer with processor Inter(R) Core(TM)i5-4570 CPU @ 3.20 GHz. For better comparison, the following root mean square error (RMSE) is firstly defined:where denotes the total Monte Carlo runs and is used here; the subscript represents the th state, and are the real state and its corresponding estimated value at time instant in the th Monte Carlo run, respectively. The compared nonlinear Kalman filters are the 3SSRCKF and 5SSRCKF [22].
Example 1. Consider the following two-dimensional nonlinear stochastic system:where .
The real initial values and their corresponding estimates are set to and , respectively. The other parameters are set as and . For all the filters, the initial covariance matrix is given by .
Table 1 shows the compared filtering accuracy of all the filters. From Table 1, we can see that the proposed MSSRCKF can obtain the best filtering performance among the three filters. In addition, it is interesting to note that the MSSRCKF with only one more point than the 3SSRCKF requires less computational burden than the 5SSRCKF. Hence, the proposed MSSRCKF with less computational complexity can implement performance improvement of the 5SSRCKF.
Example 2. The well-known vertically falling body model is utilized in this section to further demonstrate the efficiency of the proposed MSSRCKF. The complete system equations are given as follows [23]:where the parameter is a known constant; , , and are the altitude, the velocity, and the ballistic coefficient in turn, all of which can be estimated using the radar range measurement. First, the fourth-order Runge-Kutta method [24] is used to discretize the continuous dynamic system (53). The discrete-time step size is seconds, and the sampling interval is 1 second.
The discrete-time range measurement at time instant is expressed bywhere denotes the horizontal distance between the falling target and the radar; is the altitude of the radar.
The real initial state values and the initial estimates are set as and , respectively. The other parameters are given as follows: , , , and . For all the filters, the initial covariance matrix is given by .
At the beginning, the altitude of the target is high and the change of the velocity can be ignored, which leads to ; the system can be considered as a linear one. When the target approaches the radar, the sensibility of the radar to the observation noise becomes more and more sensitive, and the nonlinearity degree of the system becomes higher. The tracking results of the altitude based on three filters are shown in Figure 2. We can clearly see from Figure 2 that the MSSRCKF always achieves the lowest RMSE among the three filters.
To further compare the filters, we also give the mean values of RMSE and the mean consumed time in Table 2. It can be seen that the proposed MSSRCKF requiring almost the same time as 3SSRCKF achieves the smallest mean value of RMSE. The same conclusions as Example 1 can be obtained in Example 2.
5. Conclusion
In this paper, the MSSRCKF is proposed to solve nonlinear estimation problems. The accuracy analysis shows that the mean estimation obtained by the MSSRCKF is accurate to the third-order moment of the true mean. In addition, the covariance estimated by the MSSRCKF agrees accurately with the true covariance matrix up to the second-order moment. Simulations show that the proposed MSSRCKF outperforms the conventional low degree SSRCKF at the cost of slightly increasing computational burden. It is also interesting to note that the MSSRCKF can achieve almost the same or even better filtering performance than the 5SSRCKF that may have the stability problem in high-dimensional nonlinear system. This is beyond our intuition, because the numerical integral rule of the 5SSRCKF is higher than that of the MSSRCKF. The theoretical analysis on this phenomenon is our future work.
Conflicts of Interest
The authors declare that there are no conflicts of interest regarding the publication of this paper.
Acknowledgments
This work was supported by National Natural Science Foundation of China (nos. 61671389, 61672436, 61372139, and 61571372), Fundamental and Frontier Research Project of Chongqing (no. cstc2014jcyjA40020), China Postdoctoral Science Foundation Funded Project (no. 2016M590853), and Fundamental Research Funds for the Central Universities (nos. XDJK2014B001, XDJK2016E029).