# Spherical Simplex-Radial Cubature Quadrature Kalman Filter

Authors: Zhaoming Li; Wenge Yang
Journal: Journal of Electrical and Computer Engineering
 (2017)

---

## Abstract

A spherical simplex-radial cubature quadrature Kalman filter (SSRCQKF) is proposed in order to further improve the nonlinear filtering accuracy. The Gaussian probability weighted integral of the nonlinear function is decomposed into spherical integral and radial integral, which are approximated by spherical simplex cubature rule and arbitrary order Gauss-Laguerre quadrature rule, respectively, and the novel spherical simplex-radial cubature quadrature rule is obtained. Combined with the Bayesian filtering framework, the general form and the specific form of SSRCQKF are put forward, and the numerical simulation results indicate that the proposed algorithm can achieve a higher filtering accuracy than CKF and SSRCKF.


---

## Body

## 1. Introduction

The nonlinear state estimation problem widely exists in signal processing, target tracking, intelligent sensing, and other fields, which is a subject undergoing intense study [1–4]. In suboptimal nonlinear filtering under Bayesian theory framework, the posterior probability density function (pdf) is assumed to be Gaussian distribution, and the core issue is to calculate the intractable integral as “nonlinear function × Gaussian pdf.” Since to achieve the analytical solution is difficult for the integral, the focus of the research is seeking a high-precision integral rule for its numerical approximation [5].The most widely used nonlinear Kalman filtering algorithms are extended Kalman filter (EKF) [6, 7] and unscented Kalman filter (UKF) [8, 9], respectively. EKF uses the first-order Taylor formula to linearize the nonlinear function directly, thereby has only first-order filtering accuracy, and needs to calculate the Jacobian matrix, which limits its further application. UKF adopts a set of sigma points to approximate the intractable integral and achieves a third-order accuracy. However, the selection of the sigma points and corresponding weights lacks rigorous mathematical basis, and the stability of numerical calculation is reduced for the high-dimensional system. Cubature Kalman filter (CKF) proposed by Arasaratnam and Haykin [10, 11] decomposes the intractable integral into spherical integral and radial integral, which are approximated by the third-order cubature rule. CKF not only has a rigorous mathematical basis in selecting the cubature points but also has a complete stability in numerical calculation [12–14]. Moreover, Wang et al. [15] proposed the spherical simplex-radial cubature Kalman filter (SSRCKF), in which the spherical simplex rule instead of spherical rule is used in calculating the spherical integral [16–18]. However, radial integral is calculated by moment matching method in both CKF and SSRCKF, which cannot guarantee the optimal solution. To solve this problem, Shovan and Swati [19] proposed cubature quadrature Kalman filter (CQKF): the algorithm adopts the same method for solving the spherical integral as CKF, but, for radial integral, arbitrary order Gaussian-Laguerre quadrature formula is used to achieve a higher radial integral accuracy, so as to improve the filtering accuracy further. It is also pointed out that CKF is a simplified form of CQKF with the first-order Gaussian-Laguerre quadrature in radial integral.In order to further improve the nonlinear Kalman filtering accuracy, this paper proposes a novel spherical simplex-radial cubature quadrature Kalman filter (SSRCQKF). The structure of this paper is as follows. The spherical simplex-radial cubature quadrature rule is proposed in Section2, and the SSRCQKF algorithm is proposed in Section 3, the numerical simulation results of strong nonlinear system and target tracking are shown in Section 4, and finally the conclusion is given in Section 5.


## 2. Spherical Simplex-Radial Cubature Quadrature Rule

### 2.1. Spherical Simplex-Radial Cubature Rule

Consider the integralIf=∫Rnfxe-xΤxdx, let x=ry, where y denotes the unit sphere surface that satisfies yΤy=1, r≥0 denotes the sphere radius, and, then, If can be decomposed into the following spherical integral and radial integral [10]:(1)Sr=∫UnfrydσyR=∫0∞Srrn-1e-r2dr.In general, the analytical solutions of above integrals are difficult to obtain, so the numerical approximate method is considered. It is pointed out in [15] that the spherical integral can be approximated using the spherical simplex rule that contains 2n+2 integral points as follows:(2)Sr=An2n+1∑i=1n+1frai+f-rai,where An=2πn/Γn/2 denotes the surface area ofn-dimensional unit sphere with Γn=∫0∞xn-1e-xdx representing the Gamma function, ai=ai,1ai,2⋯ai,nΤ, and i=1,2,…,n+1 denotes the ith vertex of the n-dimensional simplex, whose elements are defined as follows:(3)ai,j=-n+1nn-j+2n-j+1,j<i0,j>in+1n-i+1nn-i+2,i=j.


### 2.2. Gaussian-Laguerre Quadrature Rule

For the radial integralR=∫0∞Srrn-1e-r2dr, let r2=t and we get R=(1/2)∫0∞Stt(n-2)/2e-tdt; furthermore, let gt=St and β=(n-2)/2; then R=(1/2)∫0∞gttβe-tdt is obtained; the integral term in R is approximated using the following Gaussian-Laguerre quadrature rule [20]:(4)∫0∞gttβe-tdt≈∑j=1mAjgtj,where tj denote the quadrature points, which can be solved from the solutions of the following m-order Chebyshev-Laguerre polynomial.(5)Lmβt=-1mt-βetdmdtmtβ+me-t=0.AndAj denote the corresponding weights, which can be solved as follows:(6)Aj=m!Γβ+m+1tjL˙mβtj2.It can be seen that the approximation accuracy of the above rule depends on the number of quadrature points.


### 2.3. Spherical Simplex-Radial Cubature Quadrature Rule

Equation (4) is substituted into rule R, and we obtain(7)R=12∫0∞gttβe-tdt=12∑j=1mAjgtj.The spherical simplex rule (2) and gt=St are plugged into (7) to obtain(8)R=12∑j=1mAjStj=An4n+1∑j=1mAj∑i=1n+1ftjai+f-tjai=πn2n+1Γn/2∑j=1mAj∑i=1n+1ftjai+f-tjai.Due to∫RnfxNx;x-,Pxdx=(1/πn)∫Rnf2Pxx+x-e-xΤxdx, the Gaussian probability weighted integral of arbitrary nonlinear function is obtained as follows:(9)∫RnfxNx;x-,Pxdx=12n+1Γn/2∑j=1mAj∑i=1n+1f2tjPxai+x-+f-2tjPxai+x-.Equation (9) is the novel spherical simplex-radial cubature quadrature rule that proposed in this paper, which requires the calculation of 2(n+1)m points and corresponding weights. In particular, it can be solved that t1=n/2, A1=Γ(n/2) when m=1, and results in the spherical simplex-radial cubature rule. Thus, the spherical simplex-radial cubature rule is the degenerate form of the proposed rule, and the proposed rule can achieve a higher approximation accuracy when m≥2.


## 2.1. Spherical Simplex-Radial Cubature Rule

Consider the integralIf=∫Rnfxe-xΤxdx, let x=ry, where y denotes the unit sphere surface that satisfies yΤy=1, r≥0 denotes the sphere radius, and, then, If can be decomposed into the following spherical integral and radial integral [10]:(1)Sr=∫UnfrydσyR=∫0∞Srrn-1e-r2dr.In general, the analytical solutions of above integrals are difficult to obtain, so the numerical approximate method is considered. It is pointed out in [15] that the spherical integral can be approximated using the spherical simplex rule that contains 2n+2 integral points as follows:(2)Sr=An2n+1∑i=1n+1frai+f-rai,where An=2πn/Γn/2 denotes the surface area ofn-dimensional unit sphere with Γn=∫0∞xn-1e-xdx representing the Gamma function, ai=ai,1ai,2⋯ai,nΤ, and i=1,2,…,n+1 denotes the ith vertex of the n-dimensional simplex, whose elements are defined as follows:(3)ai,j=-n+1nn-j+2n-j+1,j<i0,j>in+1n-i+1nn-i+2,i=j.


## 2.2. Gaussian-Laguerre Quadrature Rule

For the radial integralR=∫0∞Srrn-1e-r2dr, let r2=t and we get R=(1/2)∫0∞Stt(n-2)/2e-tdt; furthermore, let gt=St and β=(n-2)/2; then R=(1/2)∫0∞gttβe-tdt is obtained; the integral term in R is approximated using the following Gaussian-Laguerre quadrature rule [20]:(4)∫0∞gttβe-tdt≈∑j=1mAjgtj,where tj denote the quadrature points, which can be solved from the solutions of the following m-order Chebyshev-Laguerre polynomial.(5)Lmβt=-1mt-βetdmdtmtβ+me-t=0.AndAj denote the corresponding weights, which can be solved as follows:(6)Aj=m!Γβ+m+1tjL˙mβtj2.It can be seen that the approximation accuracy of the above rule depends on the number of quadrature points.


## 2.3. Spherical Simplex-Radial Cubature Quadrature Rule

Equation (4) is substituted into rule R, and we obtain(7)R=12∫0∞gttβe-tdt=12∑j=1mAjgtj.The spherical simplex rule (2) and gt=St are plugged into (7) to obtain(8)R=12∑j=1mAjStj=An4n+1∑j=1mAj∑i=1n+1ftjai+f-tjai=πn2n+1Γn/2∑j=1mAj∑i=1n+1ftjai+f-tjai.Due to∫RnfxNx;x-,Pxdx=(1/πn)∫Rnf2Pxx+x-e-xΤxdx, the Gaussian probability weighted integral of arbitrary nonlinear function is obtained as follows:(9)∫RnfxNx;x-,Pxdx=12n+1Γn/2∑j=1mAj∑i=1n+1f2tjPxai+x-+f-2tjPxai+x-.Equation (9) is the novel spherical simplex-radial cubature quadrature rule that proposed in this paper, which requires the calculation of 2(n+1)m points and corresponding weights. In particular, it can be solved that t1=n/2, A1=Γ(n/2) when m=1, and results in the spherical simplex-radial cubature rule. Thus, the spherical simplex-radial cubature rule is the degenerate form of the proposed rule, and the proposed rule can achieve a higher approximation accuracy when m≥2.


## 3. SSRCQKF Algorithm

### 3.1. The General Form of SSRCQKF

Consider the following discrete nonlinear system with additive noise:(10)xk=fxk-1+wk-1zk=hxk+vkwk-1~0,Qk-1vk~0,Rk,where xk∈Rn denotes the state vector, zk∈Rnz denotes the measurement vector, and the noises wk, vk are uncorrelated Gaussian white noise. With the system dimension n and the order m of Chebyshev-Laguerre polynomial being known, tj, j=1,…,m can be solved by (5), and the corresponding weights can be calculated as follows:(11)ωi=m!Γβ+m+12n+1Γn/2t1L˙mβt12,i=1,…,2n+2m!Γβ+m+12n+1Γn/2t2L˙mβt22,i=2n+3,…,4n+1⋮m!Γβ+m+12n+1Γn/2tmL˙mβtm2,i=2mn+m-n-1,…,2n+1m.The matrixa=a1a2⋯an+1 consists of ai that is used to construct the following expansion matrix a-a=a1a2⋯an+1-a1-a2⋯-an+1. The subscript i in ·i denotes the ith column of matrix; based on the rule (9) and Bayesian filtering framework, the primary calculation process of the general form of SSRCQKF algorithm is listed as follows:Calculate the following points:(12)x^k-1i=x^k-1++2t1Pk-1+a-ai,i=1,…,2n+1x^k-1++2t2Pk-1+a-ai,i=2n+3,…,4n+1⋮x^k-1++2tmPk-1+a-ai,i=2mn+m-n-1,…,2n+1m.Calculate the nonlinear propagation of the points:(13)Xki=fk-1x^k-1i.Calculate the prior state estimation and prior error covariance matrix:(14)x^k-=∑i=12n+1mωiXkiPk-=∑i=12n+1mωiXki-x^k-Xki-x^k-Τ+Qk-1.Calculate the following points:(15)x^ki=x^k-+2t1Pk-a-ai,i=1,…,2n+1x^k-+2t2Pk-a-ai,i=2n+3,…,4n+1⋮x^k-+2tmPk-a-ai,i=2mn+m-n-1,…,2n+1m.Calculate the nonlinear propagation of the points:(16)Zki=hkx^ki.Calculate the predicted measurement value:(17)z^k=∑i=12n+1mωiZki.Calculate the predicted measurement covariance matrix:(18)Pz=∑i=12n+1mωiZki-z^kZki-z^kΤ+Rk.Calculate the cross covariance matrix:(19)Pxz=∑i=12n+1mωix^ki-x^k-Zki-z^kΤ.Calculate the Kalman filtering gain:(20)Kk=PxzPz-1Calculate the a posteriori state estimation:(21)x^k+=x^k-+Kkzk-z^k.Calculate the a posteriori error covariance matrix:(22)Pk+=Pk--KkPzKkΤ.It can be seen from the algorithm process that the filtering accuracy depends on the order of the Gaussian-Laguerre quadrature rule; the higher the order is, the higher that estimation accuracy is achieved, but the more the points and weights are required. However, for the identifiedm and n, the points can be calculated in advance and stored offline and called directly from the memory in the process of implementation, that is necessary to improve the real-time performance of the algorithm.


### 3.2. The Specific Form of SSRCQKF Whenm=2

The general form of SSRCQKF is presented in Section3.1, and, in this section, the specific form of SSRCQKF algorithm when m=2 is given. When m=2, (7) is simplified as follows:(23)R=12A1St1+A2St2.The values oft1 and t2 are needed to be calculated. Plug m=2 into (5), we obtain(24)L2βt=t-βetd2dt2tβ+2e-t.The item(d2/dt2)tβ+2e-t is expanded to achieve L2βt and its derivative L˙2βt as follows:(25)L2βt=t2-2β+2t+β+1β+2L˙2βt=2t-2β+2.LetL2βt=0, the solutions are t=β+2±β+2, combined with β=(n-2)/2, and we obtain(26)t1=n2+1+n2+1t2=n2+1-n2+1.Then,A1 and A2 are solved from (6) as follows:(27)A1=nΓn/22n+4+22n+4A2=nΓn/22n+4-22n+4.Furthermore, the weightsωi are solved as follows:(28)ωi=n4n+1n+2+2n+4,i=1,2,…,2n+2n4n+1n+2-2n+4,i=2n+3,…,4n+4.The spherical simplex-radial cubature quadrature rule withm=2 is obtained by plugging t1, t2, A1, and A2 into rule (9) as follows:(29)∫RnfxNx;x-,Pxdx=n4n+1n+2+2n+4∑i=1n+1fx-+n+2+2n+4Pxai+fx--n+2+2n+4Pxai+n4n+1n+2-2n+4∑i=1n+1fx-+n+2-2n+4Pxai+fx--n+2-2n+4Pxai.Based on (29), the calculation steps of the specific form of SSRCQKF when m=2 are given as follows.Step 1 (filter initialization).
One has(30)x^0+=Ex0P0+=Ex0-x^0+x0-x^0+Τ.
Cyclek=1,2,…, and complete the following steps.Step 2 (time update).
One has(31)x^k-1i=x^k-1++n+2+2n+4Pk-1+a-ai,i=1,2,…,2n+2x^k-1++n+2-2n+4Pk-1+a-ai-2n-2,i=2n+3,…,4n+4Xki=fk-1x^k-1ix^k-=n4n+1n+2+2n+4∑i=12n+2Xki+n4n+1n+2-2n+4∑i=2n+34n+4XkiPk-=n4n+1n+2+2n+4∑i=12n+2Xki-x^k-Xki-x^k-Τ+n4n+1n+2-2n+4∑i=2n+34n+4Xki-x^k-Xki-x^k-Τ+Qk-1.Step 3 (measurement update).
One has(32)x^ki=x^k-+n+2+2n+4Pk-a-ai,i=1,2,…,2n+2x^k-+n+2-2n+4Pk-a-ai-2n-2,i=2n+3,…,4n+4Zki=hkx^kiz^k=n4n+1n+2+2n+4∑i=12n+2Zki+n4n+1n+2-2n+4∑i=2n+34n+4ZkiPz=n4n+1n+2+2n+4∑i=12n+2Zki-z^kZki-z^kΤ+n4n+1n+2-2n+4∑i=2n+34n+4Zki-z^kZki-z^kΤ+RkPxz=n4n+1n+2+2n+4∑i=12n+2x^ki-x^k-Zki-z^kΤ+n4n+1n+2-2n+4∑i=2n+34n+4x^ki-x^k-Zki-z^kΤ.Step 4 (state update).
One has(33)Kk=PxzPz-1x^k+=x^k-+Kkzk-z^kPk+=Pk--KkPzKkΤ.


## 3.1. The General Form of SSRCQKF

Consider the following discrete nonlinear system with additive noise:(10)xk=fxk-1+wk-1zk=hxk+vkwk-1~0,Qk-1vk~0,Rk,where xk∈Rn denotes the state vector, zk∈Rnz denotes the measurement vector, and the noises wk, vk are uncorrelated Gaussian white noise. With the system dimension n and the order m of Chebyshev-Laguerre polynomial being known, tj, j=1,…,m can be solved by (5), and the corresponding weights can be calculated as follows:(11)ωi=m!Γβ+m+12n+1Γn/2t1L˙mβt12,i=1,…,2n+2m!Γβ+m+12n+1Γn/2t2L˙mβt22,i=2n+3,…,4n+1⋮m!Γβ+m+12n+1Γn/2tmL˙mβtm2,i=2mn+m-n-1,…,2n+1m.The matrixa=a1a2⋯an+1 consists of ai that is used to construct the following expansion matrix a-a=a1a2⋯an+1-a1-a2⋯-an+1. The subscript i in ·i denotes the ith column of matrix; based on the rule (9) and Bayesian filtering framework, the primary calculation process of the general form of SSRCQKF algorithm is listed as follows:Calculate the following points:(12)x^k-1i=x^k-1++2t1Pk-1+a-ai,i=1,…,2n+1x^k-1++2t2Pk-1+a-ai,i=2n+3,…,4n+1⋮x^k-1++2tmPk-1+a-ai,i=2mn+m-n-1,…,2n+1m.Calculate the nonlinear propagation of the points:(13)Xki=fk-1x^k-1i.Calculate the prior state estimation and prior error covariance matrix:(14)x^k-=∑i=12n+1mωiXkiPk-=∑i=12n+1mωiXki-x^k-Xki-x^k-Τ+Qk-1.Calculate the following points:(15)x^ki=x^k-+2t1Pk-a-ai,i=1,…,2n+1x^k-+2t2Pk-a-ai,i=2n+3,…,4n+1⋮x^k-+2tmPk-a-ai,i=2mn+m-n-1,…,2n+1m.Calculate the nonlinear propagation of the points:(16)Zki=hkx^ki.Calculate the predicted measurement value:(17)z^k=∑i=12n+1mωiZki.Calculate the predicted measurement covariance matrix:(18)Pz=∑i=12n+1mωiZki-z^kZki-z^kΤ+Rk.Calculate the cross covariance matrix:(19)Pxz=∑i=12n+1mωix^ki-x^k-Zki-z^kΤ.Calculate the Kalman filtering gain:(20)Kk=PxzPz-1Calculate the a posteriori state estimation:(21)x^k+=x^k-+Kkzk-z^k.Calculate the a posteriori error covariance matrix:(22)Pk+=Pk--KkPzKkΤ.It can be seen from the algorithm process that the filtering accuracy depends on the order of the Gaussian-Laguerre quadrature rule; the higher the order is, the higher that estimation accuracy is achieved, but the more the points and weights are required. However, for the identifiedm and n, the points can be calculated in advance and stored offline and called directly from the memory in the process of implementation, that is necessary to improve the real-time performance of the algorithm.


## 3.2. The Specific Form of SSRCQKF Whenm=2

The general form of SSRCQKF is presented in Section3.1, and, in this section, the specific form of SSRCQKF algorithm when m=2 is given. When m=2, (7) is simplified as follows:(23)R=12A1St1+A2St2.The values oft1 and t2 are needed to be calculated. Plug m=2 into (5), we obtain(24)L2βt=t-βetd2dt2tβ+2e-t.The item(d2/dt2)tβ+2e-t is expanded to achieve L2βt and its derivative L˙2βt as follows:(25)L2βt=t2-2β+2t+β+1β+2L˙2βt=2t-2β+2.LetL2βt=0, the solutions are t=β+2±β+2, combined with β=(n-2)/2, and we obtain(26)t1=n2+1+n2+1t2=n2+1-n2+1.Then,A1 and A2 are solved from (6) as follows:(27)A1=nΓn/22n+4+22n+4A2=nΓn/22n+4-22n+4.Furthermore, the weightsωi are solved as follows:(28)ωi=n4n+1n+2+2n+4,i=1,2,…,2n+2n4n+1n+2-2n+4,i=2n+3,…,4n+4.The spherical simplex-radial cubature quadrature rule withm=2 is obtained by plugging t1, t2, A1, and A2 into rule (9) as follows:(29)∫RnfxNx;x-,Pxdx=n4n+1n+2+2n+4∑i=1n+1fx-+n+2+2n+4Pxai+fx--n+2+2n+4Pxai+n4n+1n+2-2n+4∑i=1n+1fx-+n+2-2n+4Pxai+fx--n+2-2n+4Pxai.Based on (29), the calculation steps of the specific form of SSRCQKF when m=2 are given as follows.Step 1 (filter initialization).
One has(30)x^0+=Ex0P0+=Ex0-x^0+x0-x^0+Τ.
Cyclek=1,2,…, and complete the following steps.Step 2 (time update).
One has(31)x^k-1i=x^k-1++n+2+2n+4Pk-1+a-ai,i=1,2,…,2n+2x^k-1++n+2-2n+4Pk-1+a-ai-2n-2,i=2n+3,…,4n+4Xki=fk-1x^k-1ix^k-=n4n+1n+2+2n+4∑i=12n+2Xki+n4n+1n+2-2n+4∑i=2n+34n+4XkiPk-=n4n+1n+2+2n+4∑i=12n+2Xki-x^k-Xki-x^k-Τ+n4n+1n+2-2n+4∑i=2n+34n+4Xki-x^k-Xki-x^k-Τ+Qk-1.Step 3 (measurement update).
One has(32)x^ki=x^k-+n+2+2n+4Pk-a-ai,i=1,2,…,2n+2x^k-+n+2-2n+4Pk-a-ai-2n-2,i=2n+3,…,4n+4Zki=hkx^kiz^k=n4n+1n+2+2n+4∑i=12n+2Zki+n4n+1n+2-2n+4∑i=2n+34n+4ZkiPz=n4n+1n+2+2n+4∑i=12n+2Zki-z^kZki-z^kΤ+n4n+1n+2-2n+4∑i=2n+34n+4Zki-z^kZki-z^kΤ+RkPxz=n4n+1n+2+2n+4∑i=12n+2x^ki-x^k-Zki-z^kΤ+n4n+1n+2-2n+4∑i=2n+34n+4x^ki-x^k-Zki-z^kΤ.Step 4 (state update).
One has(33)Kk=PxzPz-1x^k+=x^k-+Kkzk-z^kPk+=Pk--KkPzKkΤ.


## 4. Simulation Results and Analysis

### 4.1. Simulation 1

The effectiveness of the proposed SSRCQKF algorithm is verified by the following three-dimensional strong nonlinear system, which includes trigonometric function operation, power operation, and exponential operation.(34)x1,k+1x2,k+1x3,k+1=3sin2⁡x2,kx1,k+e-0.05x3,k0.2x1,kx2,k+x3,k+111wkzk=cos⁡x1,k+x2,kx3,k+vk,where Q=0.1, R=1, the theoretical initial value of the nonlinear system is x0=-0.711Τ, the filtering initial value is x^0=000Τ, the initial covariance matrix is P0=I3×3. The SSRCQKF-2 (when m=2) and SSRCQKF-3 (when m=3) are compared with CKF and SSRCKF, the simulation step is 1, and the total step size is 100. The root mean square error (RMSE) is used to describe the filtering accuracy and run 500 times Monte-Carlo simulation, and the results are shown in Figures 1–3. In order to show the details of the curve more clearly, the data only between 30 and 80 are captured in figures. It can be seen from the figures that the RMSE of the proposed SSRCQKF is significantly smaller than the other two algorithms.Figure 1
RMSE of state 1.Figure 2
RMSE of state 2.Figure 3
RMSE of state 3.In order to compare the four filters quantitatively, the average value and variance of RMSE of the four filters in 100 steps are counted and listed in Tables1 and 2, respectively. From Table 1, we see that CKF achieves a higher filtering accuracy than SSRCKF in this simulation. Compared with CKF, the SSRCQKF-2 proposed in this paper improves the estimation accuracy of state 1 by 1.4%, improves the estimation accuracy of state 2 by 6.26%, and improves the estimation accuracy of state 3 by 10.17%. Compared with SSRCQKF-2, the SSRCQKF-3 improves the estimation accuracy of the three states by 0.59%, 0.56%, and 2.76%, respectively. All these indicate that the proposed filter has the optimal performance in terms of estimation accuracy. Moreover, the RMSE variance of the two SSRCQKF are smaller than that of the other two filters, which indicates that the estimated fluctuation is more stable.Table 1
Average RMSE of state.
Filters
State  1
State  2
State  3
CKF
0.8743
0.6120
0.4928
SSRCKF
0.9540
0.7152
0.6429
SSRCQKF-2
0.8621
0.5737
0.4427
SSRCQKF-3
0.8570
0.5705
0.4305Table 2
Variance RMSE of state 1.
Filters
State  1
State  2
State  3
CKF
0.0108
0.0108
0.0097
SSRCKF
0.0120
0.0115
0.0217
SSRCQKF-2
0.0102
0.0089
0.0060
SSRCQKF-3
0.0099
0.0087
0.0056


### 4.2. Simulation 2

The proposed SSRCQKF is applied in target tracking in this section. The target is assumed to be in constant velocity (CV) motion; the state equation of CV model in two-dimensional case is described as follows:(35)Xk=FCVXk-1+Gwk-1,where Xk=xk,x˙k,yk,y˙kΤ represents the target states (position and velocity) at time index k, wk denotes the process noise, FCV and G denote the state transformation matrix and the noise driven matrix, which are defined, respectively, as follows:(36)FCV=1T000100001T0001,G=T220T00T220T,where T is the sampling interval.In target tracking system, the bearings-only measurement equation is written as follows:(37)Zk=arctan⁡yk-yrxk-xr+vk,where Zk is the radar measurement at time k, xr,yr is the location of radar, and vk is the measurement noise.In the simulation, the radar location is set to bexr,yr=200m,300m, the simulation time is 40 s, T=1, and the initial state of the target is x0,x˙0,y0,y˙0=100m,2m/s,200m,20m/s. The initial state and covariance matrix are x^0+=100m,2m/s,200m,20m/sΤ and P0+=diag⁡0.01,0.01,0.01,0.01, respectively. The standard deviation of the measurement noise is 0.1 deg. The Monte-Carlo simulation is performed 500 times, and the tracking accuracy is also evaluated using the RMSE.The simulation results, including the position RMSE and velocity RMSE of various filters, are shown in Figures4 and 5, respectively, and the partial magnification is given to show the details of the curve. It can be seen that the tracking accuracy of CKF is significantly lower than that of other filters, and the proposed SSRCQKF-2 and SSRCQKF-3 have achieved higher target tracking accuracy than that of SSRCKF. The average RMSE of the various filters is shown in Table 3, and compared with CKF, the estimated position accuracy of SSRCKF is improved by 20.85%, indicating that the spherical simplex-radial cubature rule has higher accuracy than spherical-radial cubature rule in this simulation. Since the second-order Gauss-Laguerre rule is used in the SSRCQKF-2, the accuracy is improved by 1.86% compared to SSRCKF. Due to the more quadrature points used in SSRCQKF-3, its accuracy is higher than that of SSRCQKF-2 by 1.50%. Through the analysis of the simulation results, the effectiveness of the proposed filter is verified.Table 3
Average RMSE of various filters.
Filters
Position RMSE/m
Velocity RMSE/(m/s)
CKF
3.0372
0.1874
SSRCKF
2.4039
0.1655
SSRCQKF-2
2.3591
0.1642
SSRCQKF-3
2.3238
0.1630Figure 4
The position RMSE of various filters.Figure 5
The velocity RMSE of various filters.


## 4.1. Simulation 1

The effectiveness of the proposed SSRCQKF algorithm is verified by the following three-dimensional strong nonlinear system, which includes trigonometric function operation, power operation, and exponential operation.(34)x1,k+1x2,k+1x3,k+1=3sin2⁡x2,kx1,k+e-0.05x3,k0.2x1,kx2,k+x3,k+111wkzk=cos⁡x1,k+x2,kx3,k+vk,where Q=0.1, R=1, the theoretical initial value of the nonlinear system is x0=-0.711Τ, the filtering initial value is x^0=000Τ, the initial covariance matrix is P0=I3×3. The SSRCQKF-2 (when m=2) and SSRCQKF-3 (when m=3) are compared with CKF and SSRCKF, the simulation step is 1, and the total step size is 100. The root mean square error (RMSE) is used to describe the filtering accuracy and run 500 times Monte-Carlo simulation, and the results are shown in Figures 1–3. In order to show the details of the curve more clearly, the data only between 30 and 80 are captured in figures. It can be seen from the figures that the RMSE of the proposed SSRCQKF is significantly smaller than the other two algorithms.Figure 1
RMSE of state 1.Figure 2
RMSE of state 2.Figure 3
RMSE of state 3.In order to compare the four filters quantitatively, the average value and variance of RMSE of the four filters in 100 steps are counted and listed in Tables1 and 2, respectively. From Table 1, we see that CKF achieves a higher filtering accuracy than SSRCKF in this simulation. Compared with CKF, the SSRCQKF-2 proposed in this paper improves the estimation accuracy of state 1 by 1.4%, improves the estimation accuracy of state 2 by 6.26%, and improves the estimation accuracy of state 3 by 10.17%. Compared with SSRCQKF-2, the SSRCQKF-3 improves the estimation accuracy of the three states by 0.59%, 0.56%, and 2.76%, respectively. All these indicate that the proposed filter has the optimal performance in terms of estimation accuracy. Moreover, the RMSE variance of the two SSRCQKF are smaller than that of the other two filters, which indicates that the estimated fluctuation is more stable.Table 1
Average RMSE of state.
Filters
State  1
State  2
State  3
CKF
0.8743
0.6120
0.4928
SSRCKF
0.9540
0.7152
0.6429
SSRCQKF-2
0.8621
0.5737
0.4427
SSRCQKF-3
0.8570
0.5705
0.4305Table 2
Variance RMSE of state 1.
Filters
State  1
State  2
State  3
CKF
0.0108
0.0108
0.0097
SSRCKF
0.0120
0.0115
0.0217
SSRCQKF-2
0.0102
0.0089
0.0060
SSRCQKF-3
0.0099
0.0087
0.0056


## 4.2. Simulation 2

The proposed SSRCQKF is applied in target tracking in this section. The target is assumed to be in constant velocity (CV) motion; the state equation of CV model in two-dimensional case is described as follows:(35)Xk=FCVXk-1+Gwk-1,where Xk=xk,x˙k,yk,y˙kΤ represents the target states (position and velocity) at time index k, wk denotes the process noise, FCV and G denote the state transformation matrix and the noise driven matrix, which are defined, respectively, as follows:(36)FCV=1T000100001T0001,G=T220T00T220T,where T is the sampling interval.In target tracking system, the bearings-only measurement equation is written as follows:(37)Zk=arctan⁡yk-yrxk-xr+vk,where Zk is the radar measurement at time k, xr,yr is the location of radar, and vk is the measurement noise.In the simulation, the radar location is set to bexr,yr=200m,300m, the simulation time is 40 s, T=1, and the initial state of the target is x0,x˙0,y0,y˙0=100m,2m/s,200m,20m/s. The initial state and covariance matrix are x^0+=100m,2m/s,200m,20m/sΤ and P0+=diag⁡0.01,0.01,0.01,0.01, respectively. The standard deviation of the measurement noise is 0.1 deg. The Monte-Carlo simulation is performed 500 times, and the tracking accuracy is also evaluated using the RMSE.The simulation results, including the position RMSE and velocity RMSE of various filters, are shown in Figures4 and 5, respectively, and the partial magnification is given to show the details of the curve. It can be seen that the tracking accuracy of CKF is significantly lower than that of other filters, and the proposed SSRCQKF-2 and SSRCQKF-3 have achieved higher target tracking accuracy than that of SSRCKF. The average RMSE of the various filters is shown in Table 3, and compared with CKF, the estimated position accuracy of SSRCKF is improved by 20.85%, indicating that the spherical simplex-radial cubature rule has higher accuracy than spherical-radial cubature rule in this simulation. Since the second-order Gauss-Laguerre rule is used in the SSRCQKF-2, the accuracy is improved by 1.86% compared to SSRCKF. Due to the more quadrature points used in SSRCQKF-3, its accuracy is higher than that of SSRCQKF-2 by 1.50%. Through the analysis of the simulation results, the effectiveness of the proposed filter is verified.Table 3
Average RMSE of various filters.
Filters
Position RMSE/m
Velocity RMSE/(m/s)
CKF
3.0372
0.1874
SSRCKF
2.4039
0.1655
SSRCQKF-2
2.3591
0.1642
SSRCQKF-3
2.3238
0.1630Figure 4
The position RMSE of various filters.Figure 5
The velocity RMSE of various filters.


## 5. Conclusion

In order to further improve the filtering accuracy of the nonlinear system, this paper proposes a SSRCQKF algorithm by combining the spherical simplex-radial cubature rule with arbitrary order Gaussian-Laguerre quadrature rule. The proposed algorithm has a higher filtering accuracy than CKF and SSRCKF. The results of the two numerical simulations, including the three-dimensional strongly nonlinear system and target tracking, verify the validity of the proposed algorithm.



---

*Source: 7863875-2017-08-08.xml*