Orthogonal Simplex Chebyshev-Laguerre Cubature Kalman Filter Applied in Nonlinear Estimation Systems

: To further improve the ﬁltering accuracy in nonlinear estimation systems, a nonlinear ﬁlter, called the orthogonal simplex Chebyshev-Laguerre cubature Kalman ﬁlter (OSCL-CKF), is proposed. The ﬁlter is built within the cubature Kalman ﬁlter framework, which transforms the multidimensional, Gaussian weighted integral into a spherical-radial coordinate system. In the spherical integral, an orthogonal method is introduced to the third-degree spherical simplex rule, and then the nonlocal sampling effects can be reduced by tuning the high order interference terms. In the radial integral, the quadrature points and corresponding weights are determined according to the Chebyshev-Laguerre (CL) equation, which enables the nonlinear ﬁlter to improve the precision by the order of the CL polynomial. Numerical results show that the proposed ﬁlter outperforms the conventional algorithms.


Introduction
Nonlinear estimation systems generally exist in civilian and military fields, such as target tracking systems, navigation systems, and communication systems. Nonlinear filtering algorithms have been widely used in these nonlinear estimation systems. For example, a multistatic radar based on nonlinear filter localizes a moving target within a small surveillance area [1]; a nonlinear-filter-based autonomous vehicle navigation system provides good positioning performance for vehicles [2]; the improved control systems based on nonlinear filter are adopted in wireless communication systems [3][4][5][6]. As the above examples show, nonlinear filtering algorithms play an important role in addressing nonlinear estimation problems.
In the Bayesian framework, the nonlinear filtering problem is essentially a computational problem of multidimensional integrals, which is typically intractable and hence the numerical approaches are considered [7]. In theory, the particle filter [8][9][10][11] based on the Monte-Carlo approximation for the multidimensional integrals can provide sufficient precision. However, the filter is usually impractical, owing to the enormous computational complexity from it suffers. The marginalized particle filters reduce the computational complexity to a certain degree, but it cannot deal with the problem whose process model and measurement model are both nonlinear. On the contrary, the filters under the Gaussian assumption of posterior probability density can be executed simply and quickly. The extended Kalman filter (EKF) [12,13], for instance, employs linearization via first-order Taylor series expansion, but it often results in unstable performance, and differentiability of the considered nonlinear functions is also required. In recent years, the deterministically sampled filters

System Model
The nonlinear discrete estimation system model can be expressed as follows: where x k ∈ R n x is the state vector at time index k with n x dimensions, and z k ∈ R n z is the measurement vector with n z dimensions. The values of n x and n z are dependent on specific applications. f (·) and h(·) are the nonlinear function, here k = 1, 2, · · · . The process noise v k−1 and the measurement noise e k are generally assumed to be independent and identically distributed zero mean Gaussian noise with the covariance matrices Q k−1 and R k , respectively.

Bayesian Gaussian Recursive Filter
The Bayesian recursive solution to the problem above can be divided into a prediction step and a measurement update step. At the prediction step, the mean and associated covariance of the Gaussian predictive density can be obtained as follows: At the measurement update step, D k−1 is denoted as the history of input measurement. Then, the predicted measurement density can be represented as follows: z k|k−1 is the predicted measurement, which is predicted based on time index k − 1 and the measurement model.ẑ k|k−1 and its associated innovation covariance P zz,k|k−1 are given by the following:ẑ and the cross-covariance is as follows: Once the new measurement is received, the filter computes the posterior density, yielding the following: wherex

Orthogonal Simplex Chebyshev-Laguerre Cubature Kalman Filter
The key of the Bayesian filter is how to compute Gaussian weighted integrals whose integrands are a product of a nonlinear function and a Gaussian density function that are present in (5)- (7). Considering the standard Gaussian distribution N(x; 0, I), the integral can be expressed as follows: where f (x) is an arbitrary nonlinear function. Now, we transform the integral from the Cartesian coordinate to the spherical-radial coordinate. Assuming that x = ry with y T y = 1 and r = √ x T x, Equation (12) can be rewritten in the spherical-radial coordinate system as follows [18]: where direction vector y and radius r are the corresponding parameters in the spherical-radial coordinate [18]. U n x = y ∈ R n x y T y = 1 is the spherical surface and dσ(·) is the spherical surface measure. As a consequence, two kinds of integrals are given by the following:

Orthogonal Spherical Simplex Rule
According to Mysovskikh and Lu [24,25], the cubature rule for Equation (14) can be obtained from the transformation group of the regular simplex with vertices, that is: where j = 1, 2, . . . , n x + 1, and Accordingly, the third-degree spherical simplex rule is constructed as follows: where n s = 2(n x + 1), A n x = 2 √ π n x /Γ(n x /2), and Γ(n x ) = ∞ 0 x n x −1 e −x dx. Compared with the cubature rule based on Arasaratnam et al. [18], two more sample points are needed in the spherical simplex rule, but it has been proven that the spherical simplex rule is more accurate [21]. According to Equation (1), the highly nonlinear nature of the nonlinear system model may lead to the nonlocal sampling effects, with the covariance being easily affected by unknown high order behavior (especially when n x > 3). Thus, we introduce the orthogonal method to tune the high order information of the spherical simplex transformation. In other words, Equation (18) can be updated as follows: The method to select an orthogonal matrix can be seen in the works of [26,27]. In this paper, we construct B according to Xiu [28]: where p = 1, 2, · · · , max{p ∈ Z|p ≤ n x /2}. If n x is odd, From Equations (18)- (21), it can be seen that the orthogonal method does not introduce additional computational costs to the spherical simplex rule, because the new orthogonal sample points can be computed offline in advance.

High Order Chebyshev-Laguerre Quadrature Rule
Due to the quadratic term r 2 /2 in the exponential function, it is difficult to deal with the radial integral (15) directly. Hence, we make an equivalent transformation that t = r 2 /2, and then Equation (15) can be rewritten as follows: The integral on the right-hand side is in the form of the generalized Gauss-Laguerre integral. Numerical integration is generally utilized because seeking an optimal or analytical solution to Equation (22) is typically intractable. Here, we evaluate it using quadrature points t k and associated weights w c i : which are determined by the Chebyshev-Laguerre polynomials [29]: where a = n x /2 − 1 and n c is the order of the Chebyshev-Laguerre polynomials. The quadrature points t k are the roots of the Chebyshev-Laguerre polynomials, and associated weights can be obtained as follows [30]: where k = 1, · · · , n c . From the theory of Laguerre polynomials, the relation between L (a) and then we obtain the following: Obviously, the precision of the quadrature formula can be readily improved with the increase of the polynomials' order, which is linear to the number of quadrature points.

Orthogonal Spherical Simplex Chebyshev-Laguerre Quadrature Rule
In Section 3.1, integral (13) is transformed into two kinds of integrals, which are dealt with in Sections 3.1 and 3.2, respectively. Substituting Equations (18) and (23) into Equation (13), the orthogonal spherical simplex Chebyshev-Laguerre rule for N(x; 0, I) is as follows: That is, Therefore, The cubature formula is easy to extend to an arbitrary Gaussian distribution N(x;x, P x ). Computing Equations (5)-(7) using the formula, the new nonlinear filter, called the orthogonal simplex Chebyshev-Laguerre cubature Kalman filter, can be obtained.
The new filter uses 2(n x + 1)n c sample points to approximate the Gaussian-weighted integral. The number of sample points is linear to that of the state dimension n x and the polynomials' order n c , respectively. It is more efficient than high-degree CKF and high-degree SCKF from the computational point of view. The accuracy of the filter depends on the order of the Chebyshev-Laguerre polynomials. In theory, the higher the order is, the more accurate the filter would be. Moreover, the high order moments of the OSCL transformation are tuned according to the designed orthogonal matrix, which can alleviate the nonlocal sampling effects efficiently.
When n c = 1 and γ = [α − α], Equation (30) can be rewritten as follows: which means that the third-degree SCKF is a specific form of the proposed filter, where the orthogonal matrix is an identity matrix and the order of the Chebyshev-Laguerre polynomials is n c = 1. The accuracy can be further improved by increasing n c . On the other hand, the advantages of the SCKF are also retained in the proposed filter, such as derivative-free and better numerical stability compared with the UKF.

Simulations
To describe the performance of the proposed algorithm, two nonlinear estimation examples as the nonlinear estimation problem and the bearings-only tracking problem, are considered.

The Highly Dimensional Nonlinear Estimation Problem
The system model of nonlinear estimation is defined as: where cos(x k−1 ) = [cos(x 1,k−1 ), cos(x 2,k−1 ), · · · , cos(x n x ,k−1 )] T . Here we set the dimensions n x = 20. v k−1 and e k are the independent Gaussian noise with zero means and covariance I n x and 1, respectively. The true initial value is x 0 = 0.1 × 1 n x ×1 , and the initial estimatex 0 used in the filters is generated randomly from N(x 0 ; x 0 , P 0 ), where the initial covariance is P 0 = I n x . The mean squared error (MSE) for each Monte-Carlo run is defined as follows: Accordingly, the average MSE can be written as follows: where the Monte-Carlo runs L = 100. The MSEs of the algorithms are shown in Figure 1. As is shown in Figure 1, the MSEs of the OSCL-CKFs are smaller than those of the CKF and the SCKF, which means that OSCL points can provide more accurate performance in this highly dimensional problem. It can be also seen that the MSEs of the OSCL-CKF4 and OSCL-CKF3 are almost the same, which illustrates that the accuracy of the filter will hardly be improved if the order further increases.

Bearings-Only Tracking Problem
Bearings-only tracking is a typical nonlinear problem which estimates the position and the velocity of a target from noisy bearing measurements.
As is shown in Figure 2, we assume that the target is in uniform linear motion with acceleration disturbance. The state vector at time index can be expressed by x y x is the position and is the velocity vector. The true state of the observation platform is , , , , and the state vector in the relative own-ship reference at time index can be defined as follows: The system model is written as follows: As is shown in Figure 1, the MSEs of the OSCL-CKFs are smaller than those of the CKF and the SCKF, which means that OSCL points can provide more accurate performance in this highly dimensional problem. It can be also seen that the MSEs of the OSCL-CKF4 and OSCL-CKF3 are almost the same, which illustrates that the accuracy of the filter will hardly be improved if the order further increases.

Bearings-Only Tracking Problem
Bearings-only tracking is a typical nonlinear problem which estimates the position and the velocity of a target from noisy bearing measurements.
As is shown in Figure 2, we assume that the target is in uniform linear motion with acceleration , and the state vector in the relative own-ship reference at time index k can be defined as follows: x The system model is written as follows: where u k−1,k is the deterministic input and F is the state transition matrix, which can be given by: where ∆ = 1min is the measurement interval. The standard deviation of measurement noise e k is σ e , and v k is the process noise vector with a covariance matrix of: where q = 10 −10 km 2 /s 3 represents the intensity of the process noise, and Γ is the noise drive matrix: A single observer is assumed to be maneuvering, where the start coordinate is (0,0). From 0 min to 12 min, the observer is in uniform linear motion, where the speed is 3 knots and the course is 2.44 rad ("rad"denotes radian). From 13 min to 17 min, the observer is in uniform turning motion, where the speed is 3 knots and the turn rate is 0.52 rad. From 18 min to 30 min, the observer is in uniform linear motion, where the speed is 3 knots and the course is 5.06 rad.
The root mean square error in position (RMSEpos) at time index k is defined as follows: where L = 100. Similarly, the RMSE in velocity (RMSEvel) can be easily written according to the RMSEpos. The mean squared error in position (MSEpos) for each Monte-Carlo run is defined as follows: 10 km / q = represents the intensity of the process noise, and Γ is the noise drive matrix: A single observer is assumed to be maneuvering, where the start coordinate is . From 0 min to 12 min, the observer is in uniform linear motion, where the speed is 3 knots and the course is 2.44 rad ("rad"denotes radian). From 13 min to 17 min, the observer is in uniform turning motion, where the speed is 3 knots and the turn rate is 0.52 rad. From 18 min to 30 min, the observer is in uniform linear motion, where the speed is 3 knots and the course is 5.06 rad. The root mean square error in position (RMSEpos) at time index is defined as follows: where . Similarly, the RMSE in velocity (RMSEvel) can be easily written according to the RMSEpos. The mean squared error in position (MSEpos) for each Monte-Carlo run is defined as follows: In the Monte-Carlo test, the target begins at the same state every time for a fair comparison. The scale parameter κ of the UKF is set as 3 − n x , and all the algorithms are set at the same initial conditions. The initial range estimate follows r ∼ N r, σ 2 r , where r is the true range. The initial bearing follows z 0 ∼ N z 0 , σ 2 e , where z 0 is the true bearing. Similarly, the initial velocity estimate s ∼ N s, σ 2 s , where s is the true initial target velocity. The target course estimate follows c ∼ N c, σ 2 c , where c is the true target course. The parameters in the simulation scenario are shown in Table 1.
("mrad" denotes milliradian in Table 1). The initial estimate state and initial estimate covariance can be obtained according to Wu et al. [31]. Hence, the RMSEpos and RMSEvel are shown in Figures 3 and 4, respectively.  Table 1. ("mrad" denotes milliradian in Table 1).  π The initial estimate state and initial estimate covariance can be obtained according to Wu et al. [31]. Hence, the RMSEpos and RMSEvel are shown in Figures 3 and 4, respectively.  The MSEpos for different noise intensities are shown in Figure 5.  Table 1. ("mrad" denotes milliradian in Table 1). π The initial estimate state and initial estimate covariance can be obtained according to Wu et al. [31]. Hence, the RMSEpos and RMSEvel are shown in Figures 3 and 4, respectively.  The MSEpos for different noise intensities are shown in Figure 5. As is shown in Figure 3, all algorithms tend to be convergent, with the measurement times increasing. During the Monte-Carlo test, the performance of CKF is better than that of UKF. One explanation is that the negative κ may lead to non-positive definiteness of the covariance matrices when 3 x n > . Moreover, the OSCL-CKF-based algorithms perform better than the other algorithms.
As the first order OSCL-CKF (OSCL-CKF1) provides better performance over the SCKF, it is proven that the orthogonal transformation can efficiently modify the uncertain higher order terms and relieve the nonlocal sampling effects. On the other hand, the high order OSCL-CKFs generally outperform the low order OSCL-CKF. At the end of the Monte-Carlo test, the RMSEpos of the OSCL-CKF4 is 3.2% better than that of the OSCL-CKF1, and 5.2% better than that of the SCKF. As is shown in Figure 4, the differences in the velocity estimate accuracy among these algorithms are not as obvious as in the position estimate. This is mainly due to the uniform linear motion of the target.
As is shown in Figure 5, the MSEspos of the filters become larger with the increasing of the noise intensity. In Case 1, the OSCL-CKF4 improves the CKF by 9.7%, and in Case 3, the OSCL-CKF4 improves the CKF by 13.3%, which means that the OSCL-CKFs show better performance to reduce the influence of the noise.

Conclusions
In order to improve the filtering accuracy of cubature Kalman filters in nonlinear estimation systems, this paper utilizes the orthogonal simplex Chebyshev-Laguerre polynomial to calculate the numerical integration of the cubature Kalman filter. The performance of the nonlinear filtering problem can be improved based on the orthogonal method, which is introduced to the spherical simplex rule without computational burden. Furthermore, unlike some high-degree sample pointsbased filtering algorithms, the filter improves the accuracy according to the order of the Chebyshev-Laguerre polynomials. This is an important observation because the computational cost increases linearly with the increase of state dimensions x n , which is more practical. In our simulation, the accuracy is improved slowly when the order exceeds 4. The choice of the order also depends on the accuracy request and the specific application. Furthermore, the OSCL-CKF has potential applications in other nonlinear systems of different fields. In future work, how to design orthogonal matrix suitably and how to choose the order of the CL polynomials in different application should be further studied. As is shown in Figure 3, all algorithms tend to be convergent, with the measurement times increasing. During the Monte-Carlo test, the performance of CKF is better than that of UKF. One explanation is that the negative κ may lead to non-positive definiteness of the covariance matrices when n x > 3. Moreover, the OSCL-CKF-based algorithms perform better than the other algorithms. As the first order OSCL-CKF (OSCL-CKF1) provides better performance over the SCKF, it is proven that the orthogonal transformation can efficiently modify the uncertain higher order terms and relieve the nonlocal sampling effects. On the other hand, the high order OSCL-CKFs generally outperform the low order OSCL-CKF. At the end of the Monte-Carlo test, the RMSEpos of the OSCL-CKF4 is 3.2% better than that of the OSCL-CKF1, and 5.2% better than that of the SCKF. As is shown in Figure 4, the differences in the velocity estimate accuracy among these algorithms are not as obvious as in the position estimate. This is mainly due to the uniform linear motion of the target.
As is shown in Figure 5, the MSEspos of the filters become larger with the increasing of the noise intensity. In Case 1, the OSCL-CKF4 improves the CKF by 9.7%, and in Case 3, the OSCL-CKF4 improves the CKF by 13.3%, which means that the OSCL-CKFs show better performance to reduce the influence of the noise.

Conclusions
In order to improve the filtering accuracy of cubature Kalman filters in nonlinear estimation systems, this paper utilizes the orthogonal simplex Chebyshev-Laguerre polynomial to calculate the numerical integration of the cubature Kalman filter. The performance of the nonlinear filtering problem can be improved based on the orthogonal method, which is introduced to the spherical simplex rule without computational burden. Furthermore, unlike some high-degree sample points-based filtering algorithms, the filter improves the accuracy according to the order of the Chebyshev-Laguerre polynomials. This is an important observation because the computational cost increases linearly with the increase of state dimensions n x , which is more practical. In our simulation, the accuracy is improved slowly when the order exceeds 4. The choice of the order also depends on the accuracy request and the specific application. Furthermore, the OSCL-CKF has potential applications in other nonlinear systems of different fields. In future work, how to design orthogonal matrix suitably and how to choose the order of the CL polynomials in different application should be further studied.
Author Contributions: Z.L. and H.W. developed the algorithm and performed the experiments. Z.L., S.C., and H.W. analyzed the experimental results and wrote this paper. F.L. supervised the study and reviewed this paper.