Mixed-Degree Spherical Simplex-Radial Cubature Kalman Filter

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 novelmixed-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 proposedMSSRCKF can perform superior results from the aspects of filtering accuracy and computational complexity.


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 pointmass 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][10][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][10][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][18][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 simplexradial 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.

Numerical Integration Based on Bayesian Filtering Framework
The general discrete-time nonlinear dynamic systems can be described as where x  ∈ R  denotes the state; z  ∈ R  denotes the measurement; q −1 and r  are both Gaussian noises with zero means and covariances Q −1 and R  , respectively.Based on (1), under the Gaussian assumption, the Bayesian filter can be divided into the following two steps: prediction and correction.
where (x −1 ; x−1 , P −1 ) represents the Gaussian distribution with the mean x−1 and the covariance P −1 . Correction where From ( 2) to (4), the Bayesian filter is generalized as the calculation of the following integral: where f(⋅) 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.

Spherical-Radial Transformation.
For simplicity, let (f) = ∑    f(  ) denote the numerical integration.(f) can be regarded as a dth-degree rule of (f), if it is exact for with ∑  =1   ≤  and is not exact for at least one polynomial of degree ∑  =1   ≤  + 1 [4,19,22].Consider the following integral: Let x = y with y  y = 0, where  = √ x  x.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   = {y ∈ R  | y  y = 1} 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 () = ∫   f(y)(y) and the radial integral  = ∫ ∞ 0 () −1  − 2 .Assume that () and  can be approximated by the following numerical integration: ≈ where y  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 a  = [ ,1 ,  ,2 , . . .,  , ]  ,  = 1, 2, . . .,  + 1 as follows: The projection from the midpoints of the vector a  on the spherical sphere leads to the following points [21,22]: For example, when the dimension  = 2, we can get that Employing the central symmetry of the cubature formula and just treating the points a  and b  of the cubature points as y  in (9), we can obtain the third-degree spherical simplex rule with 2 + 2 points as [22] and the fifth-degree spherical simplex rule with  2 + 3 + 2 points as [22] where   = 2Γ  (1/2)/Γ(/2) = 2 √   /Γ(/2) denotes the surface area of the sphere; the Gamma function is defined as

Radial Rule.
The radial integral in (10) can be approximated by Substituting () with the th-degree monomial   , we obtain where  represents an even integer.
Since the resultant spherical simplex-radial cubature rule is fully symmetric, we only need to match the evendegree monomials.Matching different even-degree monomials yields different quadrature points and weights.The thirddegree radial rule [4,19,22] can be expressed as Similarly, the fifth-degree radial rule [22] with   = 2 can be obtained by 2.4.Conventional Spherical Simplex-Radial Rule.Based on (11), the spherical simplex-radial rule for the standard Gaussian distribution (x; 0, I  ) can be obtained as For the calculation of the multidimensional Gaussian distribution (x; x, P), a linear transformation of √ P  + x is required [22], where   is the final quadrature point combining the spherical simplex rule with the radial rule, and √ P denotes the square root matrix of P; that is, P = √ P √ P  .
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].

Mixed-Degree Spherical Simplex-Radial Cubature Kalman
Filter.To improve filtering accuracy and reduce computational complexity, a novel mixed-degree spherical simplexradial rule (MSSR), namely, the third-degree spherical simplex rule and the fifth-degree radial rule, is proposed in this paper.
(I) Initialization.Set the initial state estimate value x0 and the covariance P 0 .
(5) Calculate the propagated points (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 2 + 2 points, the proposed MSSRCKF with 2 + 3 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 2+3 requires less computational complexity than the 5SSRCKF with  2 + 3 + 3 points.
Remark 3. The other combination of the fifth-degree spherical simplex rule and third-degree radial rule, which requires  2 + 3 + 2 quadrature points, has a relatively higher computational complexity.Similar to the 5SSRCKF, this mixedorder combination may introduce negative weights, which may lead to the filter instability in some high-dimensional systems.Hence, only the MSSRCKF that combines the thirddegree spherical simplex rule and fifth-degree radial rule is presented in this paper.

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.

Accuracy in Estimating the Mean of a Nonlinear Function.
Assume that a small disturbance Δx with zero mean and covariance P is introduced around the mean x.The Taylor series of a nonlinear function y = f(x) can be expressed as [1] where the operator  Δ f = [(Δx  ∇)f(x)  ]  | x=x is the differential of f(⋅) regarding the mean x, which can also be denoted using the Jacobian matrix of f(⋅); that is,  Δ f = FΔx.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 by where the second term can be transformed as Further, since [ΔxΔx  ] = P, we have Substituting ( 36) into (34), the true mean of nonlinear function can be expressed by In (22), for simplicity, let   = [ √ Pa]  and   = √  + 2  .The propagation of the th point through the nonlinear transformation as a Taylor expansion about x 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 x are zero, we can obtain After denoting (12) by the matrix form of a with some mathematical operation, we have aa  = (( + 1)/)I  .The second-order term in (39) can be written as  2 ( + 2) ( + 1) 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.

Accuracy in Estimating the Covariance of a Nonlinear
Function.Given the true mean y true , 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 Δx, the expected values of all odd ordered terms will be zero.Hence, the true covariance matrix can be expressed as Since [ Δ f( Δ f)  ] = [FΔx(FΔx)  ] = FPF  , (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 by where Since  2 ( + 2) ( + 1) the 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.

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  = 100 is used here; the subscript  represents the th state,   , and x , 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  = 0.001.
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 MSSR-CKF 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;  1 (),  2 (), and  3 () 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 1/64 seconds, and the sampling interval is 1 second.The discrete-time range measurement at time instant  is expressed by  where  1 denotes the horizontal distance between the falling target and the radar;  2 is the altitude of the radar.The real initial state values and the initial estimates are set as x 0 = [3 × 10 5 , 2 × 10 4 , 1 × 10 −3 ]  and x0 = [3 × 10 5 , 2 × 10 4 , 3 × 10 −5 ]  , respectively.The other parameters are given as follows:  = 5 × 10 −5 ,  1 =  2 = 1 × 10 5 , Q −1 = 0, and   = 1 × 10 4 .For all the filters, the initial covariance matrix is given by P 0 = diag([1 × 10 6 , 4 × 10 6 , 1 × 10 −4 ]).
At the beginning, the altitude of the target is high and the change of the velocity can be ignored, which leads to − − 1 () ≈ 0; 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.

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.

Figure 1 :
Figure 1: Comparison of the number of points required by different filters versus dimension.

Figure 2 :
Figure 2: RMSE of all the filters about the target falling altitude.

Table 1 :
Comparison of the mean values of RMSE about state  1 among all the filters.

Table 2 :
Filtering performance comparison about the target falling altitude.