Time-Varying Noise Statistic Estimator Based Adaptive Simplex Cubature Kalman Filter

To address the problem that filtering accuracy is reduced with the inaccurate time-varying noise statistic in conventional cubature Kalman filter, a noise statistic estimator based adaptive simplex cubature Kalman filter is put forward in this paper. First, the simplex cubature rule is adopted to approximate the intractable nonlinear Gaussian weighted integral in the filter. Secondly, a suboptimal unbiased constant noise statistic estimator is derived based on themaximum a posteriori estimation criterion. For the time-varying noise, the above estimator is modified using an exponential weighted attenuation method to realize the oblivion of stale data which results in a fading memory estimator, which has the ability to estimate the time-varying noise statistic to revise the filter online. The simulation results indicate that the proposed filter can achieve higher accuracy than conventional filters with inaccurate noise statistic.


Introduction
Control accuracy is generally affected by the state feedback accuracy in a closed-loop control system.On account of the state feedback being easily contaminated by noise or being prevented from being measured directly, the optimal estimate of the state should be considered.The Kalman filter is a type of real-time signal processing method which adopts the state space method to model linear systems and achieve the minimum variance estimate of the state in the form of recursion [1] and has been widely used in control systems.
In practical applications, the actual systems generally contain nonlinear characteristics which are described by nonlinear models.Correspondingly, the system states are required to be estimated using the nonlinear Kalman filters of which the extended Kalman filter (EKF) has been most widely used over the past few decades [2,3].The EKF adopts the Taylor series expansion to obtain the linear approximation of the nonlinear systems, and then the standard Kalman filter is applied.However, the accuracy of EKF is unsatisfactory for strong nonlinear systems in which the linearity error of the model may seriously affect filtering accuracy and even lead to the filter divergence [4].Moreover, the EKF requires differentiation of the nonlinear system models where the Jacobian matrix needs to be calculated, which is computationally cumbersome in many applications [5].
Julier et al. [6] propose the unscented transformation (UT) based unscented Kalman filter (UKF), which has thirddegree filtering accuracy [7,8].The UKF is a deterministic filter where a series of sigma points are selected according to the certain criteria and the a posteriori mean and covariance are approximated using nonlinear propagation of these sigma points [9].The UKF is a derivative-free filter which has no restrictions on the system models.However, as the core of the UKF, UT has some tunable parameters, and the selection of which lacks a rigorous mathematical basis.As for high-dimensional systems, the weight on the center point is negative and the semidefinite of the covariance cannot be guaranteed, which may reduce the numerical stability of the system [10].
Arasaratnam et al. [11,12] propose the cubature Kalman filter (CKF) based on the cubature rule (CR).By means of coordinate transformation, the intractable Gaussian weighted integral is decomposed into the spherical integral and radial integral, which are approximated using various numerical rules to deduce the CR.The CKF uses a set of cubature points with equal weights to approximate the a posteriori mean and covariance and achieves higher stability than the UKF [13][14][15].Furthermore, the CKF can be regarded as a special case of the UKF with  = 0 [16,17], whereas the CKF gives the reason for  = 0 theoretically for the first time from another perspective.In the calculation framework of the CKF, Wang et al. [18] and Shovan and Swati [19,20] modify the spherical integral and radial integral, respectively, and put forward the spherical simplex-radial cubature Kalman filter and the cubature quadrature Kalman filter, both of which have better performance compared with the CKF.
The prior noise statistic is required exactly for the abovementioned filters.However, in practical applications, the system noise statistic is often unknown and varies as a function of time, so the estimate accuracy may be significantly reduced.In order to address the problem that the filtering accuracy of the conventional filters reduces in the case of inaccurate time-varying noise statistic, a noise statistic estimator based adaptive simplex cubature Kalman filter (ASCKF) is proposed in this paper.The spherical simplex rule and the first-order generalized Gauss-Laguerre quadrature rule are adopted to derive the simplex cubature rule, which is used to approximate the a posteriori mean and covariance in the nonlinear Kalman filter framework.The noise statistic estimator is designed based on the maximum a posteriori (MAP) estimation criterion [21], and the fading memory exponential weighted attenuation method is utilized to achieve the oblivion of stale data and emphasize the new data.This allows the online real-time estimation of the time-varying noise statistic.The designed noise statistic estimator is used to revise the filter and to improve the filtering accuracy with the inaccurate noise statistic.Simulation results have verified the validity of the proposed filter.
The rest of this paper is organized as follows: the simplex cubature Kalman filter is provided in Section 2. The noise statistic estimator is derived in Section 3. The simulation results and analysis are presented in Section 4. The conclusions are given in Section 5.

Simplex Cubature Kalman Filter
2.1.Simplex Cubature Rule.The key problem in nonlinear Kalman filter is to calculate the intractable nonlinear Gaussian weighted integral as   = ∫ R  g(x)(x; x, P  )x, where x ∈ R  , g(x) represents the arbitrary nonlinear function, and (x; x, P  ) denotes the Gaussian distribution with mean x and covariance P  .It is generally difficult to obtain the analytical solution of   ; hence the high accuracy numerical approximation method should be taken into account.

Spherical Simplex Rule for Spherical
Integral.Specifically, the coordinate transformation for integral of the form   = ∫ R  g(x)exp(−x Τ x)x is considered first [11].Let x = s, where s = ( 1 ,  2 , . . .,   ) Τ represents the direction vector such that s Τ s = 1 with  = √ x Τ x ≥ 0. Then   can be decomposed into the following two types of integrals, namely, the spherical integral () and the radial integral , respectively.
The third-degree spherical simplex rule [18], which consists of 2 + 2 points, is adopted to approximate the spherical integral () in (1) as follows.
where   and ω denote the quadrature points and the corresponding weights, respectively. represents the number of the points.The points   can be obtained by solving the following -order Chebyshev-Laguerre polynomial [20].
The weights ω are calculated using For the third-degree accuracy of the radial integral,  should be chosen 1, and  1 = /2 with ω1 = Γ(/2) are obtained; thus the radial integral turns to be the following form.
has the following equivalent form: The simplex cubature rule, which is used to approximate the nonlinear Gaussian weighted integral, is derived by combining ( 9) and (10) as follows.
where the cubature points x() and the weights   are given as follows: where a = [a 1 , a 2 , . . ., a +1 ] and the matrix subscript [⋅]  represents the th column.

The Simplex Cubature Kalman Filter.
The following discrete-time nonlinear system is taken into account.
where x  ∈ R  and z  ∈ R  denote the state and measurement vectors, respectively, and w  and k  represent the white Gaussian process noise and measurement noise, respectively, with the mean being q  and r  , respectively, and the covariance being Q  and R  , respectively.
In the nonlinear Kalman filter framework, the a posteriori mean and covariance are calculated using the simplex cubature rule given in (11), and the simplex cubature Kalman filter (SCKF) is derived as follows.
The a posteriori state estimate x+ −1 and covariance P + −1 at time  − 1 are used instead of x and P  in (12) to calculate the cubature points x() −1 , which are propagated using the nonlinear function f(⋅) to obtain the following points X ()   .
The prior state estimate x−  at time  is calculated using the propagated points X ()   as follows. x where   is given in (13).
The prior error covariance P −  at time  is calculated as follows.
The prior state estimate x−  and covariance P −  at time  are used instead of x and P  in (12) to calculate the cubature points x()  , which are propagated using the nonlinear function h(⋅) to obtain the following points Z ()   .
The predicted measurement ẑ is calculated using the propagated points Z ()   as follows.
The measurement covariance P , is calculated as follows.
The cross covariance P , is calculated as follows.
The Kalman filter gain K  is calculated as follows.
The a posteriori state estimate x+  at time  is calculated as follows.
The a posteriori error covariance P +  is calculated as follows.
It can be seen from ( 15), ( 17), (18), and (20) that the exact noise statistics are required in SCKF, and the filtering accuracy may reduce if the inaccurate noise statistics are substituted.

Noise Statistic Estimator
In this section, two noise statistic estimators, including the constant noise statistic estimator and the time-varying noise statistic estimator, are given to realize the online estimation of the noise statistic.
Mathematical Problems in Engineering 3.1.Noise Statistic Estimator.The MAP estimates of q, Q, r, R, and X  can be obtained by calculating the maximum value of the following conditional probability density function [21].
According to the property of the conditional probability function, (25) has the equivalent form as L = (X  , q, Q, r, R, Z  )/(Z  ), and the problem is transformed into calculating the maximum value of the following equation on account of (Z  ) being independent of the optimization.
where (q, Q, r, R) can be regarded as a constant since it is obtained using the prior information.
The terms (X  | q, Q, r, R) and (Z  | X  , q, Q, r, R) in (26) can be calculated using the multiplicative theorem of conditional probability as follows.

𝜕
where e  = z  − ẑ is the filtering residual.

Time-Varying Noise Statistic
Estimator.For the timevarying noise statistic, we should emphasize the effect of new data and gradually forget the effect of the stale data, so as to achieve real-time estimation of the time-varying noise statistic.For this, the fading memory exponential weighted attenuation method is considered.The weighting factor   is selected such that   =  −1 , ∑  =1   = 1, where 0 <  < 1 denotes the forgetting factor, and then we have Each term in (32) is multiplied by  +1− to replace the original factor 1/, and the fading memory time-varying noise statistic estimator is derived as follows.
However, for the high-dimensional systems, Q and R may dissatisfy the semipositive definite and positive definite conditions, respectively, and result in the filter divergence.To solve this problem, the following modified noise statistic estimator can be used instead to ensure positivity all through the process of recursion.
From the above, the time-varying noise statistic estimator, which is suitable for nonlinear filter, is summarized in the recursive form as follows.
The above derived time-varying noise statistic estimator is used in the SCKF to carry out the online estimation of the unknown noise and results in the ASCKF, whose specific calculation steps are given below.
Step 2 (time update).The cubature points x() −1 are calculated as SCKF in Section 2.2, and the noise estimator q−1 is used instead of q −1 to obtain the propagated points X ()   as follows.
The prior state estimate x−  at time  is calculated as (16).The prior error covariance P −  at time  is calculated using the noise estimator Q−1 as follows.
Step 3 (measurement update).The cubature points x()  are calculated as SCKF in Section 2.2, and the noise estimator r is used instead of r  to obtain the propagated points Z ()   as follows.The predicted measurement ẑ is calculated as (19).The measurement covariance P , is calculated using the noise estimator R as follows.
The cross covariance P , , the Kalman filter gain K  , the a posteriori state estimate x+  , and the a posteriori error covariance P +  are calculated as (21) to (24), respectively.
On account of the recursion equations of noise statistic estimator being not independent, the noise w  and k  cannot be estimated at the same time in general; otherwise it may cause the filtering divergence.

Simulation Results
In this section, a three-dimensional nonlinear system is taken into account to test the performance of the proposed filter.The nonlinear system model is given as follows.
[  where x  = ( 1, ,  2, ,  3, ) Τ denotes the system state, w  represents the process noise with the mean and covariance being zero and Q  , respectively, and V  is the measurement noise with the mean and covariance being   and   , respectively.
In this simulation, the true initial state and covariance are set to be x 0 = (0.2, 0.5, 0.2) Τ and Q  = 0.01I 3 , respectively.The true values of the noise, including the mean and the timevarying covariance, are set to be   = 0.5 and   given below.
However, the true values of the noise are supposed to be unknown in application, so that the mean and the covariance of the noise used in the filters are chosen to be 0 and 0.1, respectively, which are inaccurate.The initial state and covariance of the filter are x+ 0 = (0.2, 0.5, 0.2) Τ and P + 0 = 0.01I 3 , respectively.The initial values of the noise statistic estimator are chosen to be r0 = 0 and R0 = 0.1, respectively.The forgetting factor,  = 0.99, and the total steps are set to be 2000.
The proposed ASCKF is compared with the CKF and SCKF, and the estimation accuracy is evaluated using the root mean square error (RMSE), which is defined below.The simulation results based on 200 Monte Carlo runs are shown in Figures 1 and 2. It can be seen from Figure 1 that the RMSEs obtained by the ASCKF are significantly smaller than those of the other two filters, indicating that the proposed ASCKF can achieve higher estimation accuracy compared with CKF and SCKF.The reason is that the mean and time-varying covariance of the noise can be estimated online effectively to revise the filter by the noise statistic estimator in ASCKF, as shown in Figure 2.However, CKF and SCKF have no ability to revise the inaccurate noise.
The mean RMSEs of the three filters are listed in Table 1.It can be seen from the table that the three states of ASCKF are improved by 38.55%, 32.61%, and 16.19%, respectively, compared with the SCKF, thus verifying the validity of the proposed filter.

Conclusion
In this paper, a noise statistic estimator based adaptive simplex cubature Kalman filter is proposed to address the problem that the filtering accuracy reduces in the case of inaccurate time-varying noise statistic.The nonlinear Gaussian weighted integral in the filter is approximated using the simplex cubature rule, and the time-varying noise statistic estimator is designed based on the maximum a posteriori estimation criterion and the fading memory exponential weighted attenuation method.The simulation results show

Mathematical
Covariance of the noise

Figure 2 :
Figure 2: The true noise statistic and the estimated one.

Table 1 :
Mean RMSEs of the three filters.
∑ =1 (   − x  ) 2 ,  = 1, 2, . . ., 2000, (43) where  is the Monte Carlo run times and    and x  represent the true state and the estimated state in the th run, respectively.