# Application of Improved Simplex Quadrature Cubature Kalman Filter in Nonlinear Dynamic System

Authors: Ting Cao; Huo-tao Gao; Chun-feng Sun; Yun Ling; Guo-bao Ru
Journal: Mathematical Problems in Engineering
 (2020)

---

## Abstract

A novel spherical simplex Gauss–Laguerre quadrature cubature Kalman filter is proposed to improve the estimation accuracy of nonlinear dynamic system. The nonlinear Gaussian weighted integral has been approximately evaluated using the spherical simplex rule and the arbitrary order Gauss–Laguerre quadrature rule. Thus, a spherical simplex Gauss–Laguerre cubature quadrature rule is developed, from which the general computing method of the simplex cubature quadrature points and the corresponding weights are obtained. Then, under the nonlinear Kalman filtering framework, the spherical simplex Gauss–Laguerre quadrature cubature Kalman filter is derived. A high-dimensional nonlinear state estimation problem and a target tracking problem are utilized to demonstrate the effectiveness of the proposed spherical simplex Gauss–Laguerre cubature quadrature rule to improve the performance.


---

## Body

## 1. Introduction

Nonlinear filtering and estimation based on Bayes framework have been widely applied in many fields, such as target tracking [1–5], navigation [5, 6] and positioning [7, 8], power system [9–11], and pattern recognition [12]. Recent years have seen a surge of interest in solving the nonlinear estimation problems. As is well known, the optimal solution to nonlinear systems is impractical. The focus has been on developing suboptimal algorithms, and many approximate nonlinear filters are generally proposed. The approximate nonlinear filters can be divided into two classes: global approach and local approach. In global approximations, one intuitive scheme is to randomly select a set of weighted particles to approximate the propagation of possible density functions (PDFs), giving birth to the particle filter (PF) [13] and grid-mass filter [14]. Although filters in this approach have high precision, the computational burden is its primary obstacle to more widespread use. Another scheme is the global linearization [15] method, in which the differential inclusion theory represents the nonlinear system via a linear differential inclusion (LDI) model. This kind of method can effectively solve the problem of local linearization error in the local linearization method. It can also handle different types of noise and model uncertainty with strong robustness. Despite the superior performance of global linearization, the problem of filter design conservatism is one of the bottlenecks. To reduce the conversion, Yu and Li [16] proposed a nested tensor product model transformation (NTPMT). The NTPMT is able to obtain multilevel nested TP models for the given systems and reduce the dimensionality of the vertex tensor such that the number of vertex systems contained in the resultant TP model is reduced effectively. The proposed transformation has indicated an attractive research direction in global linearization.The local approximations include the analytical approximations and sampling-based approaches. The most widely used nonlinear filter is the extended Kalman filter (EKF) [17], which is the typical analytical approximate nonlinear filter. The state estimation is approximated by a Gaussian random variable (GRV), which is then propagated analytically through the first-order linearization of the nonlinear process and measurement system. However, the EKF has slow convergence and low estimated accuracy and may suffer from large errors when the system has high nonlinearity. The unscented Kalman filter (UKF) [18], the Gauss–Hermite quadrature filter (GHQF) [19], and the cubature Kalman filter (CKF) [20] fall under the latter category. The UKF addresses the flaws of the EKF by using a set of deterministic points to calculate the mean and covariance of nonlinear functions. Based on the Gauss–Hermite quadrature (GHQ) rule, the GHQF is more stable and accurate than UKF. Unfortunately, the experimental growth with the dimension of the number of quadrature points and computational complexity has prohibited its widespread use.Recently, a cubature Kalman filter [20] has been proposed and widely used in many applications for its simplicity and effectiveness. The heart of the CKF is a spherical-radial cubature rule, which makes it possible to numerically compute multivariate moment integrals met in the nonlinear Bayesian framework. Since its inception in 2009, CKF has been an active research area for many researchers all over the world resulting in various variants of the basic algorithm with improved performance. A new class of CKFs with arbitrary degrees of accuracy in computing the spherical and radial integrals was put forward by Jia [21]. Based on a spherical simplex-radial rule, Wang et al. [22] proposed the spherical simplex-radial cubature Kalman filter (SSRCKF). In SSRCKF, the transformation group of the regular simplex and the moment matching method are used to calculate the spherical and the radial integrals, respectively. Simulation results show that the SSRCKF can obtain higher accuracy and better efficiency than the traditional CKFs for high-dimension systems.However, the radial integral in both CKFs and SSRCKFs is solved by the moment matching method [22], which is ambiguous and does not provide the best possible solution available [23]. To tackle this problem, Bhaumik and Swati proposed the cubature quadrature Kalman filter (CQKF) [24]. In CQKF, the spherical integral is calculated the same as the CKFs, but the radial integral is solved with arbitrary order Gauss–Laguerre quadrature rule. Thanks to the arbitrary order Gauss–Laguerre quadrature rule, the filtering accuracy has been further improved. It is also indicated that the CKF is a special case of CQKF with the first-order Gaussian–Laguerre quadrature in radial integral. To further improve the performance of the CQKF, a higher degree cubature quadrature Kalman filter (HDCQKF) [23] is proposed.The contribution of this paper is to propose a new class of CKFs with the spherical simplex rule to approximate the spherical integral and arbitrary order Gauss–Laguerre quadrature rule to solve the radial integral. The proposed filter is termed as spherical simplex Gauss–Laguerre quadrature cubature Kalman filter (SSGQKF). The SSGQKF would be a generalized form of SSRCKF, and under single Gauss–Laguerre quadrature point evaluation, it coincides with the third-degree SSRCKF. The accuracy of the SSGQKF depends on the number of the simplex cubature points. The higher the number of simplex quadrature points, the better the accuracy. Moreover, the computational burden of the SSGQKF does not increase exponentially with the dimension, and thus it does not have the curse of dimensionality problem. In addition, it is derivative free. The proposed nonlinear filter can provide more accurate results with less computational costs than HDCQKF in high-dimensional nonlinear state estimation systems. Two examples have been provided to demonstrate the superiority of the SSGQKF.The rest of this paper is organized as follows. Section2 reviews the cubature quadrature Kalman filter in the Bayesian framework of filtering and estimation. In Section 3, we present the proposed SSGQKF method. To verify the superiority of the SSGQKF method, Section 4 shows the compared performance for the conventional nonlinear filtering and the proposed algorithm. Finally, several concluding remarks are given in Section 5 to end this paper.


## 2. Cubature Quadrature Kalman Filter

### 2.1. Nonlinear System Model

Consider a class of nonlinear discrete estimation systems described by(1)xk=fxk−1+vk−1,yk=hxk+nk,where xk∈ℝnx is the state vector of the dynamic system at discrete time k; yk∈ℝm is the measurement vector; and vk−1 and nk are independent process and measurement Gaussian white noise with zero means and covariances Qk−1 and Rk, respectively.Under the Gaussian domain, Bayesian filters can be generalized by computing the following integral:(2)If=1Σ2πn∫Rnfxe−1/2x−μTΣ−1x−μdx.Letx=Crz+μ, where μ is the mean and Σ=CCT. Then, the integral If can be written as(3)If=12πn∫r=0∞∫UnfCrz+μdσzrn−1e−r2/2dr.If we assume zero mean and unity covariance, equation (3) becomes(4)If=12πn∫r=0∞∫Unfrzdσzrn−1e−r2/2dr,where Un=z∈ℝnzTz=1 is the spherical surface and σ⋅ is the spherical surface measure or the area element on Un. To compute the integral If, we decompose it into a surface integral Sr and a line integral. We may thus write the line integral(5)I=∫r=0∞Srrn−1e−r2/2dr,in which Sr is defined by the spherical integral with the unit weighting function wz=1:(6)Sr=∫Unfrzdσz.In this paper, the spherical and the line integrals are numerically computed by the spherical simplex rule adopted from [22] and the arbitrary order Gauss–Laguerre quadrature rule, respectively.


### 2.2. Spherical Simplex Rule

The spherical integral of the form∫Unfrzdσz can be efficiently evaluated by the transformation group of the regular n–simplex with vertices aj=aj,1,aj,2,⋯,aj,nT,j=1,2,…,n+1 [25, 26], i.e.,(7)aj,i≡−n+1nn−i+2n−i+1,i<j,n+1n−j+1nn−j+2,i=j,0,i>j.The set of midpoints of the verticesaj projected onto the spherical surface yields the following point set [25]:(8)bj≡n2n−1ai+al:i<l,l=1,2,…,n+1.Thus, the third-degree spherical simplex rule that contains2n+2 integral points is as follows:(9)S3r=An2n+1∑j=1n+1fraj+f−raj,where An=2πn/Γn/2 is the surface area of the unit sphere and Γn=∫0∞xn−1e−xdx. Similarly, the fifth-degree spherical simplex rule with n2+3n+2 quadrature points is obtained as follows [25, 26]:(10)S5r=An7−nn2n+12n+2∑j=1n+1fraj+f−raj+2n−12nn+12n+2∑j=1nn+1/2frbj+f−rbj.Therefore, the spherical integral can be approximately byNs point spherical simplex rule as(11)∫Unfrzdσz≈∑i=1Nsωs,ifrzi.


### 2.3. Gauss–Laguerre Quadrature Rule

It is known that any integral in the form of(12)∫λ=0∞fλλαe−λdλ,can be approximately evaluated using quadrature points and weights associated with them. The quadrature points can be derived from the roots of the n′ order of Chebyshev–Laguerre polynomial equation [27, 28]:(13)Ln′αλ=−1n′λ−αeλdn′dλn′λα+n′e−λ=0.The weights of the quadrature pointsλn′ can be determined as(14)Ai′=n′!Γα+n′+1λi′Ln′αλi′2.So, integral (12) can be written approximately using the Gauss–Laguerre quadrature rule as(15)∫λ=0∞fλλαe−λdλ≈∑i′=1n′Ai′fλi′.


## 2.1. Nonlinear System Model

Consider a class of nonlinear discrete estimation systems described by(1)xk=fxk−1+vk−1,yk=hxk+nk,where xk∈ℝnx is the state vector of the dynamic system at discrete time k; yk∈ℝm is the measurement vector; and vk−1 and nk are independent process and measurement Gaussian white noise with zero means and covariances Qk−1 and Rk, respectively.Under the Gaussian domain, Bayesian filters can be generalized by computing the following integral:(2)If=1Σ2πn∫Rnfxe−1/2x−μTΣ−1x−μdx.Letx=Crz+μ, where μ is the mean and Σ=CCT. Then, the integral If can be written as(3)If=12πn∫r=0∞∫UnfCrz+μdσzrn−1e−r2/2dr.If we assume zero mean and unity covariance, equation (3) becomes(4)If=12πn∫r=0∞∫Unfrzdσzrn−1e−r2/2dr,where Un=z∈ℝnzTz=1 is the spherical surface and σ⋅ is the spherical surface measure or the area element on Un. To compute the integral If, we decompose it into a surface integral Sr and a line integral. We may thus write the line integral(5)I=∫r=0∞Srrn−1e−r2/2dr,in which Sr is defined by the spherical integral with the unit weighting function wz=1:(6)Sr=∫Unfrzdσz.In this paper, the spherical and the line integrals are numerically computed by the spherical simplex rule adopted from [22] and the arbitrary order Gauss–Laguerre quadrature rule, respectively.


## 2.2. Spherical Simplex Rule

The spherical integral of the form∫Unfrzdσz can be efficiently evaluated by the transformation group of the regular n–simplex with vertices aj=aj,1,aj,2,⋯,aj,nT,j=1,2,…,n+1 [25, 26], i.e.,(7)aj,i≡−n+1nn−i+2n−i+1,i<j,n+1n−j+1nn−j+2,i=j,0,i>j.The set of midpoints of the verticesaj projected onto the spherical surface yields the following point set [25]:(8)bj≡n2n−1ai+al:i<l,l=1,2,…,n+1.Thus, the third-degree spherical simplex rule that contains2n+2 integral points is as follows:(9)S3r=An2n+1∑j=1n+1fraj+f−raj,where An=2πn/Γn/2 is the surface area of the unit sphere and Γn=∫0∞xn−1e−xdx. Similarly, the fifth-degree spherical simplex rule with n2+3n+2 quadrature points is obtained as follows [25, 26]:(10)S5r=An7−nn2n+12n+2∑j=1n+1fraj+f−raj+2n−12nn+12n+2∑j=1nn+1/2frbj+f−rbj.Therefore, the spherical integral can be approximately byNs point spherical simplex rule as(11)∫Unfrzdσz≈∑i=1Nsωs,ifrzi.


## 2.3. Gauss–Laguerre Quadrature Rule

It is known that any integral in the form of(12)∫λ=0∞fλλαe−λdλ,can be approximately evaluated using quadrature points and weights associated with them. The quadrature points can be derived from the roots of the n′ order of Chebyshev–Laguerre polynomial equation [27, 28]:(13)Ln′αλ=−1n′λ−αeλdn′dλn′λα+n′e−λ=0.The weights of the quadrature pointsλn′ can be determined as(14)Ai′=n′!Γα+n′+1λi′Ln′αλi′2.So, integral (12) can be written approximately using the Gauss–Laguerre quadrature rule as(15)∫λ=0∞fλλαe−λdλ≈∑i′=1n′Ai′fλi′.


## 3. Spherical Simplex Gauss–Laguerre Quadrature Evaluation of Integrals

### 3.1. Spherical Simplex Gauss–Laguerre Quadrature Rule

From (12) and (16), the spherical simplex Gauss–Laguerre rule for Nx;0,I can be approximated by(16)If=12πn∫r=0∞∑i=1Nsωs,ifrzirn−1e−r2/2dr,(17)=12πn∫t=0∞∑i=1Nsωs,if2tzitn/2−1e−tdt,(18)=12πn∑i′=1n′ωi′∑i=1Nsωs,if2tzi.Note that to transform the integralIf into the form of equation (12), we make an assumption that t=r2/2. Let the numerical method for ∫​RnfxNx;0,Idx be represented by Qf. Substituting (9) and (15) into (18) yields the third-degree spherical simplex Gauss–Laguerre quadrature rule as(19)Qf=12πn∑i′=1n′Ai″An2n+1∑j=1n+1fraj+f−raj.Similarly, substituting (10) and (15) into (18), we have the fifth-degree spherical simplex Gauss–Laguerre quadrature rule as(20)Qf=12πn∑i′=1n′Ai′An7−nn2n+12n+2∑j=1n+1fraj+f−raj+2n−12nn+12n+2∑j=1nn+1/2frbj+f−rbj.The Gaussian probability weighted integral∫RnfxNx;x¯,Pxdx can be performed by the linear transformation of Pxi+x^, where P is the square root of the matrix P and xi is the quadrature points from (19) or (20).For an dimension of state-space problem solved with third-order spherical simplex rule and n′ order Gauss–Laguerre quadrature points, total 2n+1n′ points and associated weights need to be calculated. The fifth-degree spherical simplex Gauss–Laguerre quadrature rule has n2+3n+2n′ points and shares 2n+1n′ points with the third-degree spherical simplex Gauss–Laguerre quadrature rule. For simplicity, these simplex cubature quadrature points are termed as SCQ points in this paper.


### 3.2. Calculation of Simplex Cubature Quadrature Points and Weights

The steps for calculating the simplex cubature quadrature (SCQ) points and associated weights are as follows:(i)
Determine the system dimensionn, the orders of spherical simplex cubature rule (i.e., m), and Chebyshev–Laguerre polynomial n′. Find out the set of sample points for a given specific degree of spherical simplex cubature rule.(ii)
For the third-order2m+1=3 spherical simplex rule, the sample points ξi=a,−a.(iii)
For the fifth-order2m+1=5 spherical simplex rule, the sample points ξi=a,−a,b,−b.(iv)
Solve then′ order Chebyshev–Laguerre polynomial (13) for α=n/2−1 to derive the quadrature points λi′.(v)
Find the SCQ points asξj=2λi′ξi.(vi)
The weights associated with SCQ points are(21)Wj=Ai′ωp2πn=n′!Γα+n′+1λi′L˙n′αλi′2×ωp2πn.(i)
For the third-order spherical simplex rulej=1,2,…,2n′n+1:(22)ωp=An2n+1,p=1,2,…,2n+1.(ii)
For the fifth-order spherical simplex rulej=1,2,…,n2+3n+2n′:(23)ωp=An7−nn2n+12n+2,p=1,2,…,2n+1,2Ann−12nn+12n+2,p=2n+3,…,n2+3n+2.The number of support SCQ points required for different ordered spherical simplex cubature rule is shown in Table1. The SCQ points and their associated weight of a second-order system for the third- and fifth-degree spherical simplex cubature and up to the third-order Gauss–Laguerre quadrature approximation are shown in Table 2. As the dimension of the state-space models increases, the computational burden also increases. We only discuss the third-degree and the fifth-degree spherical simplex rule here.Table 1
Support points required for different ordered spherical simplex cubature andn′ order Gauss–Laguerre quadrature rule.
Degree of spherical simplex cubature ruleNumber of SCQ points32n+1n′5n2+3n+2n′7n+1n2+8n+6n′/3Table 2
Points and weights for different degree of simplex cubature and higher-order Gauss–Laguerre rules for a second-order system.
2m + 1n′SCQ pointsWeights31(1.4142, 0), (−0.7071, 1.2247), (−0.7071, −1.2247), (−1.4142, 0), (0.7071, −1.2247), (0.7071, 1.2247)1/6, 1/6, 1/6, 1/6, 1/6, 1/62(2.6131, 0), (−1.3066, 2.2630), (−1.3066, −2.2630), (−2.6131, 0), (1.3066, −2.2630), (1.3066, 2.2630), (1.0824, 0), (−0.5412, 0.9374), (−0.5412, −0.9374), (−1.0824, 0), (0.5412, −0.9374), (0.5412, 0.9374)0.0244,0.0244, 0.0244, 0.0244, 0.0244, 0.0244, 0.1423, 0.1423, 0.1423, 0.1423, 0.1423, 0.14233(3.5468, 0), (−1.7734, 3.0716), (−1.7734, −3.0716), (−3.5468, 0), (1.7734, −3.0716), (1.7734, 3.0716), (2.1421, 0), (−1.0710, 1.8551), (−1.0710, −1.8551), (−2.1421, 0), (1.0710, −1.8551), (1.0710, 1.8551), (0.9119, 0), (−0.4559, 0.7897), (−0.4559, −0.7897), (−0.9119, 0), (0.4559, −0.7897), (0.4559, 0.7897)0.0017, 0.0017, 0.0017, 0.0017, 0.0017, 0.0017, 0.0464, 0.0464, 0.0464, 0.0464, 0.0464, 0.0464, 0.1185, 0.1185, 0.1185, 0.1185, 0.1185, 0.118551(1.4142, 0), (−0.7071, 1.2247), (−0.7071, −1.2247), (−1.4142, 0), (0.7071, −1.2247), (0.7071, 1.2247), (0.7071, 1.2247), (0.7071, −1.2247), (−1.4142, 0), (−0.7071, −1.2247), (1.4142, 0)5/36, 5/36, 5/36, 5/36, 5/36, 5/36, 1/36, 1/36, 1/36, 1/36, 1/36, 1/362(2.6131, 0), (−1.3066, 2.2630), (−1.3066, −2.2630), (−2.6131, 0), (1.3066, −2.2630), (1.3066, 2.2630), (1.3066, 2.2630), (1.3066, −2.2630), (−2.6131, 0), (−1.3066, −2.2630), (−1.3066, 2.2630), (2.6131, 0), (1.0824, 0), (−0.5412, 0.9374), (−0.5412, −0.9374), (−1.0824, 0), (0.5412, −0.9374), (0.5412, 0.9374), (0.5412, 0.9374), (0.5412, −0.9374), (−1.0824, 0), (−0.5412, −0.9374), (−0.5412, 0.9374), (1.0824, 0)0.0203, 0.0203, 0.0203, 0.0203, 0.0203, 0.0203, 0.0041, 0.0041, 0.0041, 0.0041, 0.0041, 0.0041, 0.1185, 0.1185, 0.1185, 0.1185, 0.1185, 0.1185, 0.0237, 0.0237, 0.0237, 0.0237, 0.0237, 0.02373(3.5468, 0), (−1.7734, 3.0716), (−1.7734, −3.0716), (−3.5468, 0), (1.7734, −3.0716), (1.7734, 3.0716), (1.7734, 3.0716), (1.7734, −3.0716), (−3.5468, 0), (−1.7734, −3.0716), (−1.7734, 3.0716), (3.5468, 0), (2.1421, 0), (−1.0710, 1.8551), (−1.0710, −1.8551), (−2.1421, 0), (1.0710, −1.8551), (1.0710, 1.8551), (1.0710, 1.8551), (1.0710, −1.8551), (−2.1421, 0), (−1.0710, −1.8551), (−1.0710, 1.8551), (2.1421, 0), (0.9119, 0), (−0.4559, 0.7897), (−0.4559, 0.7897), (−0.9119, 0), (0.4559, −0.7897), (0.4559, 0.7897), (0.4559, 0.7897), (0.4559, −0.7897), (−0.9119, 0), (−0.4559, −0.7897), (−0.4559, 0.7897), (0.9119, 0)0.0014, 0.0014, 0.0014, 0.0014, 0.0014, 0.0014, 0.0003, 0.0003, 0.0003, 0.0003, 0.0003, 0.0003, 0.0387, 0.0387, 0.0387, 0.0387, 0.0387, 0.0387, 0.0077, 0.0077, 0.0077, 0.0077, 0.0077, 0.0077, 0.0988, 0.0988, 0.0988, 0.0988, 0.0988, 0.0988, 0.0198, 0.0198, 0.0198, 0.0198, 0.0198, 0.0198


### 3.3. The Spherical Simplex Gauss–Laguerre Quadrature Cubature Kalman Filter (SSGQKF)

Applying the spherical simplex Gauss–Laguerre quadrature rule described in Section3.2 in the Bayesian filter framework yields the new spherical simplex Gauss–Laguerre quadrature cubature Kalman filter (SSGQKF).The algorithm of the proposed SSGQKF could be summarised as follows.Step 1.
Initialization(i)
Initialize the filter withx^00 and P00.(ii)
Calculate the SCQ pointsξj and their corresponding weights Wj as described in Section 3.2.Step 2.
Prediction step(i)
Factorise covariance matrix using Cholesky decomposition:(24)Pkk=SkkSkkT.(ii)
Evaluate the SCQ points:(25)χj,kk=Skkξj+x^kk.(iii)
Update the SCQ points:(26)χj,k+1k=fχj,kk.(iv)
Estimate the predicted state and the predicted error covariance:(27)x^k+1k=∑j=1tχj,k+1kWj,Pk+1k=∑j=1tWjχj,k+1k−x^k+1kχj,k+1k−x^k+1kT+Qk,where t is the total number of SCQ points.Step 3.
Measurement step(i)
Factorise the predicted error covariance matrixPk+1k using the Cholesky decomposition:(28)Pk+1k=Sk+1kSk+1kT.(ii)
Evaluate the SCQ points:(29)χj,k+1k=Sk+1kξj+x^k+1k.(iii)
Find the propagated measurements at each SCQ points:(30)Yj,k+1k=hχj,k+1k.(iv)
Estimate the predicted measurement:(31)y^k+1=∑j=1tYj,k+1kWj.(v)
Calculate the innovation covariance matrix and the cross-covariance matrix:(32)Pyy=∑j=1tWjYj,k+1k−y^k+1Yj,k+1k−y^k+1T+Rk,Pxy=∑j=1tWjχj,k+1k−x^k+1Yj,k+1k−y^k+1T.(vi)
Compute the Kalman gain:(33)Kk+1=PxyPyy−1.(vii)
Compute the filtering estimation:(34)x^k+1k+1=x^k+1k+Kk+1yk+1−y^k+1.(viii)
Compute the filtering estimation error covariance matrix:(35)Pk+1k+1=Pk+1k−Kk+1PyyKk+1T.
Remarks(i)
The spherical simplex cubature Gauss–Laguerre quadrature rule constructed using the above theorems is fully symmetric.(ii)
For a specific system dimensionn, there are two tuning parameters, namely, m and n′, which could be tuned to achieve satisfied accuracy on affordable computational load.(iii)
Compared to EKF, the SSGQKF is derivative free. It is easy to implement as neither a Jacobian nor Hessian matrix needs to be computed. It is also computationally more efficient than the PF and GHF because instead of generating large number of particles or quadrature points in the state space, few SCQ points are required to be generated, updated, and predicted.(iv)
For a specific order of Chebyshev–Laguerre polynomialn′, the number of SCQ points of the fifth-degree SSGQKF is less than that of the existing fifth-degree HDCQKF 2n2n′ when n>4. Therefore, from the perspective of computation, the proposed fifth-degree SSGQKF is much less complex than HDCQKF, making it easier to implement in high-dimensional nonlinear systems. That is to say, the SSGQKF will be a potential and promising method in practical engineering implementation.(v)
Whenn>4, the fifth-degree spherical-radial rule has some negative weights. Negative weights may lead to divergence of filters [22]. Despite some of the weights from (20) in the fifth-degree spherical simplex cubature rule becoming negative when n>7, they go to 0 when n⟶∞. Thus, the proposed fifth-degree SSGQKF is more efficient than the existing HDCQKF for high-dimensional systems. Consequently, the SSGQKF can be a very promising alternative to the HDCQKF under the Gaussian assumption, especially in the case of high-dimensional cases.(vi)
For a predefined ordered cubature and Gauss–Laguerre quadrature, the SCQ points and associated weights can be calculated and stored offline. In other words, the computation burden could be further alleviated.
In the following, a few discussions are added for the practical implementation of the proposed method Like the traditional Kalman filter (KF) and its suboptimal extensions (e.g., EKF, UKF, and CKF), the proposed SSGQKF is also developed under the Gaussian assumption and the minimum mean square error (MMSE) criterion. To deal with non-Gaussian noises and for further improvement, the proposed filter can be combined with other techniques such as the extendedH∞ filtering EH∞F [29], Huber’s M-estimation theory [30], maximum correntropy criterion (MCC) [31], and global linearization method to get better and more robust performance. (1) Since the extended H∞ filter does not make any assumptions about the statistics of the process or measurement noise but does require Jacobian matrices during the state estimation of nonlinear systems, by using the derivative-free property of SSGQKF and the linear propagation property, it is possible to embed the EH∞F in the SSGQKF framework. (2) Similarly, a robust scheme integrating Huber’s M-estimation theory and the SSGQKF framework is beneficial where Huber’s M-en is adopted to reformulate the measurement information of the SSGQKF. (3)The MCC is a statistical metric of the similarity between two random variables using information from higher-order signal statistics, which may perform much better than MMSE in non-Gaussian noise environments, since the MMSE captures only the second-order statistics of the error signal and is sensitive to large outliers. (4) Based on the polytopic linear differential inclusion, the estimation error system for the nonlinear system can be represented by an uncertain polytopic linear model. Then, the rectification equations for the predicted errors are designed. Finally, the state estimates for the nonlinear system are given through updating the predictions of SSGQKF. Generally speaking, the proposed SSGQKF can be applied to the nonlinear non-Gaussian environment to obtain a robust performance, which can be our future work.
Since the proposed method is computationally more efficient than the HDCQKF, it can also be expanded to the distributed fusion estimation problem for multisensor nonlinear systems [32, 33].
The estimation problem sometimes needs to process a huge number of data, costing time and energy waste [34]. The event trigger or data driven method has been successfully applied in the practical system [9–11], which can reduce the sampling so as to reduce the computation with the guaranteed accuracy. For a proper m and n′ and a given general model in nonlinear system, the SCQ points and corresponding weight can be prepared offline. Then, similar to event-triggered cubature Kalman filter (ETCKF) proposed in [11], the event-trigger schedule can also be embedded into the SSGQKF method. Motivated by the event-trigger heterogeneous nonlinear filter (ET-HNF) [10], which utilizes a master-slave filter at generator node and estimation center and a low computational slave filter to execute the event-trigger strategy, particle flow filters and the proposed SSGQKF3 method can serve as the master and slave filter, respectively.
Generally, particle filter serves as the master due to its effectiveness to nonlinear dynamic and/or measurement filtering problem. However, as explained in [35, 36], particle filter commonly suffers from the curse of dimensionality (COD) owing to particle degeneracy, which is caused by Bayes’ rule. Recent advances in particle flow filters provide a promising avenue to avoid weight degeneracy and COD. Instead of using any proposal and resample schedule, the particle flow filters compute Bayes’ rule by particle flow, making it much easier to implement and perform better than the PF.


## 3.1. Spherical Simplex Gauss–Laguerre Quadrature Rule

From (12) and (16), the spherical simplex Gauss–Laguerre rule for Nx;0,I can be approximated by(16)If=12πn∫r=0∞∑i=1Nsωs,ifrzirn−1e−r2/2dr,(17)=12πn∫t=0∞∑i=1Nsωs,if2tzitn/2−1e−tdt,(18)=12πn∑i′=1n′ωi′∑i=1Nsωs,if2tzi.Note that to transform the integralIf into the form of equation (12), we make an assumption that t=r2/2. Let the numerical method for ∫​RnfxNx;0,Idx be represented by Qf. Substituting (9) and (15) into (18) yields the third-degree spherical simplex Gauss–Laguerre quadrature rule as(19)Qf=12πn∑i′=1n′Ai″An2n+1∑j=1n+1fraj+f−raj.Similarly, substituting (10) and (15) into (18), we have the fifth-degree spherical simplex Gauss–Laguerre quadrature rule as(20)Qf=12πn∑i′=1n′Ai′An7−nn2n+12n+2∑j=1n+1fraj+f−raj+2n−12nn+12n+2∑j=1nn+1/2frbj+f−rbj.The Gaussian probability weighted integral∫RnfxNx;x¯,Pxdx can be performed by the linear transformation of Pxi+x^, where P is the square root of the matrix P and xi is the quadrature points from (19) or (20).For an dimension of state-space problem solved with third-order spherical simplex rule and n′ order Gauss–Laguerre quadrature points, total 2n+1n′ points and associated weights need to be calculated. The fifth-degree spherical simplex Gauss–Laguerre quadrature rule has n2+3n+2n′ points and shares 2n+1n′ points with the third-degree spherical simplex Gauss–Laguerre quadrature rule. For simplicity, these simplex cubature quadrature points are termed as SCQ points in this paper.


## 3.2. Calculation of Simplex Cubature Quadrature Points and Weights

The steps for calculating the simplex cubature quadrature (SCQ) points and associated weights are as follows:(i)
Determine the system dimensionn, the orders of spherical simplex cubature rule (i.e., m), and Chebyshev–Laguerre polynomial n′. Find out the set of sample points for a given specific degree of spherical simplex cubature rule.(ii)
For the third-order2m+1=3 spherical simplex rule, the sample points ξi=a,−a.(iii)
For the fifth-order2m+1=5 spherical simplex rule, the sample points ξi=a,−a,b,−b.(iv)
Solve then′ order Chebyshev–Laguerre polynomial (13) for α=n/2−1 to derive the quadrature points λi′.(v)
Find the SCQ points asξj=2λi′ξi.(vi)
The weights associated with SCQ points are(21)Wj=Ai′ωp2πn=n′!Γα+n′+1λi′L˙n′αλi′2×ωp2πn.(i)
For the third-order spherical simplex rulej=1,2,…,2n′n+1:(22)ωp=An2n+1,p=1,2,…,2n+1.(ii)
For the fifth-order spherical simplex rulej=1,2,…,n2+3n+2n′:(23)ωp=An7−nn2n+12n+2,p=1,2,…,2n+1,2Ann−12nn+12n+2,p=2n+3,…,n2+3n+2.The number of support SCQ points required for different ordered spherical simplex cubature rule is shown in Table1. The SCQ points and their associated weight of a second-order system for the third- and fifth-degree spherical simplex cubature and up to the third-order Gauss–Laguerre quadrature approximation are shown in Table 2. As the dimension of the state-space models increases, the computational burden also increases. We only discuss the third-degree and the fifth-degree spherical simplex rule here.Table 1
Support points required for different ordered spherical simplex cubature andn′ order Gauss–Laguerre quadrature rule.
Degree of spherical simplex cubature ruleNumber of SCQ points32n+1n′5n2+3n+2n′7n+1n2+8n+6n′/3Table 2
Points and weights for different degree of simplex cubature and higher-order Gauss–Laguerre rules for a second-order system.
2m + 1n′SCQ pointsWeights31(1.4142, 0), (−0.7071, 1.2247), (−0.7071, −1.2247), (−1.4142, 0), (0.7071, −1.2247), (0.7071, 1.2247)1/6, 1/6, 1/6, 1/6, 1/6, 1/62(2.6131, 0), (−1.3066, 2.2630), (−1.3066, −2.2630), (−2.6131, 0), (1.3066, −2.2630), (1.3066, 2.2630), (1.0824, 0), (−0.5412, 0.9374), (−0.5412, −0.9374), (−1.0824, 0), (0.5412, −0.9374), (0.5412, 0.9374)0.0244,0.0244, 0.0244, 0.0244, 0.0244, 0.0244, 0.1423, 0.1423, 0.1423, 0.1423, 0.1423, 0.14233(3.5468, 0), (−1.7734, 3.0716), (−1.7734, −3.0716), (−3.5468, 0), (1.7734, −3.0716), (1.7734, 3.0716), (2.1421, 0), (−1.0710, 1.8551), (−1.0710, −1.8551), (−2.1421, 0), (1.0710, −1.8551), (1.0710, 1.8551), (0.9119, 0), (−0.4559, 0.7897), (−0.4559, −0.7897), (−0.9119, 0), (0.4559, −0.7897), (0.4559, 0.7897)0.0017, 0.0017, 0.0017, 0.0017, 0.0017, 0.0017, 0.0464, 0.0464, 0.0464, 0.0464, 0.0464, 0.0464, 0.1185, 0.1185, 0.1185, 0.1185, 0.1185, 0.118551(1.4142, 0), (−0.7071, 1.2247), (−0.7071, −1.2247), (−1.4142, 0), (0.7071, −1.2247), (0.7071, 1.2247), (0.7071, 1.2247), (0.7071, −1.2247), (−1.4142, 0), (−0.7071, −1.2247), (1.4142, 0)5/36, 5/36, 5/36, 5/36, 5/36, 5/36, 1/36, 1/36, 1/36, 1/36, 1/36, 1/362(2.6131, 0), (−1.3066, 2.2630), (−1.3066, −2.2630), (−2.6131, 0), (1.3066, −2.2630), (1.3066, 2.2630), (1.3066, 2.2630), (1.3066, −2.2630), (−2.6131, 0), (−1.3066, −2.2630), (−1.3066, 2.2630), (2.6131, 0), (1.0824, 0), (−0.5412, 0.9374), (−0.5412, −0.9374), (−1.0824, 0), (0.5412, −0.9374), (0.5412, 0.9374), (0.5412, 0.9374), (0.5412, −0.9374), (−1.0824, 0), (−0.5412, −0.9374), (−0.5412, 0.9374), (1.0824, 0)0.0203, 0.0203, 0.0203, 0.0203, 0.0203, 0.0203, 0.0041, 0.0041, 0.0041, 0.0041, 0.0041, 0.0041, 0.1185, 0.1185, 0.1185, 0.1185, 0.1185, 0.1185, 0.0237, 0.0237, 0.0237, 0.0237, 0.0237, 0.02373(3.5468, 0), (−1.7734, 3.0716), (−1.7734, −3.0716), (−3.5468, 0), (1.7734, −3.0716), (1.7734, 3.0716), (1.7734, 3.0716), (1.7734, −3.0716), (−3.5468, 0), (−1.7734, −3.0716), (−1.7734, 3.0716), (3.5468, 0), (2.1421, 0), (−1.0710, 1.8551), (−1.0710, −1.8551), (−2.1421, 0), (1.0710, −1.8551), (1.0710, 1.8551), (1.0710, 1.8551), (1.0710, −1.8551), (−2.1421, 0), (−1.0710, −1.8551), (−1.0710, 1.8551), (2.1421, 0), (0.9119, 0), (−0.4559, 0.7897), (−0.4559, 0.7897), (−0.9119, 0), (0.4559, −0.7897), (0.4559, 0.7897), (0.4559, 0.7897), (0.4559, −0.7897), (−0.9119, 0), (−0.4559, −0.7897), (−0.4559, 0.7897), (0.9119, 0)0.0014, 0.0014, 0.0014, 0.0014, 0.0014, 0.0014, 0.0003, 0.0003, 0.0003, 0.0003, 0.0003, 0.0003, 0.0387, 0.0387, 0.0387, 0.0387, 0.0387, 0.0387, 0.0077, 0.0077, 0.0077, 0.0077, 0.0077, 0.0077, 0.0988, 0.0988, 0.0988, 0.0988, 0.0988, 0.0988, 0.0198, 0.0198, 0.0198, 0.0198, 0.0198, 0.0198


## 3.3. The Spherical Simplex Gauss–Laguerre Quadrature Cubature Kalman Filter (SSGQKF)

Applying the spherical simplex Gauss–Laguerre quadrature rule described in Section3.2 in the Bayesian filter framework yields the new spherical simplex Gauss–Laguerre quadrature cubature Kalman filter (SSGQKF).The algorithm of the proposed SSGQKF could be summarised as follows.Step 1.
Initialization(i)
Initialize the filter withx^00 and P00.(ii)
Calculate the SCQ pointsξj and their corresponding weights Wj as described in Section 3.2.Step 2.
Prediction step(i)
Factorise covariance matrix using Cholesky decomposition:(24)Pkk=SkkSkkT.(ii)
Evaluate the SCQ points:(25)χj,kk=Skkξj+x^kk.(iii)
Update the SCQ points:(26)χj,k+1k=fχj,kk.(iv)
Estimate the predicted state and the predicted error covariance:(27)x^k+1k=∑j=1tχj,k+1kWj,Pk+1k=∑j=1tWjχj,k+1k−x^k+1kχj,k+1k−x^k+1kT+Qk,where t is the total number of SCQ points.Step 3.
Measurement step(i)
Factorise the predicted error covariance matrixPk+1k using the Cholesky decomposition:(28)Pk+1k=Sk+1kSk+1kT.(ii)
Evaluate the SCQ points:(29)χj,k+1k=Sk+1kξj+x^k+1k.(iii)
Find the propagated measurements at each SCQ points:(30)Yj,k+1k=hχj,k+1k.(iv)
Estimate the predicted measurement:(31)y^k+1=∑j=1tYj,k+1kWj.(v)
Calculate the innovation covariance matrix and the cross-covariance matrix:(32)Pyy=∑j=1tWjYj,k+1k−y^k+1Yj,k+1k−y^k+1T+Rk,Pxy=∑j=1tWjχj,k+1k−x^k+1Yj,k+1k−y^k+1T.(vi)
Compute the Kalman gain:(33)Kk+1=PxyPyy−1.(vii)
Compute the filtering estimation:(34)x^k+1k+1=x^k+1k+Kk+1yk+1−y^k+1.(viii)
Compute the filtering estimation error covariance matrix:(35)Pk+1k+1=Pk+1k−Kk+1PyyKk+1T.
Remarks(i)
The spherical simplex cubature Gauss–Laguerre quadrature rule constructed using the above theorems is fully symmetric.(ii)
For a specific system dimensionn, there are two tuning parameters, namely, m and n′, which could be tuned to achieve satisfied accuracy on affordable computational load.(iii)
Compared to EKF, the SSGQKF is derivative free. It is easy to implement as neither a Jacobian nor Hessian matrix needs to be computed. It is also computationally more efficient than the PF and GHF because instead of generating large number of particles or quadrature points in the state space, few SCQ points are required to be generated, updated, and predicted.(iv)
For a specific order of Chebyshev–Laguerre polynomialn′, the number of SCQ points of the fifth-degree SSGQKF is less than that of the existing fifth-degree HDCQKF 2n2n′ when n>4. Therefore, from the perspective of computation, the proposed fifth-degree SSGQKF is much less complex than HDCQKF, making it easier to implement in high-dimensional nonlinear systems. That is to say, the SSGQKF will be a potential and promising method in practical engineering implementation.(v)
Whenn>4, the fifth-degree spherical-radial rule has some negative weights. Negative weights may lead to divergence of filters [22]. Despite some of the weights from (20) in the fifth-degree spherical simplex cubature rule becoming negative when n>7, they go to 0 when n⟶∞. Thus, the proposed fifth-degree SSGQKF is more efficient than the existing HDCQKF for high-dimensional systems. Consequently, the SSGQKF can be a very promising alternative to the HDCQKF under the Gaussian assumption, especially in the case of high-dimensional cases.(vi)
For a predefined ordered cubature and Gauss–Laguerre quadrature, the SCQ points and associated weights can be calculated and stored offline. In other words, the computation burden could be further alleviated.
In the following, a few discussions are added for the practical implementation of the proposed method Like the traditional Kalman filter (KF) and its suboptimal extensions (e.g., EKF, UKF, and CKF), the proposed SSGQKF is also developed under the Gaussian assumption and the minimum mean square error (MMSE) criterion. To deal with non-Gaussian noises and for further improvement, the proposed filter can be combined with other techniques such as the extendedH∞ filtering EH∞F [29], Huber’s M-estimation theory [30], maximum correntropy criterion (MCC) [31], and global linearization method to get better and more robust performance. (1) Since the extended H∞ filter does not make any assumptions about the statistics of the process or measurement noise but does require Jacobian matrices during the state estimation of nonlinear systems, by using the derivative-free property of SSGQKF and the linear propagation property, it is possible to embed the EH∞F in the SSGQKF framework. (2) Similarly, a robust scheme integrating Huber’s M-estimation theory and the SSGQKF framework is beneficial where Huber’s M-en is adopted to reformulate the measurement information of the SSGQKF. (3)The MCC is a statistical metric of the similarity between two random variables using information from higher-order signal statistics, which may perform much better than MMSE in non-Gaussian noise environments, since the MMSE captures only the second-order statistics of the error signal and is sensitive to large outliers. (4) Based on the polytopic linear differential inclusion, the estimation error system for the nonlinear system can be represented by an uncertain polytopic linear model. Then, the rectification equations for the predicted errors are designed. Finally, the state estimates for the nonlinear system are given through updating the predictions of SSGQKF. Generally speaking, the proposed SSGQKF can be applied to the nonlinear non-Gaussian environment to obtain a robust performance, which can be our future work.
Since the proposed method is computationally more efficient than the HDCQKF, it can also be expanded to the distributed fusion estimation problem for multisensor nonlinear systems [32, 33].
The estimation problem sometimes needs to process a huge number of data, costing time and energy waste [34]. The event trigger or data driven method has been successfully applied in the practical system [9–11], which can reduce the sampling so as to reduce the computation with the guaranteed accuracy. For a proper m and n′ and a given general model in nonlinear system, the SCQ points and corresponding weight can be prepared offline. Then, similar to event-triggered cubature Kalman filter (ETCKF) proposed in [11], the event-trigger schedule can also be embedded into the SSGQKF method. Motivated by the event-trigger heterogeneous nonlinear filter (ET-HNF) [10], which utilizes a master-slave filter at generator node and estimation center and a low computational slave filter to execute the event-trigger strategy, particle flow filters and the proposed SSGQKF3 method can serve as the master and slave filter, respectively.
Generally, particle filter serves as the master due to its effectiveness to nonlinear dynamic and/or measurement filtering problem. However, as explained in [35, 36], particle filter commonly suffers from the curse of dimensionality (COD) owing to particle degeneracy, which is caused by Bayes’ rule. Recent advances in particle flow filters provide a promising avenue to avoid weight degeneracy and COD. Instead of using any proposal and resample schedule, the particle flow filters compute Bayes’ rule by particle flow, making it much easier to implement and perform better than the PF.


## 4. Simulations and Discussion

In this section, the effectiveness of the proposed SSGQKF is demonstrated by comparing the SSGQKF with the CKF, the third-degree SSRCKF (SSRCKF3), the CQKF, and the HDCQKF in terms of tracking accuracy.Two problems are considered. The first problem is a highly nonlinear state estimation issue. The second problem is a tracking model of a vehicle. Both of the problems have significant nonlinearity.In all the problems, the CQKF (or SSGQKF3) is implemented with the third-degree spherical (simplex) cubature rule and second-order Gauss–Laguerre quadrature rule. The HDCQKF (or SSGQKF5) is implemented with the fifth-degree spherical (simplex) cubature rule and second-order Gauss–Laguerre quadrature rule.All algorithms were implemented on a personal computer with a 64-bit operating system, 8 GB RAM, and 2.20GHZ i7 processor. All the computations associated with cubature points were done offline.


### 4.1. Problem 1

We consider the following nonlinear system for which the state and measurement models are [22, 23, 37, 38](36)xk+1=20cosxk+ηk,yk=1+xkTxk+vk,where xk∈ℝn is an n dimensional state vector at k-th instant of time and cosxk=cosx1,k,cosx2,k,…,cosxn,kT; yk is the measurement at time k; and ηk and vk are independent process and measurement Gaussian noise sequences with zero means and covariances In and 1, respectively.The true initial state isx0=0.1×In×1, and the filter is initialized with a value of x^0 and P0, where x^00=0n×1 and P00=In. The states are estimated for 100 steps and the accuracy of the estimators is compared in terms of root mean square error (RMSE). For M number of Monte Carlo runs, the RMSE is defined as(37)RMSEk=1M∑i=1Mxi,k−x^i,k2,i=1,2,…,M,k=1,2,…,N.During the simulation, a ten (i.e.,n=10) dimensional system is considered and 500 Monte Carlo runs are used to evaluate the RMSE. Considering the limitation of this article, the RSMEs of the first six states using different filters in an interval of 20–70 s are shown in Figures 1–3, respectively.Figure 1
(a) RMSE of first state. (b) RMSE of second state.
(a)(b)Figure 2
(a) RMSE of third state. (b) RMSE of fourth state.
(a)(b)Figure 3
(a) RMSE of fifth state. (b) RMSE of sixth state.
(a)(b)The average RMSE of the six filters in 100 steps is listed in Table3. The average Monte Carlo run times of CKF, SSRCKF3, CQKF, HDCQKF, SSGQKF3, and SSGQKF5 for the above problem are 0.1823, 0.2083,0.3016, 1.5313, 0.3262, and 1.1719 seconds, respectively.Table 3
RMSE averaged over time horizon.
StateCKFSSRCKF3CQKFHDCQKFSSGQKF3SSGQKF5119.254416.891819.097714.667116.597114.4069218.471016.730318.386014.633215.501514.4000317.815116.279417.682814.645615.351514.4251417.105115.954316.921014.652215.077914.3768516.418815.546216.226714.589514.809814.3143615.897615.329715.632714.645714.738414.3693715.330515.023515.178714.632814.605614.3941815.000514.862314.822714.644214.584614.3755914.791814.719014.604914.693114.507214.42731014.714414.650014.530814.653114.430914.3736It is clear to see from Figures1–3 and Table 3 that RMSEs of SSGQKF3 are smaller than those of CKF, SSRCKF3, and CQKF. Similarly, the RMSEs of SSGQKF5 are smaller than those of HDCQKF. The RMSE reflects the accuracy of the algorithm. Besides, we can also see that the proposed SSGQKF3 (or SSGQKF5) is more efficient than the CQKF (or HDCQKF) in terms of computation burden. Thus, the proposed filter has better filtering performance as compared with the CQKFs and existing third-degree SSRCKF.


### 4.2. Problem 2

In this section, a computer simulation is performed in a target tracking application, which has been used as a benchmark problem to validate the performance of different nonlinear filtering algorithms.We consider a typical air-traffic control scenario, where an aircraft makes maneuvering turn in a horizontal plane to the ground, i.e., during maneuver, the height of the vehicle remains constant. The turn rateω is an unknown constant, which needs to be estimated, making the process model to be a set of nonlinear equations. The target dynamic equation is given by [17, 20, 21, 39–41](38)xk=1sinωΔtω0cosωΔt−1ω00cosωΔt0−sinωΔt001−cosωΔtω1sinωΔtω00sinωΔt0cosωΔt000001xk−1+vk.The state of the aircraft denotesx=ξ,ξ˙,η,η˙,ωT; ξ and η represent positions, and ξ˙ and η˙ represent velocities in the x and y directions, respectively; Δt is the sampling time. vk is the white Gaussian noise with zero mean and covariance Qk, where(39)Qk=Δt33Δt22000Δt22Δt00000Δt33Δt22000Δt22Δt000001.75×10−3Δt.In this problem, a sensor is used to measure the range and angle between the target and the sensor. The measurement equation for the sensor can be expressed by(40)rkθk=ξk−xo2+ηk−yo2atanξk−yoηk−xo+nk,where xo,yo=200m,300m is the sensor position and nk is the white Gaussian measurement noise with zero mean and covariance Rk=diag1000m2,100mrad2. The measurement sampling interval Δt=1s.The initial true statex0=1000m,300m/s,1000m,0,−3°/sT, and the associated covariance P00=diag100m2,10m2/s2,100m2,10m2/s2,100mrad2/s2. The initial state estimate x^00 is chosen randomly from the Gaussian distribution Nx0,P00 in each run. For a fair comparison, we make 100 independent Monte Carlo runs.Performance Metrics. To compare the performances of all filters, we use the root mean square error (RMSE) of the position, velocity, and the turn rate. The RMSE in position at time index k is defined as follows:(41)RMSEposk=1MC∑n=1MCξkn−ξ^kn2+ηkn−η^kn2,where n is the number of Monte Carlo runs; ξkn and ηkn are the position elements of the true state at time k in the n-th Monte Carlo run; and ξ^kn and η^kn are the corresponding estimated state vectors. Similarly, we may easily write the formulas of RMSEs in velocity and the turn rate.The simulation results, including the position RMSE, velocity RMSE, and the turn rate RMSE of various filters, are shown in Figures4–6, respectively. Due to the fact that there is not much difference among the filters in the interval of 0–30 s, the RMSEs of these steps are not shown here. The average time in Table 4 is the running time for 100 times' simulation, each time 100 steps.Figure 4
RMSE for position.Figure 5
RMSE for velocity.Figure 6
RMSE for turn rate.Table 4
The RMSE and average time of various filters.
RMSECKFSSRCKF3CQKFHDCQKFSSGQKF3SSGQKF5Position (m)54.531253.478851.908951.185951.715850.6870Velocitym/s34.957334.693634.002933.635833.920033.2904ω (°)4.17604.17264.15374.14124.15344.1160Average time (s)0.10940.12500.15630.46880.20310.4063From the results, we can see that SSGQKF5 improves the CKF by7.05% in position, 4.7684% in velocity, and 1.4368% in turn rate. We can also learn that the RMSE of position estimation error, velocity error, and turn rate error of the SSGQKF5 is much smaller than that of the HDCQKF, which means the SSGQKF5 can provide more accurate estimation results. That is because the fifth-degree simplex rule is utilized to compute the spherical integral; hence, the filter precision can be improved.Table4 shows the runtime of the six filters; the runtime of SSGQKFs is slower than that of HDCQKF, which is because the fifth-degree spherical simplex rule requires fewer points than the fifth-degree CKF when the dimension is larger than four. To sum up, we claim that the SSGQKFs is a balance between filtering accuracy and computational cost for some high-dimensional applications.


## 4.1. Problem 1

We consider the following nonlinear system for which the state and measurement models are [22, 23, 37, 38](36)xk+1=20cosxk+ηk,yk=1+xkTxk+vk,where xk∈ℝn is an n dimensional state vector at k-th instant of time and cosxk=cosx1,k,cosx2,k,…,cosxn,kT; yk is the measurement at time k; and ηk and vk are independent process and measurement Gaussian noise sequences with zero means and covariances In and 1, respectively.The true initial state isx0=0.1×In×1, and the filter is initialized with a value of x^0 and P0, where x^00=0n×1 and P00=In. The states are estimated for 100 steps and the accuracy of the estimators is compared in terms of root mean square error (RMSE). For M number of Monte Carlo runs, the RMSE is defined as(37)RMSEk=1M∑i=1Mxi,k−x^i,k2,i=1,2,…,M,k=1,2,…,N.During the simulation, a ten (i.e.,n=10) dimensional system is considered and 500 Monte Carlo runs are used to evaluate the RMSE. Considering the limitation of this article, the RSMEs of the first six states using different filters in an interval of 20–70 s are shown in Figures 1–3, respectively.Figure 1
(a) RMSE of first state. (b) RMSE of second state.
(a)(b)Figure 2
(a) RMSE of third state. (b) RMSE of fourth state.
(a)(b)Figure 3
(a) RMSE of fifth state. (b) RMSE of sixth state.
(a)(b)The average RMSE of the six filters in 100 steps is listed in Table3. The average Monte Carlo run times of CKF, SSRCKF3, CQKF, HDCQKF, SSGQKF3, and SSGQKF5 for the above problem are 0.1823, 0.2083,0.3016, 1.5313, 0.3262, and 1.1719 seconds, respectively.Table 3
RMSE averaged over time horizon.
StateCKFSSRCKF3CQKFHDCQKFSSGQKF3SSGQKF5119.254416.891819.097714.667116.597114.4069218.471016.730318.386014.633215.501514.4000317.815116.279417.682814.645615.351514.4251417.105115.954316.921014.652215.077914.3768516.418815.546216.226714.589514.809814.3143615.897615.329715.632714.645714.738414.3693715.330515.023515.178714.632814.605614.3941815.000514.862314.822714.644214.584614.3755914.791814.719014.604914.693114.507214.42731014.714414.650014.530814.653114.430914.3736It is clear to see from Figures1–3 and Table 3 that RMSEs of SSGQKF3 are smaller than those of CKF, SSRCKF3, and CQKF. Similarly, the RMSEs of SSGQKF5 are smaller than those of HDCQKF. The RMSE reflects the accuracy of the algorithm. Besides, we can also see that the proposed SSGQKF3 (or SSGQKF5) is more efficient than the CQKF (or HDCQKF) in terms of computation burden. Thus, the proposed filter has better filtering performance as compared with the CQKFs and existing third-degree SSRCKF.


## 4.2. Problem 2

In this section, a computer simulation is performed in a target tracking application, which has been used as a benchmark problem to validate the performance of different nonlinear filtering algorithms.We consider a typical air-traffic control scenario, where an aircraft makes maneuvering turn in a horizontal plane to the ground, i.e., during maneuver, the height of the vehicle remains constant. The turn rateω is an unknown constant, which needs to be estimated, making the process model to be a set of nonlinear equations. The target dynamic equation is given by [17, 20, 21, 39–41](38)xk=1sinωΔtω0cosωΔt−1ω00cosωΔt0−sinωΔt001−cosωΔtω1sinωΔtω00sinωΔt0cosωΔt000001xk−1+vk.The state of the aircraft denotesx=ξ,ξ˙,η,η˙,ωT; ξ and η represent positions, and ξ˙ and η˙ represent velocities in the x and y directions, respectively; Δt is the sampling time. vk is the white Gaussian noise with zero mean and covariance Qk, where(39)Qk=Δt33Δt22000Δt22Δt00000Δt33Δt22000Δt22Δt000001.75×10−3Δt.In this problem, a sensor is used to measure the range and angle between the target and the sensor. The measurement equation for the sensor can be expressed by(40)rkθk=ξk−xo2+ηk−yo2atanξk−yoηk−xo+nk,where xo,yo=200m,300m is the sensor position and nk is the white Gaussian measurement noise with zero mean and covariance Rk=diag1000m2,100mrad2. The measurement sampling interval Δt=1s.The initial true statex0=1000m,300m/s,1000m,0,−3°/sT, and the associated covariance P00=diag100m2,10m2/s2,100m2,10m2/s2,100mrad2/s2. The initial state estimate x^00 is chosen randomly from the Gaussian distribution Nx0,P00 in each run. For a fair comparison, we make 100 independent Monte Carlo runs.Performance Metrics. To compare the performances of all filters, we use the root mean square error (RMSE) of the position, velocity, and the turn rate. The RMSE in position at time index k is defined as follows:(41)RMSEposk=1MC∑n=1MCξkn−ξ^kn2+ηkn−η^kn2,where n is the number of Monte Carlo runs; ξkn and ηkn are the position elements of the true state at time k in the n-th Monte Carlo run; and ξ^kn and η^kn are the corresponding estimated state vectors. Similarly, we may easily write the formulas of RMSEs in velocity and the turn rate.The simulation results, including the position RMSE, velocity RMSE, and the turn rate RMSE of various filters, are shown in Figures4–6, respectively. Due to the fact that there is not much difference among the filters in the interval of 0–30 s, the RMSEs of these steps are not shown here. The average time in Table 4 is the running time for 100 times' simulation, each time 100 steps.Figure 4
RMSE for position.Figure 5
RMSE for velocity.Figure 6
RMSE for turn rate.Table 4
The RMSE and average time of various filters.
RMSECKFSSRCKF3CQKFHDCQKFSSGQKF3SSGQKF5Position (m)54.531253.478851.908951.185951.715850.6870Velocitym/s34.957334.693634.002933.635833.920033.2904ω (°)4.17604.17264.15374.14124.15344.1160Average time (s)0.10940.12500.15630.46880.20310.4063From the results, we can see that SSGQKF5 improves the CKF by7.05% in position, 4.7684% in velocity, and 1.4368% in turn rate. We can also learn that the RMSE of position estimation error, velocity error, and turn rate error of the SSGQKF5 is much smaller than that of the HDCQKF, which means the SSGQKF5 can provide more accurate estimation results. That is because the fifth-degree simplex rule is utilized to compute the spherical integral; hence, the filter precision can be improved.Table4 shows the runtime of the six filters; the runtime of SSGQKFs is slower than that of HDCQKF, which is because the fifth-degree spherical simplex rule requires fewer points than the fifth-degree CKF when the dimension is larger than four. To sum up, we claim that the SSGQKFs is a balance between filtering accuracy and computational cost for some high-dimensional applications.


## 5. Conclusions

In this paper, a new method has been proposed for solving the nonlinear filtering problems. The proposed filter is named as spherical simplex Gauss–Laguerre quadrature cubature Kalman filter. The intractable integrals encountered during the filtering are decomposed into surface and line integrals. The surface integral is solved by spherical simplex cubature rule, while the line integral is solved by any ordered Gauss–Laguerre quadrature rule. SSGQKFs use the spherical simplex Gauss–Laguerre quadrature rule and choose a set of SCQ points, which do not require the evaluation of Jacobian matrices during the prediction and update stage. The proposed filter is tested in two problems that involve a high nonlinear state estimation problem and the use of a target tracking problem of practical significance. The accuracy of the proposed filter is found to be better than the CKF, the CQKF, and the HDCKF. The proposed nonlinear filters can provide more accurate results with lower computational costs than HDCQKF in high-dimensional nonlinear systems. To enhance accuracy, the proposed filter can be potentially used in state estimation where high nonlinearity exists in systems.



---

*Source: 1072824-2020-05-14.xml*