A comparison of estimation accuracy by the use of KF, EKF & UKF filters

Transcription

1 Computational Methods and Eperimental Measurements XIII 779 A comparison of estimation accurac b the use of KF EKF & UKF filters S. Konatowski & A. T. Pieniężn Department of Electronics Militar Universit of Technolog Poland Abstract This paper considers the prlem of appling the Kalman filters to nonlinear sstems. The Kalman filter (KF) is an optimal linear estimator when the process noise and the measurement noise can be modeled b white Gaussian noise. The KF onl utilies the first two moments of the state (mean and covariance) in its update rule. In situations when the prlems are nonlinear or the noise that distorts the signals is non-gaussian the Kalman filters provide a solution that ma be far from optimal. Nonlinear prlems can be solved with the etended Kalman filter (EKF). This filter is based upon the principle of lineariation of the state transition matri and the servation matri with Talor series epansions. Eploiting the assumption that all transformations are quasi-linear the EKF simpl makes linear all nonlinear transformations and substitutes Jacian matrices for the linear transformations in the KF equations. The lineariation can lead to poor performance and divergence of the filter for highl non-linear prlems. An improvement to the etended Kalman filter is the unscented Kalman filter (UKF). The UKF approimates the prabilit densit resulting from the nonlinear transformation of a random variable. It is done b evaluating the nonlinear function with a minimal set of carefull chosen sample points. The posterior mean and covariance estimated from the sample points are accurate to the second order for an nonlinearit. The paper presents a comparison of the estimation qualit for two nonlinear measurement models of the following Kalman filters: covariance filter (KF) etended filter (EKF) and unscented filter (UKF). Kewords: nonlinear model discrete Kalman filter etended Kalman filter unscented Kalman filter integrated navigation sstem. doi: /cmem070761

2 780 Computational Methods and Eperimental Measurements XIII 1 Introduction The classical Kalman filter is used for linear dnamic sstems [1] moreover etended Kalman filter EKF for nonlinear sstems [1 ] or unscented Kalman filter UKF [2 4 8]. Unscented Kalman filter with comparison to EKF is not based on linear model but operates on the statistical parameters of the measurement and state vectors that are subsequentl nonlinearl transformed. The unscented Kalman filter is based on the unscented transform (UT). Kalman filtration [1 ] is based on the following models of state and measurement vectors respectivel: ( k 1 ) ( k) ( k) ( k) for ( k) ~ N ( k) + = f u w w 0 Q ( k 1 ) ( k) ( k) for ( k) ~ N ( k) + = h v v 0 R. (1) Vector (k) is n-dimensional state vector in the moment k (k) is p-dimensional measurement vector in the moment k f( u w) denotes nonlinear state function describing dnamic behavior of the sstem between k+1 and k moments u is the input sstem vector w is the noise state vector Q is the covariance matri of the noise state (denotes uncertaint in the dnamic model during transition from k+1 to k moments h( v) denotes nonlinear measurement function v p-dimensional vector of measurement noise R is covariance matri of measurement errors with dimensions p p P is covariance matri of state vector with dimensions n n. 2 Object dnamics and measurement models In this paper process model is described b state vector in the following form: T p p p v v v (2) = p v where p is three-dimensional position vector (in Cartesian coordinates). Vector v is three-dimensional speed vector. State matri has the form: F I ti = 0 I () where t is the time step between moments k and k+1 I is identit matri. In this case the state vector (2) and state matri () are identical for covariance filter (KF) etended filter (EKF) and unscented filter (UKF). 2.1 Sstem with constant velocit The ject moves with constant velocit (acceleration is ero). Velocit components (v ) are additive Gaussian noise. Fig.1 presents analed model in the graphic form.

3 Computational Methods and Eperimental Measurements XIII 781 Object (p p p ) φ r θ Observer Figure 1: Positioning principle. The measured parameters in the measurement model are: r distance from ject to server (radar) θ aimuth φ elevation. Dependenc between ject position in the Cartesian coordinates and measurements in spherical coordinates is described b nonlinear function. Thus the measurement vector is given b: ( ) ( ) T arccos = r θ ϕ p arctan p p p p p p p p. (4) Measurement matri for discrete and unscented filters is given b: [ ] H = I 0. (5) The vector function h(*) has the following form: [ r θ ϕ] T h =. (6) In the case of etended filter lineariation of the measurement function h for each measurement step b the use of partial derivative relativel to all elements of state vector should be made. The final measurement matri for etended Kalman filter is as follows: p r p r p r h H = ( ) p ( ) p ( ) = = k r p r p (7) 2 2 pp p p r p r r p r r r p where Cartesian ject coordinates are given b: p = rcosθ sinϕ 2.2 Sstem with comple movement p = rsinθcosϕ p = rcosϕ. As an eample of comple dnamics the ject movement around ais is presented. This situation causes nonlinear relationships both in state and measurement matri. Figure 2 in detail illustrates considered ject.

4 782 Computational Methods and Eperimental Measurements XIII Figure 2: Positioning principle. One can see that a nonlinear relation eists between measurements from the sstem and elements of the motion: tanθ = v v v = vsinθ v = v cosθ v = v + v. (8) 2 2 In the presented sstem initial values of vector state are described b the followings: [ r h v ] T (0) = (9) Nonlinear state functions (in the moment k) are defined as follows: ( ) ω ( ) ( ) ω ( ) r cos arctan p p + k t f1 f r sin arctan p p + k t 2 f p ( k) + v ( k) t f ( k ) = = (10) f4 v sin arctan ( p p ) ω ( k) t + f 5 v cos arctan ( p p ) + ω ( k) t f 6 v ( k) where the aimuth and the angular velocit of the ject can be calculated via the following formulas: ( k+ 1) = ( k) + ( k) t ( k) v ( k) r ( k) θ θ ω ω = for r = p + p. (11) 2 2 This nonlinear equation requires lineariation which in etended Kalman filter is performed around the estimated ject s trajector. For the EKF state matri has been calculated as a matri of derivatives of nonlinear f(*) function with respect to the components of the state vector. where: F = (12)

5 Computational Methods and Eperimental Measurements XIII = 2 0 = 1 p v tp p = cosγ + r + sin γ 2 r r pβ p 1 v tp 1 = cosγ + r sin γ r r pβ 1 tv tv 1 1 = sinγ = sinγ v v 2 p p v tp = sinγ r + cosγ 2 r pβ r p 2 1 v tp = sin γ + r cosγ r pβ r 2 tv tv 2 2 = cosγ = cosγ v v = 1 = t p 4 v tp 4 1 v tp = v cosγ 2 = v cos γ pβ r pβ r 4 ( r 4 sinγ + v tcosγ) = v r v ( r γ + v t γ) sin cos 4 = r v v 4 5 p v tp 5 v tp 1 = v + sinγ 2 = v cos γ pβ r r pβ 5 ( r 5 cosγ v tsinγ) = v r v 6 ( r γ v t γ) cos sin 5 = r v for: arctan ( ) 6 6 γ = p p + t v r 6 v p p β = = 1 For discrete and unscented Kalman filter state matri has a form given b (). An servation matri H in the measurement model is identical as in the first model (11).

6 784 Computational Methods and Eperimental Measurements XIII Simulation results The accurac comparisons have been eamined b the use of simulation in the Matlab environment. In order to ensure the same conditions research of filters were realied with identical form of state vector covariance matri Q measurement vector R and initial state vector covariance matri P(0) in both sstems. Similarl to Julier et al [4] the following parameters of unscented transform have been assumed: λ = β = 1 κ =. Furthermore the following values of noise covariance matri have been applied: 2 diag [ m ] 0 Q = 2 2 (1) 0 diag [ 0.49 ms ] covariance matri of measurement noise: R (14) = diag m deg deg initial covariance matri of vector state errors: P ( 0) 2 [ ] diag 1 m = diag [ 1 ms ].1 Eamination of a sstem with constant velocit 0. (15) Eamination results are presented for covariance filter (DKF green line) etended filter (EKF red line) and for unscented filter (UKF blue line). The eaminations results include values of mean square error (mse) of estimated state vector: T ( ˆ ) ( ˆ ) mse *KF = real real n (16) and covariance error P according to Kalman filtering theor estimated component of state vector (Fig. 5): ( k ) ( k) ( k) ( k) T ( k) cov *KF = P + 1 = F P F + Q. (17) Mean square error and covariance error of speed components (Fig. 6 8) has also been done..2 Eamination of a sstem with comple movement The sstem eamination conditions are the same as for sstem with constant velocit. Results for covariance filter DKF etended filter EKF and unscented filter UKF are presented. The include values of mse error of estimated state vector and covariance error estimated components of the position (Fig. 9 11) and values of mse error and covariance error of speed components (Fig ).

7 Computational Methods and Eperimental Measurements XIII 785 Figure : Mean square error of the estimated component p and covariance error P p of the position. Figure 4: Mean square error of the estimated component p and covariance error P p of the position. Figure 5: Mean square error of the estimated component p and covariance error P p of the position. Figure 6: Mean square error of estimated v and covariance error P v of speed component Figure 7: Mean square error of estimated v and covariance error P v of speed component. Figure 8: Mean square error of estimated v and covariance error P v of

8 786 Computational Methods and Eperimental Measurements XIII Figure 9: Mean square error of the estimated component p and covariance error P p of the position. Figure 10: Mean square error of the estimated component p and covariance error P p of the position. Figure 11: Mean square error of the estimated component p and covariance error P p of the position Figure 12: Mean square error of estimated v and covariance error P v of Figure 1: Mean square error of estimated v and covariance error P v of Figure 14: Mean square error of estimated v and covariance error P v of

9 Computational Methods and Eperimental Measurements XIII 787 The estimation of ject speed in the Cartesian coordinates (Fig ) and determination of mean square error and covariance error of speed components (Figs ) has also been done. Figure 15: Estimated v speed component. Figure 16: Estimated v speed component. Figure 17: Estimated v speed component. Figure 18: Mean square error of estimated v and covariance error P v of Figure 19: Mean square error of estimated v and covariance error P v of Figure 20: Mean square error of estimated v and covariance error P v of

10 788 Computational Methods and Eperimental Measurements XIII. Analsis of simulation results For nonlinear measurement model etended and unscented filters estimate the ject position nearl identicall. Discrete Kalman filter is becoming nonoptimal filter in the sense of minimiing the mean square error. When the mean square error is included within the area determined b the covariance error of the estimated vector state then filter is performing correctl. Last principle is satisfied for etended and unscented Kalman filters but not for DKF. During speed estimation one can see that difference between etended and unscented filters is minimal. UKF gives smaller errors what results from nature of speed components which are band-limited Gaussian processes. The bigger are the jumps of noise values the worse of etended Kalman filter performance is. 4 Conclusions Results of estimation using Discrete and Etended and Unscented Kalman Filter for sstem with constant velocit and for sstem with comple movement show that Unscented Kalman Filter operating as algorithm of data processing in sstem with nonlinear dnamics guarantees the best qualit. Nonlinear transform makes Discrete Kalman Filter not to be optimal in sense of minimum mean square error. Loss stabiliation in EKF is possible for long measurement steps whereas decreasing of measurement steps enlarges computational costs as a result of complicated calculations of Jacians. UKF algorithm does not require calculation of Jacians which simplifies its compleit. The Unscented Kalman Filter provides effective estimation in case of strongl nonlinear models what recommends its use in practice. References [1] Brown R.G. Hwang P.Y.C.: Introduction to Random Signals and Applied Kalman Filtering with MATLAB Eercises and Solutions. John Wile & Sons Canada [2] Gordon N.J. Ristic B. Arulampalam S.: Beond the Kalman Filter Particle Filters for Tracking Applications. Artech House London [] Grewal M.S. Andrews A.P: Kalman filtering Theor and Practice Using MATLAB. John Wile & Sons Canada [4] Julier S.J. Uhlmann J.K. Durrant-Whte H.F.: A New Method for the Nonlinear Transformation of Means and Covariances in Filters and Estimators. IEEE Transactions on Automatic Control vol. 45 no. March 2000 pp [5] van der Merwe R. Wan E.A.: The Square-root Unscented Kalman Filter for State and Parameter-estimation. Proceedings of International Conference on Acoustics Speech and Signal Processing vol. 6 Salt Lake Cit Ma 2001 pp

11 Computational Methods and Eperimental Measurements XIII 789 [6] van der Merwe R. Wan E.A.: The Unscented Kalman Filter. Department of Electrical and Computer Engineering Oregon Graduate Institute of Science and Technolog Beaverton Oregon [7] Wan E.A. van der Merwe R.: The Unscented Kalman Filter to appear in Kalman Filtering and Neural Networks. Chapter 7. Edited b Simon Hakin John Wile & Sons USA [8] Wan E.A. van der Merwe R.: The Unscented Kalman Filter for Nonlinear Estimation. Proc. IEEE Smp. Adaptive Sstems for Signal Proc. Communication and Control (AS-SPCC) Lake Louise Alberta Canada Octer 2000 pp