Nonlinear Estimation and Multiple Sensor Fusion Using ... - Google Sites

> IEEE SIGNAL PROCESSING LETTERS, 2008, DEOK-JIN LEE, SPL-05409-2008.R1

1

Nonlinear Estimation and Multiple Sensor Fusion Using Unscented Information Filtering Deok-Jin Lee, Member, IEEE

Abstract—This paper represents a new unscented information filtering algorithm for nonlinear estimation and multiple sensor information fusion. The proposed information fusion algorithm is derived by embedding the unscented transformation method used in the sigma point filter into the extended information filtering architecture. The new information filter achieves not only the accuracy and robustness of the sigma point filter, but also the flexibility of the information filter for multiple sensor estimation. Performance comparison of the proposed filter with the extended information filter is demonstrated through a target-tracking simulation study.

Index Terms— multiple sensor estimation, sensor data fusion, sigma point filtering, unscented information filtering

I. INTRODUCTION

S

ENSOR data fusion techniques are widely used in various applications such as target tracking, surveillance, robot navigation, signal and image processing, and large-scale systems [1]. It can be loosely defined as how to best extract useful information

from multiple sensor observations, and the Kalman filter (KF) has been used extensively in the processing of multiple sensor data. The KF, however, gives rise to a high computational load when all sensor measurements are processed centrally to yield a solution [2]. On the other hand, information filtering, which is essentially a Kalman filter expressed in terms of the inverse of the covariance matrix, has been widely used in multiple sensor estimation and control applications [3], [4] due to its advantages over the standard Kalman filter; the structure of the information estimation is computationally simpler than the KF update equations, and it is easily initialized compared to the KF algorithms without knowing a priori information of the state of the systems [4]. For nonlinear estimation problems, the information filter can be extended by applying a linearized estimation algorithm used in the extended Kalman filter (EKF), which is called the extended information filtering (EIF) [4]. However, some of the drawbacks inherited from the EKF can affect the EIF in terms of the truncation errors due to the approximation in the first and second order

The author is with the Unmanned Systems Lab in the Department of Mechanical Engineering, Naval Postgraduate School, Monterey, CA, 93943. (phone: 831-656-2202; fax: 831-656-2313; e-mail: [email protected]/[email protected]) .

> IEEE SIGNAL PROCESSING LETTERS, 2008, DEOK-JIN LEE, SPL-05409-2008.R1

2

moments. The drawbacks mentioned above can be compensated for by utilizing the sigma point filters (SPFs) including the unscented Kalman filter [6] and the divided difference filter [7]. Since the advent of the SPFs, many applications and extensions of the sigma point filtering techniues also have been made in order to enhance the performance of the nonlinear estimation and sensor fusion problems [8]-[11]. For a decentralized estimation, the sigma point information filter [12] was proposed by utilizing the statistical linear regression Kalman filtering methodology for the linearization of stochastic process and measurement equations. In this paper, an unscented information filtering (UIF) algorithm is derived by embedding a statistical linear error propagation technique, based on the unscented transformation used in the SPF, into the EIF architecture for nonlinear estimation and multiple sensor fusion problems. The motivation behind this paper comes from the fact that the unscented filtering provides a more accurate estimate than the EIF for nonlinear estimation problems, and the simplicity of the information filtering architecture makes it suitable for multiple sensor estimation applications. The remainder of this paper is organized as follows. Section II presents an overview of the extended information filter. In section III, the unscented information filtering algorithm for nonlinear estimation is derived, and is further extended for multiple sensor estimation. Finally, simulation results are presented in Section V.

II. EXTENDED INFORMATION FILTERING Consider discrete-time nonlinear dynamic and measurement equations x k +1 = f (x k , k ) + w k

(1)

z k = h( x k , k ) + v k

(2)

where x k ∈ ℜn and z k ∈ ℜm are the state vector and the observation vector, respectively, and w k ∈ ℜq ∼ N ( 0, Q k ) and v k ∈ ℜl ∼ N ( 0, R k ) denote the noise vectors with zero-mean and white Gaussian sequences. The extended information filtering

(EIF) [1] for the nonlinear system is derived from the extended Kalman filtering (EKF) algorithm in terms of the Fisher information matrix J k

Pk−|1k and the information state vector yˆ k |k

Yk |k

Pk−|1k = J k

(3)

yˆ k |k

Pk−|1k xˆ k |k = Yk |k xˆ k |k

(4)

The update equations for the information matrix and the information state vector are obtained by Yk |k = Pk−|1k −1 + HTk R −k 1H k = Yk |k −1 + I k

(5)

> IEEE SIGNAL PROCESSING LETTERS, 2008, DEOK-JIN LEE, SPL-05409-2008.R1 yˆ k |k = yˆ k |k −1 + HTk R k−1[υk + H k xˆ k |k −1 ] = yˆ k |k −1 + i k

3

(6)

where i k is the information state contribution and I k is its associated information matrix defined by ik

HTk R k−1[υk + H k xˆ k |k −1 ]

(7)

HTk R k−1H k

(8)

Ik

and H k is the partial of the nonlinear measurement equation, and υk is the innovation vector, υk = z k − h(xˆ k |k −1 , k ) . The predicted information state vector and covariance are obtained from yˆ k |k −1 = (Pk |k −1 ) −1 xˆ k |k −1 = Yk |k −1f (xˆ k −1|k −1 , k − 1) (Pk |k −1 ) −1 = [Fk Pk −1|k −1FkT + Q k ]−1

Yk |k −1

(9) (10)

where Fk is a partial of the nonlinear dynamic equation.

III. UNSCENTED INFORMATION FILTERING In this section, an unscented information filtering (UIF) algorithm is developed by embedding a statistical linear error propagation approach based on the unscented transformation into the extended information filtering structure. A. Unscented Information Filtering

An augmented state vector xˆ ak |k ∈ ℜn + q along with noise variables and the corresponding augmented covariance matrix is defined ⎡ xˆ ⎤ ⎡P xˆ ak|k = ⎢ k ⎥ , Pka|k = ⎢ k ˆ w ⎣ k⎦ ⎣0

0 ⎤ Q k ⎦⎥

(11)

i = 1,… , na

(12)

A set of weighted sigma points Xia, k is generated by X0,a k = xˆ ak |k

( −(

Xia, k = xˆ ka|k +

( na + λ ) Pka|k

Xia, k = xˆ ak |k

( na + λ ) Pka|k

), ), i

i

i = na + 1,… , 2na

where λ = α 2 (na + κ ) − na is a scaling parameter with the constant parameters 0 ≤ α ≤ 1 and κ = 3 − na [6]. The corresponding weights for the mean and covariance are defined by

> IEEE SIGNAL PROCESSING LETTERS, 2008, DEOK-JIN LEE, SPL-05409-2008.R1 ⎧⎪λ / (na + λ ) Wi ( m ) = ⎨ ⎪⎩1/ {2(na + λ )}

i=0 i = 1,… , 2na

⎪⎧λ / (na + λ ) + (1 − α + β ) =⎨ ⎪⎩1/ {2(na + λ )} 2

Wi

(c )

4

i=0

(13)

i = 1,… , 2na

where β is a third parameter for incorporating extra higher order effects [9]. Then, the information prediction equations are derived by implementing the unscented transformation [6] into (9) and (10) as 2 na

yˆ k +1|k = Yk +1|k ∑ Wi ( m )Xix, k +1

(14)

Yk +1|k = ( Pk +1|k )

(15)

i =0

−1

where Xix, k +1|k is the predicted sigma point vector obtained from Xix, k +1 = f ( Xix, k , Xiw, k , k ) , and the predicted state covariance matrix is computed by 2 na

Pk +1|k = ∑ Wi ( c ) [X ix,k +1 − xˆ k +1|k ][X ix,k +1 − xˆ k +1|k ]T

(16)

i =0

Recall that the derivation of the update equations in the EKF is based on the inverse of the covariance matrix and the linearized measurement equation. The UKF update equation, however, is not an explicit function of the measurement matrix H k +1 , thus the unscented Kalman filtering algorithm can be directly embedded into the extended information update equations. Instead, based on the assumption that the nonlinear measurement equation in (2) can be mapped into a function of its statistical estimates such as mean and variances, the information update equations of the EIF can be reformulated by utilizing the statistical linear error propagation methodology [11]. First, using the error propagation the observation covariance and its cross-correlation covariance are approximated by T ˆ ˆ PkYY +1| k = E [( z k +1 − z k +1| k )( z k +1 − z k +1| k ) ]

H k +1Pk +1|k HTk +1 T ˆ ˆ PkXY +1| k = E [( x k +1 − x k +1| k )( z k +1 − z k +1| k ) ]

Pk +1HTk +1

(17)

(18)

where z k +1 = h(x k +1 ) and H k +1 is the linearized measurement matrix. Now, multiplying the predicted covariance and its inverse term on the right side of the information matrix equation in (8) and replacing Pk +1HTk +1 with PkXY +1| k leads to the following I k +1 = HTk +1R k−1+1H k +1 = (Pk +1|k ) −1 Pk +1|k HTk +1R k−1+1H k +1| k (Pk +1|k )T (Pk +1|k ) −T −1

XY k +1

(Pk +1|k ) P

R

−1 k +1

XY T k +1

(P

) (Pk +1|k )

−T

(19)

> IEEE SIGNAL PROCESSING LETTERS, 2008, DEOK-JIN LEE, SPL-05409-2008.R1

5

where the predicted state covariance matrix Pk +1|k is obtained from (16), and the cross-correlation matrix PkXY +1| k is obtained by PkXY +1| k

2 na

∑W

(c )

i

i =1

[Xix, k +1|k − xˆ k +1|k ][Yi , k +1|k − zˆ k +1|k ]T

(20) 2 na

where Yi , k +1 = h(Xix, k +1 ) and the predicted measurement vector zˆ k +1|k is obtained by zˆ k +1|k = ∑ Wi ( m ) Yi , k +1 . Similarly, the i =0

information state contribution i k can be rewritten by i k +1 = HTk +1R k−1+1[υk +1 + H k +1xˆ k +1|k ] = (Pk +1|k ) −1 Pk +1|k HTk +1R −k 1+1[υk +1 + H k +1 (Pk +1|k )T (Pk +1|k ) −T xˆ k +1|k ] −1

XY k +1| k

(Pk +1|k ) P

R

−1 k +1

[υk +1 + (P

XY T k +1| k

(21)

−T

) (Pk +1|k ) xˆ k +1|k ]

Now, to make the information contribution and its associated information matrix equations compatible to those of the EIF, a pseudo-measurement matrix Hk +1 is defined as HkT+1

(P ) k +1| k

−1

PkXY +1|k

(22)

Then, in term of the pseudo-measurement matrix Hk +1 the information state contribution and matrix equations are expressed by i k +1 = HkT+1R −k 1+1[υk +1 + Hk +1xˆ k +1|k ] = HkT+1R −k 1+1[z k +1 − h(xˆ k +1|k ) + Hk +1xˆ k +1|k ] I k +1 = HkT+1R −k 1+1Hk +1

(23)

(24)

Based on the above results, it is seen that there exists a mapping which can approximate the nonlinear measurement equation in (2) in terms of the statistical error variances and its mean as z k +1 = h(x k +1 , k + 1) Hk +1x k +1 + u k +1

(25)

where u k +1 = h(xˆ k +1|k ) − Hk +1xˆ k +1|k is a measurement residual term. The mapping in (25) is verified by showing that the information contribution terms in (23) and (24) are obtained directly by implementing the transformed function of (25) into (7) and (8). Finally, the update equations for the unscented information filter can be obtained directly by implementing the pseudo information contribution terms of (23) and (24) into (5) and (6), and the update state estimate is computed by xˆ k +1|k +1 = Pk +1|k +1yˆ k +1|k +1 using (2). B. Multiple Sensor Estimation The unscented information filter is further extended to multiple sensor estimation to increase the reliability of the estimation. Suppose an observation vector z s , k +1 is available from N different sensor sites and each sensor observes a common state according to the local observation model expressed by

> IEEE SIGNAL PROCESSING LETTERS, 2008, DEOK-JIN LEE, SPL-05409-2008.R1 z s , k +1 = Hs , k +1x k +1 + u s , k +1 + v s , k +1 , s = 1,… , N

6 (26)

where the noise vector v s , k +1 is assumed to be white Gaussian and uncorrelated between sensors. The variance of a composite observation noise vector v k +1 comprising a stacked vector of observations is expressed in terms of the block diagonal matrix, R k +1 = diag ([R1, k +1 ,… , R N , k +1 ]) . Then, each local information state contribution i s and its associated information matrix I s at T

the s sensor site are described by i s , k +1 = HsT, k +1R −s ,1k +1[υs , k +1 + Hs , k +1xˆ k +1|k ]

(27)

I s , k +1 = HsT, k +1R −s ,1k +1Hs , k +1

(28)

Since the information contribution terms have group-diagonal structure in terms of the innovation and measurement matrix, the update equations for the multiple sensor estimation and data fusion are expressed by a linear combination of the local information contribution terms as N

yˆ k +1|k +1 = yˆ k +1|k + ∑ HsT, k +1R −s ,1k +1[υs , k +1 + Hs , k +1 xˆ k +1|k ]

(29)

s =1

N

Yk +1|k +1 = Yk +1|k + ∑ HsT, k +1R −s ,1k +1Hs , k +1

(30)

s =1

On the other hand, the prediction equations are calculated by using (14) and (15) for the multiple sensor estimation problem. IV. SIMULATION EXAMPLE Consider the target tracking of a re-entry vehicle entering into an atmosphere from space. The state vector x ∈ ℜ5×1 consists of the position, velocity, and a parameter related to the aerodynamic force, i.e., x = [x y x y γ ]T . The equations of motion of the vehicle are expressed by ⎧⎪ ( r0 − r v = β 0 exp {γ } exp ⎨ ⎩⎪ h0 γ = w3

) ⎫⎪ v ⎬ ⎭⎪

v−

μ r

3

r + wa

(31)

where r = x 2 + y 2 is the distance from the center of the earth, and v = x 2 + y 2 is the speed of the vehicle. The parameter values used in this study are β 0 = −0.59783 , h0 = 13.406 km , μ = 3.9860 × 105 km3 / s 2 , and r0 = 6378 km . The process noise vector is defined by w = [wTa , w3 ]T with zero-mean white Gaussian processes. The motion of the vehicle is measured by radars located at each position, ( xm , s , ym , s ) and the observations of each radar consists of a range and bearing angle obtained at 10 Hz . The measurement equations of each sensor site s are given by

> IEEE SIGNAL PROCESSING LETTERS, 2008, DEOK-JIN LEE, SPL-05409-2008.R1 rm , s =

( x − x ) + (y − y ) 2

m, s

m, s

2

7

+ v1, s

(32)

⎛ y − y m, s ⎞ ⎟⎟ + v 2, s ⎝ x − x m, s ⎠

θ m , s = tan −1 ⎜⎜

(33)

where v k , s = [v1, s v 2, s ]T is the measurement noise vector. The initial true state value vector for the target trajectory and the initial estimate vector for the filtering are given by

x0 = [ 6500.40km, 349.14 km, − 1.8093 km/s, − 6.7967 km/s, 0.69320]

(34)

xˆ 0 = [ 6499.94 km, 349.11 km, − 1.8091km/s, − 6.7962 km/s, 0.69315]

(35)

T

T

The a priori state covariance matrix is given by P0 = diag ([10−6 10−6 10−6 10−6 1]) , and the process noise covariance matrix is set to Q(t ) = diag[2.4064 ×10−5 2.4064 × 10−5 10−6 ] . Tracking radar sites are located at ( xm ,1 = 6374 km, ym ,1 = 0.0 km) and ( xm ,2 = 6375 km, ym ,2 = 0.0 km) with

each

sensor

noise

covariance

matrix,

R1, k = diag ([0.032 km 2 0.02 2 deg 2 ])

and

R 2, k = diag ([0.04 2 km 2 0.02 2 deg 2 ]) , respectively.

The performance of the proposed unscented information filter is compared with the extended information filter in terms of the estimation accuracy and convergence for multiple sensor estimation using a set of sensor obervations. The filters use the same simulation conditions mentioned in the above, and the parameters used for the UIF are α = 10−3 , and β = 2 . Fig. 1 describes the position estimation errors from the EIF and the UIF, where the convergence of the UIF is faster than the EIF and also the estimation accuracy of the UKF is better than that of the EIF. This is because the UIF is based on the unscented transformation which provides more accurate prediction performance than the first-order approximation in the EIF. A similar result on the velocity estimation is shown in Fig. 2. The parameter estimate related to the atmospheric drag is shown is Fig. 3, where accurate and fast converged parameter estimation is achieved by the UIF. The simulation results indicate that the performance of the UIF is superior to the EIF in terms of not only the estimation accuracy, but also the fast convergence.

V. CONCLUSION A new unscented information fusion algorithm is derived by embedding the unscented transformation into the extended information filtering architecture by utilizing the statistical linear error propagation method. The unscented information filter is further extended for the multiple sensor estimation. The simulation results show that the unscented information filter provides not only accurate and robust estimation, but also the flexibility of the information filter needed for multiple sensor fusion.

> IEEE SIGNAL PROCESSING LETTERS, 2008, DEOK-JIN LEE, SPL-05409-2008.R1

8

REFERENCES [1]

A. G. O. Mutambara, Decentralized Estimation and Control for Multi-sensor Systems. Boca Raton, FL, CRC, 1998, ch. 1.

[2]

B. S. Y. Rao, H. F. Durrant-Whyte, and J. A. Sheen, “A Fully Decentralized Multi-Sensor System for Tracking and Surveillance,” The International Journal of Robotics Research, vol. 12, no. 1, pp. 20-44, Feb. 1993.

[3]

M. E. Liggins, C.-Y. Chong, I. Kadar, M. G. Alford, V. Vannicola, and S. Thomopoulos, “Distributed Fusion Architectures and Algorithms for Target Tracking,” Proceedings of the IEEE, vol. 85, no. 1, pp. 95–107, Jan. 1997.

[4]

A. G. O. Mutambara, and H. F. Durrant-Whyte, “Estimation and Control for a Modular Wheeled Mobile Robot,” IEEE Transactions on Control Systems Technology, vol. 8, no. 1, pp. 35-46, Jan. 2000.

[5]

M. Wei, and K. P. Schwarz, “Testing a Decentralized Filter for GPS/INS Integration,” in Proceedings of IEEE Position Location and Navigation Symposium, 1990, pp.429 –435.

[6]

S. J. Julier, J. K. Uhlmann, and H. F. Durrant-Whyte, “A New Method for Nonlinear Transformation of Means and Covariances in Filters and Estimators,” IEEE Transactions on Automatic Control, vol. 45, no 3, pp. 477-482, March 2003.

[7]

M. Norgaard, N. K. Poulsen, and O Rawn, “New Developments in State Estimation for Nonlinear Systems,” Automatica, vol. 36, no. 11, pp. 1627-1638, Nov. 2000.

[8]

D. -J. Lee, and K. T. Alfriend, “Sigma Point Filtering for Sequential Orbit Estimation and Prediction,” Journal of Spacecraft and Rockets, vol. 44, no.2, March-April 2007, pp. 1627-1638.

[9]

R. van der Merwe, E. A. Wan, and S. J. Julier, “Sigma-Point Kalman Filters for Nonlinear Estimation and Sensor Fusion: Applications to Integrated Navigation,” AIAA Guidance, Navigation, and Control Conference and Exhibit, Providence, Rhode Island, Aug. 2004, AIAA 2004-5120.

[10] D. -J. Lee, and K. T. Alfriend, “Adaptive Sigma Point Filtering for State and Parameter Estimation,” AIAA/AAS Astrodynamics Specialist Conference and Exhibit, Providence, Rhode Island, Aug. 2004, AIAA 2004-5101. [11] G. Sibley, G. S. Sukhatme, and L. Matthies, “The Iterated Sigma Point Filter with Applications to Long Range Stereo,” Online Proceedings of the 2nd Robotics: Science and Systems Conference, Philadelphia, Pennsylvania, Aug. 16-19, 2006. [12] T. Vercauteren, and X. Wang, “Decentralized Sigma-Point Information Filters for Target Tracking in Collaborative Sensor Networks,” IEEE Transactions on Signal Processing, vol. 53, no. 8, pp. 2997–3009, Aug. 2005.

> IEEE SIGNAL PROCESSING LETTERS, 2008, DEOK-JIN LEE, SPL-05409-2008.R1

Fig. 1. Position errors from multiple sensor estimation

Fig. 2. Velocity errors from multiple sensor estimation

9

> IEEE SIGNAL PROCESSING LETTERS, 2008, DEOK-JIN LEE, SPL-05409-2008.R1

Fig. 3. Ballistic parameter errors from multiple sensor estimation

10