Nonlinear Gaussian Filter with Multi-Step Colored Noise

: 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 ﬁltering 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 ﬁltering framework, resulting in an analytic form of posterior state estimate. Compared with the existing nonlinear ﬁltering 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 veriﬁed with a classical target tracking system.


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: R r and R r×s denote the r-dimensional and r × s dimensional Euclidean spaces, respectively. E{·} denotes mathematical expectation. diag{·} stands for block diagonal matrix. blk{·} denotes block matrix. O ij ∈ 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 "*". AA T A( * ) T , where A is an arbitrary matrix. N(a; a, b) indicates that random vector a follows Gaussian distribution with mean a and covariance b.

Problem Statement
Consider a nonlinear system described by the following state-space model: where x k+1 ∈ R n x and z ∈ R n z are the state vector and measurement, respectively. f (·) and h(·) represent the nonlinear vector function. ω k and υ k are multi-step colored noise with multi-step autoregressive models, witch can be formulated as where n = min{k, s} and m = min{k, t} are the known correlated step, a 0 k = O n x ×n x , b 0 k = O n z ×n z , k = 1, 2, · · · . ξ k and η k are uncorrelated white Gaussian noise with covariance Q k and R k , respectively. a i k , i = 1, 2, · · · , n and b j k , j = 1, 2, . . . , m are known correlation parameters. Gaussian filtering is done by approximating the system with Gaussian distribution to obtain posteriori estimates based on the measurement information z 1:k = {z 1 , z 2 , . . . , z k }. However, ω k and υ 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 0 k and b 0 k 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 Figure 1 of Ref. [18]. ζ(t) = n c (t) is the result of input noise n(t) spread over the local code, ψ(t) = B I p f is the low-pass filter bandwidth equivalent to the integral totalizer.

Colored Noises Whitening
In order to process the colored noise in system (1), it is first necessary to whiten it: 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: 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 then Equations (5) and (8) constitute the system with noise whitening processing, and notice that {y 1 , y 2 , . . . , y k } can be transformed back into {z 1 , z 2 , . . . , 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

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 α and β, we define The following Lemma will be used later.

One-Step Prediction
According to the definition in Equation (5), the one-step predictive estimate X k+1|k can be given by where Since ξ k is uncorrelated with y 1:k , we have ω k|k =E[ω k |y 1:k ] = E[a 0 k ω k + a 1 k ω k−1 + · · · + a n k ω k−n + ξ k |y 1: Notice that the posteriori estimate of X k at the previous moment is According to Equation (6), we have In this case, ρ k|k , θ k−1|k and ω k−i , i = 1, 2, . . . , 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 XX k+1|k has the following expressions: Next, the terms in Equation (21) will be calculated. The error of state one-step prediction can be represented as utilizing the Equation (22), we can obtain According to Equations (2) and (6), the P ωθ k,k−1|k , P ωω k,k|k and P ρω k,k|k can be given by where In addition, using Equation (19) one can obtain Comparing Equations (21) Therefore, P ωω k−i,k−j|k , i, j = 1, 2, . . . , n and P ρω k,k−i|k , i = 1, 2 . . . , n in Equations (21) and (28) can also be derived from P XX k,k|k .

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: where φ ∈ 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: where Using Equation (44)  1: At k + 1 moment, input X k|k and P XX k|k 2: Obtain ρ k|k , θ k−1|k and ω k−i|k , i = 1, 2, . . . , n from X k|k ; 3: Calculate ω k|k by Equation (18); 4: Calculate f (x k )N(X k ; X k|k , P XX k,k|k )dX k using Equation (42), then get x k+1|k ; 5: Get P ρρ k,k|k , P ρθ k,k−1|k , P θθ k−1,k−1|k , P ωω k−i,k−j|k , i, j = 1, 2, . . . , n and P ρω k,k−i|k , i = 1, 2 . . . , n from P XX k|k ; 6: Calculate P ωθ k,k−1|k and P ωω k,k−j|k , j = 1, 2, · · · , n using Equations (27)   15: Return to step 1 and implement steps 1-14 for obtaining the estimation of next time; 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 where (s x,k (i), s y,k (i)) T and (ŝ x,k (i),ŝ y,k (i)) 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. The proposed CKF-MCN The UKF with colored noise in [17] The GF with colored noise in [18] The CKF in [7] Figure 3. Comparisons of the position RMSE obtained by the proposed CKF-MCN, the UKF with colored noise in Ref. [17], the GF with colored noise in Ref. [18] and the CKF in Ref. [7].

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.

Conflicts of Interest:
The authors declare no conflict of interest.

Abbreviations x k
True state z k Measurement w k Color process noise v k Color measurement noise X k Whitening state y k Whitening measurement ξ k Whitening process noise η k Whitening measurement noise y 1:k The set of measurement from moment 1 to k X k+1|k State one-step prediction of X k+1 P XX k+1,k+1|k Covariance one-step prediction of X k+1 ρ k Augmented state θ k Augmented process noise a i|j Mean of a i given y 1:j P ab i,j|k Cross-covariance between a i and b j given y 1:k