Nonlinear Gaussian Filter with the Colored Measurement Noise
Nonlinear Gaussian Filter with the Colored Measurement Noise Xiaoxu Wang Northwestern Polytechnical University Xi’an, China [email protected] Abstract—This paper is concerned with the Gaussian approximation (GA) filtering for a class of nonlinear stochastic systems in the case that the colored measurement noise is modeled as a first-order autoregressive process. First, through the augmentation of the standard measurements, the problem of designing the GA filter with the colored measurement noise is transformed into that of deriving the GA one with the delayed state in the augmented measurement function. Second, through presenting Gaussian approximation about the joint posterior probability density functions (PDF) of the present state, the delayed state and the augmented measurement, the novel GA filter with the delayed state are proposed, which recursively operate by analytical computation and nonlinear Gaussian integrals. The proposed GA filter provides a general and common framework, from which many variations can be developed by utilizing different numerical technologies for computing such nonlinear Gaussian integrals, for example the modified cubature Kalman filter (CKF) in this paper using the spherical-radial cubature rule. The performance of the new method is demonstrated with a simulation example. Keywords—nonlinear system, colored measurement noise, Gaussian approximation, filter
I.
INTRODUCTION
A. Motivation Nonlinear state filtering problem for the discrete-time stochastic systems has attracted considerable interest because of its widespread applications in control, signal processing, computer vision and econometrics [1]-[5]. Usually, a standard assumption in the classical estimation theory is that the process and measurement noises should be Gaussian white ones. However, in actual engineering applications, it is quite common that the measurement noise is coloured, rather than a white one. For example, the glint noise of the measured target in radar tracking system [6], and the gyro random slow drift in inertial navigation system [7], are all coloured, since the sampling frequency is too high to ignore the noise correlation in the time direction. Moreover, in the multipath parameters estimation problem of weak GPS signal [8], the measurement noise can be also regarded as the driven output by the white noise through integral and feedback parts. Therefore, there is a great need to develop the nonlinear estimation approaches with the coloured measurement noise. Based on Bayesian approach [9], the general optimal estimation scheme is given by the functional recursive relations for
Quan Pan Northwestern Polytechnical University Xi’an, China [email protected]
computation of probability density functions (PDFs) of the state conditioned by the measurements. For linear systems, these PDFs can be analytically obtained through constructing the first two order moments (mean and covariance), which results in the well-known Kalman filter (KF). However, for the nonlinear system, the analytical computation of such PDFs is unavailable and intractable since the Bayesian solution requires the propagation of full probability density which results in the computation-intensive in numerical implementation. Approximation is thus necessary [10], and the Gaussian approximation (GA) to such PDFs is widely accepted by the fact that: 1) the estimated mean and covariance of the state is the computation of the first two moments of posterior PDF, and GA just meets this requirement and further guarantees the state estimation accuracy, and 2) the recursive operation is available and the corresponding GA filter is analytical. In other words, the GA method can give an effective balance between estimation accuracy and computation demands, and is very suitable for dealing with the filtering problem for the nonlinear systems with the coloured measurement noise. B. Related Works The most prominent and widely used method for nonlinear systems with the coloured measurement noise is the extended Kalman filter (EKF), which is obtained by first-order linearization of the nonlinear system equations so that the traditional KF with the coloured measurement noise is applied at each step. But the first-order linearization may provide insufficient accuracy, resulting in significant bias, or even divergence in the case of large initial errors or high nonlinearities. Moreover, the derivation of the Jacobian matrix is non-trivial in many applications, especially in high-dimensional state or measurement case. For overcoming the above shortcomings in first-order linearization, many novel GA filters have been proposed as an improvement to the EKF, typically including the unscented Kalman filter (UKF) based on the unscented transformation (UT) [11], the Gauss-Hermit filter (GHF) based on the quadrature rule [1], the divided difference filter (DDF) [12], the central difference filter (CDF) based on the Stirling’s polynomial interpolation [13], the cubature Kalman filter (CKF) based on the spherical-radial cubature rule [14], and sparse-grid quadrature filter (SGQF) [15]. The above methods don’t require evaluation of the Jacobian matrix so that they have the computational complexity similar to the EKF [16] but are much sim-
pler to implement. Moreover, they can achieve at least the second-order accuracy while the EKF is the only first-order. So far, some modification about the above GA filters has been developed for filtering the nonlinear state with the coloured measurement noise. Ma, et al [17] and Yin, et al [18] considered the speech signals with the measurement noise modelled as the multi-order autoregressive (AR) process, and respectively proposed the dual perceptually constrained UKF and the modified unscented particle filter (UPF) for the speech enhancement. Unfortunately, the two algorithms suffered from the limitation that the measurement function must be linear, and thus they could not be extended to the situation that the dynamic and measurement functions are all nonlinear. In addition, Yuan, et al [8] and Xiong, et al [20] modelled the measurement noise as the coloured one with the first-order AR process, and improved the UKF to estimate the multipath parameter of weak signal from global position satellite (GPS) and track the target by using the multi sensors, respectively. However, these improved UKFs in [8] and [20] still used the firstorder linearization to decorrelate the colored measurement noise, so that they have to face the similar shortcomings of the low precision and the non-trivial computation of the Jacobian matrix in EKF. Specially for the strong nonlinearities and highdimension cases, such methods may be fail, or even divergent. C. Results and Organization In this paper, we present the GA filtering problem for the nonlinear stochastic systems with the colored measurement noise, modeled as the first-order autoregressive (AR) process. First, a new augmented measurement is artificially generated to decorrelate the colored noise, and further the problem of deriving the GA filter with the colored noise is transformed into that of designing the GA ones with both the delayed state and the uncorrelated noises cases. Second, by presenting the novel Gaussian assumptions about the posterior PDFs of joint the present state, the delayed state and the augmented measurement, the general GA filtering framework with the delayed state are proposed, which recursively operate by combining the analytical computation and the nonlinear Gaussian integrals. Third, the implementation of the proposed GA filter is hence transformed to the computation of such nonlinear integrals, which is solved by using the spherical-radial cubature rule for developing the new variations, i.e., the CKF with the colored measurement noise. Finally, a simulation example shows the superior performance of the new methods. The proposed GA filter is somewhat different from those in the literature. Comparing with [17]-[18], this paper considers the situation that the dynamic and measurement functions are all nonlinear, and aims to develop a general GA filtering framework such that various variations of this framework can be developed by different numerical technologies, such as UT, polynomial interpolation and cubature rule, etc. In addition, instead of the used linearization in [19]-[20], we apply the augmented measurement approach for decorrealting the coloured noise, in order to facilitate the direct application of the cubature rule for computing the nonlinear Gaussian integrals in second-order accuracy, and to further develop the modified CKF as the variation of the derived GA filtering framework. Consequently, the estimation precision superiority of the proposed CKF to the existing one in [19], is also demonstrated by the following simulation results.
The rest of the paper is organized as follows. The investigated problem is formulated and transformed in Section II. In Sections III-A, the general GA filter with the delayed state in the augmented measurement is derived. Based on the sphericalradial cubature rule, the new CKF algorithms are developed as the variations of the proposed GA filter in Section III-B. The simulation analysis is given in Section IV. Some conclusions are drawn in Section V. D. Notification Throughout this paper, the superscripts “ −1 ” and “ T ” represent the inverse and transpose operations of matrix, respectively; i denotes the matrix determinant; N( x; μ, P) denotes that the variable x obeys Gaussian distribution with mean μ and covariance P ; E[i] denotes mathematical expectation; I denotes the unit matrix; The superscripts “ ∧ ” and “ ∼ ”, used as the hat of random variables, represent the estimate and the estimation error, respectively. For example, xˆ denotes the estimate of variable x and its estimation error is x = x − xˆ .
II.
PROBLEM FORMULATION AND PRELIMINARIES
Consider the discrete-time nonlinear stochastic system xk = f k −1 ( xk −1 ) + wk −1 ,
(1)
zk = hk ( xk ) + v k ,
(2) n
where k ∈{1, 2, } is the discrete time, xk ∈ is the state vector at time k , zk ∈ m is the measurement vector at time k , f k (i) and hk (i) are some known nonlinear functions, wk is the process noise, vk is the colored measurement noise which obeys a first-order autoregressive (AR) model, i.e. vk = ψ k , k −1vk −1 + ζ k −1 .
(3)
Here, ψ k , k −1 is the known correlation parameter, wk and ζ k are uncorrelated zero-mean Gaussian white noises satisfying Εw [ kwlT] T = Qkδkl and Ε[vkvl ]= Rkδkl respectively, where δ kl is the Kronecker delta function, and the initial state x0 is a Gaussian random vector with mean xˆ 0 and covariance P0 , which is independent of ζ k and wk . Remark 1. The AR model (3) has been frequently considered for describing the colored noise in the practical applications, including the radar target tracking [21], the inertial navigation [22], the speech processing [23], the channel and spectral estimations [24]-[25]. The reasons causing the colored measurement noise are analyzed as follows. On the one hand, the measurement frequency is high enough so that the correlation between the successive samples of the measured noise cannot be ignored without degrading the estimation performance. On the other hand, feedback and integral control may cause the colored measurement noise, which, as shown in Fig.1, can be regarded as the excited output by the white noise ζ (t) through integral and feedback parts. For example, the colored noise model (3) has been widely used in multipath parameters estimation problem of weak GPS signal [8]. ζ (t )
+
−
∫
v(t )
ψ (t )
Figure 1. The colored noise caused by feedback
Our aim is to design the GA filter, including the filter and smoother for the system in (1)-(2). For this purpose, a new augmented measurement zk* is artificially generated to decorrelate the colored measurement noise [19]
1). the one-step predicted PDF of the state xk conditioned on Z k*−1 is Gaussian, i.e.
⎧⎪zk −ψ k ,k −1zk −1 = hk ( xk ) −ψ k ,k −1hk −1 ( xk −1 ) + ζ k −1， k > 1, zk* = ⎨ (4) k = 1. ⎪⎩z1 = h1 ( x1 ) + ζ 0 ,
with the state prediction xˆ k +1|k and covariance Pk +1|k
Hence, the original measurement model in (2) is transformed into the new augmented measurement one in (4) with delayed state. Here, wk and ζ k are uncorrelated zero-mean Gaussian white noises, zk* ∈ m is the augmented measurement vector at time k . Consequently, the problem of designing the GA estimators for the nonlinear system in (1)-(2) with the colored measurement noise is transformed into that of deriving the GA ones for the system in (1) and (4) with both uncorrelated white noises and delayed state. In other words, we need to find Gaussian approximations to the filtering PDF p(xk | Zk*) for k>0, where Z *j = {zi* }ij=1 denotes the augmented measurements set. Then, in the minimum mean square error (MMSE) sense, the filtering estimates are obtained by computing the first two moments of the above PDFs
2). the one-step predicted PDF of the augmented measurement zk* conditioned on Z k*−1 is Gaussian, i.e.
* k
T k
* k
xˆ k = E[ xk | Z ] , Pk = E[ xk x | Z ] ,
(5)
Remark 2. Maybe, a well-known method for designing the GA filter for the nonlinear system in (1)-(2) with the colored measurement noise is that, by augmenting the state xk with the noise vk , the existing standard GA filter can be extended to be applicable for the considered problem. However, such augmentation method may lead to that the covariance updating becomes ill-conditioned, and further results in the poor estimation accuracy, or even the divergence. The reason is that as shown in following (14), the inverse calculation calls for the positive definiteness of the predicted auto-covariance of the measurement, which is generally guaranteed by assuming that the measurement noise covariance is positive. While such augmentation method would cause no the measurement noise in the observation equation, and the resulting computation of the predicted auto-covariance of the measurement may be non-positive definiteness. This results in the unavailability in computing its inverse, and further causes the estimator to halt its operation. The standard GA filter [10] has been presented for the case that wk and vk are all the white noises and uncorrelated with each other. Here, all the Gaussian approximations to the state posterior filtering PDFs are based on the standard measurekorN ments generated by (2), i.e. Z j = {zi }i= 1 . However, when considering vk is the colored noise as shown in (3), such Gaussian approximations are unavailable for deriving the new GA filter for the system in (1) and (4) since the estimation should be based on the augmented measurements Z *j obtained by (4), instead of Z j . Specially, considering the following analysis below (18) in next Section III, a novel Gaussian assumption is proposed as follows. Assumption 1. The joint posterior PDF of xk −1 , xk and zk* conditioned by Z k*−1 i.e. p( xk −1 , xk , zk* | Z k*−1 ) can be approximated by a Gaussian distribution. Obviously, according to Assumption 1, the following marginal PDFs are also Gaussian, including:
p( xk | Z k*−1 ) = N ( xk ; xˆ k |k −1 , Pk |k −1 ) , k ≥ 1 ,
(6)
xˆ k |k −1 = E[ xk | Z k*−1 ] , Pk |k −1 = E[ xk |k −1 xkT|k −1 | Z k*−1 ] .
(7)
p( zk* | Z k*−1 ) = N ( z k* ; zˆk*|k −1 , Pkz|k* −1 ) , k > 1 ,
(8)
with the prediction zˆk*|k −1 and covariance Pkz*|k −1
zˆk*|k −1 = E[ z k* | Z k*−1 ] , Pkz*|k −1 = E[ z k*|k −1 ( zk*|k −1 )T | Z k*−1 ] . III.
(9)
GAUSSIAN FILTER
A. GA Filtering Framework Define the following covariances ( k ≥ 1 ) Pkxz|k*−1 = E[ xk |k −1 ( zk*|k −1 )T | Z k*−1 ] ,
(10)
Pk −1, k|k −1 = E[ xk −1 xkT|k −1 | Z k*−1 ] .
(11)
Theorem 1 (Analytical computation). Consider the system in (1) and (4), under Assumption 1, the Gaussian approximation of p(xk | Zk*) ( k ≥ 1 ) as the filtering estimation xˆ k and the covariance Pk of the state as the unified form: * xˆ k = xˆ k|k −1 + K k ( zk* − zˆk|k −1 ) ,
(12)
Pk = Pk|k −1 − K k Pk|kz *−1 K kT ,
(13)
K k = Pk|kxz *−1 ( Pk|kz*−1 ) −1 .
(14)
Proof. According to Assumption 1, the joint posterior PDF of xk and zk* conditioned by Z k*−1 is also Gaussian, i.e.
⎛ ⎛ x ⎞ ⎛ xˆ k |k −1 ⎞ ⎞ p( xk , zk | Z k*−1 ) = N ⎜ ⎜ k ⎟ ; ⎜ * ⎟ , M k |k −1 ⎟ , ⎜ zk ⎟ ˆ z ⎝ ⎝ ⎠ ⎝ k |k −1 ⎠ ⎠ =
1
( (2π )
n
Pk |k −1 − Pkxz|k*−1 ( Pkz|k*−1 )−1 ( Pkxz|k *−1 )T
)
1/ 2
⎡ 1 ⎛ xk|k −1 ⎞T ⎛ 0 ⎞⎞⎛ xk|k −1 ⎞⎤ * * ⎛0 ⎥ p zk Zk −1 . (15) exp ⎢− ⎜ * ⎟ ⎜ Mk−|1k −1 −⎜ * z −1 ⎟ ⎟ ⎟⎜ * ⎟ ⎢⎣ 2 ⎝ zk|k −1 ⎠ ⎜⎝ ⎝ 0 (Pk|k −1) ⎠⎠⎝ zk|k−1 ⎠⎥⎦
(
)
⎛ P Pkxz|k *−1 ⎞ where M k |k −1 = ⎜ xzk |k* −1 T ⎟ . From the Bayesian rule [9] ⎜ (P ) Pkz|k*−1 ⎟⎠ ⎝ k |k −1 and the Gaussian computation rules [26], we have p( xk | Z k* ) = with
p( xk , zk* | Z k*−1 ) = N ( xk ; xˆ k , Pk ) , p( zk* | Z k*−1 )
(16)
* xˆ k = xˆ k|k −1 + Pkxz|k*−1 ( Pkz|k* −1 ) −1 ( zk* − zˆk|k −1 ) ,
(17)
Pk = Pk|k −1 − Pkxz|k*−1 (Pkz|k*−1 )−1 (Pkxz|k*−1 )T .
(18)
Then rearranging (17) and (18) by (14) gives (12)-(13). □ In the standard GA filter [15] with the uncorrelated process and measurement white noises, as shown in Fig. 2, only the propagation of the state prediction at time k through the nonlinear measurement transition hk (xk ) , is needed for computing the prediction of the original measurement zk .
= ∫ hk ( xk )N ( xk ; xˆ k |k −1 , Pk |k −1 )dxk −ψ k , k −1 ∫ hk −1 ( xk −1 )N ( xk −1 ; xˆ k −1 , Pk −1 )dxk −1 ,
(25)
Pkz|k*−1 = ∫ hk* ( xka )[hk* ( xka )]T N ( xka ; xˆ ka|k −1 , Pka|k −1 )dxka −zˆk*|k −1 ( zˆk*|k −1 )T + Rk −1 ,
(26)
Pkxz|k*−1 = ∫ xk [hk* ( xka )]T N( xka ; xˆ ka|k −1 , Pka|k −1 )dxka − xˆ k|k −1 (zˆk*|k −1 )T , (27) Pk −1, k|k −1 =∫ xk −1 fkT−1 ( xk −1 )N( xk −1 ; xˆ k −1 , Pk −1 )dxk −1 − xˆ k −1 xˆ kT|k −1 . (28)
p ( xk −1 Z k −1 )
p ( z k | xk )
xˆ k −1 Pk −1
xˆ k|k −1 Pk|k −1
p( xk xk −1 )
k −1 ← k
zˆk|k −1 Pkz|k −1 xˆ k|k −1 Pk|k −1
p( xk Z k −1 )
xˆ k Pk
where hk*(xka )= hk (xk ) −ψk,k−1hk−1(xk−1) .
p ( zk | Z k −1 )
zk
xˆ k |k −1 = E[( f k −1 ( xk −1 ) + wk −1 ) | Z k*−1 ] ,
p ( xk | Z k )
(29)
Pk|k −1 = E[ xk xkT | Zk*−1 ] − xˆk|k −1 xˆkT|k −1 = E[ fk −1 ( xk −1 ) fkT−1 ( xk −1 ) | Zk*−1 ]
Figure 2. Diagram of the standard GA filtering framework
However, when vk is modeled as the colored noise in (3), by substituting (4) into the definition of zˆk*|k −1 and Pkz*|k −1 in (9), we can find that, the derivation of zˆk*|k −1 and Pkz*|k −1 for k > 1 ,is dependent on the computation of not only the propagation result of the filtering estimation at time k−1through measurement transition hk−1(xk−1) , but also the necessary propagation of state prediction at time k through hk (xk ) ,Therefore, define the following augmented state for computing the predictive estimates of the augmented measurement, i.e. xka = ⎡⎣ xkT
Proof. Substituting the dynamic equation in (1) into the definition of xˆ k |k −1 and Pk |k −1 in (7) gives
T
xkT−1 ⎤⎦ ,
(19)
Obviously, from the Assumption 1, the posterior predicted PDF of the augmented state also obeys Gaussian, i.e.
p( xka | Zk*−1 ) = N( xka ; xˆka|k −1, Pka|k −1 ) , k ≥1 ,
(20)
with
⎡ xˆ ⎤ xˆ ka|k −1 = E[ xka | Z k*−1 ] = ⎢ k |k −1 ⎥ , ˆ x ⎣ k −1 ⎦ ⎡ P Pka|k −1 = E[ xka|k −1 ( xka|k −1 )T | Z k*−1 ] = ⎢ k |k −1 ⎣⎢ Pk −1, k|k −1
(21) PkT−1, k|k −1 ⎤ ⎥. Pk −1 ⎦⎥
(22)
Theorem 2 (Nonlinear integrals). Consider the system in (1) and (4), under Assumption 1, the prediction estimates of the state and augmented measurement have the following form: xˆ k |k −1 = ∫ f k −1 ( xk −1 )N ( xk −1 ; xˆ k −1 , Pk −1 )dxk −1 , (23)
+E[wk −1wkT−1 | Zk*−1 ] − xˆ k |k −1 xˆ kT|k −1 , Pk −1, k|k −1 = E[ xk −1 xkT | Z k*−1 ] − xˆ k −1 xˆ kT|k −1
= E[ xk −1 ( f k −1 ( xk −1 ) + wk −1 )T | Zk*−1 ] − xˆ k −1 xˆ kT|k −1 .
zˆk*|k −1 = ∫ hk* ( xka )N ( xka ; xˆ ka|k −1 , Pka|k −1 )dxka
(24)
(31)
Note that wk −1 is zero-mean Gaussian white noise with covariance Qk −1 and is independent of Z k*−1 . Then under Gaussian approximation in (16), we can easily obtain (23)-(24) and (28) from (29)-(31). Similarly, by the fact that ζ k −1 is the zero-mean Gaussian white noise with covariance Rk −1 and is independent of Z k*−1 , under Gaussian approximation of p(xka | Zk*−1) in (20), we can easily get (25)-(27) by inserting the augmented measurement equation in (4) to the definition of zˆk*|k −1 , Pkz*|k −1 in (9) and Pkxz|k*−1 in (10). □ Remark 3. The proposed GA filter in Theorems 1-2 operates by combining the analytical computation in(12)-(14) with the nonlinear integrals in (23)-(28). Moreover, as shown in Fig. 3, the procedure in Theorems 1-2 is a recursion. That means that, given the initial state estimation xˆ 0 and P0 , the state filtering estimates at time k − 1 is used by Theorem 2 for computing the predictive estimates of the state and augmented measurement at time k , which is further input to Theorem 1 for computing the state filtering estimates at time k , then the output of Theorem 1 is the initial value of Theorem 2 for carrying out the following cycle and for proceeding forward to the terminating step ( k
I.
INTRODUCTION
A. Motivation Nonlinear state filtering problem for the discrete-time stochastic systems has attracted considerable interest because of its widespread applications in control, signal processing, computer vision and econometrics [1]-[5]. Usually, a standard assumption in the classical estimation theory is that the process and measurement noises should be Gaussian white ones. However, in actual engineering applications, it is quite common that the measurement noise is coloured, rather than a white one. For example, the glint noise of the measured target in radar tracking system [6], and the gyro random slow drift in inertial navigation system [7], are all coloured, since the sampling frequency is too high to ignore the noise correlation in the time direction. Moreover, in the multipath parameters estimation problem of weak GPS signal [8], the measurement noise can be also regarded as the driven output by the white noise through integral and feedback parts. Therefore, there is a great need to develop the nonlinear estimation approaches with the coloured measurement noise. Based on Bayesian approach [9], the general optimal estimation scheme is given by the functional recursive relations for
Quan Pan Northwestern Polytechnical University Xi’an, China [email protected]
computation of probability density functions (PDFs) of the state conditioned by the measurements. For linear systems, these PDFs can be analytically obtained through constructing the first two order moments (mean and covariance), which results in the well-known Kalman filter (KF). However, for the nonlinear system, the analytical computation of such PDFs is unavailable and intractable since the Bayesian solution requires the propagation of full probability density which results in the computation-intensive in numerical implementation. Approximation is thus necessary [10], and the Gaussian approximation (GA) to such PDFs is widely accepted by the fact that: 1) the estimated mean and covariance of the state is the computation of the first two moments of posterior PDF, and GA just meets this requirement and further guarantees the state estimation accuracy, and 2) the recursive operation is available and the corresponding GA filter is analytical. In other words, the GA method can give an effective balance between estimation accuracy and computation demands, and is very suitable for dealing with the filtering problem for the nonlinear systems with the coloured measurement noise. B. Related Works The most prominent and widely used method for nonlinear systems with the coloured measurement noise is the extended Kalman filter (EKF), which is obtained by first-order linearization of the nonlinear system equations so that the traditional KF with the coloured measurement noise is applied at each step. But the first-order linearization may provide insufficient accuracy, resulting in significant bias, or even divergence in the case of large initial errors or high nonlinearities. Moreover, the derivation of the Jacobian matrix is non-trivial in many applications, especially in high-dimensional state or measurement case. For overcoming the above shortcomings in first-order linearization, many novel GA filters have been proposed as an improvement to the EKF, typically including the unscented Kalman filter (UKF) based on the unscented transformation (UT) [11], the Gauss-Hermit filter (GHF) based on the quadrature rule [1], the divided difference filter (DDF) [12], the central difference filter (CDF) based on the Stirling’s polynomial interpolation [13], the cubature Kalman filter (CKF) based on the spherical-radial cubature rule [14], and sparse-grid quadrature filter (SGQF) [15]. The above methods don’t require evaluation of the Jacobian matrix so that they have the computational complexity similar to the EKF [16] but are much sim-
pler to implement. Moreover, they can achieve at least the second-order accuracy while the EKF is the only first-order. So far, some modification about the above GA filters has been developed for filtering the nonlinear state with the coloured measurement noise. Ma, et al [17] and Yin, et al [18] considered the speech signals with the measurement noise modelled as the multi-order autoregressive (AR) process, and respectively proposed the dual perceptually constrained UKF and the modified unscented particle filter (UPF) for the speech enhancement. Unfortunately, the two algorithms suffered from the limitation that the measurement function must be linear, and thus they could not be extended to the situation that the dynamic and measurement functions are all nonlinear. In addition, Yuan, et al [8] and Xiong, et al [20] modelled the measurement noise as the coloured one with the first-order AR process, and improved the UKF to estimate the multipath parameter of weak signal from global position satellite (GPS) and track the target by using the multi sensors, respectively. However, these improved UKFs in [8] and [20] still used the firstorder linearization to decorrelate the colored measurement noise, so that they have to face the similar shortcomings of the low precision and the non-trivial computation of the Jacobian matrix in EKF. Specially for the strong nonlinearities and highdimension cases, such methods may be fail, or even divergent. C. Results and Organization In this paper, we present the GA filtering problem for the nonlinear stochastic systems with the colored measurement noise, modeled as the first-order autoregressive (AR) process. First, a new augmented measurement is artificially generated to decorrelate the colored noise, and further the problem of deriving the GA filter with the colored noise is transformed into that of designing the GA ones with both the delayed state and the uncorrelated noises cases. Second, by presenting the novel Gaussian assumptions about the posterior PDFs of joint the present state, the delayed state and the augmented measurement, the general GA filtering framework with the delayed state are proposed, which recursively operate by combining the analytical computation and the nonlinear Gaussian integrals. Third, the implementation of the proposed GA filter is hence transformed to the computation of such nonlinear integrals, which is solved by using the spherical-radial cubature rule for developing the new variations, i.e., the CKF with the colored measurement noise. Finally, a simulation example shows the superior performance of the new methods. The proposed GA filter is somewhat different from those in the literature. Comparing with [17]-[18], this paper considers the situation that the dynamic and measurement functions are all nonlinear, and aims to develop a general GA filtering framework such that various variations of this framework can be developed by different numerical technologies, such as UT, polynomial interpolation and cubature rule, etc. In addition, instead of the used linearization in [19]-[20], we apply the augmented measurement approach for decorrealting the coloured noise, in order to facilitate the direct application of the cubature rule for computing the nonlinear Gaussian integrals in second-order accuracy, and to further develop the modified CKF as the variation of the derived GA filtering framework. Consequently, the estimation precision superiority of the proposed CKF to the existing one in [19], is also demonstrated by the following simulation results.
The rest of the paper is organized as follows. The investigated problem is formulated and transformed in Section II. In Sections III-A, the general GA filter with the delayed state in the augmented measurement is derived. Based on the sphericalradial cubature rule, the new CKF algorithms are developed as the variations of the proposed GA filter in Section III-B. The simulation analysis is given in Section IV. Some conclusions are drawn in Section V. D. Notification Throughout this paper, the superscripts “ −1 ” and “ T ” represent the inverse and transpose operations of matrix, respectively; i denotes the matrix determinant; N( x; μ, P) denotes that the variable x obeys Gaussian distribution with mean μ and covariance P ; E[i] denotes mathematical expectation; I denotes the unit matrix; The superscripts “ ∧ ” and “ ∼ ”, used as the hat of random variables, represent the estimate and the estimation error, respectively. For example, xˆ denotes the estimate of variable x and its estimation error is x = x − xˆ .
II.
PROBLEM FORMULATION AND PRELIMINARIES
Consider the discrete-time nonlinear stochastic system xk = f k −1 ( xk −1 ) + wk −1 ,
(1)
zk = hk ( xk ) + v k ,
(2) n
where k ∈{1, 2, } is the discrete time, xk ∈ is the state vector at time k , zk ∈ m is the measurement vector at time k , f k (i) and hk (i) are some known nonlinear functions, wk is the process noise, vk is the colored measurement noise which obeys a first-order autoregressive (AR) model, i.e. vk = ψ k , k −1vk −1 + ζ k −1 .
(3)
Here, ψ k , k −1 is the known correlation parameter, wk and ζ k are uncorrelated zero-mean Gaussian white noises satisfying Εw [ kwlT] T = Qkδkl and Ε[vkvl ]= Rkδkl respectively, where δ kl is the Kronecker delta function, and the initial state x0 is a Gaussian random vector with mean xˆ 0 and covariance P0 , which is independent of ζ k and wk . Remark 1. The AR model (3) has been frequently considered for describing the colored noise in the practical applications, including the radar target tracking [21], the inertial navigation [22], the speech processing [23], the channel and spectral estimations [24]-[25]. The reasons causing the colored measurement noise are analyzed as follows. On the one hand, the measurement frequency is high enough so that the correlation between the successive samples of the measured noise cannot be ignored without degrading the estimation performance. On the other hand, feedback and integral control may cause the colored measurement noise, which, as shown in Fig.1, can be regarded as the excited output by the white noise ζ (t) through integral and feedback parts. For example, the colored noise model (3) has been widely used in multipath parameters estimation problem of weak GPS signal [8]. ζ (t )
+
−
∫
v(t )
ψ (t )
Figure 1. The colored noise caused by feedback
Our aim is to design the GA filter, including the filter and smoother for the system in (1)-(2). For this purpose, a new augmented measurement zk* is artificially generated to decorrelate the colored measurement noise [19]
1). the one-step predicted PDF of the state xk conditioned on Z k*−1 is Gaussian, i.e.
⎧⎪zk −ψ k ,k −1zk −1 = hk ( xk ) −ψ k ,k −1hk −1 ( xk −1 ) + ζ k −1， k > 1, zk* = ⎨ (4) k = 1. ⎪⎩z1 = h1 ( x1 ) + ζ 0 ,
with the state prediction xˆ k +1|k and covariance Pk +1|k
Hence, the original measurement model in (2) is transformed into the new augmented measurement one in (4) with delayed state. Here, wk and ζ k are uncorrelated zero-mean Gaussian white noises, zk* ∈ m is the augmented measurement vector at time k . Consequently, the problem of designing the GA estimators for the nonlinear system in (1)-(2) with the colored measurement noise is transformed into that of deriving the GA ones for the system in (1) and (4) with both uncorrelated white noises and delayed state. In other words, we need to find Gaussian approximations to the filtering PDF p(xk | Zk*) for k>0, where Z *j = {zi* }ij=1 denotes the augmented measurements set. Then, in the minimum mean square error (MMSE) sense, the filtering estimates are obtained by computing the first two moments of the above PDFs
2). the one-step predicted PDF of the augmented measurement zk* conditioned on Z k*−1 is Gaussian, i.e.
* k
T k
* k
xˆ k = E[ xk | Z ] , Pk = E[ xk x | Z ] ,
(5)
Remark 2. Maybe, a well-known method for designing the GA filter for the nonlinear system in (1)-(2) with the colored measurement noise is that, by augmenting the state xk with the noise vk , the existing standard GA filter can be extended to be applicable for the considered problem. However, such augmentation method may lead to that the covariance updating becomes ill-conditioned, and further results in the poor estimation accuracy, or even the divergence. The reason is that as shown in following (14), the inverse calculation calls for the positive definiteness of the predicted auto-covariance of the measurement, which is generally guaranteed by assuming that the measurement noise covariance is positive. While such augmentation method would cause no the measurement noise in the observation equation, and the resulting computation of the predicted auto-covariance of the measurement may be non-positive definiteness. This results in the unavailability in computing its inverse, and further causes the estimator to halt its operation. The standard GA filter [10] has been presented for the case that wk and vk are all the white noises and uncorrelated with each other. Here, all the Gaussian approximations to the state posterior filtering PDFs are based on the standard measurekorN ments generated by (2), i.e. Z j = {zi }i= 1 . However, when considering vk is the colored noise as shown in (3), such Gaussian approximations are unavailable for deriving the new GA filter for the system in (1) and (4) since the estimation should be based on the augmented measurements Z *j obtained by (4), instead of Z j . Specially, considering the following analysis below (18) in next Section III, a novel Gaussian assumption is proposed as follows. Assumption 1. The joint posterior PDF of xk −1 , xk and zk* conditioned by Z k*−1 i.e. p( xk −1 , xk , zk* | Z k*−1 ) can be approximated by a Gaussian distribution. Obviously, according to Assumption 1, the following marginal PDFs are also Gaussian, including:
p( xk | Z k*−1 ) = N ( xk ; xˆ k |k −1 , Pk |k −1 ) , k ≥ 1 ,
(6)
xˆ k |k −1 = E[ xk | Z k*−1 ] , Pk |k −1 = E[ xk |k −1 xkT|k −1 | Z k*−1 ] .
(7)
p( zk* | Z k*−1 ) = N ( z k* ; zˆk*|k −1 , Pkz|k* −1 ) , k > 1 ,
(8)
with the prediction zˆk*|k −1 and covariance Pkz*|k −1
zˆk*|k −1 = E[ z k* | Z k*−1 ] , Pkz*|k −1 = E[ z k*|k −1 ( zk*|k −1 )T | Z k*−1 ] . III.
(9)
GAUSSIAN FILTER
A. GA Filtering Framework Define the following covariances ( k ≥ 1 ) Pkxz|k*−1 = E[ xk |k −1 ( zk*|k −1 )T | Z k*−1 ] ,
(10)
Pk −1, k|k −1 = E[ xk −1 xkT|k −1 | Z k*−1 ] .
(11)
Theorem 1 (Analytical computation). Consider the system in (1) and (4), under Assumption 1, the Gaussian approximation of p(xk | Zk*) ( k ≥ 1 ) as the filtering estimation xˆ k and the covariance Pk of the state as the unified form: * xˆ k = xˆ k|k −1 + K k ( zk* − zˆk|k −1 ) ,
(12)
Pk = Pk|k −1 − K k Pk|kz *−1 K kT ,
(13)
K k = Pk|kxz *−1 ( Pk|kz*−1 ) −1 .
(14)
Proof. According to Assumption 1, the joint posterior PDF of xk and zk* conditioned by Z k*−1 is also Gaussian, i.e.
⎛ ⎛ x ⎞ ⎛ xˆ k |k −1 ⎞ ⎞ p( xk , zk | Z k*−1 ) = N ⎜ ⎜ k ⎟ ; ⎜ * ⎟ , M k |k −1 ⎟ , ⎜ zk ⎟ ˆ z ⎝ ⎝ ⎠ ⎝ k |k −1 ⎠ ⎠ =
1
( (2π )
n
Pk |k −1 − Pkxz|k*−1 ( Pkz|k*−1 )−1 ( Pkxz|k *−1 )T
)
1/ 2
⎡ 1 ⎛ xk|k −1 ⎞T ⎛ 0 ⎞⎞⎛ xk|k −1 ⎞⎤ * * ⎛0 ⎥ p zk Zk −1 . (15) exp ⎢− ⎜ * ⎟ ⎜ Mk−|1k −1 −⎜ * z −1 ⎟ ⎟ ⎟⎜ * ⎟ ⎢⎣ 2 ⎝ zk|k −1 ⎠ ⎜⎝ ⎝ 0 (Pk|k −1) ⎠⎠⎝ zk|k−1 ⎠⎥⎦
(
)
⎛ P Pkxz|k *−1 ⎞ where M k |k −1 = ⎜ xzk |k* −1 T ⎟ . From the Bayesian rule [9] ⎜ (P ) Pkz|k*−1 ⎟⎠ ⎝ k |k −1 and the Gaussian computation rules [26], we have p( xk | Z k* ) = with
p( xk , zk* | Z k*−1 ) = N ( xk ; xˆ k , Pk ) , p( zk* | Z k*−1 )
(16)
* xˆ k = xˆ k|k −1 + Pkxz|k*−1 ( Pkz|k* −1 ) −1 ( zk* − zˆk|k −1 ) ,
(17)
Pk = Pk|k −1 − Pkxz|k*−1 (Pkz|k*−1 )−1 (Pkxz|k*−1 )T .
(18)
Then rearranging (17) and (18) by (14) gives (12)-(13). □ In the standard GA filter [15] with the uncorrelated process and measurement white noises, as shown in Fig. 2, only the propagation of the state prediction at time k through the nonlinear measurement transition hk (xk ) , is needed for computing the prediction of the original measurement zk .
= ∫ hk ( xk )N ( xk ; xˆ k |k −1 , Pk |k −1 )dxk −ψ k , k −1 ∫ hk −1 ( xk −1 )N ( xk −1 ; xˆ k −1 , Pk −1 )dxk −1 ,
(25)
Pkz|k*−1 = ∫ hk* ( xka )[hk* ( xka )]T N ( xka ; xˆ ka|k −1 , Pka|k −1 )dxka −zˆk*|k −1 ( zˆk*|k −1 )T + Rk −1 ,
(26)
Pkxz|k*−1 = ∫ xk [hk* ( xka )]T N( xka ; xˆ ka|k −1 , Pka|k −1 )dxka − xˆ k|k −1 (zˆk*|k −1 )T , (27) Pk −1, k|k −1 =∫ xk −1 fkT−1 ( xk −1 )N( xk −1 ; xˆ k −1 , Pk −1 )dxk −1 − xˆ k −1 xˆ kT|k −1 . (28)
p ( xk −1 Z k −1 )
p ( z k | xk )
xˆ k −1 Pk −1
xˆ k|k −1 Pk|k −1
p( xk xk −1 )
k −1 ← k
zˆk|k −1 Pkz|k −1 xˆ k|k −1 Pk|k −1
p( xk Z k −1 )
xˆ k Pk
where hk*(xka )= hk (xk ) −ψk,k−1hk−1(xk−1) .
p ( zk | Z k −1 )
zk
xˆ k |k −1 = E[( f k −1 ( xk −1 ) + wk −1 ) | Z k*−1 ] ,
p ( xk | Z k )
(29)
Pk|k −1 = E[ xk xkT | Zk*−1 ] − xˆk|k −1 xˆkT|k −1 = E[ fk −1 ( xk −1 ) fkT−1 ( xk −1 ) | Zk*−1 ]
Figure 2. Diagram of the standard GA filtering framework
However, when vk is modeled as the colored noise in (3), by substituting (4) into the definition of zˆk*|k −1 and Pkz*|k −1 in (9), we can find that, the derivation of zˆk*|k −1 and Pkz*|k −1 for k > 1 ,is dependent on the computation of not only the propagation result of the filtering estimation at time k−1through measurement transition hk−1(xk−1) , but also the necessary propagation of state prediction at time k through hk (xk ) ,Therefore, define the following augmented state for computing the predictive estimates of the augmented measurement, i.e. xka = ⎡⎣ xkT
Proof. Substituting the dynamic equation in (1) into the definition of xˆ k |k −1 and Pk |k −1 in (7) gives
T
xkT−1 ⎤⎦ ,
(19)
Obviously, from the Assumption 1, the posterior predicted PDF of the augmented state also obeys Gaussian, i.e.
p( xka | Zk*−1 ) = N( xka ; xˆka|k −1, Pka|k −1 ) , k ≥1 ,
(20)
with
⎡ xˆ ⎤ xˆ ka|k −1 = E[ xka | Z k*−1 ] = ⎢ k |k −1 ⎥ , ˆ x ⎣ k −1 ⎦ ⎡ P Pka|k −1 = E[ xka|k −1 ( xka|k −1 )T | Z k*−1 ] = ⎢ k |k −1 ⎣⎢ Pk −1, k|k −1
(21) PkT−1, k|k −1 ⎤ ⎥. Pk −1 ⎦⎥
(22)
Theorem 2 (Nonlinear integrals). Consider the system in (1) and (4), under Assumption 1, the prediction estimates of the state and augmented measurement have the following form: xˆ k |k −1 = ∫ f k −1 ( xk −1 )N ( xk −1 ; xˆ k −1 , Pk −1 )dxk −1 , (23)
+E[wk −1wkT−1 | Zk*−1 ] − xˆ k |k −1 xˆ kT|k −1 , Pk −1, k|k −1 = E[ xk −1 xkT | Z k*−1 ] − xˆ k −1 xˆ kT|k −1
= E[ xk −1 ( f k −1 ( xk −1 ) + wk −1 )T | Zk*−1 ] − xˆ k −1 xˆ kT|k −1 .
zˆk*|k −1 = ∫ hk* ( xka )N ( xka ; xˆ ka|k −1 , Pka|k −1 )dxka
(24)
(31)
Note that wk −1 is zero-mean Gaussian white noise with covariance Qk −1 and is independent of Z k*−1 . Then under Gaussian approximation in (16), we can easily obtain (23)-(24) and (28) from (29)-(31). Similarly, by the fact that ζ k −1 is the zero-mean Gaussian white noise with covariance Rk −1 and is independent of Z k*−1 , under Gaussian approximation of p(xka | Zk*−1) in (20), we can easily get (25)-(27) by inserting the augmented measurement equation in (4) to the definition of zˆk*|k −1 , Pkz*|k −1 in (9) and Pkxz|k*−1 in (10). □ Remark 3. The proposed GA filter in Theorems 1-2 operates by combining the analytical computation in(12)-(14) with the nonlinear integrals in (23)-(28). Moreover, as shown in Fig. 3, the procedure in Theorems 1-2 is a recursion. That means that, given the initial state estimation xˆ 0 and P0 , the state filtering estimates at time k − 1 is used by Theorem 2 for computing the predictive estimates of the state and augmented measurement at time k , which is further input to Theorem 1 for computing the state filtering estimates at time k , then the output of Theorem 1 is the initial value of Theorem 2 for carrying out the following cycle and for proceeding forward to the terminating step ( k
