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

Authors: Zhaoming Li; Wenge Yang; Dan Ding
Journal: Mathematical Problems in Engineering
 (2017)

---

## Abstract

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 the maximum 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.


---

## Body

## 1. 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 third-degree 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–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 Section2. 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.


## 2. 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 asIN=∫RngxNx;x^,Pxdx, where x∈Rn, gx represents the arbitrary nonlinear function, and Nx;x^,Px denotes the Gaussian distribution with mean x^ and covariance Px. It is generally difficult to obtain the analytical solution of IN; hence the high accuracy numerical approximation method should be taken into account.


#### 2.1.1. Spherical Simplex Rule for Spherical Integral

Specifically, the coordinate transformation for integral of the formIg=∫Rngxexp-xΤxdx is considered first [11]. Let x=rs, where s=s1,s2,…,snΤ represents the direction vector such that sΤs=1 with r=xΤx≥0. Then Ig can be decomposed into the following two types of integrals, namely, the spherical integral Sr and the radial integral R, respectively.(1)Sr=∫Ungrsdσs(2)R=∫0∞Srrn-1exp⁡-r2dr,where Un=s∈Rn:s12+s22+⋯+sn2=1 represents the spherical surface and σ· denotes the area element on Un.The third-degree spherical simplex rule [18], which consists of 2n+2 points, is adopted to approximate the spherical integral Sr in (1) as follows.(3)Sr≈Tn2n+1∑i=1n+1grai+g-rai,where Tn=2πn/Γn/2 is the surface area of unit sphere and Γx=∫0∞exp⁡-ttx-1dt denotes the Gamma function. The vectors ai=ai,1,ai,2,…,ai,nΤ,i=1,2,…,n+1 are the vertices of the n-dimensional simplex, and the elements are defined as follows.(4)ai,j=-n+1nn-j+2n-j+1,j<in+1n-i+1nn-i+2,i=j0,j>i.


#### 2.1.2. Generalized Gauss-Laguerre Quadrature Rule for Radial Integral

As for the radial integral shown in (2), let r=t and R=1/2∫0∞Sttn/2-1exp⁡-tdt is deduced. Further, R can be written in the form of R=1/2∫0∞S~ttβexp⁡-tdt by letting S~t=St with β=n/2-1, and it is convenient to use the generalized Gauss-Laguerre quadrature rule [19] for approximation as follows.(5)∫0∞S~ttβexp⁡-tdt≈∑i=1pω~iS~ti,where ti and ω~i denote the quadrature points and the corresponding weights, respectively. p represents the number of the points. The points ti can be obtained by solving the following m-order Chebyshev-Laguerre polynomial [20].(6)Lpβt=-1pt-βexp⁡tdpdtptβ+pexp⁡-t=0.The weightsω~i are calculated using(7)ω~i=p!Γβ+p+1tiL˙pβti2.For the third-degree accuracy of the radial integral,p should be chosen 1, and t1=n/2 with ω~1=Γn/2 are obtained; thus the radial integral turns to be the following form.(8)R≈12ω~1S~t1=12Γn2Sn2.


#### 2.1.3. Simplex Cubature Rule

The integralIg is approximated by substituting (3) into (8) as follows.(9)Ig≈πn2n+1∑i=1n+1gn2ai+g-n2ai.I
N has the following equivalent form:(10)IN=1πn∫Rng2Pxx+x^exp⁡-xΤxdx.The simplex cubature rule, which is used to approximate the nonlinear Gaussian weighted integral, is derived by combining (9) and (10) as follows.(11)IN≈12n+1∑i=1n+1gx^+nPxai+gx^-nPxai=∑i=12n+2ωigx^i,where the cubature points x^i and the weights ωi are given as follows:(12)x^i=x^+nPxa,-ai,i=1,2,…,2n+2(13)ωi=12n+1,i=1,2,…,2n+2,where a=a1,a2,…,an+1 and the matrix subscript ·i represents the ith column.


### 2.2. The Simplex Cubature Kalman Filter

The following discrete-time nonlinear system is taken into account.(14)xk=fxk-1+wkzk=hxk+vk,where xk∈Rn and zk∈Rq denote the state and measurement vectors, respectively, and wk and vk represent the white Gaussian process noise and measurement noise, respectively, with the mean being qk and rk, respectively, and the covariance being Qk and Rk, 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 estimatex^k-1+ and covariance Pk-1+ at time k-1 are used instead of x^ and Px in (12) to calculate the cubature points x^k-1i, which are propagated using the nonlinear function f· to obtain the following points Xki.(15)Xki=fx^k-1i+qk-1.The prior state estimatex^k- at time k is calculated using the propagated points Xki as follows.(16)x^k-=∑i=12n+2ωiXki,where ωi is given in (13).The prior error covariancePk- at time k is calculated as follows.(17)Pk-=∑i=12n+2ωiXki-x^k-Xki-x^k-Τ+Qk-1.The prior state estimatex^k- and covariance Pk- at time k are used instead of x^ and Px in (12) to calculate the cubature points x^ki, which are propagated using the nonlinear function h· to obtain the following points Zki.(18)Zki=hx^ki+rk.The predicted measurementz^k is calculated using the propagated points Zki as follows.(19)z^k=∑i=12n+2ωiZki.The measurement covariancePz,k is calculated as follows.(20)Pz,k=∑i=12n+2ωiZki-z^kZki-z^kΤ+Rk.The cross covariancePxz,k is calculated as follows.(21)Pxz,k=∑i=12n+2ωix^ki-x^k-Zki-z^kΤ.The Kalman filter gainKk is calculated as follows.(22)Kk=Pxz,kPz,k-1.The a posteriori state estimatex^k+ at time k is calculated as follows.(23)x^k+=x^k-+Kkzk-z^k.The a posteriori error covariancePk+ is calculated as follows.(24)Pk+=Pk--KkPz,kKkΤ.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.


## 2.1. Simplex Cubature Rule

The key problem in nonlinear Kalman filter is to calculate the intractable nonlinear Gaussian weighted integral asIN=∫RngxNx;x^,Pxdx, where x∈Rn, gx represents the arbitrary nonlinear function, and Nx;x^,Px denotes the Gaussian distribution with mean x^ and covariance Px. It is generally difficult to obtain the analytical solution of IN; hence the high accuracy numerical approximation method should be taken into account.


### 2.1.1. Spherical Simplex Rule for Spherical Integral

Specifically, the coordinate transformation for integral of the formIg=∫Rngxexp-xΤxdx is considered first [11]. Let x=rs, where s=s1,s2,…,snΤ represents the direction vector such that sΤs=1 with r=xΤx≥0. Then Ig can be decomposed into the following two types of integrals, namely, the spherical integral Sr and the radial integral R, respectively.(1)Sr=∫Ungrsdσs(2)R=∫0∞Srrn-1exp⁡-r2dr,where Un=s∈Rn:s12+s22+⋯+sn2=1 represents the spherical surface and σ· denotes the area element on Un.The third-degree spherical simplex rule [18], which consists of 2n+2 points, is adopted to approximate the spherical integral Sr in (1) as follows.(3)Sr≈Tn2n+1∑i=1n+1grai+g-rai,where Tn=2πn/Γn/2 is the surface area of unit sphere and Γx=∫0∞exp⁡-ttx-1dt denotes the Gamma function. The vectors ai=ai,1,ai,2,…,ai,nΤ,i=1,2,…,n+1 are the vertices of the n-dimensional simplex, and the elements are defined as follows.(4)ai,j=-n+1nn-j+2n-j+1,j<in+1n-i+1nn-i+2,i=j0,j>i.


### 2.1.2. Generalized Gauss-Laguerre Quadrature Rule for Radial Integral

As for the radial integral shown in (2), let r=t and R=1/2∫0∞Sttn/2-1exp⁡-tdt is deduced. Further, R can be written in the form of R=1/2∫0∞S~ttβexp⁡-tdt by letting S~t=St with β=n/2-1, and it is convenient to use the generalized Gauss-Laguerre quadrature rule [19] for approximation as follows.(5)∫0∞S~ttβexp⁡-tdt≈∑i=1pω~iS~ti,where ti and ω~i denote the quadrature points and the corresponding weights, respectively. p represents the number of the points. The points ti can be obtained by solving the following m-order Chebyshev-Laguerre polynomial [20].(6)Lpβt=-1pt-βexp⁡tdpdtptβ+pexp⁡-t=0.The weightsω~i are calculated using(7)ω~i=p!Γβ+p+1tiL˙pβti2.For the third-degree accuracy of the radial integral,p should be chosen 1, and t1=n/2 with ω~1=Γn/2 are obtained; thus the radial integral turns to be the following form.(8)R≈12ω~1S~t1=12Γn2Sn2.


### 2.1.3. Simplex Cubature Rule

The integralIg is approximated by substituting (3) into (8) as follows.(9)Ig≈πn2n+1∑i=1n+1gn2ai+g-n2ai.I
N has the following equivalent form:(10)IN=1πn∫Rng2Pxx+x^exp⁡-xΤxdx.The simplex cubature rule, which is used to approximate the nonlinear Gaussian weighted integral, is derived by combining (9) and (10) as follows.(11)IN≈12n+1∑i=1n+1gx^+nPxai+gx^-nPxai=∑i=12n+2ωigx^i,where the cubature points x^i and the weights ωi are given as follows:(12)x^i=x^+nPxa,-ai,i=1,2,…,2n+2(13)ωi=12n+1,i=1,2,…,2n+2,where a=a1,a2,…,an+1 and the matrix subscript ·i represents the ith column.


## 2.1.1. Spherical Simplex Rule for Spherical Integral

Specifically, the coordinate transformation for integral of the formIg=∫Rngxexp-xΤxdx is considered first [11]. Let x=rs, where s=s1,s2,…,snΤ represents the direction vector such that sΤs=1 with r=xΤx≥0. Then Ig can be decomposed into the following two types of integrals, namely, the spherical integral Sr and the radial integral R, respectively.(1)Sr=∫Ungrsdσs(2)R=∫0∞Srrn-1exp⁡-r2dr,where Un=s∈Rn:s12+s22+⋯+sn2=1 represents the spherical surface and σ· denotes the area element on Un.The third-degree spherical simplex rule [18], which consists of 2n+2 points, is adopted to approximate the spherical integral Sr in (1) as follows.(3)Sr≈Tn2n+1∑i=1n+1grai+g-rai,where Tn=2πn/Γn/2 is the surface area of unit sphere and Γx=∫0∞exp⁡-ttx-1dt denotes the Gamma function. The vectors ai=ai,1,ai,2,…,ai,nΤ,i=1,2,…,n+1 are the vertices of the n-dimensional simplex, and the elements are defined as follows.(4)ai,j=-n+1nn-j+2n-j+1,j<in+1n-i+1nn-i+2,i=j0,j>i.


## 2.1.2. Generalized Gauss-Laguerre Quadrature Rule for Radial Integral

As for the radial integral shown in (2), let r=t and R=1/2∫0∞Sttn/2-1exp⁡-tdt is deduced. Further, R can be written in the form of R=1/2∫0∞S~ttβexp⁡-tdt by letting S~t=St with β=n/2-1, and it is convenient to use the generalized Gauss-Laguerre quadrature rule [19] for approximation as follows.(5)∫0∞S~ttβexp⁡-tdt≈∑i=1pω~iS~ti,where ti and ω~i denote the quadrature points and the corresponding weights, respectively. p represents the number of the points. The points ti can be obtained by solving the following m-order Chebyshev-Laguerre polynomial [20].(6)Lpβt=-1pt-βexp⁡tdpdtptβ+pexp⁡-t=0.The weightsω~i are calculated using(7)ω~i=p!Γβ+p+1tiL˙pβti2.For the third-degree accuracy of the radial integral,p should be chosen 1, and t1=n/2 with ω~1=Γn/2 are obtained; thus the radial integral turns to be the following form.(8)R≈12ω~1S~t1=12Γn2Sn2.


## 2.1.3. Simplex Cubature Rule

The integralIg is approximated by substituting (3) into (8) as follows.(9)Ig≈πn2n+1∑i=1n+1gn2ai+g-n2ai.I
N has the following equivalent form:(10)IN=1πn∫Rng2Pxx+x^exp⁡-xΤxdx.The simplex cubature rule, which is used to approximate the nonlinear Gaussian weighted integral, is derived by combining (9) and (10) as follows.(11)IN≈12n+1∑i=1n+1gx^+nPxai+gx^-nPxai=∑i=12n+2ωigx^i,where the cubature points x^i and the weights ωi are given as follows:(12)x^i=x^+nPxa,-ai,i=1,2,…,2n+2(13)ωi=12n+1,i=1,2,…,2n+2,where a=a1,a2,…,an+1 and the matrix subscript ·i represents the ith column.


## 2.2. The Simplex Cubature Kalman Filter

The following discrete-time nonlinear system is taken into account.(14)xk=fxk-1+wkzk=hxk+vk,where xk∈Rn and zk∈Rq denote the state and measurement vectors, respectively, and wk and vk represent the white Gaussian process noise and measurement noise, respectively, with the mean being qk and rk, respectively, and the covariance being Qk and Rk, 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 estimatex^k-1+ and covariance Pk-1+ at time k-1 are used instead of x^ and Px in (12) to calculate the cubature points x^k-1i, which are propagated using the nonlinear function f· to obtain the following points Xki.(15)Xki=fx^k-1i+qk-1.The prior state estimatex^k- at time k is calculated using the propagated points Xki as follows.(16)x^k-=∑i=12n+2ωiXki,where ωi is given in (13).The prior error covariancePk- at time k is calculated as follows.(17)Pk-=∑i=12n+2ωiXki-x^k-Xki-x^k-Τ+Qk-1.The prior state estimatex^k- and covariance Pk- at time k are used instead of x^ and Px in (12) to calculate the cubature points x^ki, which are propagated using the nonlinear function h· to obtain the following points Zki.(18)Zki=hx^ki+rk.The predicted measurementz^k is calculated using the propagated points Zki as follows.(19)z^k=∑i=12n+2ωiZki.The measurement covariancePz,k is calculated as follows.(20)Pz,k=∑i=12n+2ωiZki-z^kZki-z^kΤ+Rk.The cross covariancePxz,k is calculated as follows.(21)Pxz,k=∑i=12n+2ωix^ki-x^k-Zki-z^kΤ.The Kalman filter gainKk is calculated as follows.(22)Kk=Pxz,kPz,k-1.The a posteriori state estimatex^k+ at time k is calculated as follows.(23)x^k+=x^k-+Kkzk-z^k.The a posteriori error covariancePk+ is calculated as follows.(24)Pk+=Pk--KkPz,kKkΤ.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.


## 3. 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.


### 3.1. Constant Noise Statistic Estimator

The MAP estimates ofq, Q, r, R, and Xk can be obtained by calculating the maximum value of the following conditional probability density function [21].(25)L~=pXk,q,Q,r,R∣Zk,where Xk=x0,x1,…,xk and Zk=z1,z2,…,zk.According to the property of the conditional probability function, (25) has the equivalent form as L~=pXk,q,Q,r,R,Zk/pZk, and the problem is transformed into calculating the maximum value of the following equation on account of pZk being independent of the optimization.(26)L=pXk,q,Q,r,R,Zk=pZk∣Xk,q,Q,r,RpXk∣q,Q,r,Rpq,Q,r,R,where pq,Q,r,R can be regarded as a constant since it is obtained using the prior information.The termspXk∣q,Q,r,R and pZk∣Xk,q,Q,r,R in (26) can be calculated using the multiplicative theorem of conditional probability as follows.(27)pXk∣q,Q,r,R=px0∏j=1kpxj∣xj-1,q,Q=M1P0-1/2·Q-k/2exp⁡-12x0-x^0+P0-12+∑j=1kxj-fxj-1-qQ-12,where M1=1/2πnk+1/2 represents a constant, · denotes the determinant, and uA2=uΤAu is the quadratic.(28)pZk∣Xk,q,Q,r,R=∏j=1kpzj∣xj,r,R=M2R-k/2exp-12∑j=1kzj-hxj-rR-12,where M2=1/2πqk/2 denotes a constant.Then, by substituting (27) and (28) into (26), we have(29)L=MQ-k/2·R-k/2exp⁡-12∑j=1kxj-fxj-1-qQ-12+∑j=1kzj-hxj-rR-12,where M=M1M2P0-1/2pq,Q,r,Rexp⁡-1/2x0-x^0+P0-12, and taking the logarithm of (29), we obtain that(30)ln⁡L=-k2ln⁡Q-k2ln⁡R-12∑j=1kxj-fxj-1-qQ-12-12∑j=1kzj-hxj-rR-12+ln⁡M.Note that (29) and (30) share the same extreme points; the following partial differentials are taken.(31)∂ln⁡L∂qq=q^kxj-1=x^j-1∣k+,xj=x^j∣k+=0∂ln⁡L∂QQ=Q^kxj-1=x^j-1∣k+,xj=x^j∣k+=0∂ln⁡L∂rr=r^kxj=x^jk+=0∂ln⁡L∂RR=R^kxj=x^jk+=0,where x^j-1∣k+ and x^j∣k+ represent the smooth values, which are replaced by the filtering estimates x^j-1+ and x^j+, or predicted estimate x^j-, and the suboptimal MAP noise statistic estimator is deduced in a recursive form as follows.(32)q^k=1kk-1q^k-1+x^k+-∑i=12n+2ωifx^k-1iQ^k=1kk-1Q^k-1+KkekekΤKkΤ+Pk+-∑i=12n+2ωiXki-x^k-Xki-x^k-Τr^k=1kk-1r^k-1+zk-∑i=12n+2ωihx^kiR^k=1kk-1R^k-1+ekekΤ-∑i=12n+2ωiZki-z^kZki-z^kΤ,where ek=zk-z^k is the filtering residual.


### 3.2. Time-Varying Noise Statistic Estimator

For the time-varying noise statistic, we should emphasize the effect of new data and gradually forget the effect of the stale data, so as to achieve the online real-time estimation of the time-varying noise statistic. For this, the fading memory exponential weighted attenuation method is considered. The weighting factorμi is selected such that μi=μi-1γ,∑i=1kμi=1, where 0<γ<1 denotes the forgetting factor, and then we have(33)μi=ρkγi-1ρk=1-γ1-γk.Each term in (32) is multiplied by μk+1-j to replace the original factor 1/k, and the fading memory time-varying noise statistic estimator is derived as follows.(34)q^k=1-ρkq^k-1+ρkx^k+-∑i=12n+2ωifx^k-1iQ^k=1-ρkQ^k-1+ρkKkekekΤKkΤ+Pk+-∑i=12n+2ωiXki-x^k-Xki-x^k-Τr^k=1-ρkr^k-1+ρkzk-∑i=12n+2ωihx^kiR^k=1-ρkR^k-1+ρkekekΤ-∑i=12n+2ωiZki-z^kZki-z^kΤ.However, for the high-dimensional systems,Q^k and R^k 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.(35)Q^k=1-ρkQ^k-1+ρkKkekekΤKkΤR^k=1-ρkR^k-1+ρkekekΤ.From the above, the time-varying noise statistic estimator, which is suitable for nonlinear filter, is summarized in the recursive form as follows.(36)q^k=1-ρkq^k-1+ρkx^k+-∑i=12n+2ωifx^k-1iQ^k=1-ρkQ^k-1+ρkKkekekΤKkΤ+Pk+-∑i=12n+2ωiXki-x^k-Xki-x^k-Τ,positive semi-definite1-ρkQ^k-1+ρkKkekekΤKkΤ,negative definiter^k=1-ρkr^k-1+ρkzk-∑i=12n+2ωihx^kiR^k=1-ρkR^k-1+ρkekekΤ-∑i=12n+2ωiZki-z^kZki-z^kΤ,positive definite1-ρkR^k-1+ρkekekΤ,nonpositive definite.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 1 (filter initialization).
One has the initial valuesx^0+, P0+, q^0, Q^0, r^0, R^0.
Cyclek=1,2,…, and complete the following steps.Step 2 (time update).
The cubature pointsx^k-1i are calculated as SCKF in Section 2.2, and the noise estimator q^k-1 is used instead of qk-1 to obtain the propagated points Xki as follows.(37)Xki=fx^k-1i+q^k-1.
The prior state estimatex^k- at time k is calculated as (16).
The prior error covariancePk- at time k is calculated using the noise estimator Q^k-1 as follows.(38)Pk-=∑i=12n+2ωiXki-x^k-Xki-x^k-Τ+Q^k-1.Step 3 (measurement update).
The cubature pointsx^ki are calculated as SCKF in Section 2.2, and the noise estimator r^k is used instead of rk to obtain the propagated points Zki as follows.(39)Zki=hx^ki+r^k.
The predicted measurementz^k is calculated as (19).
The measurement covariancePz,k is calculated using the noise estimator R^k as follows.(40)Pz,k=∑i=12n+2ωiZki-z^kZki-z^kΤ+R^k.
The cross covariancePxz,k, the Kalman filter gain Kk, the a posteriori state estimate x^k+, and the a posteriori error covariance Pk+ are calculated as (21) to (24), respectively.Step 4 (noise statistic estimator update).
The noise statistic estimator is updated using (36).
On account of the recursion equations of noise statistic estimator being not independent, the noisewk and vk cannot be estimated at the same time in general; otherwise it may cause the filtering divergence.


## 3.1. Constant Noise Statistic Estimator

The MAP estimates ofq, Q, r, R, and Xk can be obtained by calculating the maximum value of the following conditional probability density function [21].(25)L~=pXk,q,Q,r,R∣Zk,where Xk=x0,x1,…,xk and Zk=z1,z2,…,zk.According to the property of the conditional probability function, (25) has the equivalent form as L~=pXk,q,Q,r,R,Zk/pZk, and the problem is transformed into calculating the maximum value of the following equation on account of pZk being independent of the optimization.(26)L=pXk,q,Q,r,R,Zk=pZk∣Xk,q,Q,r,RpXk∣q,Q,r,Rpq,Q,r,R,where pq,Q,r,R can be regarded as a constant since it is obtained using the prior information.The termspXk∣q,Q,r,R and pZk∣Xk,q,Q,r,R in (26) can be calculated using the multiplicative theorem of conditional probability as follows.(27)pXk∣q,Q,r,R=px0∏j=1kpxj∣xj-1,q,Q=M1P0-1/2·Q-k/2exp⁡-12x0-x^0+P0-12+∑j=1kxj-fxj-1-qQ-12,where M1=1/2πnk+1/2 represents a constant, · denotes the determinant, and uA2=uΤAu is the quadratic.(28)pZk∣Xk,q,Q,r,R=∏j=1kpzj∣xj,r,R=M2R-k/2exp-12∑j=1kzj-hxj-rR-12,where M2=1/2πqk/2 denotes a constant.Then, by substituting (27) and (28) into (26), we have(29)L=MQ-k/2·R-k/2exp⁡-12∑j=1kxj-fxj-1-qQ-12+∑j=1kzj-hxj-rR-12,where M=M1M2P0-1/2pq,Q,r,Rexp⁡-1/2x0-x^0+P0-12, and taking the logarithm of (29), we obtain that(30)ln⁡L=-k2ln⁡Q-k2ln⁡R-12∑j=1kxj-fxj-1-qQ-12-12∑j=1kzj-hxj-rR-12+ln⁡M.Note that (29) and (30) share the same extreme points; the following partial differentials are taken.(31)∂ln⁡L∂qq=q^kxj-1=x^j-1∣k+,xj=x^j∣k+=0∂ln⁡L∂QQ=Q^kxj-1=x^j-1∣k+,xj=x^j∣k+=0∂ln⁡L∂rr=r^kxj=x^jk+=0∂ln⁡L∂RR=R^kxj=x^jk+=0,where x^j-1∣k+ and x^j∣k+ represent the smooth values, which are replaced by the filtering estimates x^j-1+ and x^j+, or predicted estimate x^j-, and the suboptimal MAP noise statistic estimator is deduced in a recursive form as follows.(32)q^k=1kk-1q^k-1+x^k+-∑i=12n+2ωifx^k-1iQ^k=1kk-1Q^k-1+KkekekΤKkΤ+Pk+-∑i=12n+2ωiXki-x^k-Xki-x^k-Τr^k=1kk-1r^k-1+zk-∑i=12n+2ωihx^kiR^k=1kk-1R^k-1+ekekΤ-∑i=12n+2ωiZki-z^kZki-z^kΤ,where ek=zk-z^k is the filtering residual.


## 3.2. Time-Varying Noise Statistic Estimator

For the time-varying noise statistic, we should emphasize the effect of new data and gradually forget the effect of the stale data, so as to achieve the online real-time estimation of the time-varying noise statistic. For this, the fading memory exponential weighted attenuation method is considered. The weighting factorμi is selected such that μi=μi-1γ,∑i=1kμi=1, where 0<γ<1 denotes the forgetting factor, and then we have(33)μi=ρkγi-1ρk=1-γ1-γk.Each term in (32) is multiplied by μk+1-j to replace the original factor 1/k, and the fading memory time-varying noise statistic estimator is derived as follows.(34)q^k=1-ρkq^k-1+ρkx^k+-∑i=12n+2ωifx^k-1iQ^k=1-ρkQ^k-1+ρkKkekekΤKkΤ+Pk+-∑i=12n+2ωiXki-x^k-Xki-x^k-Τr^k=1-ρkr^k-1+ρkzk-∑i=12n+2ωihx^kiR^k=1-ρkR^k-1+ρkekekΤ-∑i=12n+2ωiZki-z^kZki-z^kΤ.However, for the high-dimensional systems,Q^k and R^k 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.(35)Q^k=1-ρkQ^k-1+ρkKkekekΤKkΤR^k=1-ρkR^k-1+ρkekekΤ.From the above, the time-varying noise statistic estimator, which is suitable for nonlinear filter, is summarized in the recursive form as follows.(36)q^k=1-ρkq^k-1+ρkx^k+-∑i=12n+2ωifx^k-1iQ^k=1-ρkQ^k-1+ρkKkekekΤKkΤ+Pk+-∑i=12n+2ωiXki-x^k-Xki-x^k-Τ,positive semi-definite1-ρkQ^k-1+ρkKkekekΤKkΤ,negative definiter^k=1-ρkr^k-1+ρkzk-∑i=12n+2ωihx^kiR^k=1-ρkR^k-1+ρkekekΤ-∑i=12n+2ωiZki-z^kZki-z^kΤ,positive definite1-ρkR^k-1+ρkekekΤ,nonpositive definite.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 1 (filter initialization).
One has the initial valuesx^0+, P0+, q^0, Q^0, r^0, R^0.
Cyclek=1,2,…, and complete the following steps.Step 2 (time update).
The cubature pointsx^k-1i are calculated as SCKF in Section 2.2, and the noise estimator q^k-1 is used instead of qk-1 to obtain the propagated points Xki as follows.(37)Xki=fx^k-1i+q^k-1.
The prior state estimatex^k- at time k is calculated as (16).
The prior error covariancePk- at time k is calculated using the noise estimator Q^k-1 as follows.(38)Pk-=∑i=12n+2ωiXki-x^k-Xki-x^k-Τ+Q^k-1.Step 3 (measurement update).
The cubature pointsx^ki are calculated as SCKF in Section 2.2, and the noise estimator r^k is used instead of rk to obtain the propagated points Zki as follows.(39)Zki=hx^ki+r^k.
The predicted measurementz^k is calculated as (19).
The measurement covariancePz,k is calculated using the noise estimator R^k as follows.(40)Pz,k=∑i=12n+2ωiZki-z^kZki-z^kΤ+R^k.
The cross covariancePxz,k, the Kalman filter gain Kk, the a posteriori state estimate x^k+, and the a posteriori error covariance Pk+ are calculated as (21) to (24), respectively.Step 4 (noise statistic estimator update).
The noise statistic estimator is updated using (36).
On account of the recursion equations of noise statistic estimator being not independent, the noisewk and vk cannot be estimated at the same time in general; otherwise it may cause the filtering divergence.


## 4. 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.(41)x1,k+1x2,k+1x3,k+1=3cos⁡x2,kx1,kx3,k0.1x1,kx2,k+x3,k+wkzk=x1,kx2,k+x3,k+vk,where xk=x1,k,x2,k,x3,kΤ denotes the system state, wk represents the process noise with the mean and covariance being zero and Qk, respectively, and vk is the measurement noise with the mean and covariance being rk and Rk, respectively.In this simulation, the true initial state and covariance are set to bex0=0.2,0.5,0.2Τ and Qk=0.01I3, respectively. The true values of the noise, including the mean and the time-varying covariance, are set to be rk=0.5 and Rk given below.(42)Rk=0.3,0<k≤5000.9,500<k≤10001.5,1000<k≤15000.6,1500<k≤2000.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 arex^0+=0.2,0.5,0.2Τ and P0+=0.01I3, respectively. The initial values of the noise statistic estimator are chosen to be r^0=0 and R^0=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.(43)RMSEk=1M∑i=1Mxki-x^ki2,k=1,2,…,2000,where M is the Monte Carlo run times and xki and x^ki represent the true state and the estimated state in the ith run, respectively.The simulation results based on 200 Monte Carlo runs are shown in Figures1 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.Figure 1
RMSEs of the three filters.
(a)
State 1
(b)
State 2
(c)
State 3Figure 2
The true noise statistic and the estimated one.
(a)
Mean of the noise
(b)
Covariance of the noiseThe mean RMSEs of the three filters are listed in Table1. 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.Table 1
Mean RMSEs of the three filters.
Filters
State 1
State 2
State 3
CKF
0.4693
0.3410
0.1423
SCKF
0.4677
0.3404
0.1421
ASCKF
0.2874
0.2294
0.1191


## 5. 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 that the proposed ASCKF has achieved better performance compared with the CKF and the SCKF in the case of inaccurate time-varying noise statistic, and the validity of the proposed filter has been verified.



---

*Source: 5349879-2017-12-14.xml*