Lecture 5: Unscented Kalman Filter And Particle Filtering

Lecture 5: Unscented Kalman filter andParticle FilteringSimo SärkkäDepartment of Biomedical Engineering and Computational ScienceHelsinki University of TechnologyApril 21, 2009Simo SärkkäLecture 5: UKF and PF

Contents1Idea of Unscented Transform2Unscented Transform3Unscented Kalman Filter Algorithm4Unscented Kalman Filter Properties5Particle Filtering6Particle Filtering Properties7Summary and DemonstrationSimo SärkkäLecture 5: UKF and PF

Linearization Based Gaussian ApproximationProblem: Determine the mean and covariance of y:x N(µ, σ 2 )y sin(x)Linearization based approximation:y sin(µ) sin(µ)(x µ) . . . µwhich givesE[y] E[sin(µ) cos(µ)(x µ)] sin(µ)Cov[y] E[(sin(µ) cos(µ)(x µ) sin(µ))2 ] cos2 (µ) σ 2 .Simo SärkkäLecture 5: UKF and PF

Principle of Unscented Transform [1/3]Form 3 sigma points as follows:X0 µX1 µ σX2 µ σ.We may now select some weights W0 , W1 , W2 such thatthe original mean and (co)variance can be alwaysrecovered byXµ Wi xii2σ XiSimo SärkkäWi (Xi µ)2 .Lecture 5: UKF and PF

Principle of Unscented Transform [2/3]Use the same formula for approximating the distribution ofy sin(x) as follows:Xµy Wi sin(Xi )iσy2 XiWi (sin(Xi ) µy )2 .For vectors x N(m, P) the generalization of standarddeviation σ is the Cholesky factor L P:P L LT .The sigma points can be formed using columns of L (herec is a suitable positive constant):X0 mXi m c LiXn i m c LiSimo SärkkäLecture 5: UKF and PF

Principle of Unscented Transform [3/3]For transformation y g(x) the approximation is:Xµy Wi g(Xi )iΣy XiWi (g(Xi ) µy ) (g(Xi ) µy )T .Joint distribution of x and y g(x) q is then given as X mxXiq E Wiµyg(x) qg(Xi )i xCovqg(x) q X(Xi m) (Xi m)T(Xi m) (g(Xi ) µy )T Wi(g(Xi ) µy ) (Xi m)T (g(Xi ) µy ) (g(Xi ) µy )TiSimo SärkkäLecture 5: UKF and PF

Unscented Transform Approximation of Non-LinearTransforms [1/3]Unscented transformThe unscented transform approximation to the joint distributionof x and y g(x) q where x N(m, P) and q N(0, Q) is xmP CU N,,yµUCTU SUThe sub-matrices are formed as follows:1Form the matrix of sigma points X as X m ··· m n λ 0P P ,[continues in the next slide. . . ]Simo SärkkäLecture 5: UKF and PF

Unscented Transform Approximation of Non-LinearTransforms [2/3]Unscented transform (cont.)2Propagate the sigma points through g(·):Yi g(Xi ),3i 1 . . . 2n 1,The sub-matrices are then given as:X (m)µU Wi 1 YiiSU XWi 1 (Yi µU ) (Yi µU )T QCU XWi 1 (Xi m) (Yi µU )T ,ii(c)(c)Simo SärkkäLecture 5: UKF and PF

Unscented Transform Approximation of Non-LinearTransforms [3/3]Unscented transform (cont.)λ is a scaling parameter defined as λ α2 (n κ) n.α and κ determine the spread of the sigma points.(m)Weights Wi(c)and Wiare given as follows:(m) λ/(n λ)(c) λ/(n λ) (1 α2 β)W0W0(m) 1/{2(n λ)},i 1, . . . , 2n(c) 1/{2(n λ)},i 1, . . . , 2n,WiWiβ can be used for incorporating prior information on the(non-Gaussian) distribution of x.Simo SärkkäLecture 5: UKF and PF

Linearization/UT Example 02 2x1 N,x20 2 3Simo Särkkädy1 exp( y1 ), y1 (0) x1dtdy21 y23 ,y2 (0) x2dt2Lecture 5: UKF and PF

Linearization Approximation Simo SärkkäLecture 5: UKF and PF

UT Approximation Simo SärkkäLecture 5: UKF and PF

Unscented Kalman Filter (UKF): Derivation [1/4]Assume that the filtering distribution of previous step isGaussianp(xk 1 y1:k 1 ) N(xk 1 mk 1 , Pk 1 )The joint distribution of xk and xk 1 f(xk 1 ) qk 1 canbe approximated with UT as Gaussian ′ ′ P′12P11m1xk 1,,p(xk 1 , xk , y1:k 1 ) Nm′2xk(P′12 )T P′22Form the sigma points Xi of xk 1 N(mk 1 , Pk 1 ) andcompute the transformed sigma points as X̂i f(Xi ).The expected values can now be expressed as:m′1 mk 1X (m)m′2 Wi 1 X̂iiSimo SärkkäLecture 5: UKF and PF

Unscented Kalman Filter (UKF): Derivation [2/4]The blocks of covariance can be expressed as:P′11 PkX (c)P′12 Wi 1 (Xi mk 1 ) (X̂i m′2 )TiP′22 Xi(c)Wi 1 (X̂i m′2 ) (X̂i m′2 )T Qk 1The prediction mean and covariance of xk are then m′2 andP′22 , and thus we getm k XWi 1 X̂iP kX TWi 1 (X̂i m k ) (X̂i mk ) Qk 1(m)i i(c)Simo SärkkäLecture 5: UKF and PF

Unscented Kalman Filter (UKF): Derivation [3/4]For the joint distribution of xk and yk h(xk ) rk wesimilarly get ′′ ′′P′′12P11m1xk,,p(xk , yk , y1:k 1 ) N(P′′12 )T P′′22m′′2yk If X i are the sigma points of xk N(mk , Pk ) andŶi f(X i ), we get:m′′1 m kX (m)′′m2 Wi 1 ŶiiP′′11 P kX (c) ′′′′ TP12 Wi 1 (X i mk ) (Ŷi m2 )iP′′22 Xi(c)Wi 1 (Ŷi m′′2 ) (Ŷi m′′2 )T RkSimo SärkkäLecture 5: UKF and PF

Unscented Kalman Filter (UKF): Derivation [4/4]Recall that if Axa N,ybCT C,Bthenx y N(a C B 1 (y b), A C B 1 CT ).Thus we get the conditional mean and covariance:′′′′ 1′′mk m k P12 (P22 ) (yk m2 )′′ 1′′(P′′12 )T .Pk P k P12 (P22 )Simo SärkkäLecture 5: UKF and PF

Unscented Kalman Filter (UKF): Algorithm [1/3]Unscented Kalman filter: Prediction step1Form the matrix of sigma points: pp Xk 1 mk 1 · · · mk 1 n λ 0Pk 1 Pk 1 .2Propagate the sigma points through the dynamic model:X̂k ,i f(Xk 1,i ),3i 1 . . . 2n 1.Compute the predicted mean and covariance:X (m)m Wi 1 X̂k ,ikiP k Xi(c) TWi 1 (X̂k ,i m k ) (X̂k ,i mk ) Qk 1 .Simo SärkkäLecture 5: UKF and PF

Unscented Kalman Filter (UKF): Algorithm [2/3]Unscented Kalman filter: Update step1Form the matrix of sigma points: X mk · · ·k2h qq i m n λ.0P P kkkPropagate sigma points through the measurement model:Ŷk ,i h(X k ,i ),3i 1 . . . 2n 1.Compute the following terms:X (m)µk Wi 1 Ŷk ,iiSk XWi 1 (Ŷk ,i µk ) (Ŷk ,i µk )T RkCk X TWi 1 (X k ,i mk ) (Ŷk ,i µk ) .ii(c)(c)Simo SärkkäLecture 5: UKF and PF

Unscented Kalman Filter (UKF): Algorithm [3/3]Unscented Kalman filter: Update step (cont.)4Compute the filter gain Kk and the filtered state mean mkand covariance Pk , conditional to the measurement yk :Kk Ck S 1kmk m k Kk [yk µk ]TPk P k Kk Sk Kk .Simo SärkkäLecture 5: UKF and PF

Unscented Kalman Filter (UKF): ExampleRecall the discretized pendulum model 1 xk1 1 xk2 1 txk0 qk 1xk2 1 g sin(xk1 1 ) txk2 {z}f(xk 1 )yk sin(xk1 ) rk , {z }h(xk ) Matlab demonstrationSimo SärkkäLecture 5: UKF and PF

Unscented Kalman Filter (UKF): AdvantagesNo closed form derivatives or expectations needed.Not a local approximation, but based on values on a largerarea.Functions f and h do not need to be differentiable.Theoretically, captures higher order moments ofdistribution than linearization.Simo SärkkäLecture 5: UKF and PF

Unscented Kalman Filter (UKF): DisadvantageNot a truly global approximation, based on a small set oftrial points.Does not work well with nearly singular covariances, i.e.,with nearly deterministic systems.Requires more computations than EKF or SLF, e.g.,Cholesky factorizations on every step.Can only be applied to models driven by Gaussian noises.Simo SärkkäLecture 5: UKF and PF

Particle Filtering: Overview [1/3]Demo: Kalman vs. Particle Filtering:Kalman filter animationParticle filter animationSimo SärkkäLecture 5: UKF and PF

Particle Filtering: Overview [2/3] The idea is to form a weighted particle presentation(x(i) , w (i) ) of the posterior distribution:Xp(x) w (i) δ(x x(i) ).iParticle filtering Sequential importance sampling, withadditional resampling step.Bootstrap filter (also called Condensation) is the simplestparticle filter.Simo SärkkäLecture 5: UKF and PF

Particle Filtering: Overview [3/3]The efficiency of particle filter is determined by theselection of the importance distribution.The importance distribution can be formed by using e.g.EKF or UKF.Sometimes the optimal importance distribution can beused, and it minimizes the variance of the weights.Rao-Blackwellization: Some components of the model aremarginalized in closed form hybrid particle/Kalman filter.Simo SärkkäLecture 5: UKF and PF

Bootstrap Filter: PrincipleState density representation is set of samples(i){xk : i 1, . . . , N}.Bootstrap filter performs optimal filtering update andprediction steps using Monte Carlo.Prediction step performs prediction for each particleseparately.Equivalent to integrating over the distribution of previousstep (as in Kalman Filter).Update step is implemented with weighting andresampling.Simo SärkkäLecture 5: UKF and PF

Bootstrap Filter: AlgorithmBootstrap Filter1Generate sample from predictive density of each old(i)sample point xk 1 :(i)(i)x̃k p(xk xk 1 ).2Evaluate and normalize weights for each new sample point(i)x̃k :(i)(i)wk p(yk x̃k ).3(i)(i)Resample by selecting new samples xk from set {x̃k }(i)with probabilities proportional to wk .Simo SärkkäLecture 5: UKF and PF

Sequential Importance Resampling: PrincipleState density representation is set of weighted samples(i)(i){(xk , wk ) : i 1, . . . , N} such that for arbitrary functiong(xk ), we haveX (i)(i)E[g(xk ) y1:k ] wk g(xk ).iOn each step, we first draw samples from an importancedistribution π(·), which is chosen suitably.The prediction and update steps are combined and consist(i)(i)of computing new weights from the old ones wk 1 wk .If the “sample diversity” i.e the effective number of differentsamples is too low, do resampling.Simo SärkkäLecture 5: UKF and PF

Sequential Importance Resampling: AlgorithmSequential Importance Resampling1(i)Draw new point xk for each point in the sample set(i){xk 1 , i 1, . . . , N} from the importance distribution:(i)(i)xk π(xk xk 1 , y1:k ),2i 1, . . . , N.Calculate new weights(i)wk (i) p(ykwk 1(i)(i)(i) xk ) p(xk xk 1 )(i)(i)π(xk xk 1 , y1:k ),i 1, . . . , N.and normalize them to sum to unity.3If the effective number of particles is too low, performresampling.Simo SärkkäLecture 5: UKF and PF

Effective Number of Particles and ResamplingThe estimate for the effective number of particles can becomputed as:1neff ,PN(i) 2i 1 wkResampling123(i)Interpret each weight wk as the probability of obtaining(i)the sample index i in the set {xk i 1, . . . , N}.Draw N samples from that discrete distribution and replacethe old sample set with this new one.(i)Set all weights to the constant value wk 1/N.Simo SärkkäLecture 5: UKF and PF

Constructing the Importance DistributionUse the dynamic model as the importance distribution Bootstrap filter.Use the optimal importance distributionπ(xk xk 1 , y1:k ) p(xk xk 1 , y1:k ).Approximate the optimal importance distribution by UKF unscented particle filter.Instead of UKF also EKF or SLF can be, for example, used.Simulate availability of optimal importance distribution auxiliary SIR (ASIR) filter.Simo SärkkäLecture 5: UKF and PF

Rao-Blackwellized Particle Filtering: Principle [1/2]Consider a conditionally Gaussian model of the formsk p(sk sk 1 )xk A(sk 1 ) xk 1 qk ,yk H(sk ) xk rk ,qk N(0, Q)rk N(0, R)The model is of the formp(xk , sk xk 1 , sk 1 ) N(xk A(sk 1 )xk 1 , Q) p(sk sk 1 )p(yk xk , sk ) N(yk H(sk ), R)The full model is non-linear and non-Gaussian in general.But given the values sk the model is Gaussian and thusKalman filter equations can be used.Simo SärkkäLecture 5: UKF and PF

Rao-Blackwellized Particle Filtering: Principle [1/2]The idea of the Rao-Blackwellized particle filter:Use Monte Carlo sampling to the values skGiven these values, compute distribution of xk with Kalmanfilter equations.Result is a Mixture Gaussian distribution, where each(i)(i)particle consist of value sk , associated weight wk and the(i)mean and covariance conditional to the history s1:kThe explicit RBPF equations can be found in the lecturenotes.If the model is almost conditionally Gaussian, it is alsopossible to use EKF, SLF or UKF instead of the linear KF.Simo SärkkäLecture 5: UKF and PF

Particle Filter: AdvantagesNo restrictions in model – can be applied to non-Gaussianmodels, hierarchical models etc.Global approximation.Approaches the exact solution, when the number ofsamples goes to infinity.In its basic form, very easy to implement.Superset of other filtering methods – Kalman filter is aRao-Blackwellized particle filter with one particle.Simo SärkkäLecture 5: UKF and PF