SEIF, EnKF, EKF SLAM. Pieter Abbeel UC Berkeley EECS

Transcription

1 SEIF, EnKF, EKF SLAM Pieer Abbeel UC Berkeley EECS

2 Informaion Filer From an analyical poin of view == Kalman filer Difference: keep rack of he inverse covariance raher han he covariance marix [maer of some linear algebra manipulaions o ge ino his form] Why ineresing? Inverse covariance marix = 0 is easier o work wih han covariance marix = infiniy (case of complee uncerainy) Inverse covariance marix is ofen sparser han he covariance marix --- for he insiders : inverse covariance marix enry (i,j) = 0 if x i is condiionally independen of x j given some se {x k, x l, } Downside: when exended o non-linear seing, need o solve a linear sysem o find he mean (around which one can hen linearize) See Probabilisic Roboics pp for more in-deph pros/cons and Probabilisic Roboics Chaper 12 for is relevance o SLAM (hen ofen referred o as he sparse exended informaion filer (SEIF) )

3 Ensemble Kalman filer (enkf) Represen he Gaussian disribuion by samples Empirically: even 40 samples can rack he amospheric sae wih high accuracy wih enkf <-> UKF: 2 * n sigma-poins, n = hen sill forms covariance marices for updaes The inellecual innovaion: Transforming he Kalman filer updaes ino updaes which can be compued based upon samples and which produce samples while never explicily represening he covariance marix

4 KF enkf Keep rack of µ, Keep rack of ensemble [x 1,, x N ] Predicion: µ Σ Correcion: = µ 1 = A + B u A Σ A + R 1 T T T K = ΣC ( C ΣC + Q ) µ = µ + K z C µ ) ( Σ = ( I K C ) Σ 1 Can updae he ensemble by simply propagaing hrough he dynamics model + adding sampled noise? Reurn µ, Σ

5 enkf correcion sep KF: T T K = ΣC ( C ΣC + Q ) µ = µ + K z C µ ) ( Σ = ( I K C ) Σ 1 Curren ensemble X = [x 1,, x N ] Build observaions marix Z = [z +v 1 z +v N ] where v i are sampled according o he observaion noise model Then he columns of X + K (Z C X) form a se of random samples from he poserior Noe: when compuing K, leave in he forma = [x 1 -µ x N -µ ] [x 1 -µ x N -µ ] T

6 How abou C? Indeed, would be expensive o build up C. However: careful inspecion shows ha C only appears as in: C X C C T = C X X T C T à can simply compue h(x) for all columns x of X and compue he empirical covariance marices required [deails lef as exercise]

7

8 References for enkf Mandel, 2007 A brief uorial on he Ensemble Kalman Filer Evensen, 2009, The ensemble Kalman filer for combined sae and parameer esimaion

9 KF Summary Kalman filer exac under linear Gaussian assumpions Exension o non-linear seing: Exended Kalman filer Unscened Kalman filer Exension o exremely large scale seings: Ensemble Kalman filer Sparse Informaion filer Main limiaion: resriced o unimodal / Gaussian looking disribuions Can alleviae by running muliple XKFs + keeping rack of he likelihood; bu his is sill limied in erms of represenaional power unless we allow a very large number of hem

10 EKF/UKF SLAM R B E H A D G C Sae: (n R, e R, θ R, n A, e A, n B, e B, nf C, e C, n D, e D, n E, e E, n F, e F, n G, e G, n H, e H ) Now map = locaion of landmarks (vs. gridmaps) Transiion model: Robo moion model; Landmarks say in place

11 Simulaneous Localizaion and Mapping (SLAM) In pracice: robo is no aware of all landmarks from he beginning Moreover: no use in keeping rack of landmarks he robo has no received any measuremens abou à Incremenally grow he sae when new landmarks ge encounered.

12 Simulaneous Localizaion and Mapping (SLAM) Landmark measuremen model: robo measures [ x k ; y k ], he posiion of landmark k expressed in coordinae frame aached o he robo: h(n R, e R, θ R, n k, e k ) = [x k ; y k ] = R(θ) ( [n k ; e k ] - [n R ; e R ] ) Ofen also some odomery measuremens E.g., wheel encoders As hey measure he conrol inpu being applied, hey are ofen incorporaed direcly as conrol inpus (why?)

13 Vicoria Park Daa Se [couresy by E. Nebo]

14 Vicoria Park Daa Se Vehicle [couresy by E. Nebo]

15 Daa Acquisiion [couresy by E. Nebo]

16 Esimaed Trajecory [couresy by E. Nebo] 18

17 EKF SLAM Applicaion [couresy by J. Leonard] 19

18 EKF SLAM Applicaion odomery esimaed rajecory [couresy by John Leonard] 20

19 Landmark-based Localizaion 21

20 EKF-SLAM: pracical challenges Defining landmarks Laser range finder: Disinc geomeric feaures (e.g. use RANSAC o find lines, hen use corners as feaures) Camera: ineres poin deecors, exures, color, Ofen need o rack muliple hypoheses à à Daa associaion/correspondence problem: when seeing feaures ha consiue a landmark --- Which landmark is i? Closing he loop problem: how o know you are closing a loop? Can spli off muliple EKFs whenever here is ambiguiy; Keep rack of he likelihood score of each EKF and discard he ones wih low likelihood score Compuaional complexiy wih large numbers of landmarks.