A NEW EVALUATION PLATFORM FOR NAVIGATION SYSTEMS

Transcription

1 A NEW EVALUATION PLATFORM FOR NAVIGATION SYSTEMS Thomas Hanefeld Sejerøe Niels Kjølstad polsen Ole Ravn Ørsted DTU, Atomation, The Technical Universit of Denmar, Bilding 6, DK-8 Kgs. Lngb, Denmar Informatics Mathematical Modelling, The Technical Universit of Denmar, Bilding, DK-8 Kgs. Lngb, Denmar Abstract: The KALMTOOL toolbo is a set of tools for state estimation for nonlinear sstems. The toolbo contains fnctions for etended Kalman filtering as well as for two new filters called the DD filter the DD filter. It also contains fnction for Uncented Kalman filters as well as three versions of particle filters. The toolbo reqires ver. 6, bt no additional toolboes are reqired. Kewords: State estimation, Kalman filtering, nonlinear sstems Atonomos robots. INTRODUCTION In this paper a newl developed platform for evalation of estimation algorithms, Kalmtool, will be described. The prpose of the platform is to mae evalation of different algorithms for solving nonlinear state estimation problems to enable a comparison with new methods. Dring the wor it was fond that the etended Kalman filter was somewhat inconvenient to se in some of or applications. A small modification of the application sometimes had serios implications on the EKF implementation. Moreover, it was often difficlt to implement. Or problem was that the EKF reqires a linearization of the sstem model. Sometimes this is eas to find bt sometimes it can be prett hard. In an case, it maes things infleible. If a small change is made in the model, one has to wor ot a new set of derivatives. This is particlarl inconvenient in model calibration where certain model parameters are temporaril inclded in the state vector estimated simltaneosl with the actal states. Since it was sggested, the etended Kalman filter (EKF) has ndobtl been the dominating techniqe for nonlinear state estimation. Nevertheless, the EKF is nown to have several drawbacs. These are mainl de to the Talor linearization of the nonlinear transformations arond the crrent state estimate. The linearization reqires that Jacobians of state transition observation eqations are derived, which is often a qite comple tas. Moreover, sometimes there are points in which the Jacobians are not defined. In addition to the difficlties with implementation, convergence problems are often encontered de to the fact that the linearized models describe the sstem poorl.

2 Previos wor inclde several toolboes other platform. Focs here is on comparision transparenc. The paper is organized as follows: first overall design philosoph behind the platform is described. Net a description of the estimation algorithms are given inclding the etended Kalman filter, the Uncented Kalman filter different tpes of particlefilters. Section 4 gives an etensive eample std as well as a demonstration of the platform for comparing algorithms for navigation of a mobile robot. Finall conclsions references are given.. THE PLATFORM The design philosoph of the simlation platform has been to provide a tool, which is simple transparent in its inner worings, et powerfl reliable. With this in mind, ease of se in applications with respect to development of new algorithms have been a high priorit. The platform itself is based on a series of fnctions written in Matlab, which are combined with a Simlin graphical interface for ease of contrction. As Simlin b design resembles the bloc-diagrams of control theor, constrction of sstems becomes ver straightforward all signal paths are visible. Control Signals ipvec w v Fnction Sstem Eqations Fnction Eqations Hold s Integrator Fnction Estimation Algorithm Inpts s var Estimates Fig.. The Simlin laot of a continos sstem. Control Signals ipvec v w Hold Hold v Hold w () v () w Fnction Sstem Eqations Fnction Eqations Hold () (+) z Unit Dela [] Fnction Estimation Algorithm () Samples Inpts s var Estimates Samples Fig.. The Simlin laot of a discrete time sstems The algorithms, which are implemented in the platform, are constrcted in a model-independent manner. The models are defined eternall as simple fnction files in Matlab, in accordance with the format sed for solving differential eqations nmericall (see the docmentation for Matlab). Depending on the application the sstem at h, continos time step or fied time step integration ma be sed, as well as discrete time sstems. Finall, the fnctions are not intended solel for se with Simlin. This means that an data set consisting of measrements inpts ma be filtered sing the fnctions, given that a sstem measrement description is provided. All in all, this provides a great deal of fleibilit, ease of testing control algorithms, as well as testing varios data filtering methods.. ESTIMATION ALGORITHMS Consider a sstem in which the evoltion of the state seqence { R n, N} is given b + = f (,,v ) () where f is a possible nonlinear fnction of the state,, the inpt (control) signal, the process noise, v. The process noise is assmed to be a seqence {v R n N} of i.i.d. stochastic vectors. The objective is to estimate from measrements = g (,e ) R m () where also g is a possible nonlinear fnction of the state the measrement noise, e. The measrement noise is assmed to be a seqence, {e R m N}, of i.i.d. stochastic vectors. More specific we see an estimate of based on all available measrements ( nown inpts) Y : = {( i, i ), i =,...,}. The soltion to this problem is embedded in the conditional degree of belief in the state, given the data, Y :. The problem is then (recrsivel) to determine the pdf. p( : ). If the initial distribtion, p( ), is nown then the soltion can in principle be determined throgh the recrsions: p( Y : ) = p( )p( Y : )d Ω () p( Y : ) = p( ) p( Y : ) p( Y : ) (4) These two recrsions are related to the dnamic (()) the inference ((4)) step, respectivel can onl in special cases be solved analticall. In the linear Gassian case the pdf. can be parameterized in terms of mean variance the recrsions reslts in the well nown Kalman filter. In that case (the linear Gassian case with stard assmptions inclding N( ˆ,P )) the sstem is assmed to be given b the recrsions: + = A + B + v v N iid (,R )

3 = C + e e N iid (,R ) The Kalman filter is given b the prediction or the time pdates ˆ + = A ˆ + B () P + = AP A T + R (6) the inference recrsion where: ˆ = ˆ + K ( ŷ ) (7) P = P K CP (8) K = P C T S ŷ = X ˆ S = CP C T + R In this case, the prediction in () reslts in () can also be fond as an application of calcls for linear operations on Gassian vectors. The inference recrsion in (7) emerge from (4) or as an application of the Projection Theorem. The varios filters differs in the wa the hle the propagation of the distribtions throgh the two nonlinearities, f g, how the inference is carried ot. The net three filters are all based on the projection Theorem.. The Etended Kalman filter The Etended Kalman filter is as its name indicate based on an etension of the application of the Kalman filter to the nonlinear case. The Etended Kalman filter (EKF) is based on a stard Talor epansion of the nonlinear fnctions can be regarded as a local approimation. In general the approimation is best for small deviations from the point of linearization. The basic idea is related to the problem of determine the distribtion of z if z = F() the distribtion of is nown to be N( ˆ,P ). The approimation is simpl to se where z N ( F( ˆ),AP A T) A = F ˆ This approimation applies both to the process eqation ( f ) the measrement eqation ( g). In fact, the onl changes with respect to ()-(8) is ˆ i+ i = f i ( ˆ i i, i,) ŷ i i = g i ( ˆ i i,) The variance pdate, (6) (8), are nchanged (ecept for the state dependent sstem matrices).. Divided difference filters The divided difference filter eists in a first order version (DD) in a second order version (DD) is based on Stirlings interpolation formla (see (Nørgaard et al., ) Figre for illstration). f() Two polnomial approimations of the same fnction 4 Fig.. Comparison of a second-order polnomial approimation obtained with the Talor (dotdashed) the Stirling method (dashed) Let again, be a stochastic variable N ( ˆ,S S T ). The approimation which taes the variation of w into accont is F() = F( ˆ)+ z F( ˆ)( ˆ)+ F( ˆ)( ˆ) + ε where { } F( ˆ) = Matr i j h [F i( ˆ+hS j ) F i ( ˆ hs j )] F( ˆ)=Matr i j { } h [F i( ˆ+ hs j )+F i ( ˆ hs j ) F i ( ˆ)] Here h is a scale parameter S j is the j th colmn in S. In the Gassian case the choice h = is in some sense optimal (see (Nørgaard et al., )). Introdce the notation F + p = F( ˆ+hS,p) F p = F( ˆ hs,p) F = F( ˆ) For the DD filter the approimation is then ẑ = h n h F + n h F p + + F p p= P z = 4h n + h 4h i= n (F + p F p )(F+ p F p )T i= (F + p + F p F )(F + p + F p F ) T For the first order filter (DD) onl the first terms in the approimations are sed. In the divided difference filters (DD DD) the propagation of mean variance is determined throgh the approimations mentioned above. The inference is based on the Projection Theorem.

4 . The Unscented alman filter The Uncented filter is based on the (ncentedi) transformation of a stochastic variable,, throgh a nonlinear fnction, F() (see (Jlier Uhlmann, 4)). Assming again the mean of is ˆ the variance matri is P = S S T, then the sigma points are defined as: () = ˆ w = κ n + κ (i) = ˆ+ κ (n + κ)s i w i = (n + κ) i =,... n ( j+n) = ˆ κ (n + κ)s, j w j+n = (n + κ) j =,...,n Here κ is a scaling parameter w i is the weight associated with a point n i= w i = Each sigma point is propagated throgh the nonlinear fnction z (i) = F( (i) ) the approimation is then P z = n i= ẑ = n i= w i z (i) i =,... n w i (z (i) ẑ)(z (i) ẑ) T The stard UKF is based on the approimation mentioned above the Projection Theorem. In the scaled version of UKF the weight is chosen in a slightl different manner (see (Jlier, ) or (Wan van der Merwe, ) for details)..4 Particle filters Particle filters comes in several versions implementations (see e.g. (Arlampalam et al., ) or (van der Merwe et al., )). In the most basic version (Ep. PF) implemented in the platform the nonlinearities are dealt with b propagating a swarm of particle throgh the nonlinearities. Again assming N( ˆ,P ) a nmber (N) of particles are generated (i) N( ˆ,P ) i =,... N propagated throgh the nonlinear fnction z (i) = F( (i) ) The approimation is then simpl ẑ = N i= z (i) P z = N i= (z (i) ẑ)(z (i) ẑ) T In the most basic version (Ep. PF) the inference is based on the Projection Theorem the nonlinearities are hled with the method mentioned above. In the generic particle filters (Gen. PF) the inference is not based on the Projection Theorem, bt is carried ot b appling 4 directl. That reslts in weights associated with each of the particles. In this version the particle are onl initiall generated as described above. After the inference step the particles are resampled from a distribtion characterized b the weights. In the last version (MH. PF) implemented here on this platform, the resampling is performed b means of the Metropolis-Hastings algorithm. 4. EXAMPLE STUDY The versatilit of the simlation framewor is most evident when implementing a nmber of eamples. For the prpose of this demonstration, a continos time differential eqation sstem a discrete time difference eqation sstem are selected. The continos time sstem is a ver simple model of a deadreconing gidance for a small mobile robot. The discrete time sstem is an academic eample of a nonlinear sstem (thogh in a simplified form), which have been sed previosl as a benchmar for testing filter algorithms (Netto et al., 978)) The model of the small mobile robot (niccle tpe) is given b the set of eqations stated below is onl slightl nonlinear. The eqations ield a position in a two dimensional space as well as a heading. An inpt signal consisting of the velocit, γ, trnrate, ω, is sed as a wa of introdcing an eternal sorce of distrbance. The observation eqations are prel linear, the noise is modelled as being additive, zero mean, Gassian distribted in both sets of eqations. ẋ = γ cos(θ)+v ẏ = γ sin(θ)+v (9) θ = ω + v The process noise sorces, v n, as well as the observation noise sorces, w n, are specified b N(,.). z = +w z = +w () z = θ + w An eample of a simlation sing the Divided Difference (nd order) as estimator can be seen in figre 4. The integral of the control signal, ω, is seen below the path traced b the robot. In order to compare a range of techniqes implemented in the framewor, accrac reslts are given in table. Attempting to find a fair estimate of the accrac, rns were made with each algorithm the average vales were fond. The particle filters all sed particles per time pdate. The table contains the "worst case" vales for the three states.

5 [m] Divided Difference KF (nd) T =. s [m] their rntimes ma well benefit from nmerical optimizations in application specific implementations. The algorithms sed a fied step integration (Matlab, Dorm-Prince, order ) to solve eqation 9. The stard Unscented Kalman filter (Std. UKF) performs ver well, while it s scaled version gives a lower mean RMSE a slightl lower mean variance estimates. The DD DD both give low mean RMSE consistent variance estimates - in this case, the second order parts of the DD does not ield mch. The second eample is a nonlinear eqation with a linear nois measrement. First, the process eqation, + is listed, net the measrement eqation,. + = cos(.)+v n = + w ; () Radians Tre vale Estimate Time [s] Note that, both the noise sorces, v w, are zero mean Gassian white noise with variances of.. respectivel. As was the case with the small robot model, a Monte Carlo series of simlations was made with a variet of estimation algorithms. Two eamples of the appearance of a simlation can be fond in figre. Fig. 4. A path traced b the small niccle robot. The estimation rotine emploed is the nd order Divided Difference filter with a sampling freqenc of Hz. At ever other estimated state, the 9% confidence intervals are drawn as ellipses or bars respectivel. The estimate is at no point otside the confidence intervals. Algorithm Ma. RMSE Ma. Var. Est. Time C.D. EKF Std. UKF Scl. UKF DD DD Ep. PF Gen. PF PF (MH) Table. Small mobile robot, worst vale of mean estimate (,,θ) maimm mean variance estimate of Monte Carlo simlations. The table is split into Kalman filter variants (top) particle filters (bottom). The particle filters all sed particles. Also listed in the table is the comptational brden of each algorithm. The latter is given as a relative nmber compared to the rntime of a continos-discrete etended Kalman filter (C.D. EKF). The times are relative, as other processor speeds tpes will ield different absolte reslts. Frthermore, the algorithms The reslt of the Monte Carlo simlation can be seen in table. The Kalman filter tpe algorithms were simlated times the means of the root mean sqare errors (RMSE) were fond as well as the means of the variance estimates. The Particle Filter tpes were simlated times with particles per time pdate in all filters. Algorithm Mean RMSE Mean Var. Est. Time C.D. EKF Std. UKF Scl. UKF DD DD Ep. PF Gen. PF PF (MH) Table. Table of reslts for a Monte Carlo series of simlations on the discrete nonlinear nois sstem. Finall, in order to compare the precision of the three particle filters as a fnction of the nmber of particles per time pdate, a Monte Carlo series of simlations was made sing the benchmar sstem. The reslts can be seen in figre 6. The series consisted of rns per particle cont, from to 6 particles in increasing steps. The algorithms converge rather qicl as the particle cont increases.

6 Divided Difference KF (nd) RMSE Particles Generic PF No. of Particles = Tre vale Estimate Mean Var. Est. 4 Ep. Unsc. PF. Generic PF. Generic PF. (MH) Particles Fig.. Two eamples of the highl nonlinear nois sstem given in eqation. The topmost is the nd order Divided Difference filter, while the bottommost is a generic Particle Filter.. CONCLUSION In this paper we have presented the toolbo KALM- TOOL ver. which a set of tools for state estimation for nonlinear sstems. It contains fnctions for etended Kalman filtering as well as for the two new filters the DD filter the DD filter. It also contains fnctions for Unscented (stard scaled) Kalman filter as well as three versions of particle filters. REFERENCES Arlampalam, M.S., S. Masell, N. Gordon T. Clapp (). A ttorial on particle filters for online nonlinear/non-gassian baesian tracing. IEEE Transactions on Signal Processing (), Jlier, S. (). The scaled nscented transformation. Proceedings of the American Control Conference pp Jlier, Simon Jeffre Uhlmann (4). Unscented filtering nonlinear estimation. Proceeding of the IEEE 9(), 4 4. Fig. 6. Two graphs depicting the effect of varing the particle cont per pdate on the benchmar sstem. The mean RMSE mean variance estimates are seen to converge rather qicl to relativel stationar vales at arond particles per time pdate. Netto, A.M.L., L. Gimeno M.J. Mendes (978). A new spline algorithm for non-linear filtering of discrete time sstems. Proceedings of the 4th IFAC Smposim on Identification Sstem Parameter Estimation, Tbilisi, U.S.S.R. pp.. Nørgaard, Magns, Niels K. Polsen Ole Ravn (). New development in state estimation for nonlinear sstems. Atomatica 6, Nørgaard, Magns, Niels Kjølstad Polsen Ole Ravn (). Kalmtool for se with matlab. In: th IFAC Smposim on Sstem Identification, SYSID, Rotterdam. IFAC. Rotterdam. pp van der Merwe, R., A. Docet, N. de Freitas E. Wan (). The nscented particle filter. Technical report. Cambridge Universit Engineering Department. Wan, Eric Rdolph van der Merwe (). The nscented alman filter for nonlinear estimation. In: Proceedings of the IEEE Adaptive Sstems for Signal Processing, Commnications, Control Smposim. pp. 8.