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

Abstract

To further improve the filtering accuracy in nonlinear estimation systems, a nonlinear filter, called the orthogonal simplex Chebyshev-Laguerre cubature Kalman filter (OSCL-CKF), is proposed. The filter is built within the cubature Kalman filter 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 filter to improve the precision by the order of the CL polynomial. Numerical results show that the proposed filter outperforms the conventional algorithms.

1. 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 have drawn much attention, such as the central difference Kalman filter (CDKF) [14], the unscented Kalman filter (UKF) [15,16,17], and the cubature Kalman filter (CKF) [18]. Among these filters, the UKF uses a set of sigma-points and unscented transformation to capture posterior statistics of the state. The CKF performs with higher accuracy than the EKF and shows better numerical stability and lower computational demands than the UKF with a non-zero scaling parameter in high dimensions [18].

It should be noted that when the system model includes highly nonlinear functions, the unknown high order terms may drastically reduce the accuracy of the covariance estimate [19]. Additionally, the performance of the CKF may be unsatisfactory in high nonlinear conditions, and the way in which to make use of the measurements efficiently should also be taken into account. Jia [20] proposed the fifth and higher-degree CKF to further improve the accuracy, but the number of sample points increases rapidly with the increase of the algebraic degree or the state dimension. Compared with the high-degree CKF, the high-degree simplex cubature Kalman filter (SCKF) [21] uses fewer sample points and shows higher accuracy. Unfortunately, the number of sample points still increases quadratically with the increase of the state dimension.

The contribution of this paper is to derive a new class of nonlinear filter with arbitrary order of the Chebyshev-Laguerre (CL) polynomial. In order to regulate the effect of the high order terms, the stochastic orthogonal method [22] is introduced to the third-degree spherical simplex rule, which improves the accuracy of the CKF with only two more sample points. Moreover, the solution to the CL polynomial equation of arbitrary order is discussed [23]. The quadrature points and weights in the radial integral can be subsequently obtained according to the nature of the CL polynomial, and hence the accuracy can be controlled by the order of CL polynomial. The filter is called the orthogonal simplex Chebyshev-Laguerre cubature Kalman filter (OSCL-CKF). Compared with the standard CKF, the proposed filter is more flexible because the accuracy can be further improved with the increase of CL polynomial order. Also, computational cost increases linearly with CL polynomial order, which is superior to other high-degree versions of the CKF. The proposed filter can potentially be used in many applications such as air traffic control, remote sensing, ballistic missile guidance, and computer vision with good filtering accuracy.

The rest of this paper is organized as follows. Section 2 describes the system model and Bayesian nonlinear filter framework. The orthogonal simplex Chebyshev-Laguerre cubature rule is presented in Section 3. Section 4 yields the performance comparison of the proposed algorithm with the conventional algorithms. Conclusions are drawn in Section 5.

2. System Model and Bayesian Filter

2.1. System Model

The nonlinear discrete estimation system model can be expressed as follows:

$$\{\begin{array}{l}{\mathit{x}}_k=f\left({\mathit{x}}_{k-1}\right)+{\mathit{v}}_{k-1}\\ {\mathit{z}}_k=h\left({\mathit{x}}_k\right)+{\mathit{e}}_k\end{array},$$

(1)

where ${\mathit{x}}_k\in {ℝ}^{n_x}$ is the state vector at time index k with $n_x$ dimensions, and ${\mathit{z}}_k\in {ℝ}^{n_z}$ is the measurement vector with n_z dimensions. The values of $n_x$ and $n_z$ are dependent on specific applications. $f(\cdot )$ and $h(\cdot )$ are the nonlinear function, here $k=1,2,\cdots$. The process noise ${\mathit{v}}_{k-1}$ and the measurement noise ${\mathit{e}}_k$ are generally assumed to be independent and identically distributed zero mean Gaussian noise with the covariance matrices ${\mathit{Q}}_{k-1}$ and ${\mathit{R}}_k$, respectively.

2.2. 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:

$${\hat{\mathit{x}}}_{k|k-1}={\displaystyle {\int }_{{ℝ}^{n_x}}f\left({\mathit{x}}_{k-1}\right)N\left({\mathit{x}}_{k-1};{\hat{\mathit{x}}}_{k-1|k-1},{\mathit{P}}_{k-1|k-1}\right)d{\mathit{x}}_{k-1}},$$

(2)

$$\begin{array}{ll}{\mathit{P}}_{k|k-1}& ={\displaystyle {\int }_{{ℝ}^{n_x}}f\left({\mathit{x}}_{k-1}\right)f^T\left({\mathit{x}}_{k-1}\right)N\left({\mathit{x}}_{k-1};{\hat{\mathit{x}}}_{k-1|k-1},{\mathit{P}}_{k-1|k-1}\right)d{\mathit{x}}_{k-1}}\\ & -{\hat{\mathit{x}}}_{k|k-1}{\hat{\mathit{x}}}_{k|k-1}^T+{\mathit{Q}}_{k-1}\end{array},$$

(3)

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:

$$p\left({\mathit{z}}_k|D_{k-1}\right)=N\left({\mathit{z}}_k;{\hat{\mathit{z}}}_{k|k-1},{\mathit{P}}_{zz,k|k-1}\right),$$

(4)

${\hat{\mathit{z}}}_{k|k-1}$ is the predicted measurement, which is predicted based on time index k − 1 and the measurement model. ${\hat{\mathit{z}}}_{k|k-1}$ and its associated innovation covariance ${\mathit{P}}_{zz,k|k-1}$ are given by the following:

$${\hat{\mathit{z}}}_{k|k-1}={\displaystyle {\int }_{{ℝ}^{n_x}}h\left({\mathit{x}}_k\right)N\left({\mathit{x}}_k;{\hat{\mathit{x}}}_{k|k-1},{\mathit{P}}_{k|k-1}\right)d{\mathit{x}}_k},$$

(5)

$${\mathit{P}}_{zz,k|k-1}={\displaystyle {\int }_{{ℝ}^{n_x}}h\left({\mathit{x}}_k\right)h^T\left({\mathit{x}}_k\right)N\left({\mathit{x}}_k;{\hat{\mathit{x}}}_{k|k-1},{\mathit{P}}_{k|k-1}\right)d{\mathit{x}}_k}-{\hat{\mathit{z}}}_{k|k-1}{\hat{\mathit{z}}}_{k|k-1}^T+{\mathit{R}}_k,$$

(6)

and the cross-covariance is as follows:

$${\mathit{P}}_{xz,k|k-1}={\displaystyle {\int }_{{ℝ}^{n_x}}{\mathit{x}}_k{h}^T\left({\mathit{x}}_k\right)N\left({\mathit{x}}_k;{\hat{\mathit{x}}}_{k|k-1},{\mathit{P}}_{k|k-1}\right)d{\mathit{x}}_k}-{\hat{\mathit{x}}}_{k|k-1}{\hat{\mathit{z}}}_{k|k-1}^T.$$

(7)

Once the new measurement is received, the filter computes the posterior density, yielding the following:

$$p\left({\mathit{x}}_k|D_k\right)=N\left({\mathit{x}}_k;{\hat{\mathit{x}}}_{k|k},{\mathit{P}}_{k|k}\right),$$

(8)

where

$${\hat{\mathit{x}}}_{k|k}={\hat{\mathit{x}}}_{k|k-1}+{\mathit{K}}_k({\mathit{z}}_k-{\hat{\mathit{z}}}_{k|k-1}),$$

(9)

$${\mathit{P}}_{k|k}={\mathit{P}}_{k|k-1}-{\mathit{K}}_k{\mathit{P}}_{zz,k|k-1}{\mathit{K}}_k^T,$$

(10)

$${\mathit{K}}_k={\mathit{P}}_{xz,k|k-1}{\mathit{P}}_{zz,k|k-1}^{-1}$$

(11)

3. 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\left(\mathit{x};0,\mathit{I}\right)$, the integral can be expressed as follows:

$$I\left(f\right)={\displaystyle {\int }_{{ℝ}^{n_x}}f\left(\mathit{x}\right)N\left(\mathit{x};0,\mathit{I}\right)d\mathit{x}}={\left(2\pi \right)}^{-\frac{n_x}{2}}{\displaystyle {\int }_{{ℝ}^{n_x}}f\left(\mathit{x}\right){\mathit{e}}^{-\frac{1}{2}{\mathit{x}}^T\mathit{x}}d\mathit{x}},$$

(12)

where $f\left(\mathit{x}\right)$ is an arbitrary nonlinear function.

Now, we transform the integral from the Cartesian coordinate to the spherical-radial coordinate. Assuming that $\mathit{x}=r\mathit{y}$ with ${\mathit{y}}^T\mathit{y}=1$ and $r=\sqrt{{\mathit{x}}^T\mathit{x}}$, Equation (12) can be rewritten in the spherical-radial coordinate system as follows [18]:

$$I\left(f\right)={\left(2\pi \right)}^{-\frac{n_x}{2}}{\displaystyle {\int }_0^{\infty }{\displaystyle {\int }_{U_{n_x}}f\left(r\mathit{y}\right)r^{n_x-1}{e}^{-\frac{r^2}{2}}d\sigma \left(\mathit{y}\right)dr}},$$

(13)

where direction vector $\mathit{y}$ and radius $r$ are the corresponding parameters in the spherical-radial coordinate [18]. $U_{n_x}=\left\{\mathit{y}\in {ℝ}^{n_x}|{\mathit{y}}^T\mathit{y}=1\right\}$ is the spherical surface and $d\sigma (\cdot )$ is the spherical surface measure. As a consequence, two kinds of integrals are given by the following:

$$S\left(r\right)={\displaystyle {\int }_{U_{n_x}}f\left(r\mathit{y}\right)d\sigma \left(\mathit{y}\right)},$$

(14)

$$G_s={\left(2\pi \right)}^{-\frac{n_x}{2}}{\displaystyle {\int }_0^{\infty }S\left(r\right)r^{n_x-1}{\mathit{e}}^{-\frac{r^2}{2}}dr},$$

(15)

3.1. 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:

$${\mathit{\alpha }}_j={\left[{\alpha }_{j,1},{\alpha }_{j,2},\dots ,{\alpha }_{j,n_x}\right]}^T,$$

(16)

where $j=1,2,\dots ,n_x+1$, and

$${\alpha }_{j,m}=\{\begin{array}{ll}-\sqrt{\frac{n_x+1}{n_x\left(n_x-m+2\right)\left(n_x-m+1\right)}}& m<j\\ \sqrt{\frac{\left(n_x+1\right)\left(n_x-j+1\right)}{n_x\left(n_x-j+2\right)}}& m=j\\ 0& m>j\end{array},$$

(17)

Accordingly, the third-degree spherical simplex rule is constructed as follows:

$$S_3\left(r\right)={\displaystyle \sum _{j=1}^{n_s}{w}_j^{s}f(\cdot )}=\frac{A_{n_x}}{2\left(n_x+1\right)}{\displaystyle \sum _{j=1}^{n_x+1}\left(f\left(r{\mathit{\alpha }}_j\right)+f\left(-r{\mathit{\alpha }}_j\right)\right)},$$

(18)

where $n_s=2\left(n_x+1\right)$, $A_{n_x}=2\sqrt{{\pi }^{n_x}}/\mathsf{\Gamma }\left(n_x/2\right)$, and $\mathsf{\Gamma }\left(n_x\right)={\displaystyle {\int }_0^{\infty }{x}^{n_x-1}{\mathit{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:

$$S_3\left(r\right)=\frac{A_{n_x}}{2\left(n_x+1\right)}{\displaystyle \sum _{j=1}^{n_s}f\left(r{\mathbf{\gamma }}_j\right)},$$

(19)

where $\mathbf{\gamma }=\left[{\mathbf{\gamma }}_1,{\mathbf{\gamma }}_2,\dots ,{\mathbf{\gamma }}_{2n_x+2}\right]=\mathit{B}\times \left[\mathit{\alpha }-\mathit{\alpha }\right]$ and $\mathit{B}=\left[{\mathit{B}}_1,{\mathit{B}}_2,\dots ,{\mathit{B}}_{n_x}\right]$ is an $n_x\times n_x$ orthogonal matrix, where ${\mathit{B}}_i={\left[{\beta }_{i,1},{\beta }_{i,2},\cdots ,{\beta }_{i,n_x}\right]}^T$. Therefore, $\mathbf{\gamma }$ is a $n_x\times \left(2n_x+2\right)$ matrix.

The method to select an orthogonal matrix can be seen in the works of [26,27]. In this paper, we construct $\mathit{B}$ according to Xiu [28]:

$${\beta }_{i,2p-1}=\sqrt{\frac{2}{n_x}}\cos \frac{\left(2p-1\right)i\pi }{n_x},$$

(20)

$${\beta }_{i,2p}=\sqrt{\frac{2}{n_x}}\sin \frac{\left(2p-1\right)i\pi }{n_x},$$

(21)

where $p=1,2,\cdots , \max \left\{p\in \mathit{Z}|p\le n_x/2\right\}$. If $n_x$ is odd, ${\beta }_{i,n_x}={\left(-1\right)}^i/\sqrt{n_x}$.

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.

3.2. 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:

$$G_s=\frac{1}{2}{\pi }^{-\frac{n_x}{2}}{\displaystyle {\int }_0^{\infty }S\left(\sqrt{2t}\right)t^{\frac{n_x}{2}-1}{e}^{-t}dt\mathrm{v},}$$

(22)

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_i^c$:

$$G_s={\displaystyle \sum _{k=1}^{n_c}{w}_i^c}S\left(\sqrt{2t}\right),$$

(23)

which are determined by the Chebyshev-Laguerre polynomials [29]:

$$L_{n_c}^{\left(a\right)}\left(t\right)={\left(-1\right)}^{n_c}{t}^{-a}{e}^t\frac{d^{n_c}}{d{t}^{n_c}}\left(t^{a+n_c}{e}^{-t}\right),$$

(24)

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]:

$$w_k^c=-{\pi }^{-\frac{n_x}{2}}\cdot \frac{n_c!\mathsf{\Gamma }\left(a+n_c+1\right)}{2{\dot{L}}_{n_c}^{\left(a\right)}\left(t_k\right)L_{n_c+1}^{\left(a\right)}\left(t_k\right)},$$

(25)

where $k=1,\cdots ,n_c$. From the theory of Laguerre polynomials, the relation between $L_{n_c}^{\left(a\right)}\left(t_k\right)$ and $L_{n_c+1}^{\left(a\right)}\left(t_k\right)$ is described as follows:

$$L_{n_c+1}^{\left(a\right)}\left(t_k\right)=-t_k{\dot{L}}_{n_c}^{\left(a\right)}\left(t_k\right),$$

(26)

and then we obtain the following:

$$w_k^c={\pi }^{-\frac{n_x}{2}}\cdot \frac{n_c!\mathsf{\Gamma }\left(a+n_c+1\right)}{2t_k{\left[{\dot{L}}_{n_c}^{\left(a\right)}\left(t_k\right)\right]}^2}.$$

(27)

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.

3.3. 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 Section 3.1 and Section 3.2, respectively. Substituting Equations (18) and (23) into Equation (13), the orthogonal spherical simplex Chebyshev-Laguerre rule for $N\left(\mathit{x};0,\mathit{I}\right)$ is as follows:

$${\displaystyle {\int }_{{ℝ}^{n_x}}f\left(\mathit{x}\right)N\left(\mathit{x};0,\mathit{I}\right)d\mathit{x}}\approx {\displaystyle \sum _{i=1}^{n_s}{\displaystyle \sum _{j=1}^{n_c}{w}_i^s{w}_j^c}}f(\cdot ),$$

(28)

That is,

$$\begin{array}{l}I\left(f\right)\approx {\displaystyle \sum _{i=1}^{n_s}{\displaystyle \sum _{j=1}^{n_c}{w}_i^s{w}_j^c}}f(\cdot )\\ ={\displaystyle \sum _{i=1}^{n_s}}{\displaystyle \sum _{j=1}^{n_c}}\frac{A_{n_x}}{n_s}\cdot \frac{n_c!\mathsf{\Gamma }\left(a+n_c+1\right)}{\sqrt{{\pi }^{n_x}}2t_j{\left[{\dot{L}}_{n_c}^{\left(a\right)}\left(t_j\right)\right]}^2}f\left(\sqrt{2t_j}\mathit{B}\times {\left[\mathit{\alpha }-\mathit{\alpha }\right]}_i\right)\\ ={\displaystyle \sum _{i=1}^{n_s}}{\displaystyle \sum _{j=1}^{n_c}}\frac{2\sqrt{{\pi }^{n_x}}/\mathsf{\Gamma }\left(n_x/2\right)}{2\left(n_x+1\right)}\cdot \frac{n_c!\mathsf{\Gamma }\left(a+n_c+1\right)}{\sqrt{{\pi }^{n_x}}2t_j{\left[{\dot{L}}_{n_c}^{\left(a\right)}\left(t_j\right)\right]}^2}f\left(\sqrt{2t_j}{\mathbf{\gamma }}_i\right)\end{array}$$

(29)

Therefore,

$$I\left(f\right)={\displaystyle \sum _{i=1}^{2\left(n_x+1\right)}}{\displaystyle \sum _{j=1}^{n_c}}\frac{n_c!\mathsf{\Gamma }\left(n_x/2+n_c\right)}{2t_j\left(n_x+1\right)\mathsf{\Gamma }\left(n_x/2\right){\left[{\dot{L}}_{n_c}^{\left(n_x/2-1\right)}\left(t_j\right)\right]}^2}f\left(\sqrt{2t_j}{\mathbf{\gamma }}_i\right)$$

(30)

The cubature formula is easy to extend to an arbitrary Gaussian distribution $N\left(\mathit{x};\hat{\mathit{x}},{\mathit{P}}_x\right)$. 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 $\mathbf{\gamma }=\left[\mathit{\alpha }-\mathit{\alpha }\right]$, Equation (30) can be rewritten as follows:

$$\begin{array}{ll}I\left(f\right)& ={\displaystyle \sum _{i=1}^{2\left(n_x+1\right)}}\frac{1!\mathsf{\Gamma }(n_x/2+1)}{2t_1\left(n_x+1\right)\mathsf{\Gamma }(n_x/2){\left[{\dot{L}}_1^{\left(n_x/2-1\right)}\left(t_1\right)\right]}^2}f\left(\sqrt{2t_1}{\left[\mathit{\alpha }-\mathit{\alpha }\right]}_i\right)\\ & ={\displaystyle \sum _{i=1}^{2\left(n_x+1\right)}\frac{\mathsf{\Gamma }\left(3\right)}{4\left(n_x+1\right)\mathsf{\Gamma }\left(2\right){\left[{\dot{L}}_1^{\left(1\right)}\left(2\right)\right]}^2}f\left(2{\left[\mathit{\alpha }-\mathit{\alpha }\right]}_i\right)}\\ & =\frac{1}{2\left(n_x+1\right)}{\displaystyle \sum _{i=1}^{2\left(n_x+1\right)}}f\left(2{\left[\mathit{\alpha }-\mathit{\alpha }\right]}_i\right)\end{array},$$

(31)

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.

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

4.1. The Highly Dimensional Nonlinear Estimation Problem

The system model of nonlinear estimation is defined as:

$${\mathit{x}}_k={\mathit{x}}_{k-1}^2\cos \left({\mathit{x}}_{k-1}\right)+{\mathit{v}}_{k-1},$$

$$z_k=\sqrt[n_x]{1+{\mathit{x}}_k^T\mathit{x}}+e_k,$$

where $\cos \left({\mathit{x}}_{k-1}\right)={\left[\cos \left(x_{1,k-1}\right),\cos \left(x_{2,k-1}\right),\cdots ,\cos \left(x_{n_x,k-1}\right)\right]}^T$. Here we set the dimensions $n_x=20$. ${\mathit{v}}_{k-1}$ and e_k are the independent Gaussian noise with zero means and covariance ${\mathit{I}}_{n_x}$ and 1, respectively.

The true initial value is ${\mathit{x}}_0=0.1\times {\mathbf{1}}_{n_x\times 1}$, and the initial estimate ${\hat{\mathit{x}}}_0$ used in the filters is generated randomly from $N\left({\hat{\mathit{x}}}_0;{\mathit{x}}_0,{\mathit{P}}_0\right)$, where the initial covariance is ${\mathit{P}}_0={\mathit{I}}_{n_x}$. The mean squared error (MSE) for each Monte-Carlo run is defined as follows:

$${\mathrm{MSE}}_i=\frac{1}{n}\sum _{k=1}^n\left[\frac{1}{n_x}{\left({\mathit{x}}_k-{\hat{\mathit{x}}}_{k,i}\right)}^T\left({\mathit{x}}_k-{\hat{\mathit{x}}}_{k,i}\right)\right],$$

(32)

Accordingly, the average MSE can be written as follows:

$$\mathrm{MSE}=\frac{1}{L}{\displaystyle \sum _{i=1}^L{\mathrm{MSE}}_i},$$

(33)

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.

4.2. 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 k can be expressed by ${\mathit{x}}_k^t={[x_k^t,y_k^t,{\dot{x}}_k^t,{\dot{y}}_k^t]}^T$, where ${\mathit{x}}_k^p={\left[x_k^t,y_k^t\right]}^T$ is the position and ${\dot{\mathit{x}}}_k^v={\left[{\dot{x}}_k^t,{\dot{y}}_k^t\right]}^T$ is the velocity vector. The true state of the observation platform is ${\mathit{x}}_k^o={\left[x_k^o,y_k^o,{\dot{x}}_k^o,{\dot{y}}_k^o\right]}^T$, and the state vector in the relative own-ship reference at time index k can be defined as follows:

$${\mathit{x}}_k\triangleq {\mathit{x}}_k^t-{\mathit{x}}_k^o={[x_k,y_k,{\dot{x}}_k,{\dot{y}}_k]}^T$$

The system model is written as follows:

$$\{\begin{array}{l}{\mathit{x}}_k=\mathit{F}{\mathit{x}}_{k-1}+{\mathit{v}}_k-{\mathit{u}}_{k-1,k}\\ z_k=h({\mathit{x}}_k)=\arctan \frac{y_k}{x_k}+e_k\end{array},$$

(34)

where ${\mathit{u}}_{k-1,k}$ is the deterministic input and F is the state transition matrix, which can be given by:

$${\mathit{u}}_{k-1,k}=\left[\begin{array}{c}{x}_k^o-x_{k-1}^o-\Delta {\dot{x}}_{k-1}^o\\ y_k^o-y_{k-1}^o-\Delta {\dot{y}}_{k-1}^o\\ {\dot{x}}_k^o-{\dot{x}}_{k-1}^o\\ {\dot{y}}_k^o-{\dot{y}}_{k-1}^o\end{array}\right], \mathit{F}=\begin{array}{cccc}1& 0& \Delta & 0\\ 0& 1& 0& \Delta \\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}$$

where $\Delta =1\min$ is the measurement interval. The standard deviation of measurement noise $e_k$ is ${\sigma }_e$, and ${\mathit{v}}_k$ is the process noise vector with a covariance matrix of:

$$\mathit{Q}={\mathit{\Gamma }}^T\mathit{\Gamma }\cdot q$$

where $q={10}^{-10}$ km²/s³ represents the intensity of the process noise, and $\mathit{\Gamma }$ is the noise drive matrix:

$$\mathit{\Gamma }=\left[\begin{array}{cc}{\Delta }^2/2& 0\\ 0& {\Delta }^2/2\\ \Delta & 0\\ 0& \Delta \end{array}\right]$$

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:

$${\mathrm{RMSE}}_{pos,k}=\sqrt{\frac{1}{L}{\displaystyle \sum _{j=1}^L\left[{({\hat{x}}_{k,j}^t-x_{k,j}^t)}^2+{({\hat{y}}_{k,j}^t-y_{k,j}^t)}^2\right]}},$$

(35)

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:

$${\mathrm{MSE}}_{\mathrm{pos},i}=\frac{1}{n}{\displaystyle \sum _{k=1}^n\left[{\left(x_k^t-{\hat{x}}_{k|k,i}^t\right)}^2+{\left(y_k^t-{\hat{y}}_{k|k,i}^t\right)}^2\right]}$$

(36)

In the Monte-Carlo test, the target begins at the same state every time for a fair comparison. The scale parameter $\kappa$ 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 $\overline{r}~N(r,{\sigma }_r^2)$, where r is the true range. The initial bearing follows $\overline{z_0}~N(z_0,{\sigma }_e^2)$, where z₀ is the true bearing. Similarly, the initial velocity estimate $\overline{s}~N(s,{\sigma }_s^2)$, where s is the true initial target velocity. The target course estimate follows $\overline{c}~N(c,{\sigma }_c^2)$, where c is the true target course. The parameters in the simulation scenario are shown in

Table 1. The parameters of the tracking.

Parameters	r	s	c	σr	σe	σc
Value	5 km	4 knots	−2.44 rad	2 km	17.5 mrad	$\pi \sqrt{12}$

. (“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 Figure 3 and Figure 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 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.

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