SEIF, EnKF, EKF SLAM Pieer Abbeel UC Berkeley EECS
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. 78-79 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) )
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 = 10 6 + 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
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 µ, Σ
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
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]
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
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
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
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.
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?)
Vicoria Park Daa Se [couresy by E. Nebo]
Vicoria Park Daa Se Vehicle [couresy by E. Nebo]
Daa Acquisiion [couresy by E. Nebo]
Esimaed Trajecory [couresy by E. Nebo] 18
EKF SLAM Applicaion [couresy by J. Leonard] 19
EKF SLAM Applicaion odomery esimaed rajecory [couresy by John Leonard] 20
Landmark-based Localizaion 21
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.