Nonlinear Gaussian Filter with Multi-Step Colored Noise

Abstract

Color noise is a special kind of noise often occurring in localization systems, and it is more suitable than the general Gaussian white noise to model time dependence due to time delay or high-frequency sampling. This paper derives a nonlinear Gaussian filtering framework for multi-step colored noise systems using noise whitening techniques and Bayes rule. Meanwhile, the cubature rule is used to solve the Gaussian-weighted integral in the proposed Gaussian filtering framework, resulting in an analytic form of posterior state estimate. Compared with the existing nonlinear filtering algorithms, the proposed method has obvious advantages in colored noise systems because it fully takes into account the time dependence of colored noise. Finally, the effectiveness and advantages of the proposed algorithm are verified with a classical target tracking system.

1. Introduction

Nonlinear estimation is a hot problem in engineering and has a wide range of applications in robot control [1,2], fault diagnosis [3], and mobile communications [4]. In the sense of minimum mean square error (MMSE), the optimal state estimate is the mean of its posterior probability density function (PDF). For the linear Gaussian case, the Kalman filter (KF) [5] gives the recursive form of the posterior PDF. However, for the nonlinear systems, the analytic solution of the posterior PDF is difficult to be obtained, in which case one has to find the suboptimal solution. The nonlinear Gaussian filter (GF) [6] derives the approximate state posteriori PDF through a series of Gaussian assumptions, but it involves a series of Gaussian weighted integrals, which are computationally significant. For this reason, a great deal of work has been devoted to search for high-precision numerical methods to approximate the Gaussian weighted integral [7,8,9,10,11]. Unscented Kalman filter (UKF) [8] makes use of unscented transformation (UT) to approximate the Gaussian weighted integral, which is comparable in accuracy to the third-order Taylor expansion. However, the weights of UKF can be negative at high state dimensions, which makes it numerical unstable. Cubature Kalman filter (CKF) [7] uses the three-degree radial rule to compute Gaussian weighted integrals, which has the same accuracy as UKF, and the weights are all positive, making it more stable than UKF. Moreover, there are also Gauss-Hermite quadrature filter (GHQF) [9], High-degree CKF (HCKF) [10] and Sparse-grid quadrature filter (SGQF) [11], etc., all of which are proposed for the idea of improving the accuracy of numerical integration.

However, the above algorithm is only applicable to the system with Gaussian white noise, which is difficult to be satisfied in real-world environments. For example, in complex underwater environments, gravity sensors that commonly used for navigation are susceptible to outliers, which cause their measurement noise to be heavy-tailed. To reflect the heavy-tailed characteristic, the student-t distribution is often used to model the statistical properties of the noise [12]. In channel estimation, no radio links are individually assigned and there is interference between different links, which makes the noise in the channel often a mixture of multiple Gaussian noises, i.e., Gaussian mixture noise [13]. In addition, in GPS navigation systems, the sensors suffer from unavoidable random delays, which makes the noise present at one moment have an impact on the measurements at multiple later moments, i.e., the system suffers from colored noise [14]. In addition, in high-frequency sampling systems and networked systems, color noise can occur. To reduce the impact of color noise on the system, a large amount of literature has been contributed. Refs. [15,16] considered the speech signals with the multi-order colored measurement noise, and respectively proposed the dual perceptually constrained UKF and the modified unscented particle filter (UPF). Refs. [17,18] modelled the measurement noise as the coloured first-order AR process, and proposed the improved UKFs to estimate the multi-path parameter of weak signal from GPS and track the target, respectively. However, the algorithm mentioned above are only applicable to the case of first-order colored noise. In the fields of target tracking, the system noise may be multi-step colored noise [19].

Motivated by the aforementioned analysis, this paper aims to propose a nonlinear filtering algorithm that is applicable to arbitrary order colored noise. The main contributions are summarized as follows: (i) A new system whose noise item is Gaussian white noise is developed by whitening and dimension expansion; (ii) A nonlinear Gaussian filter for the new system is designed by using Gaussian update rule. Finally, a target localization system are used to show the effectiveness and advantages of the proposed methods.

Notations: ${\mathbb{R}}^r$ and ${\mathbb{R}}^{r\times s}$ denote the r-dimensional and $r\times s$ dimensional Euclidean spaces, respectively. $E\{·\}$ denotes mathematical expectation. $\mathrm{diag}\{·\}$ stands for block diagonal matrix. $\mathrm{blk}\{·\}$ denotes block matrix. $O_{ij}\in {\mathbb{R}}^{ij}$ is zero matrix. I stands for identity matrix. $e_i$ denotes the i-th column of I. Superscript T represents transpose. $|·|$ denotes absolute value. The symmetric terms in a symmetric matrix are denoted by “*”. $A{A}^T\triangleq A{(*)}^T$, where A is an arbitrary matrix. $N(a;\overline{a},b)$ indicates that random vector a follows Gaussian distribution with mean $\overline{a}$ and covariance b.

2. Problem Statement

Consider a nonlinear system described by the following state-space model:

$$\begin{array}{c}\hfill \left\{\begin{array}{c}{x}_{k+1}=f\left(x_k\right)+{\omega }_k\hfill \\ z_{k+1}=h\left(x_{k+1}\right)+{\upsilon }_k\hfill \end{array}\right.k=0,1,2,\dots \end{array}$$

(1)

where $x_{k+1}\in {\mathbb{R}}^{n_x}$ and $z\in {\mathbb{R}}^{n_z}$ are the state vector and measurement, respectively. $f(·)$ and $h(·)$ represent the nonlinear vector function. ${\omega }_k$ and ${\upsilon }_k$ are multi-step colored noise with multi-step autoregressive models, witch can be formulated as

$$\begin{array}{c}\hfill \left\{\begin{array}{c}{\omega }_k={\sum }_{i=0}^n{a}_k^i{\omega }_{k-i}+{\xi }_k\hfill \\ {\upsilon }_k={\sum }_{i=0}^m{b}_k^i{\upsilon }_{k-i}+{\eta }_k\hfill \end{array}\right.k=0,1,2,\dots \end{array}$$

(2)

where $n=min\{k,s\}$ and $m=min\{k,t\}$ are the known correlated step, $a_k^0=O_{n_x\times n_x}$, $b_k^0=O_{n_z\times n_z}$, $k=1,2,\dots$${\xi }_k$ and ${\eta }_k$ are uncorrelated white Gaussian noise with covariance $Q_k$ and $R_k$, respectively. $a_k^i$, $i=1,2,\dots ,n$ and $b_k^j$, $j=1,2,\dots ,m$ are known correlation parameters. Gaussian filtering is done by approximating the system with Gaussian distribution to obtain posteriori estimates

$$x_{k|k}=E\left[x_k\right|z_{1:k}],P_{k,k|k}^{xx}=E\left[(x_k-x_{k|k}){(*)}^T\right|z_{1:k}]$$

(3)

based on the measurement information $z_{1:k}=\{z_1,z_2,\dots ,z_k\}$. However, ${\omega }_k$ and ${\upsilon }_k$ in system (1) are not Gaussian white noise, and the classical Gaussian filtering is no longer applicable here. Our aim is to design a new Gaussian filter to obtain a posteriori estimates of system (1).

Remark 1: 

It should be emphasized that$a_k^0$ and $b_k^0$ are only added to facilitate the modeling of the case where the colored noise is of order 0. Although it appears in the model, the computational procedure for the terms associated with these two values will not be given later since they are equal to zero matrices and remain zero matrices when multiplied by any matrix.

Remark 2: 

When the sampling frequency of the system is sufficiently high, the noise at a given moment can have an effect on several adjacent sampling periods, and the noise is correlated in the time direction, which is referred to as multi-step colored noise [18]. In addition, feedback control is also one of the causes of colored noise, for example, in the GPS multi-path delay nonlinear estimation, the formation process of the colored noise is shown in Fig. 1 of Ref. [18].$\zeta \left(t\right)=n_c\left(t\right)$is the result of input noise$n\left(t\right)$spread over the local code,$\psi \left(t\right)=B_{Ipf}$is the low-pass filter bandwidth equivalent to the integral totalizer.

3. Main Results

3.1. Colored Noises Whitening

In order to process the colored noise in system (1), it is first necessary to whiten it:

$$\begin{array}{cc}\hfill y_{k+1}=& z_{k+1}-b_{k+1}^0{z}_{k+1}-b_{k+1}^1{z}_k-\dots -b_{k+1}^m{z}_{k+1-m}\hfill \\ \hfill =& h\left(x_{k+1}\right)-b_{k+1}^{0}h\left(x_{k+1}\right)-b_{k+1}^{1}h\left(x_k\right)-\dots \hfill \\ \hfill & -b_{k+1}^{m}h\left(x_{k+1-m}\right)+{\eta }_k.\hfill \end{array}$$

(4)

We can see that the noise term of $y_{k+1}$ is Gaussian white noise. Moreover, notice that the new measurement $y_{k+1}$ becomes a function of $x_{k+1-m}$, $x_{k+2-m}$,…, $x_{k+1}$, and the process noise is colored noise. In this case an expansion of the state is required:

$$X_{k+1}=\left[\begin{array}{c}{x}_{k+1}\\ {\rho }_k\\ {\omega }_k\\ {\theta }_{k-1}\end{array}\right]=\left[\begin{array}{c}f\left(x_k\right)+{\sum }_{i=0}^n{a}_k^i{\omega }_{k-i}\\ {\rho }_k\\ {\sum }_{i=0}^n{a}_k^i{\omega }_{k-i}\\ {\theta }_{k-1}\end{array}\right]+\left[\begin{array}{c}{\xi }_k\\ O_{m{n}_x\times 1}\\ {\xi }_k\\ O_{m{n}_x\times 1}\end{array}\right]$$

(5)

where

$${\rho }_k=\left[\begin{array}{c}{x}_k\\ ⋮\\ x_{k+1-m}\end{array}\right],{\theta }_{k-1}=\left[\begin{array}{c}{\omega }_{k-1}\\ ⋮\\ {\omega }_{k-n}\end{array}\right].$$

(6)

The noise term of $X_{k+1}$ becomes Gaussian white noise after dimension expansion, and it is clear that $y_{k+1}$ is a function of $X_{k+1}$. Let us define

$$\begin{array}{cc}\hfill H\left(X_{k+1}\right)\triangleq & h\left(x_{k+1}\right)-b_{k+1}^{0}h\left(x_{k+1}\right)-b_{k+1}^{1}h\left(x_k\right)-\dots \hfill \\ \hfill & -b_{k+1}^{m}h\left(x_{k+1-m}\right),\hfill \end{array}$$

(7)

then

$$y_{k+1}=H\left(X_{k+1}\right)+{\eta }_k.$$

(8)

Equations (5) and (8) constitute the system with noise whitening processing, and notice that $\{y_1,y_2,\dots ,y_k\}$ can be transformed back into $\{z_1,z_2,\dots ,z_k\}$ by the linear transformation in Equation (4), which means that the information contained in $y_{1:k}$ and $z_{1:k}$ is equivalent, so Equation (3) is equivalent to

$$\begin{array}{c}\hfill x_{k|k}=E\left[x_k\right|y_{1:k}],P_{k,k|k}^{xx}=E\left[(x_k-x_{k|k}){(*)}^T\right|y_{1:k}].\end{array}$$

(9)

3.2. Design Gaussian Filter with Multi-Step Colored Noise

In this section, in the case of multi-step colored noise, a posteriori estimates of the $k+1$ moments are derived based on the posteriori estimates of the k moments. Firstly, for arbitrary random variable $\alpha$ and $\beta$, we define

$$\begin{array}{c}\hfill \left\{\begin{array}{c}{\alpha }_{i|j}=E\left[{\alpha }_i\right|y_{1:j}]\hfill \\ {\tilde{\alpha }}_{i|j}={\alpha }_i-{\alpha }_{i|j}\hfill \\ P_{i,j|r}^{\alpha \beta }=E\left[{\tilde{\alpha }}_{i|r}{\tilde{\beta }}_{j|r}^T\right|y_{1:r}]\hfill \end{array}\right..\end{array}$$

(10)

The following Lemma will be used later.

Lemma 1 [20]. 

When the joint PDF of${\alpha }_k$and${\beta }_k$conditioned on${\beta }_{l:k-1}$is Gaussian, that is,$p({\alpha }_k,{\beta }_k|{\beta }_{1:k})$is Gaussian, then$p\left({\alpha }_k\right|{\beta }_{1:k})$can be computed as Gaussian with mean${\alpha }_{k|k}$and corresponding covariance matrix$P_{k,k|k}^{\alpha \alpha }$as the unified form:

$${\alpha }_{k|k}={\alpha }_{k|k-1}+K_k^{\alpha }({\beta }_k-{\beta }_{k|k-1}),$$

(11)

$$P_{k,k|k}^{\alpha \alpha }=P_{k,k|k-1}^{\alpha \alpha }-K_k^{\alpha }{P}_{k,k|k-1}^{\beta \beta }{\left(K_k^{\alpha }\right)}^T,$$

(12)

$$K_k^{\alpha }=P_{k,k|k-1}^{\alpha \beta }{\left(P_{k,k|k-1}^{\beta \beta }\right)}^{-1},$$

(13)

where${\alpha }_{k|k-1}=E\left[{\alpha }_k\right|{\beta }_{1:k-1}]$, $P_{k,k|k-1}^{\alpha \alpha }=E\left[({\alpha }_k-{\alpha }_{k|k-1}){(*)}^T\right]$, ${\beta }_{k|k-1}=E\left[{\beta }_k\right|{\beta }_{1:k-1}]$, $P_{k,k|k-1}^{\beta \beta }=E\left[({\beta }_k-{\beta }_{k|k-1}){(*)}^T\right]$. $P_{k,k|k-1}^{\alpha \beta }=E\left[({\alpha }_k-{\alpha }_{k|k-1}){({\beta }_k-{\beta }_{k|k-1})}^T\right]$.

Assumption 1: 

Based on the measurement from moments 1 to k, the one-step predictive PDF for$X_{k+1}$and$y_{k+1}$are Gaussian, that is

$$p\left(X_{k+1}\right|y_{1:k})=N(X_{k+1};X_{k+1|k},P_{k+1|k}^{XX}),$$

(14)

$$p\left(y_{k+1}\right|y_{1:k})=N(y_{k+1};y_{k+1|k},P_{k+1|k}^{yy}),$$

(15)

where$X_{k+1|k}=E\left[X_{k+1}\right|y_{1:k}]$, $P_{k+1|k}^{XX}=E\left[(X_{k+1}-X_{k+1|k}){(*)}^T\right]$, $y_{k+1|k}=E\left[y_{k+1}\right|y_{1:k}]$, $P_{k+1|k}^{yy}=E\left[(y_{k+1}-y_{k+1|k}){(*)}^T\right]$.

3.2.1. One-Step Prediction

According to the definition in Equation (5), the one-step predictive estimate $X_{k+1|k}$ can be given by

$$X_{k+1|k}=E\left[X_{k+1}\right|y_{1:k}]=\left[\begin{array}{c}E\left[x_{k+1}\right|y_{1:k}]\\ E\left[{\rho }_k\right|y_{1:k}]\\ E\left[{\omega }_k\right|y_{1:k}]\\ E\left[{\theta }_{k-1}\right|y_{1:k}]\end{array}\right]=\left[\begin{array}{c}{x}_{k+1|k}\\ {\rho }_{k|k}\\ {\omega }_{k|k}\\ {\theta }_{k-1|k}\end{array}\right]$$

(16)

where

$$x_{k+1|k}=E\left[x_{k+1}\right|y_{1:k}]=E[f\left(x_k\right)+{\omega }_k|y_{1:k}]=\int f\left(x_k\right)N(X_k;X_{k|k},P_{k,k|k}^{XX})d{X}_k+{\omega }_{k|k}.$$

(17)

Since ${\xi }_k$ is uncorrelated with $y_{1:k}$, we have

$${\omega }_{k|k}=E\left[{\omega }_k\right|y_{1:k}]=E[a_k^0{\omega }_k+a_k^1{\omega }_{k-1}+\dots +a_k^n{\omega }_{k-n}+{\xi }_k|y_{1:k}]=\sum _{i=0}^n{a}_k^i{\omega }_{k-i|k}.$$

(18)

Notice that the posteriori estimate of $X_k$ at the previous moment is

$$X_{k|k}=E\left[X_k\right|y_{1:k}]=\left[\begin{array}{c}E\left[x_k\right|y_{1:k}]\\ E\left[{\rho }_{k-1}\right|y_{1:k}]\\ E\left[{\omega }_{k-1}\right|y_{1:k}]\\ E\left[{\theta }_{k-2}\right|y_{1:k}]\end{array}\right]=\left[\begin{array}{c}E\left[{\rho }_k\right|y_{1:k}]\\ E\left[x_{k-m}\right|y_{1:k}]\\ E\left[{\theta }_{k-1}\right|y_{1:k}]\\ E\left[{\omega }_{k-1-n}\right|y_{1:k}].\end{array}\right]$$

(19)

According to Equation (6), we have

$$\begin{array}{c}\hfill E\left[{\theta }_{k-1}\right|y_{1:k}]=\left[\begin{array}{c}{\omega }_{k-1|k}\\ {\omega }_{k-2|k}\\ ⋮\\ {\omega }_{k-n|k}.\end{array}\right]\end{array}$$

(20)

In this case, ${\rho }_{k|k}$, ${\theta }_{k-1|k}$ and ${\omega }_{k-i}$, $i=1,2,\dots ,n$ can be obtained from the posteriori estimate at the previous moment, i.e, $X_{k|k}$.

Moreover, according to the definition in Equation (5), the one-step predictive error covariance $P_{k+1|k}^{XX}$ has the following expressions:

$$P_{k+1,k+1|k}^{XX}=E[(\left[\begin{array}{c}{\tilde{x}}_{k+1|k}\\ {\tilde{\rho }}_{k|k}\\ {\tilde{\omega }}_{k|k}\\ {\tilde{\theta }}_{k-1|k}\end{array}\right]){(*)}^T|y_{1:k}]=\left[\begin{array}{cccc}{P}_{k+1,k+1|k}^{xx}& P_{k+1,k|k}^{x\rho }& P_{k+1,k|k}^{x\omega }& P_{k+1,k-1|k}^{x\theta }\\ *& P_{k,k|k}^{\rho \rho }& P_{k,k|k}^{\rho \omega }& P_{k,k-1|k}^{\rho \theta }\\ *& *& P_{k,k|k}^{\omega \omega }& P_{k,k-1|k}^{\omega \theta }\\ *& *& *& P_{k-1,k-1|k}^{\theta \theta }\end{array}\right].$$

(21)

Next, the terms in Equation (21) will be calculated. The error of state one-step prediction can be represented as

$${\tilde{x}}_{k+1|k}=x_{k+1}-x_{k+1|k}=f\left(x_k\right)-E\left[f\left(x_k\right)\right|y_{1:k}]+{\tilde{\omega }}_{k|k},$$

(22)

utilizing the Equation (22), we can obtain

$$\begin{array}{cc}\hfill P_{k+1,k+1|k}^{xx}=& E[(f\left(x_k\right)-E\left[f\left(x_k\right)\right|y_{1:k}]+{\tilde{\omega }}_{k|k}){(*)}^T|y_{1:k}]\hfill \\ \hfill =& \int (f\left(x_k\right)-E\left[f\left(x_k\right)\right|y_{1:k}]){(\ast )}^{T}N(X_k;X_{k|k},P_{k|k}^{XX}))d{X}_k\hfill \\ \hfill & +\sum _{i=0}^n\int (f\left(x_k\right)-E\left[f\left(x_k\right)\right|y_{1:k}]){\tilde{\omega }}_{k-i|k}^T{\left(a_k^i\right)}^{T}N(X_k;X_{k|k},P_{k|k}^{XX})d{X}_k\hfill \\ \hfill & +{(\sum _{i=0}^n\int (f\left(x_k\right)-E\left[f\left(x_k\right)\right|y_{1:k}]){\tilde{\omega }}_{k-i|k}^T{\left(a_k^i\right)}^{T}N(X_k;X_{k|k},P_{k,k|k}^{XX})d{X}_k)}^T+P_{k,k|k}^{\omega \omega }\hfill \end{array}$$

(23)

$$\begin{array}{cc}\hfill P_{k+1,k|k}^{x\rho }=& E[{\tilde{x}}_{k+1|k}{\tilde{\rho }}_{k|k}^T|y_{1:k}]\hfill \\ \hfill =& E[(f\left(x_k\right)-E\left[f\left(x_k\right)\right|y_{1:k}]+{\tilde{\omega }}_{k|k}){\tilde{\rho }}_{k|k}^T|y_{1:k}]\hfill \\ \hfill =& \int (f\left(x_k\right)-E\left[f\left(x_k\right)\right|y_{1:k}]){\tilde{\rho }}_{k|k}^{T}N(X_k;X_{k|k},P_{k,k|k}^{XX})d{X}_k+P_{k,k|k}^{\omega \rho }\hfill \end{array}$$

(24)

$$\begin{array}{cc}\hfill P_{k+1,k|k}^{x\omega }=& E[{\tilde{x}}_{k+1|k}{\tilde{\omega }}_{k|k}^T|y_{1:k}]\hfill \\ \hfill =& E[(f\left(x_k\right)-E\left[f\left(x_k\right)\right|y_{1:k}]+{\tilde{\omega }}_{k|k}){\tilde{\omega }}_{k|k}^T|y_{1:k}]\hfill \\ \hfill =& \sum _{i=0}^n\int (f\left(x_k\right)-E\left[f\left(x_k\right)\right|y_{1:k}]){\tilde{\omega }}_{k-i|k}^T{\left(a_k^i\right)}^{T}N(X_k;X_{k|k},P_{k|k}^{XX})d{X}_k+P_{k,k|k}^{\omega \omega }\hfill \end{array}$$

(25)

$$\begin{array}{cc}\hfill P_{k+1,k-1|k}^{x\theta }=& E[{\tilde{x}}_{k+1|k}{\tilde{\theta }}_{k-1|k}^T|y_{1:k}]\hfill \\ \hfill =& E[(f\left(x_k\right)-E\left[f\left(x_k\right)\right|y_{1:k}]+{\tilde{\omega }}_{k|k}){\tilde{\theta }}_{k-1|k}^T|y_{1:k}]\hfill \\ \hfill =& \int (f\left(x_k\right)-E\left[f\left(x_k\right)\right|y_{1:k}]){\tilde{\theta }}_{k-1|k}^{T}N(X_k;X_{k|k},P_{k,k|k}^{XX})d{X}_k+P_{k,k-1|k}^{\omega \theta }.\hfill \end{array}$$

(26)

According to Equations (2) and (6), the $P_{k,k-1|k}^{\omega \theta }$, $P_{k,k|k}^{\omega \omega }$ and $P_{k,k|k}^{\rho \omega }$ can be given by

$$\begin{array}{cc}\hfill P_{k,k-1|k}^{\omega \theta }=& E[{\tilde{\omega }}_{k|k}{\tilde{\theta }}_{k-1|k}^T|y_{1:k}]=E[{\tilde{\omega }}_{k|k}{[{\tilde{\omega }}_{k-1|k}\dots {\tilde{\omega }}_{k-n|k}]}^T|y_{1:k}]=\left[\begin{array}{c}{P}_{k,k-1|k}^{\omega \omega }\dots P_{k,k-n|k}^{\omega \omega }\end{array}\right]\hfill \end{array}$$

(27)

$$P_{k,k|k}^{\omega \omega }=E[{\tilde{\omega }}_{k|k}{\tilde{\omega }}_{k|k}^T|y_{1:k}]=\sum _{i=0}^{n}E[{\tilde{\omega }}_{k|k}{\tilde{\omega }}_{k-i|k}^T|y_{1:k}]{\left(a_k^i\right)}^T+Q_k=\sum _{i=0}^n{P}_{k,k-i|k}^{\omega \omega }{\left(a_k^i\right)}^T+Q_k$$

(28)

$$\begin{array}{cc}\hfill P_{k,k|k}^{\rho \omega }=& E[{\tilde{\rho }}_k{\tilde{\omega }}_k^T|y_{1:k}]\hfill \\ \hfill =& \sum _{i=0}^{n}E[{\tilde{\rho }}_k{\tilde{\omega }}_{k-i|k}^T|y_{1:k}]{\left(a_k^i\right)}^T=\sum _{i=0}^n{P}_{k,k-i|k}^{\rho \omega }{\left(a_k^i\right)}^T,\hfill \end{array}$$

(29)

where

$$\begin{array}{cc}\hfill P_{k,k-j|k}^{\omega \omega }=& E[{\tilde{\omega }}_{k|k}{\tilde{\omega }}_{k-j|k}^T|y_{1:k}]=\sum _{i=0}^n{a}_k^{i}E[{\tilde{\omega }}_{k-i|k}{\tilde{\omega }}_{k-j|k}^T|y_{1:k}]\hfill \\ \hfill =& \sum _{i=0}^n{a}_k^i{P}_{k-i,k-j|k}^{\omega \omega },j=0,1,\dots ,n.\hfill \end{array}$$

(30)

In addition, using Equation (19) one can obtain

$$\begin{array}{cc}\hfill P_{k,k|k}^{XX}=& E[{\tilde{X}}_{k|k}{\tilde{X}}_{k|k}^T|y_{1:k}]=E[\left[\begin{array}{c}{\tilde{x}}_{k|k}\\ {\tilde{\rho }}_{k-1|k}\\ {\tilde{\omega }}_{k-1|k}\\ {\tilde{\theta }}_{k-2|k}\end{array}\right]{(*)}^T|y_{1:k}]\hfill \\ \hfill =& E[\left[\begin{array}{c}{\tilde{\rho }}_{k|k}\\ {\tilde{x}}_{k-m|k}\\ {\tilde{\theta }}_{k-1|k}\\ {\tilde{\omega }}_{k-1-n|k}\end{array}\right]{(*)}^T|y_{1:k}]\hfill \\ \hfill =& \left[\begin{array}{cccc}{P}_{k,k|k}^{\rho \rho }& P_{k,k-m|k}^{\rho x}& P_{k,k-1|k}^{\rho \theta }& P_{k,k-1-n|k}^{\rho \omega }\\ *& P_{k-m,k-m|k}^{xx}& P_{k,k|k}^{x\theta }& P_{k-m,k-1-n|k}^{x\omega }\\ *& *& P_{k-1,k-1|k}^{\theta \theta }& P_{k-1,k-1-n|k}^{\theta \omega }\\ *& *& *& P_{k-1-n,k-1-n|k}^{\omega \omega }\end{array}\right].\hfill \end{array}$$

(31)

Comparing Equations (21) and (31), it can be seen that $P_{k,k|k}^{\rho \rho }$, $P_{k,k-1|k}^{\rho \theta }$, and $P_{k-1,k-1|k}^{\theta \theta }$ in Equation (21) can be obtained from Equation (31). Moreover, utilizing Equation (6), one obtains

$$\begin{array}{cc}\hfill P_{k-1,k-1|k}^{\theta \theta }=& E[{\tilde{\theta }}_{k|k}{\tilde{\theta }}_{k|k}^T|y_{1:k}]\hfill \\ \hfill =& \left[\begin{array}{cccc}{P}_{k-1,k-1|k}^{\omega \omega }& P_{k-1,k-2|k}^{\omega \omega }& \dots & P_{k-1,k-n|k}^{\omega \omega }\\ *& P_{k-2,k-2|k}^{\omega \omega }& \dots & P_{k-2,k-n|k}^{\omega \omega }\\ *& *& \dots & \dots \\ *& *& *& P_{k-n,k-n|k}^{\omega \omega }\end{array}\right]\hfill \end{array}$$

(32)

$$P_{k,k-1|k}^{\rho \theta }=E[{\tilde{\rho }}_{k|k}{\left[\begin{array}{c}{\tilde{\omega }}_{k-1|k}\\ {\tilde{\omega }}_{k-2|k}\\ ⋮\\ {\tilde{\omega }}_{k-n|k}\end{array}\right]}^T|y_{1:k}]=\left[\begin{array}{c}{P}_{k,k-1|k}^{\rho \omega }{P}_{k,k-2|k}^{\rho \omega }\dots P_{k,k-n|k}^{\rho \omega }\end{array}\right]$$

(33)

Therefore, $P_{k-i,k-j|k}^{\omega \omega }$, $i,j=1,2,\dots ,n$ and $P_{k,k-i|k}^{\rho \omega }$, $i=1,2\dots ,n$ in Equations (21) and (28) can also be derived from $P_{k,k|k}^{XX}$.

3.2.2. Measurement Update

Based on Assumption 1 and the previous analysis, we have

$$p\left(X_{k+1}\right|y_{1:k})=N(X_{k+1};X_{k+1|k},P_{k+1,k+1|k}^{XX}).$$

(34)

${\eta }_{k+1}$ is uncorrelated with $y_{1:k}$, so, measurement one-step prediction can be obtained by

$$\begin{array}{cc}\hfill y_{k+1|k}=& E\left[y_{k+1}\right|y_{1:k}]=E[H\left(X_{k+1}\right)+{\eta }_{k+1}|y_{1:k}]\hfill \\ \hfill =& \int H\left(X_{k+1}\right)N(X_{k+1};X_{k+1|k},P_{k+1,k+1|k}^{XX})d{X}_{k+1}\hfill \end{array}$$

(35)

The error of measurement one-step prediction is

$$\begin{array}{cc}\hfill {\tilde{y}}_{k+1|k}=& y_{k+1}-y_{k+1|k}\hfill \\ \hfill =& H\left(X_{k+1}\right)-E\left[H\left(X_{k+1}\right)\right|y_{1:k}]+{\eta }_{k+1},\hfill \end{array}$$

(36)

and thus, we have

$$\begin{array}{cc}\hfill P_{k+1,k+1|k}^{yy}=& E[{\tilde{y}}_{k+1|k}{\tilde{y}}_{k+1|k}^T|y_{1:k}]\hfill \\ \hfill =& \int (H\left(X_{k+1}\right)-E\left[H\left(X_{k+1}\right)\right|y_{1:k}]){(*)}^T\hfill \\ \hfill & \times N(X_{k+1};X_{k+1|k},P_{k+1,k+1|k}^{XX})d{X}_{k+1}.\hfill \end{array}$$

(37)

$$\begin{array}{cc}\hfill P_{k+1,k+1|k}^{Xy}=& E[{\tilde{X}}_{k+1|k}{\tilde{y}}_{k+1|k}^T|y_{1:k}]\hfill \\ \hfill =& \int (X_{k+1}-X_{k+1|k}){(H\left(X_{k+1}\right)-E\left[H\left(X_{k+1}\right)\right|y_{1:k}])}^T\hfill \\ \hfill & \times N(X_{k+1};X_{k+1|k},P_{k+1,k+1|k}^{XX})d{X}_{k+1}.\hfill \end{array}$$

(38)

Substituting Equations (34), (35), (37) and (38) into Lemma 1, we have

$$X_{k+1|k+1}=X_{k+1|k}+K_{k+1}^X(y_{k+1}-y_{k+1|k}),$$

(39)

$$P_{k+1,k+1|k+1}^{XX}=P_{k+1,k+1|k}^{XX}-K_{k+1}^X{P}_{k+1,k+1|k}^{yy}{\left(K_{k+1}^X\right)}^T,$$

(40)

$$K_{k+1}^X=P_{k+1,k+1|k}^{Xy}{(P_{k+1,k+1|k}^{yy})}^{-1}.$$

(41)

Obviously, $x_{k+1|k+1}$ and $P_{k+1,k+1|k}^{xx}$ can be obtained from $X_{k+1|k+1}$ and $P_{k+1,k+1|k}^{XX}$.

3.3. Implementing the Gaussian Filter Using Third-Degree Spherical-Radial Rule

The previous section derived the posteriori estimate at $k+1$ moment based on the posteriori estimate at moment k. However, notice that Equations (23)–(26), (35), (37) and (38) contain integral operations, which have a uniform form:

$$\int g\left(\varphi \right)N(\varphi ;\overline{\varphi },P)d\varphi$$

(42)

where $\varphi \in {\mathbb{R}}^L$, $g(·)$ is an arbitrary function. The core of the Gaussian filter is the calculation of the integral in the form of Equation (42). Various methods have been proposed in Refs. [6,7,8,9,10,11] to calculate the above equation. The third-degree spherical-radial rule [7] has been widely used to approximate Equation (42) because of its good integration accuracy, stability, and ease of implementation:

$$\int g\left(\varphi \right)N(\varphi ;\overline{\varphi },P)d\varphi \approx \sum _{i=1}^{2L}\frac{1}{2L}g(S{\mu }_i+\overline{\varphi })$$

(43)

where

$$P=S{S}^T,{\mu }_i=\left\{\begin{array}{c}\sqrt{L}{e}_i,i=1,2,\dots ,L\hfill \\ -\sqrt{L}{e}_{i-n},i=L+1,L+2,\dots ,2L.\hfill \end{array}\right.$$

(44)

Using Equation (44) to compute the Gaussian weighted integral in Equations (23)–(26), (35), (37) and (38) yields the Cubature Kalman filter with multi-step colored noise (CKF-MCN) easily, and we summarize the algorithmic procedure of the CKF-MCN in Algorithm 1. In addition, to better understand the operation of CKF-MCN, its main steps are summarized in the form of a flow chart in Figure 1.

Algorithm 1:Cubature Kalman filter with multi-step colored noise.

1: At $k+1$ moment, input $X_{k|k}$ and $P_{k|k}^{XX}$ 2: Obtain ${\rho }_{k|k}$, ${\theta }_{k-1|k}$ and ${\omega }_{k-i|k}$, $i=1,2,\dots ,n$ from $X_{k|k}$; 3: Calculate ${\omega }_{k|k}$ by Equation (18); 4: Calculate $\int f\left(x_k\right)N(X_k;X_{k|k},P_{k,k|k}^{XX})d{X}_k$ using Equation (42), then get $x_{k+1|k}$; 5: Get $P_{k,k|k}^{\rho \rho }$, $P_{k,k-1|k}^{\rho \theta }$, $P_{k-1,k-1|k}^{\theta \theta }$, $P_{k-i,k-j|k}^{\omega \omega }$, $i,j=1,2,\dots ,n$ and $P_{k,k-i|k}^{\rho \omega }$, $i=1,2\dots ,n$ from $P_{k|k}^{XX}$; 6: Calculate $P_{k,k-1|k}^{\omega \theta }$ and $P_{k,k-j|k}^{\omega \omega }$, $j=1,2,\dots ,n$ using Equations (27) and (30); 7: Calculate $P_{k,k|k}^{\rho \omega }$ and $P_{k,k|k}^{\omega \omega }$ using Equations (29) and (28); 8: Calculate $\int (f\left(x_k\right)-E\left[f\left(x_k\right)\right|y_{1:k}]){(\ast )}^T\times N(X_k;X_{k|k},P_{k|k}^{XX}))d{X}_k$, $\int (f\left(x_k\right)-E\left[f\left(x_k\right)\right|y_{1:k}]){\tilde{\rho }}_{k|k}^T\times N(X_k;X_{k|k},P_{k,k|k}^{XX})d{X}_k$, $\sum _{i=0}^n\int (f\left(x_k\right)-E\left[f\left(x_k\right)\right|y_{1:k}]){\tilde{\omega }}_{k-i|k}^T\times N(X_k;X_{k|k},P_{k|k}^{XX})d{X}_k$$\times {\left(a_k^i\right)}^T$, and $\int (f\left(x_k\right)-E\left[f\left(x_k\right)\right|y_{1:k}]){\tilde{\theta }}_{k-1|k}^T\times N(X_k;X_{k|k},P_{k,k|k}^{XX})d{X}_k$ by Equation (42); 9: Obtain $P_{k+1,k+1|k}^{xx}$, $P_{k+1,k|k}^{x\rho }$, $P_{k+1,k|k}^{x\omega }$ and $P_{k+1,k-1|k}^{x\theta }$ from Equations (23)–(26), then $P_{k+1,k+1|k}^{XX}$ can be given by Equation (21); 10: Calculate $\int H\left(X_{k+1}\right)N(X_{k+1};X_{k+1|k},P_{k+1,k+1|k}^{XX})d{X}_{k+1}$ by Equation (42), and then get $y_{k+1|k}$ from Equation (35); 11: Calculate $y_{k+1|k}$, $P_{k+1,k+1|k}^{yy}$ and $P_{k+1,k+1|k}^{Xy}$ by Equation (42). 12: Calculate $K_{k+1}^X$ from Equation (41); 13: Calculate $X_{k+1|k+1}$ and $P_{k+1,k+1|k+1}^{XX}$; 14: $x_{k+1|k+1}$ and $P_{k+1,k+1|k+1}^{xx}$ can be obtained from $X_{k+1|k+1}$ and $P_{k+1,k+1|k+1}^{XX}$ 15: Return to step 1 and implement steps 1–14 for obtaining the estimation of next time;

Remark 3: 

Compared with the extended Kalman filter (EKF), the proposed CKF-MCN has two main advantages: (1). Higher accuracy. The EKF utilizes Taylor expansion to approximate the first-order and second-order moments of the random variable with an accuracy of 1st-order Taylor expansion. However, the CKF-MCN uses cubature rules to approximate the statistical properties of random variables with an accuracy of 3rd-order Taylor expansion; (2). Better application prospects. The EKF is only applicable to white noise systems, while the CKF-MCN is applicable to multi-step colored noise systems.

4. Simulation Examples

In this section, the effectiveness of the proposed CKF-MCN is verified by a target tracking simulation. The motion model [7] of the target is

$$\begin{array}{c}\hfill x_{k+1}=\left[\begin{array}{ccccc}1& \frac{\sin \left(\mathsf{\Omega }T\right)}{\mathsf{\Omega }}& 0& -\left(\frac{1-\cos \left(\mathsf{\Omega }T\right)}{\mathsf{\Omega }}\right)& 0\\ 0& \cos \left(\mathsf{\Omega }T\right)& 0& -\sin \left(\mathsf{\Omega }T\right)& 0\\ 0& \frac{1-\cos \left(\mathsf{\Omega }T\right)}{\mathsf{\Omega }}& 1& \frac{\sin \left(\mathsf{\Omega }T\right)}{\mathsf{\Omega }}& 0\\ 0& \sin \left(\mathsf{\Omega }T\right)& 0& \cos \left(\mathsf{\Omega }T\right)& 0\\ 0& 0& 0& 0& 1\end{array}\right]x_k+{\omega }_k\end{array}$$

$$\begin{array}{cc}\hfill z_{k+1}=& \left[\begin{array}{c}{d}_{k+1}\\ {\beta }_{k+1}\end{array}\right]\hfill \\ \hfill =& \left[\begin{array}{c}\sqrt{{(s_{x,k+1}-a)}^2+{(s_{y,k+1}-b)}^2}\\ {\tan }^{-1}(\frac{s_{y,k+1}}{s_{x,k+1}})\end{array}\right]+{\upsilon }_k\hfill \end{array}$$

where the state of the target $x_k={\left[s_{x,k}{v}_{x,k}{s}_{y,k}{v}_{y,k}\eta \right]}^T$, $s_{x,k}$ and $s_{y,k}$ are respectively the coordinates of x-axis and y-axis direction. $v_{x,k}$ and $v_{y,k}$ denote the velocities of x-axis and y-axis direction, respectively. $\mathsf{\Omega }$ is the turn rate. a and b are respectively the location of the sensor in the x- and y-coordinates. T is the sample period.

The simulation parameters are selected as follows: initial state is ${\widehat{x}}_0={[1000 \mathrm{m}300 \mathrm{m/s}1000 \mathrm{m}0 \mathrm{m/s}\frac{3\pi }{180}\mathrm{rad/s}]}^T$, $P_0=[100 {\mathrm{m}}^{2}10 {\mathrm{m}}^2/{\mathrm{s}}^{2}100 {\mathrm{m}}^2/{\mathrm{s}}^{2}10 {\mathrm{m}}^2/{\mathrm{s}}^2$$100 {\mathrm{mrad}}^2/{\mathrm{s}}^2]$, $T=0.25 \mathrm{s}$, $R_k=[100 {\mathrm{m}}^{2}10 {\mathrm{mrad}}^2/{\mathrm{m}}^2]$, $Q_k=diag\left(q_{1}G{q}_{1}G{q}_{2}T\right)$ where

$$\begin{array}{c}\hfill \left\{\begin{array}{c}G=\left[\begin{array}{cc}\frac{T^3}{3}& \frac{T^2}{2}\\ \frac{T^2}{2}& T\end{array}\right]\hfill \\ q_1=0.1 {\mathrm{m}}^2/{\mathrm{s}}^3,q_2=1.75\times {10}^{-4}/{\mathrm{s}}^3.\hfill \end{array}\right.\end{array}$$

We consider the following scenario:

$$\begin{array}{c}\hfill \left\{\begin{array}{c}{\omega }_k=\sum _{i=0}^{min(2,k)}{a}_k^i{\omega }_{k-i}+{\xi }_k,a_k^1=0.25I,a_k^2=0.05I\hfill \\ {\upsilon }_k=\sum _{i=0}^{min(2,k)}{b}_k^i{\upsilon }_{k-i}+{\eta }_k,b_k^1=0.6I,b_k^2=0.2I.\hfill \end{array}\right.\end{array}$$

By implementing Algorithm 1, the true and estimated trajectories are plotted in Figure 2. As can be seen from this figure, the CKF-MCN can track the target well. Moreover, position root mean square error (RMSE) is selected as performance indicator to compare the different algorithms, which is given by

$$\begin{array}{cc}\hfill & {\mathrm{RMSE}}_k=\sqrt{\frac{1}{m}\sum _{i=1}^m{(s_{x,k}\left(i\right)-{\widehat{s}}_{y,k}\left(i\right))}^2+{(s_{y,k}\left(i\right)-{\widehat{s}}_{y,k}\left(i\right))}^2}\hfill \end{array}$$

where ${(s_{x,k}\left(i\right),s_{y,k}\left(i\right))}^T$ and ${({\widehat{s}}_{x,k}\left(i\right),{\widehat{s}}_{y,k}\left(i\right))}^T$ are the true and estimated coordinate of target at k instant of the i-th Monte carlo run. To obtain the exact position RMSE, 100 Monte Carlo runs were performed and the simulation results are shown in Figure 3. As can be seen from Figure 3, the CKF-MCN has the highest accuracy in several different methods, which indicates the superior performance of our proposed algorithm in handling multi-step colored noise.

5. Conclusions

In this paper, the problem of state estimation for nonlinear systems with multi-step colored noise is studied. A linear-transform-based technique is proposed to whiten the system with colored noise by which the colored noise system can be transform to an equivalent Gaussian white noise system. A nonlinear Gaussian filtering framework is designed for the whitened system, which is suitable for multi-step colored noise systems. Compared with existing methods that only apply to white noise or first-order colored noise, the proposed method has a wider application scope. Simulation results verify the effectiveness and advantages of the proposed algorithm. Furthermore, it should be emphasized that the proposed menthod expands the dimension of the state, which often leads to the instability of numerical integration. Therefore, in the future work, we aim to propose a new method to solve the state estimation problem under colored noise without dimensionality expansion.