EKF SLAM vs. FastSLAM A Comparison

Transcription

1 vs. A Comparison Michael Calonder, Compuer Vision Lab Swiss Federal Insiue of Technology, Lausanne EPFL) michael.calonder@epfl.ch The wo algorihms are described wih a planar robo applicaion in mind. Generalizaion is o any spaial SLAM scenarios is sraighforward. For simpliciy, we assume here is no conrol inpu. The pose consiss of he robo s posiion x, y) and is heading direcion δ: s := x, y, δ). The landmarks are denoed θ i, simply consising of a pair of planar coordinaes. 1 Probabilisical EKF Formulaion The Exended Kalman Filer EKF) can be viewed as a varian of a Bayesian Filer; EKFs provide a recursive esimae of he sae of a dynamic sysem, or more precise, solve an unobservable, nonlinear esimaion problem. Roughly speaking, he sae x of a sysem a ime can be considered a random variable where he uncerainy abou his sae is represened by a probabiliy disribuion. One is ineresed in he poserior densiy p x z ), where z := {z 1,..., z } is he se of measuremens up o ime and x := s, θ 0, θ 1,..., θ K) is he sae vecor. Noe ha he superscrip refers o he se of variables a ime.) In general, he complexiy of compuing such a densiy grows exponenially wih ime; o make he compuaion racable, he rue sae is being assumed o be an unobserved Markov process implying ha he rue sae is condiionally independen of all earlier saes excep he previous sae: p x x 1) = p x x 1 ) where x := {x 1,..., x } denoes he se of saes. he measuremen a he -h imesep depends only upon he curren sae and is condiionally independen of all oher saes: p z x ) = p z x ). EKF uses a predicion and an updae sep. The predicion sep calculaes he probabiliy of he curren sae x, while he measuremen z for he curren ime sep k is no ye available: p x z 1) = p x x 1 ) p x 1 z 1) dx 1. The lef PDF erm of he inegrand is he PDF of he moion model see below) where he righ par is he PDF of he sae esimaed in he las ime sep, rendering he esimaion recursive. Once we know p x z 1) we can find he poserior or correced esimae using a new measuremen z : p x z ) = p z x ) p x z 1) p z z 1 ) wih p z z 1) = p z x ) p x z 1) dx being some consan) normalizaion erm. The PDF p z x ) is where he measuremen model comes in. EKF filering makes he following assumpions abou he respecive PDFs: p x x 1 ) Nfx 1 ), Q ) p z x ) Nhx ), R ) p x 1 z 1) Nx 1, P 1 ) where Nµ, Σ) denoes he Gaussian disribuion wih mean µ and covariance marix Σ, fx) he moion model and hx) he measuremen model. The error covariance P is in he predicion sep a ime k projeced ahead by P = A P 1 A + W Q 1 W ekf fasslam comp 1/5

2 and finally updaed in he correcion sep according o P = I K H ) P where K is he Kalman gain marix [8]. The Jacobians involved are A = f x, W = f x 1 w, and H = h x 1 x wih w being he process noise vecor [8]. 2 Algorihm The key idea of explois he fac ha knowledge of he robo s pah s 1, s 2,..., s renders he individual landmark measuremens independen [3], as originally observed by Murphy [6]. decomposes he SLAM problem ino one robo localizaion problem, and a collecion of K landmark esimaion problems. In, alike in, poses are assumed o behave according o a probabilisic law named moion model wih an underlying densiy p s s 1 ). Likewise, he measuremens are governed by he probabilisic) measuremen model p z s, θ, n ) wih z measuremen, θ = {θ 1,..., θ K } he se of landmarks, and n {1,..., K} he index of he observed landmark a ime only one a a ime). The ulimae goal is o esimae he poserior p s, θ z ). The exac facored represenaion employed by reads p s, θ z, n ) = p s z, n ) p θ z, n ) = p s z, n ) p θ k s, z, n ). 1) 1 k K Noe he condiional dependence on n. The K + 1 facors in 1) are compued as follows. Esimaion of pah p s z, n ) This is achieved by a paricle filer: mainains a se of M paricles, {s,[m] } = {s [m] 1,..., s [m] }, where [m] refers o he m-h paricle in he se. To creae he paricle se a ime, s,[m], one firs ) samples from he moion model M paricles ino a emporary se; s [m] p s s [m] 1. Then he imporance facor weighs w [m] are calculaed; hey deermine he probabiliy ha a cerain paricle from he emporary se eners he final se. Esimaion of landmark locaions p θ k s, z, n ) The poserior updae depends on wheher he k-h landmark has been observed or no, p { θ k s, z, n ) p z θ k, s, n ) p θ k s 1, z 1, n 1) if k = n p θ k s 1, z 1, n 1) oherwise implemens he above updae equaion by means of an EKF. Daa associaion problem as mos exising SLAM soluions based on EKF) solves he daa associaion problem via maximum likelihood esimaion, see secion Similariies Basically, and solve he same problem while making use of he idenical probabilisic moion and measuremen models. Furhermore, boh use Kalman filering: EKF SLAM applies he filer once o a high dimensional filering problem where employes M K iny EKFs K of hem in each paricle). x ekf fasslam comp 2/5

3 4 Differences 4.1 Sae Vecor A fundamenal advanage of over EKF based approaches o he SLAM problem is ha he EKF suffers from a OK 2 ) complexiy where K being he number of landmarks. In conras, has an OM log K) complexiy wih M = cons denoing he number of paricles. Sae vecor is comprised of curren pose esimae as well as landmark esimaes: x = s, θ 0, θ 1,..., θ K). The map managemen will have o change he size of he sae vecor a runime and updae a dimx ) dimx ) marix in each sep, dimx ) = 2K Poserior Densiy All densiies involved in he calculaion of he poserior p x z ) = p z x ) p x z 1) p z z 1 ) are modeled as Gaussians. No sae vecor as in, bu loose se of small pars insead; he K covariance marices in each paricle soring he landmark uncerainies have a small consan size here: 2 2). New observaions jus slighly aler some ree srucure soring he parameers of he Gaussians of each paricle. The problem is decomposed ino K +1 subproblems: one problem of esimaing a poserior over robo pahs s, and K problems of esimaing he locaions of he K landmarks, p s, θ z, n ) = p s z, n ) p θ k s, z, n ). k This facorizaion is exac. 4.3 Daa Associaion Differen approaches, e.g. iniializaion of a 3D line ino he map and successive esimaion of deph before inroducing i definiely. Bu: all aemps use jus one daa associaion, if i is wrong for some reason, e.g. because of an ambigous environmen, he filer will probably diverge. 4.4 Observaion Model The observaion model is simply assumed o have an underlying Normal disribuion, p z x ) Nhx ), R ). The covariance marix R is updaed according o a Jacobian calculaion in every ime sep. Each paricle has is own hypohesis of daa associaion, n [m]. The concep of resampling according o imporance weighs les wrong associaions disappear in expecaion) and makes he filer more robus. In addiion o he s observaion model, makes explici use of daa associaion informaion in n : p z θ k, s, n ). ekf fasslam comp 3/5

4 5 1.0 vs. 2.0 Basically, he only modificaion proposed by Version 2 is ha he proposal disribuion 2) should no only rely on he previous esimae of he pose s 1, bu also on he acual measuremen z, p s s 1,[m], z, n ). s [m] 5.1 Proposal Disribuion This secion roughly inroduces he consequences of he new choice for he proposal disribuion. I can be expressed as p s s 1,[m], z, n ) = η [m] wih p z θ n, s, n ) Nh θ n, s )), R )) p θ n s 1,[m], z 1, n 1) ) N µ [m] n, 1, Σ[m] n, 1 ) p s s [m] 1 N f s [m] 1 p z θ n, s, n ) p θ n s 1,[m], z 1, n 1) ) dθ n p s s [m] 1 ) ), P where h ) and f ) arise in he general SLAM problem as he measuremen model p z θ, s, n ) = h θ n, s ) + ɛ and he moion model p s s 1 ) = f s 1 ) + δ, respecively, are inroduced. Furhermore, ɛ N0, R ) and δ N0, Q ). However, he new proposal disribuion does no have a closed form unless h ) is linearized around he landmark posiion µ [m] n, 1 and he prediced pose esimae ŝ[m], ) ) h θ n, s ) ẑ [m] + H θ θ n µ [m] n, 1 + H s s ŝ [m]. ˆθ[m] where we used he prediced measurumen ẑ [m] = h n ). H θ and H s sand for he respecive Jacobians evaluaed a ŝ [m] [m], ˆθ n ). 5.2 Imporance Weighs The ways he imporance weighs are calculaed for Version 1 and 2 differ insofar ha in Fas- SLAM 2.0 one has o accoun for he normalizer η [m] ha was no here before. However, he imporance weighs in Version 2 are sill normally disribued wih mean ẑ [m] and covariance H s P H s + H θ Σ [m] n, 1 H θ + R. 5.3 Unknown Daa Associaion Similar o Version 1, 2.0 selecs ha associaion n ha maximizes he probabiliy of measuremen z for he m-h paricle cf. Secion 6.1). However, his probabiliy has o be modified in order o consider he sampled pose. Linearizing ) h ) and calculaing he new probabiliy leads o a Gaussian over z wih mean h µ [m] n, 1, s[m] and covariance R. 6 wih Unknown Daa Associaion Solving he unknown daa associaion problem is a crucial sep in since oherwise i would no be possible o decompose he poserior 1). Monemerlo e al. suggesed four ways o handle his problem [4]. ekf fasslam comp 4/5

5 6.1 Per-Paricle Maximum Likelihood Daa Associaion Wih Bayes and Markov one can show ha p n z ) ) p z s [m], n, n = arg max p n z ), n m where n is he ML esimae. This procedure is also adoped by EKFs, even hough here i is used on a per-paricle basis. Consequences are a) Noise in he pose esimaion will be filered ou, given a reasonable number of paricles. b) Delayed decision-making: pose ambiguiies ha appear reasonable a he ime he decision is aken) are resolved since hey will laer receive low imporance weighs. 6.2 Mone-Carlo Daa Associaion The mehod above can be aken a sep furher: each paricle can draw a random associaion weighed by he probabiliies of each landmark having generaed he observaion. However, uniformly high measuremen errors lead o an exponenial increase in he number of paricles required in comparison o he same scenario wih known daa associaion. 6.3 Muual Exclusion Muual Exclusion requires o consider more han one observaion per ime sep and allows o eliminae daa associaion hypoheses ha assign muliple measuremens o he same landmark. They propose o apply he muual exclusion consrain in a greedy fashion: each observaion is associaed wih he mos likely landmark in each paricle ha has no received an observaion ye compuaionally inexpensive since already mainains a se of daa associaion hypoheses). Thus, muual exclusion forces he creaion of a new landmark whenever he observaion canno be explained by he acual associaions. 6.4 Negaive Informaion Areas wihou known) landmarks canno be assumed o be empy, bu sill one can draw an inference from such informaion: if he sysem is expecing o see a paricular landmark bu acually does no, i should become less confiden abou he exisence of he landmark. This can be achieved by borrowing a echnique for making evidence grids a deailed descripion is given in [7]). References [1] A. Davison. Real-ime simulaneous localisaion and mapping wih a single camera, [2] Suar Kramer. A Bayesian perspecive on why he exended Kalman filer fails in passive racking. Technical repor, Air Force Insiue of Technology, [3] M. Monemerlo, S. Thrun, D. Koller, and B. Wegbrei. : A facored soluion o he simulaneous localizaion and mapping problem [4] Michael Monemerlo and Sebasian Thrun. Simulaneous localizaion and mapping wih unknown daa associaion using fasslam. In ICRA, pages , [5] Michael Monemerlo, Sebasian Thrun, Daphne Koller, and Ben Wegbrei. Fasslam 2.0: An improved paricle filering algorihm for simulaneous localizaion and mapping ha provably converges. In IJCAI, pages , [6] K. Murphy. Bayesian map learning in dynamic environmens [7] Sebasian Thrun. Learning Meric-Topological Maps Maps for Indoor Mobile Robo Navigaion. Arificial Inelligence, 991):21 71, [8] Greg Welch and Gary Bishop. An inroducion o he kalman filer. Technical Repor TR , ekf fasslam comp 5/5