# Mixed-Degree Spherical Simplex-Radial Cubature Kalman Filter

Authors: Shiyuan Wang; Yali Feng; Shukai Duan; Lidan Wang
Journal: Mathematical Problems in Engineering
 (2017)

---

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


---

## Body

## 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 formnonlinear 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 as(1)xk=gxk-1+qk-1,zk=hxk+rk,where xk∈Rn denotes the state; zk∈Rm denotes the measurement; qk-1 and rk are both Gaussian noises with zero means and covariances Qk-1 and Rk, respectively.Based on (1), under the Gaussian assumption, the Bayesian filter can be divided into the following two steps: prediction and correction.Prediction
(2)
x
^
k
∣
k
-
1
=
∫
R
n
g
x
k
-
1
N
x
k
-
1
;
x
^
k
-
1
,
P
k
-
1
d
x
k
-
1
,
P
k
∣
k
-
1
=
∫
R
n
g
x
k
-
1
g
T
x
k
-
1
N
x
k
-
1
;
x
^
k
-
1
,
P
k
-
1
d
x
k
-
1
-
x
^
k
∣
k
-
1
x
^
k
∣
k
-
1
T
+
Q
k
-
1
,where Nxk-1;x^k-1,Pk-1 represents the Gaussian distribution with the mean x^k-1 and the covariance Pk-1.Correction
(3)
x
^
k
=
x
^
k
∣
k
-
1
+
G
k
z
k
-
z
^
k
∣
k
-
1
,
P
k
=
P
k
∣
k
-
1
-
G
k
P
z
z
G
k
T
,where(4)Gk=PxzPzz-1,z^k∣k-1=∫RnhxkNxk;x^k∣k-1,Pk∣k-1dxk,Pxz=∫Rnxk-x^k∣k-1hxk-z^k∣k-1T×Nxk;x^k∣k-1,Pk∣k-1dxk,Pzz=∫Rnhxk-z^k∣k-1hxk-z^k∣k-1T×Nxk;x^k∣k-1,Pk∣k-1dxk+Rk.From (2) to (4), the Bayesian filter is generalized as the calculation of the following integral:(5)If=∫RnfxNx;x¯,Pdx,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,(6)If=∫RnfxNx;x¯,Pdx≈∑i=0Lwifξi,where L is the total number of points; ξi represents the quadrature point and wi is the corresponding weight.


### 2.1. Spherical-Radial Transformation

For simplicity, letQ(f)=∑iwif(ξi) denote the numerical integration. Q(f) can be regarded as adth-degree rule of I(f), if it is exact for f(x)=∏i=0nxidi with ∑i=1ndi≤d and is not exact for at least one polynomial of degree ∑i=1ndi≤d+1 [4, 19, 22].Consider the following integral:(7)If=∫Rnfxe-xTxdx.Letx=ry with yTy=0, where r=xTx. 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.(8)If=∫0∞∫Unfryrn-1e-r2dσydr,where Un={y∈Rn∣yTy=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 S(r)=∫Unfrydσy and the radial integral R=∫0∞Srrn-1e-r2dr. Assume that Sr and R can be approximated by the following numerical integration:(9)Sr≈∑i=1Lsws,ifryi,(10)R≈∑j=1Lrwr,jfrj,where yi and ws,i denote the quadrature point and the corresponding weight of Sr; rj and wr,j denote the quadrature point and the corresponding weight of R; Ls and Lr are the number of the points, respectively.Therefore, applying (9) and (10) into (8) generates the following approximation:(11)If≈∑j=1Lr∑i=1Lswr,jws,ifrjyi.


### 2.2. Spherical Simplex Rule

The aforementioned spherical integrationS(r) in (9) can be efficiently calculated using the transformation group of the regular n-simplex [21, 22] with the vector ai=ai,1,ai,2,…,ai,nT,  i=1,2,…,n+1 as follows:(12)ai,j=-n+1nn-j+2n-j+1,i>jn+1n-i+1nn-i+2,i=j0,i<j.The projection from the midpoints of the vectorai on the spherical sphere leads to the following points [21, 22]:(13)bi=n2n-1aj+al:j<l,l=1,2,…,n+1.For example, when the dimension n=2, we can get that {ai}=1,0T,-1/2,3/2T,-1/2,-3/2T and bi={1/2,3/2T,1/2,-3/2T,-1,0T}.Employing the central symmetry of the cubature formula and just treating the pointsai and bi of the cubature points as yi in (9), we can obtain the third-degree spherical simplex rule with 2n+2 points as [22](14)S3r=An2n+1∑i=1n+1frai+f-raiand the fifth-degree spherical simplex rule with n2+3n+2 points as [22](15)S5r=An7-nn2n+12n+2∑i=1n+1frai+f-rai+2n-12nn+12n+2∑i=1nn+1/2frbi+f-rbi,where An=2Γn(1/2)/Γ(n/2)=2πn/Γ(n/2) denotes the surface area of the sphere; the Gamma function is defined as Γn=∫0∞e-xxn-1dx with the properties of Γ(1/2)=π and Γ(n+1)=nΓ(n).


### 2.3. Radial Rule

The radial integral in (10) can be approximated by(16)R=∫0∞Srrn-1e-r2dr≈∑j=1Lrwr,jSrj.Substituting S(r) with the lth-degree monomial rl, we obtain(17)∫0∞rlrn-1e-r2dr=12Γn+l2,where l 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(18)R3=12Γn2Sn2.Similarly, the fifth-degree radial rule [22] with Lr=2 can be obtained by(19)R5=1n+2Γn2S0+nΓn/22n+2Sn2+1.


### 2.4. Conventional Spherical Simplex-Radial Rule

Based on (11), the spherical simplex-radial rule for the standard Gaussian distribution N(x;0,In) can be obtained as(20)∫RnfxNx;0,Indx≈1πn∑i=1Ls∑j=1Lrws,iwr,jf2rjyi.For the calculation of the multidimensional Gaussian distributionN(x;x¯,P), a linear transformation of Pξi+x¯ is required [22], where ξi 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=PPT.Conventional spherical simplex-radial cubature Kalman filters are all based on the samedth-degree spherical simplex rule and radial rule, respectively [22]. The degree for the spherical simplex-radial rule is thus up to the dth-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 dth-degree can also be used to achieve the dth-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(21)∫RnfxNx;x¯,Pdx=2n+2fx¯+n2n+1n+2∑i=1n+1fn+2Pai+x¯+f-n+2Pai+x¯.The quadrature points and weights based on the MSSR are therefore denoted by(22)ξ0=x¯,w0=2n+2,ξi=x¯+n+2Pai,i=1,2,…,n+1,ξi=x¯-n+2Pai,i=n+2,n+3,…,2n+2,wi=n2n+2n+1,i=1,2,…,2n+2.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 x^0 and the covariance P0.
(II) Prediction
(1) Evaluate the sampling pointsξi,k-1 and wi by (22), where x¯=x^k-1, and P=Pk-1.
(2) Calculate the points propagating through the state process equation(23)ξi,k∣k-1∗=gξi,k-1.
(3) Estimate the predicted state value(24)x^k∣k-1=∑i=1Lwiξi,k∣k-1∗ and the predicted error covariance matrix(25)Pk∣k-1=∑i=1Lwiξi,k∣k-1∗ξi,k∣k-1∗T-x^k∣k-1x^k∣k-1T+Qk-1.(III) Correction
(4) Regenerate the pointsξi,k∣k-1 and wi by (22), where x¯=x^k∣k-1, and P=Pk∣k-1.
(5) Calculate the propagated points(26)Zi,k∣k-1=hξi,k∣k-1.
(6) Estimate the predicted measurement value(27)z^k∣k-1=∑i=1LwiZi,k∣k-1.
(7) Estimate the cross covariance matrix and the innovation covariance matrix(28)Pxz=∑i=1Lwiξi,k∣k-1-x^k∣k-1Zi,k∣k-1-z^k∣k-1T,Pzz=∑i=1LwiZi,k∣k-1Zi,k∣k-1T-z^k∣k-1z^k∣k-1T+Rk.
(8) Calculate the Kalman gain(29)Gk=PxzPzz-1.
(9) Update the state estimate and the corresponding error covariance matrix(30)x^k=x^k∣k-1+Gkzk-z^k∣k-1,Pk=Pk∣k-1-GkPzzGkT.Remark 2.
In comparison with the 3SSRCKF [22] with 2n+2 points, the proposed MSSRCKF with 2n+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 2n+3 requires less computational complexity than the 5SSRCKF with n2+3n+3 points.Figure 1
Comparison of the number of points required by different filters versus dimension.Remark 3.
The other combination of the fifth-degree spherical simplex rule and third-degree radial rule, which requiresn2+3n+2 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.


## 2.1. Spherical-Radial Transformation

For simplicity, letQ(f)=∑iwif(ξi) denote the numerical integration. Q(f) can be regarded as adth-degree rule of I(f), if it is exact for f(x)=∏i=0nxidi with ∑i=1ndi≤d and is not exact for at least one polynomial of degree ∑i=1ndi≤d+1 [4, 19, 22].Consider the following integral:(7)If=∫Rnfxe-xTxdx.Letx=ry with yTy=0, where r=xTx. 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.(8)If=∫0∞∫Unfryrn-1e-r2dσydr,where Un={y∈Rn∣yTy=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 S(r)=∫Unfrydσy and the radial integral R=∫0∞Srrn-1e-r2dr. Assume that Sr and R can be approximated by the following numerical integration:(9)Sr≈∑i=1Lsws,ifryi,(10)R≈∑j=1Lrwr,jfrj,where yi and ws,i denote the quadrature point and the corresponding weight of Sr; rj and wr,j denote the quadrature point and the corresponding weight of R; Ls and Lr are the number of the points, respectively.Therefore, applying (9) and (10) into (8) generates the following approximation:(11)If≈∑j=1Lr∑i=1Lswr,jws,ifrjyi.


## 2.2. Spherical Simplex Rule

The aforementioned spherical integrationS(r) in (9) can be efficiently calculated using the transformation group of the regular n-simplex [21, 22] with the vector ai=ai,1,ai,2,…,ai,nT,  i=1,2,…,n+1 as follows:(12)ai,j=-n+1nn-j+2n-j+1,i>jn+1n-i+1nn-i+2,i=j0,i<j.The projection from the midpoints of the vectorai on the spherical sphere leads to the following points [21, 22]:(13)bi=n2n-1aj+al:j<l,l=1,2,…,n+1.For example, when the dimension n=2, we can get that {ai}=1,0T,-1/2,3/2T,-1/2,-3/2T and bi={1/2,3/2T,1/2,-3/2T,-1,0T}.Employing the central symmetry of the cubature formula and just treating the pointsai and bi of the cubature points as yi in (9), we can obtain the third-degree spherical simplex rule with 2n+2 points as [22](14)S3r=An2n+1∑i=1n+1frai+f-raiand the fifth-degree spherical simplex rule with n2+3n+2 points as [22](15)S5r=An7-nn2n+12n+2∑i=1n+1frai+f-rai+2n-12nn+12n+2∑i=1nn+1/2frbi+f-rbi,where An=2Γn(1/2)/Γ(n/2)=2πn/Γ(n/2) denotes the surface area of the sphere; the Gamma function is defined as Γn=∫0∞e-xxn-1dx with the properties of Γ(1/2)=π and Γ(n+1)=nΓ(n).


## 2.3. Radial Rule

The radial integral in (10) can be approximated by(16)R=∫0∞Srrn-1e-r2dr≈∑j=1Lrwr,jSrj.Substituting S(r) with the lth-degree monomial rl, we obtain(17)∫0∞rlrn-1e-r2dr=12Γn+l2,where l 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(18)R3=12Γn2Sn2.Similarly, the fifth-degree radial rule [22] with Lr=2 can be obtained by(19)R5=1n+2Γn2S0+nΓn/22n+2Sn2+1.


## 2.4. Conventional Spherical Simplex-Radial Rule

Based on (11), the spherical simplex-radial rule for the standard Gaussian distribution N(x;0,In) can be obtained as(20)∫RnfxNx;0,Indx≈1πn∑i=1Ls∑j=1Lrws,iwr,jf2rjyi.For the calculation of the multidimensional Gaussian distributionN(x;x¯,P), a linear transformation of Pξi+x¯ is required [22], where ξi 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=PPT.Conventional spherical simplex-radial cubature Kalman filters are all based on the samedth-degree spherical simplex rule and radial rule, respectively [22]. The degree for the spherical simplex-radial rule is thus up to the dth-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 dth-degree can also be used to achieve the dth-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(21)∫RnfxNx;x¯,Pdx=2n+2fx¯+n2n+1n+2∑i=1n+1fn+2Pai+x¯+f-n+2Pai+x¯.The quadrature points and weights based on the MSSR are therefore denoted by(22)ξ0=x¯,w0=2n+2,ξi=x¯+n+2Pai,i=1,2,…,n+1,ξi=x¯-n+2Pai,i=n+2,n+3,…,2n+2,wi=n2n+2n+1,i=1,2,…,2n+2.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 x^0 and the covariance P0.
(II) Prediction
(1) Evaluate the sampling pointsξi,k-1 and wi by (22), where x¯=x^k-1, and P=Pk-1.
(2) Calculate the points propagating through the state process equation(23)ξi,k∣k-1∗=gξi,k-1.
(3) Estimate the predicted state value(24)x^k∣k-1=∑i=1Lwiξi,k∣k-1∗ and the predicted error covariance matrix(25)Pk∣k-1=∑i=1Lwiξi,k∣k-1∗ξi,k∣k-1∗T-x^k∣k-1x^k∣k-1T+Qk-1.(III) Correction
(4) Regenerate the pointsξi,k∣k-1 and wi by (22), where x¯=x^k∣k-1, and P=Pk∣k-1.
(5) Calculate the propagated points(26)Zi,k∣k-1=hξi,k∣k-1.
(6) Estimate the predicted measurement value(27)z^k∣k-1=∑i=1LwiZi,k∣k-1.
(7) Estimate the cross covariance matrix and the innovation covariance matrix(28)Pxz=∑i=1Lwiξi,k∣k-1-x^k∣k-1Zi,k∣k-1-z^k∣k-1T,Pzz=∑i=1LwiZi,k∣k-1Zi,k∣k-1T-z^k∣k-1z^k∣k-1T+Rk.
(8) Calculate the Kalman gain(29)Gk=PxzPzz-1.
(9) Update the state estimate and the corresponding error covariance matrix(30)x^k=x^k∣k-1+Gkzk-z^k∣k-1,Pk=Pk∣k-1-GkPzzGkT.Remark 2.
In comparison with the 3SSRCKF [22] with 2n+2 points, the proposed MSSRCKF with 2n+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 2n+3 requires less computational complexity than the 5SSRCKF with n2+3n+3 points.Figure 1
Comparison of the number of points required by different filters versus dimension.Remark 3.
The other combination of the fifth-degree spherical simplex rule and third-degree radial rule, which requiresn2+3n+2 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Δx with zero mean and covariance P is introduced around the mean x¯. The Taylor series of a nonlinear function y=fx can be expressed as [1](31)y=fx¯+Δx=fx¯+DΔxf+DΔx2f2!+DΔx3f3!+DΔx4f4!+⋯,where the operator DΔxf=ΔxT∇fxTTx=x¯ is the differential of f· regarding the mean x¯, which can also be denoted using the Jacobian matrix of f·; that is, DΔxf=FΔx.Thus, thekth term in the Taylor series is given as(32)1k!DΔxkf=1k!∑i=1nΔxi∂∂xikfxx=x¯.Taking expectations of (31), we can get that(33)y¯true=Efx¯+Δx=fx¯+EDΔxf+DΔx2f2!+DΔx3f3!+DΔx4f4!+⋯.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(34)y¯true=fx¯+EDΔx2f2!+DΔx4f4!+DΔx6f6!+⋯,where the second term can be transformed as(35)EDΔx2f2!=EDΔxDΔxf2!=E∇TΔxΔxT∇2!f.Further, sinceEΔxΔxT=P, we have(36)EDΔx2f2!=∇TP∇2!f.Substituting (36) into (34), the true mean of nonlinear function can be expressed by(37)y¯true=fx¯+∇TP∇2!f+EDΔx4f4!+DΔx6f6!⋯.In (22), for simplicity, let δi=Pai and σi=n+2δi. The propagation of the ith point through the nonlinear transformation as a Taylor expansion about x¯ can be written as(38)yi=fξi=fx¯+Dσif+Dσi2f2!+Dσi3f3!+Dσi4f4!+⋯.According to (24), the mean of the estimated nonlinear transformation can be expressed by(39)y¯est=2n+2fx¯+n2n+2n+1×∑i=12n+1fx¯+Dσif+Dσi2f2!+Dσi3f3!+⋯=fx¯+n2n+2n+1×∑i=12n+1Dσif+Dσi2f2!+Dσi3f3!+Dσi4f4!+⋯.Since the odd moments for the points symmetrically distributed aroundx¯ are zero, we can obtain(40)y¯est=fx¯+n2n+2n+1∑i=12n+1Dσi2f2!+Dσi4f4!+⋯.After denoting (12) by the matrix form of a with some mathematical operation, we have aaT=n+1/nIn. The second-order term in (39) can be written as(41)n2n+2n+1∑i=12n+1Dσi2f2!=n2n+2n+1∑i=12n+1∇TσiσiT∇2!f=2nn+22n+2n+1∑i=1n+1∇TδiδiT∇2!f=n2!n+1∇TPaaTPT∇f=∇TP∇2!f.Therefore, (40) can be rewritten as(42)y¯est=fx¯+∇TP∇2!f+n2n+2n+1×∑i=12n+1Dσi4f4!+Dσi6f6!+⋯.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 meany¯true, the true covariance matrix can be calculated by(43)Ptrue=Ey-y¯truey-y¯trueT.Combining (31) and (34), we can obtain that(44)y-y¯true=fx¯+DΔxf+DΔx2f2!+DΔx3f3!+DΔx4f4!+⋯-fx¯+EDΔx2f2!+DΔx4f4!+⋯=DΔxf+DΔx2f2!+⋯-12!EDΔx2f+14!EDΔx4f+⋯.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(45)Ptrue=Ey-y¯truey-y¯trueT=EDΔxfDΔxfT+DΔxfDΔx3fT3!+DΔx2fDΔx2fT2!2!+DΔx3fDΔxfT3!-EDΔx2f2!EDΔx2f2!T+⋯.SinceEDΔxfDΔxfT=EFΔxFΔxT=FPFT, (45) can be rewritten as(46)Ptrue=FPFT+EDΔxfDΔx3fT3!+DΔx2fDΔx2fT2!2!+DΔx3fDΔxfT3!-∇TP∇2!f∇TP∇2!fT.Given the mean of the estimated nonlinear transformation in (39), the estimated covariance matrix of the proposed MSSR is calculated by(47)Pest=2n+2y0-y¯esty0-y¯estT+n2n+2n+1∑i=12n+1yi-y¯estyi-y¯estT,where(48)y0-y¯est=-n2n+2n+1∑i=12n+1Dσi2f2!+Dσi4f4!+⋯,yi-y¯est=Dσif+Dσi2f2!+Dσi3f3!+Dσi4f4!+⋯-n2n+2n+1∑i=12n+1Dσi2f2!+Dσi4f4!+⋯.Since(49)n2n+2n+1∑i=12n+1DσifDσifT=n2n+2n+1F∑i=12n+1σiσiTFT=nn+1F∑i=1n+1δiδiTFT=FPFT,the estimated covariance matrix (47) can be simplified as follows:(50)Pest=FPFT+n2n+2n+1×∑i=12n+1DσifDσi3fT3!+Dσi2fDσi2fT2!2!+Dσi3fDσifT3!-∇TP∇2!f∇TP∇2!fT.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.


## 3.1. 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=fx can be expressed as [1](31)y=fx¯+Δx=fx¯+DΔxf+DΔx2f2!+DΔx3f3!+DΔx4f4!+⋯,where the operator DΔxf=ΔxT∇fxTTx=x¯ is the differential of f· regarding the mean x¯, which can also be denoted using the Jacobian matrix of f·; that is, DΔxf=FΔx.Thus, thekth term in the Taylor series is given as(32)1k!DΔxkf=1k!∑i=1nΔxi∂∂xikfxx=x¯.Taking expectations of (31), we can get that(33)y¯true=Efx¯+Δx=fx¯+EDΔxf+DΔx2f2!+DΔx3f3!+DΔx4f4!+⋯.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(34)y¯true=fx¯+EDΔx2f2!+DΔx4f4!+DΔx6f6!+⋯,where the second term can be transformed as(35)EDΔx2f2!=EDΔxDΔxf2!=E∇TΔxΔxT∇2!f.Further, sinceEΔxΔxT=P, we have(36)EDΔx2f2!=∇TP∇2!f.Substituting (36) into (34), the true mean of nonlinear function can be expressed by(37)y¯true=fx¯+∇TP∇2!f+EDΔx4f4!+DΔx6f6!⋯.In (22), for simplicity, let δi=Pai and σi=n+2δi. The propagation of the ith point through the nonlinear transformation as a Taylor expansion about x¯ can be written as(38)yi=fξi=fx¯+Dσif+Dσi2f2!+Dσi3f3!+Dσi4f4!+⋯.According to (24), the mean of the estimated nonlinear transformation can be expressed by(39)y¯est=2n+2fx¯+n2n+2n+1×∑i=12n+1fx¯+Dσif+Dσi2f2!+Dσi3f3!+⋯=fx¯+n2n+2n+1×∑i=12n+1Dσif+Dσi2f2!+Dσi3f3!+Dσi4f4!+⋯.Since the odd moments for the points symmetrically distributed aroundx¯ are zero, we can obtain(40)y¯est=fx¯+n2n+2n+1∑i=12n+1Dσi2f2!+Dσi4f4!+⋯.After denoting (12) by the matrix form of a with some mathematical operation, we have aaT=n+1/nIn. The second-order term in (39) can be written as(41)n2n+2n+1∑i=12n+1Dσi2f2!=n2n+2n+1∑i=12n+1∇TσiσiT∇2!f=2nn+22n+2n+1∑i=1n+1∇TδiδiT∇2!f=n2!n+1∇TPaaTPT∇f=∇TP∇2!f.Therefore, (40) can be rewritten as(42)y¯est=fx¯+∇TP∇2!f+n2n+2n+1×∑i=12n+1Dσi4f4!+Dσi6f6!+⋯.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 meany¯true, the true covariance matrix can be calculated by(43)Ptrue=Ey-y¯truey-y¯trueT.Combining (31) and (34), we can obtain that(44)y-y¯true=fx¯+DΔxf+DΔx2f2!+DΔx3f3!+DΔx4f4!+⋯-fx¯+EDΔx2f2!+DΔx4f4!+⋯=DΔxf+DΔx2f2!+⋯-12!EDΔx2f+14!EDΔx4f+⋯.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(45)Ptrue=Ey-y¯truey-y¯trueT=EDΔxfDΔxfT+DΔxfDΔx3fT3!+DΔx2fDΔx2fT2!2!+DΔx3fDΔxfT3!-EDΔx2f2!EDΔx2f2!T+⋯.SinceEDΔxfDΔxfT=EFΔxFΔxT=FPFT, (45) can be rewritten as(46)Ptrue=FPFT+EDΔxfDΔx3fT3!+DΔx2fDΔx2fT2!2!+DΔx3fDΔxfT3!-∇TP∇2!f∇TP∇2!fT.Given the mean of the estimated nonlinear transformation in (39), the estimated covariance matrix of the proposed MSSR is calculated by(47)Pest=2n+2y0-y¯esty0-y¯estT+n2n+2n+1∑i=12n+1yi-y¯estyi-y¯estT,where(48)y0-y¯est=-n2n+2n+1∑i=12n+1Dσi2f2!+Dσi4f4!+⋯,yi-y¯est=Dσif+Dσi2f2!+Dσi3f3!+Dσi4f4!+⋯-n2n+2n+1∑i=12n+1Dσi2f2!+Dσi4f4!+⋯.Since(49)n2n+2n+1∑i=12n+1DσifDσifT=n2n+2n+1F∑i=12n+1σiσiTFT=nn+1F∑i=1n+1δiδiTFT=FPFT,the estimated covariance matrix (47) can be simplified as follows:(50)Pest=FPFT+n2n+2n+1×∑i=12n+1DσifDσi3fT3!+Dσi2fDσi2fT2!2!+Dσi3fDσifT3!-∇TP∇2!f∇TP∇2!fT.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:(51)RMSEk=1M∑i=1Mxj,ki-x^j,ki2,where M denotes the total Monte Carlo runs and M=100 is used here; the subscript j represents the jth state, xj,ki and x^j,ki are the real state and its corresponding estimated value at time instant k in the ith 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:(52)x1,k=x1,k-1+τx2,k-1,x2,k=x2,k-1+τ-x1,k-1+x1,k-12+x2,k-12-1x2,k-1,zk=x1,k+rk,where τ=0.001.
The real initial values and their corresponding estimates are set tox1,0,x2,0T=0.8,0.2T and x^1,0,x^2,0T=2.3,5.2T, respectively. The other parameters are set as Qk-1=0.0032I2×2 and Rk=0.0012. For all the filters, the initial covariance matrix is given by P0=I2×2.
Table1 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.Table 1
Comparison of the mean values ofRMSE about state x1 among all the filters.
Filters
Mean values ofRMSE
Time (s)
Points
3SSRCKF
0.36675522
0.12705626
2
n
+
2
5SSRCKF
0.36545063
0.23100751
n
2
+
3
n
+
3
MSSRCKF
0.36545053
0.13303506
2
n
+
3Example 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]:(53)x˙1t=-x2t,x˙2t=-e-αx1tx22tx3t,x˙3t=0,where the parameter α is a known constant; x1(t), x2(t), and x3(t) 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 instantk is expressed by(54)zk=D12+x1,k-D22+rk,where D1 denotes the horizontal distance between the falling target and the radar; D2 is the altitude of the radar.
The real initial state values and the initial estimates are set asx0=3×105,2×104,1×10-3T and x^0=3×105,2×104,3×10-5T, respectively. The other parameters are given as follows: α=5×10-5, D1=D2=1×105, Qk-1=0, and Rk=1×104. For all the filters, the initial covariance matrix is given by P0=diag1×106,4×106,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-e-αx1(t)≈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 Table2. 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.Table 2
Filtering performance comparison about the target falling altitude.
Filters
Mean values ofRMSE
Time (s)
Points
3SSRCKF
393.56981130
0.00816492
2
n
+
2
5SSRCKF
358.05064678
0.01639527
n
2
+
3
n
+
3
MSSRCKF
336.41120229
0.00939589
2
n
+
3Figure 2
RMSE of all the filters about the target falling altitude.


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



---

*Source: 6969453-2017-03-19.xml*