Double-Layer Cubature Kalman Filter for Nonlinear Estimation

The cubature Kalman filter (CKF) has poor performance in strongly nonlinear systems while the cubature particle filter has high computational complexity induced by stochastic sampling. To address these problems, a novel CKF named double-Layer cubature Kalman filter (DLCKF) is proposed. In the proposed DLCKF, the prior distribution is represented by a set of weighted deterministic sampling points, and each deterministic sampling point is updated by the inner CKF. Finally, the update mechanism of the outer CKF is used to obtain the state estimations. Simulation results show that the proposed algorithm has not only high estimation accuracy but also low computational complexity, compared with the state-of-the-art filtering algorithms.


Introduction
Nonlinear estimation has been widely used in many fields, such as information fusion, computer vision, engineering, and economics [1][2][3][4]. Some classic estimation algorithms are put forward to deal with nonlinear systems. These filters can be classified as two categories: point estimation (e.g., the extended Kalman filter [5], unscented Kalman filter [6,7] and cubature Kalman filter [8][9][10]), and density estimation (e.g., the particle filter [11,12]). The former uses moment and density function to characterize variables [13] as the latter approximates posterior density function on the basis of bayesian formula [14]. The extended Kalman filter (EKF) [5] has been widely used in dealing with nonlinear systems. By using Taylor series expansion, it linearizes nonlinear system equations and can be applied in nonlinear systems. However, as it must obey the Gaussian distribution, in strongly nonlinear [15] and non-Gaussian systems, the EKF will perform poorly [16]. Based on unscented transform (UT), the unscented Kalman filter (UKF) [6,7] can obtain a higher filtering accuracy than the EKF. However, the weights of the sigma points in the UKF are prone to be negative in high-dimension systems, leading to numerical instability as well as low estimation accuracy. The cubature Kalman filter (CKF) [8][9][10] uses the spherical-radial cubature rule to generate some weighted sampling points to approximate integral in Bayesian estimation. It has estimation accuracy up to three order and is widely used in high order nonlinear systems [17]. Therefore, the CKF algorithm is mainly discussed in this paper. However, in non-Gaussian systems, the CKF will degrade. Further, it has numerical instability in the process of practical applications [18].
in the outer CKF of the DLCKF is added as well as the flowsheet of the DLCKF is revised for better formulations; (d) four algorithms (UKF, ICKF, UPF, RUCKF) and applications in real-world scenario are added in the simulation to demonstrate the excellent performance of the DLCKF.
The rest of this paper is organized as follows. In Section 2, the fundamentals of the CKF and CPF are briefly reviewed. Section 3 presents a new DLCKF algorithm based on the CKF. Then, simulation results are provided to demonstrate the performance of the proposed algorithm in Section 4. At last, some conclusions are given in Section 5.

Cubature Rule
The problem of non-linear integrating in Gauss estimation can be expressed as In order to compute the above numerical integration, the following two steps are taken. First, the integration is transformed into a form of spherical-radial cubature, and then three-degree spherical-radial cubature rule is designed. The specific method is as follows: In the cubature rule, the key is the transformation from vector x to radius r and direction vector y. Supposing x = ry, y T y = 1,then x T x = r 2 , r ∈ [0, +∞). So, Equation (1) in the spherical-radial coordinate system can be expressed as where U n x denotes sphere surface with radius 1, that is U n x = {y ∈ R n x |y T y = 1}, σ(·) represents spherical measure. So, Equation (2) can be rewritten. m r points and m s points are generated by the radial rule and the spherical rule respectively [18]. So, m r × m s points produced by spherical-radial cubature rule can be expressed as For the three-degree spherical-radial cubature rule, there are m r = 1 and m s = 2n x . Therefore, 2n points are needed. Accordingly, calculating a standard Gauss weight integral can be expressed as where m = 2n x ; Given x ∼ N(x;x, P),P = SS T ,then the Equation (8) can be transformed into

Cubature Kalman Filter
Consider the discrete-time state stochastic system, whitch is also used in [20,24] : where k denotes the discrete time index, x k ∈ R m is the state vector, z k ∈ R n is the measurement vector. w k−1 ∈ R m and v k ∈ R n denote the process noise vector and the measurement noise vector. The process noise and the measurement noise are zero-mean random variables with known probability density functions. The functions f (·) and h(·) are assumed to be known. The process of the CKF Algorithm 1 is summarized as follows: The CKF algorithm [8] for one round Input:x k−1 , P k−1 , f , h Output:x k , P k 1: Time update: Generate m cubature points and corresponding weights: Obtain predicted state and predicted covariance: Generate m integration points and corresponding weights: ζ j,k|k−1 = chol(P k|k−1 )χ i +x k|k−1 , w j = 1/m Propagate the integration point: ε j,k|k−1 = h(ζ j,k|k−1 ) 3: Obtain predicted measurement,covariance and mutual covariance: Obtain state estimation and corresponding covariance:

Cubature Particle Filter
Based on particle filter (PF), the cubature particle filter (CPF) [29,30] adopts the CKF as the importance probability distribution function. Although it is suitable for Gaussian systems and non-Gaussian systems, it may result in heavy computational load owing to the use of a large number of particles. The process of the CPF can be summarized as follows: (2) Use CKF to update each particle; (3) Use latest measurements to update each weight w k−1 , y k , and then normalize them; (4) Calculate the number of effective particles N e f f , if N e f f < N threshold , resample; (5) Obtain state estimationx k and covariance P k by accumulating particles at time k; (6) Get state estimation at each time by repeating steps (2) to (5).

Double-Layer Cubature Kalman Filter (DLCKF)
Since CPF needs a large number of random particles to approximate the posterior density function of the state, it results in heavy computational load. In contrast to CPF, the proposed DLCKF uses deterministic weighted sampling points to approximate the posterior density function. The core idea of DLCKF is using the inner CKF to update each sampling point, and then the weight of each sampling point is updated with the latest measurement. Next, weighted summation of the updated sampling points is performed to obtain the initial state estimation for the next time. The obtained initial state estimation is used as the predictive value to run the outer CKF algorithm. Final, the state estimation is obtained.
The proposed DLCKF consists of inner-layer CKF and outer-layer CKF. The process of DLCKF is given as follows: Initial value isx 0 = E [x 0 ], and initial covariance can be expressed as are obtained by cubature rule. Then the inner-layer CKF is used to update each cubature point.

The inner-layer CKF:
The cubature point χ j,i = √ m/2 [1] j,i with corresponding weight w j,i = 1/m are obtained by three-degree spherical-radial cubature rule, where m denotes the total number of cubature points, which is twice the state dimension, that is m = 2n x , and n x is the dimension of the state. [1] ∈ R n x can be written as Then, based on the cubature point χ j,i m j=1 and its weight w j,i m j=1 , the cubature point of the inner-layer CKF is obtained. Then, we have the time update and measurement update steps of the inner-layer CKF as follows.
Time update: Measurement update: The outer-layer CKF: After the cubature point updated by the inner CKF, its weight can be caculated as follows.
Normlize the weight as follows: Then, the initial estimation with corresponding covariance at time k can be expressed aŝ Based onx k|k−1 and P k|k−1 , the cubature points can be updated as: Repeating Equations from Equation (10) to (36) state estimationx of the proposed DLCKF at each time can be obtained.
The process of the proposed DLCKF algorithm is summarized as Figure 1:

Simulation Experiments
In this section, to illustrate the effectiveness of the proposed algorithm, the DLCKF is compared with CKF [8][9][10], UKF [6,7], ICKF [22], CPF [29,30], UPF [34,35] and RUCKF [24] in two classical filtering applications and a real-world scenario. The root mean squared error (RMSE) is used to indicate the estimation performance. All simulations are executed on an Intel Core i5-4200H CPU 2.80 GHz personal computer with 8 GB of random-access memory.

Single-Dimensional Scenario
We first consider the following univariate nonstationary growth model, which has been also used in [36,37] for its high nonlinearity. Its state space model can be written as: where process noise ω k obeys gamma distribution Ga (3, 2), and measurement noise v k+1 obeys Gaussian distribution with mean of 0 with variance of R = 10 −5 . The initial position is x 0 = 3. The iterative number of ICKF is 10 and the particle number of the CPF as well as UPF is set as 100.
The Monte Carlo times are 300 and the simulation time is 30 s. The simulation results are shown in Figure 2. It can be seen from Figure 2 that the RMSE of the CKF is similar to that of the UKF because they both have three-order accuracy of Gaussian distribution. The performance of the CPF and the UPF is slightly better than those of the CKF and the UKF, since the former make certain improvements to the latter. The ICKF shows better performance than all above methods to some extent as the result of its iterative process which applies state estimation and covariance of each timestep as the initial value. The RUCKF is adaptable in single-dimensional scenario and therefore its RMSE is low. The proposed algorithm performs best at each timestep, which indicates the improvement of the DLCKF is more significant than that of others.
The number of the particles in the CPF and UPF is gradually increased from 100 to 500. The single run time and average RMSE of each algorithm are shown in Table 1.  Table 1 illustrates the performance of the run time and average RMSE of each algorithm. It can be seen that the CKF and the UKF take the least time cost, and the CPF and the UPF takes the longest time cost. In the CPF and the UPF, as the number of particles increases, the run time gradually becomes longer. In terms of RMSE, the DLCKF is much smaller than that of the other five methods. The ICKF and RUCKF have lower RMSE than the CKF due to their iteration. As the number of the CPF and UPF increases, their RMSE also decreases. When the particle number is 500, the RMSE of the CPF and the UPF is still twice that of the DLCKF. The above results all indicate that in single-dimensional scenario, the proposed DLCKF has better performance than the other algorithms.

Multi-Dimensional Scenario
Consider tracking a 2-D uniform linear motion [38], while the state space model expressed as: where, x k = [x k ,ẋ k , y k ,ẏ k ] is the state variable that represents the position and velocity of the x axis and y axis, respectively , the process noise ω k obeys a Gaussian distribution with zeros mean and variance matrix Q , and F and Q are written as In Equation (40), z k+1 = [r k+1 , θ k+1 ] T denotes the measurement variable, which respectively represents the radial distance and azimuth angle of the target, v k+1 denotes the measurement noise, which is flicker noise and can be expressed as: The measurement equation h(·) can be written as: In Equation (43), R 1 and R 2 can be expressed as In this simulation, the initial value of the state is (20,000 m, −160 m/s, 40,000 m, −150 m/s). The iterative times of the ICKF is set as 10, the particle number of the CPF and the UPF is 300. Other parameters are set as Table 2. The simulation time is 100 s, and the Monte Carlo times is set as 300. The RMSE of position can be expressed as: The result of simulation is as Figure 3. Figure 3 shows the position RMSE of each algorithm. It can be seen that the ICKF, the CPF and the UPF are superior to the CKF and the UKF and the RUCKF is not suitable for the multi-dimensional scenario. The performance of the DLCKF is the best. As a result, the RMSE of the DLCKF is the lowest in the above filtering methods. This also shows that the proposed DLCKF performs greatly on the 2-D uniform linear motion.
In the 2-D uniform linear motion, the number of the particles in the CPF and UPF increases gradually from 300 to 1000. The single run time and the average RMSE of each algorithm are shown as Table 3.
From Table 3, it can be seen that in terms of the running time, the proposed DLCKF is slightly longer than that of the CKF and UKF, but it is far less than that of the CPF and UPF. In the CPF and UPF, as the number of particles increases, their computational time also increases. In terms of average RMSE, as the particle number of the CPF and UPF increases, their average RMSE also gradually reduces. However, compared with the DLCKF, the RMSE of the CPF and UPF is still sizable. All of this indicate that the proposed DLCKF has a great estimation effect in 2-D uniform linear motion.

Real-World Scenario
Measurement data of the real-world scenario is obtained by a real radar sensor. A given target appears in a 6 km × 2 km area covered by the sensor. The given target is a car driving along a road in this area with a constant velocity. The initial position of the target is (−1443.6 m, −18.5454 m ), the running speed of the target is almost 32 km/h, and scanning period is 8 s. All illustration of the sensor and the target is presented in Figure 4. In this experiment, a 2-D uniform linear motion model is applied and measurement value comes from the real radar sensor. We manually wipe off the measurements which are not from the certain target we concerned.
The results of experiments are shown in Figures 5 and 6.   Figures 5 and 6 shows the estimation error of position X, Y of each algorithm in real-world scenario. It can be seen that the CPF and the UPF are superior to the CKF, the UKF, the ICKF and the RUCKF. The DLCKF performs best, which indicates that the proposed DLCKF can obtain great estimation in the real-world scenario.
In this simulation, the number of the particles in the CPF and UPF is 300. The single run time and the average estimation error (AEE) of position X, Y of each algorithm are shown as Table 4. From Table 4, it can be seen that in terms of the running time, the proposed DLCKF has slightly longer timecost than that of the CKF and UKF, but it is far less than that of the CPF and UPF. In terms of average estimation error of position X and Y, the CPF and the UPF perform better compared with the CKF and the UKF, and the DLCKF performs best. All of those demonstrate that the proposed DLCKF has better performance than other algorithms in real-world scenario.

Conclusions
The DLCKF that is based on CKF is proposed in this paper, which uses the cubature points to approximate the prior density function of the state. The cubature points are updated by the inner-layer CKF, meanwhile, the weight of each cubature point in the outer-layer CKF is updated with the latest measurement. Finally, the state estimations at each time are obtained through the update mechanism of the outer-layer CKF. The experiments results show that in one-dimensional and multi-dimensional simulation scenarios, the DLCKF performs best compared with the CKF, UKF, ICKF, CPF, UPF and RUCKF. In addition, the superiority of the DLCKF is its effectiveness of in a more challenging environment, which brings great challenges to classic algorithms. The experiments in real-world scenario confirm it. All of those indicate that the proposed DLCKF proposed can obtain an outstanding estimation accuracy with low time cost, compared to classic algorithms.