FastSLAM 2.0: An Improved Particle Filtering Algorithm for Simultaneous Localization and Mapping that Provably Converges

Transcription

1 Proceedings of IJCAI 2003 FasSLAM 2.0: An Improved Paricle Filering Algorihm for Simulaneous Localizaion and Mapping ha Provably Converges Michael Monemerlo and Sebasian Thrun School of Compuer Science Carnegie Mellon Universiy Pisburgh, PA Absrac In [15], Monemerlo e al. proposed an algorihm called FasSLAM as an efficien and robus soluion o he simulaneous localizaion and mapping problem. This paper describes a modified version of FasSLAM which overcomes imporan deficiencies of he original algorihm. We prove convergence of his new algorihm for linear SLAM problems and provide real-world experimenal resuls ha illusrae an order of magniude improvemen in accuracy over he original FasSLAM algorihm. 1 Inroducion Simulaneous localizaion and mapping (SLAM) is a highly acive research area in roboics and AI. The SLAM problem arises when a moving vehicle (e.g. a mobile robo, submarine, or drone) simulaneously esimaes a map of is environmen and is pose relaive o ha map. In he absence of global posiion informaion, he vehicle s pose esimae will become increasingly inaccurae, as will is map. Since maps may conain housands of eniies, acquiring large, accurae maps is a challenging saisical esimaion problem, especially when performed in real-ime. Mos presen-day research on SLAM originaes from a seminal paper by Smih and Cheeseman [21], which proposed he use of he exended Kalman filer (EKF) for solving SLAM. This paper is based on he insighs ha errors in he map and pose errors are naurally correlaed, and ha he covariance marix mainained by he EKF expresses such correlaions. Newmann [18] recenly proved ha he EKF converges for linear SLAM problems, where he moion model and observaion model are linear funcions wih Gaussian noise (see below). Unforunaely, EKF covariance marices are quadraic in he size of he map, and updaing hem requires ime quadraic in he number of landmarks N. This quadraic complexiy has long been recognized o be a major obsacle in scaling SLAM algorihms o maps wih more han a few hundred feaures. I also limis he applicabiliy of SLAM algorihms o problems wih ambiguous landmarks, which induces a daa associaion problem [2; 22]. Today s mos robus algorihms for SLAM wih unknown daa associaion mainain muliple hypoheses (racks), which increase heir compuaional complexiy. Consequenly, here has been a flurry on research on more efficien SLAM echniques (see e.g., [11]). One group of researchers has developed echniques ha recursively divide maps ino submaps, hereby confining mos compuaion o small regions. Some of hese approaches sill mainain global correlaions among hose submaps, hence are quadraic bu Daphne Koller and Ben Wegbrei Compuer Science Deparmen Sanford Universiy Sanford, CA wih a much reduced consan facor [1; 7; 23; 26]. Ohers resric he updae exclusively o local maps [12], hence operae in consan ime (assuming known daa associaion). A second group of researchers has developed echniques ha represen maps hrough poenial funcions beween adjacen landmarks, similar o Markov random fields. The resuling represenaions require memory linear in he number of landmarks [19; 24]. Under appropriae approximaions, such echniques have been shown o provide consan ime updaing (again for known daa associaion). Unforunaely, no convergence proof exiss for any of hese exensions of he EKF, even for he generic case of linear SLAM. Furhermore, if landmarks are ambiguous, all of hese approaches have o perform search o find appropriae daa associaion hypoheses, adding a logarihmic facor o heir updae complexiy. The FasSLAM algorihm, proposed in [15] as an efficien approach o SLAM based on paricle filering [6], does no fall ino eiher of he caegories above. FasSLAM akes advanage of an imporan characerisic of he SLAM problem (wih known daa associaion): landmark esimaes are condiionally independen given he robo s pah [17]. FasSLAM uses a paricle filer o sample over robo pahs. Each paricle possesses N low-dimensional EKFs, one for each of he N landmarks. This represenaion requires O(NM) memory, where M is he number of paricles in he paricle filer. Updaing his filer requires O(M log N) ime, wih or wihou knowledge of he daa associaions. However, he number of paricles M needed for convergence is unknown and has been suspeced o be exponenial in he size of he map, in he wors-case. This paper proposes an improved version of he FasSLAM algorihm. The modificaion is concepually simple: When proposing a new robo pose an essenial sep in FasSLAM s paricle filer our proposal disribuion relies no only on he moion esimae (as is he case in FasSLAM), bu also on he mos recen sensor measuremen. Such an approach is less waseful wih is samples han he original FasSLAM algorihm, especially in siuaions where he noise in moion is high relaive o he measuremen noise. To obain a suiable proposal disribuion, our algorihm linearizes he moion and he measuremen model in he same manner as he EKF. As a resul, he proposal disribuion can be calculaed in closed form. This exension parallels prior work by Douce and colleagues, who proposed a similar modificaion for general paricle filers [6] and MCMC echniques for neural neworks [4]. I is similar o he arc reversal echnique proposed for paricle filers applied o Bayes neworks [10], and i is similar o recen work by van der Merwe [25], who uses an unscened filering sep [9] for generaing proposal disribuions ha accommodae he measuremen.

2 While his modificaion is concepually simple, i has imporan ramificaions. A key conribuion of his paper is a convergence proof for linear SLAM problems using a single paricle. The resuling algorihm requires consan updaing ime. To our knowledge, he bes previous SLAM algorihm for which convergence was shown requires quadraic updae ime. Furhermore, we observe experimenally ha our new FasSLAM algorihm, even wih a single paricle, yields significanly more accurae resuls on a challenging real-world benchmark [7] han he previous version of he algorihm. These findings are of significance, as many mobile robo sysems are plagued by conrol noise, bu possess relaively accurae sensors. Moreover, hey conradic a common belief ha mainaining he enire covariance marix is required for convergence [5]. 2 Simulaneous Localizaion and Mapping Simulaneous localizaion and mapping (SLAM) addresses he problem of simulaneously recovering a map and a vehicle pose from sensor daa. The map conains N feaures (landmarks) and shall be denoed Θ = θ 1,..., θ N. The pah of he vehicle will be denoed s = s 1,..., s, where is a ime index and s is he pose of he vehicle a ime. Mos sae-of-he-ar SLAM algorihms calculae (or approximae) varians of he following poserior disribuion: p(θ, s z, u, n ) (1) where z = z 1,..., z is a sequence of measuremens (e.g., range and bearing o nearby landmarks), and u = u 1,..., u is a sequence of robo conrols (e.g., velociies for robo wheels). (As usual, we assume wihou loss of generaliy ha only a single landmark is observed a each ime.) The variables n = n 1,..., n are daa associaion variables each n specifies he ideniy of he landmark observed a ime. Iniially, we assume n is known; we relax his assumpion below. To calculae he poserior (1), he vehicle is given a probabilisic moion model, in he form of he condiional probabiliy disribuion p(s u, s 1 ). This disribuion describes how a conrol u, assered in he ime inerval [ 1; ), affecs he resuling pose. Addiionally, he vehicle is given a probabilisic measuremen model, denoed p(z s, Θ, n ), describing how measuremens evolve from sae. In accordance o he rich SLAM lieraure, we will model boh models by nonlinear funcions wih independen Gaussian noise: p(z s, Θ, n ) = g(s, θ n ) + ε (2) p(s u, s 1) = h(u, s 1) + δ (3) Here g and h are nonlinear funcions, and ε and δ are Gaussian noise variables wih covariance R and P, respecively. 3 FasSLAM FasSLAM [15] is based on he imporan observaion [17] ha he poserior can be facored p(θ, s z, u, n ) = p(s z, u, n ) n p(θ n s, z, u, n ) (4) This facorizaion is exac and universal in SLAM problems. I saes ha if one (hypoheically) knew he pah of he vehicle, he landmark posiions could be esimaed independenly of each oher (hence he produc over n). In pracice, of course, one does no know he vehicle s pah. Neverheless, he independence makes i possible o facor he poserior ino a erm ha esimaes he probabiliy of each pah, and N erms ha esimae he posiion of he landmarks, condiioned on each (hypoheical) pah. FasSLAM samples he pah using a paricle filer. Each paricle has aached is own map, consising of N exended Kalman filers. Formally, he m-h paricle S conains a pah s, along wih Gaussian N landmark esimaes, described by he mean n, and covariance Σ n, : = s,, 1,, Σ 1,,..., µ N,, Σ N, (5) landmark θ 1 landmark θ N S We briefly reviews he key equaions of he regular Fas- SLAM algorihm, and refer he reader o [15]. Each updae in FasSLAM begins wih sampling new poses based on he mos recen moion command u : p(s s, u). (6) s Noe ha his proposal disribuion only uses he moion command u, bu ignores he measuremen z. Nex, FasSLAM updaes he esimae of he observed landmark(s), according o he following poserior. This poserior akes he measuremen z ino consideraion: 1 p(θ n s,, n, z ) (7) = η p(z θ n, s, n ) N (z ;g(θ n,s ),R ) p(θ n s 1,, z 1, n 1 ) N (θ n ; n, 1,Σ n, 1 ) Here η is a consan. This poserior is he normalized produc of wo Gaussians as indicaed. However, if g is non-linear, he produc will no be Gaussian in general. To make he resul Gaussian, FasSLAM employs he sandard EKF rick [13]: g is approximaed by a linear funcion (see below). Under his approximaion, (7) is equivalen o he measuremen updae equaion familiar from he EKF lieraure [13]. In a final sep, FasSLAM correcs for he fac ha he pose sample s has been generaed wihou consideraion of he mos recen measuremen. I does so by resampling he paricles [20]. The probabiliy for he m-h paricle o be sampled (wih replacemen) is given by he following variable w, commonly referred o as imporance facor: w = η p(z θ n, s, n ) N (z ;g(θ n,s ),R ) p(θ n s 1,, z 1, n 1 ) N ( n, 1,Σ n, 1 ) dθ n As shown in [15], he resampling operaion can be implemened in O(M log N) ime using rees, where M is he number of samples and N he number of landmarks in he map. However, he number of paricles M needed for convergence remains an open quesion. FasSLAM has been exended o SLAM wih unknown daa associaions [14]. If he daa associaion is unknown, each paricle m in FasSLAM makes is own local daa associaion decision ˆn, by maximizing he measuremen likelihood. The formula for finding he mos likely daa associaion maximizes he resuling imporance weigh: ˆn = argmax n w (n ) (8)

3 Here w (n ) makes he dependence of he facor w on he variable n explici. A key characerisic of FasSLAM is ha each paricle makes is own local daa associaion. In conras, EKF echniques mus commi o a single daa associaion hypohesis for he enire filer. Resuls in [14] show empirically ha his difference renders FasSLAM significanly more robus o noise han EKF-syle algorihms. 4 FasSLAM 2.0 Our new FasSLAM algorihm is based on an obvious inefficiency arising from he proposal disribuion of regular Fas- SLAM. In regular FasSLAM, he pose s is sampled in accordance o he predicion arising from he moion command u, as specified in (6). I does no consider he measuremen z acquired a ime ; insead, he measuremen is incorporaed hrough resampling. This approach is paricularly roublesome if he noise in he vehicle moion is large relaive o he measuremen noise. In such siuaions, sampled poses will mosly fall ino areas of low measuremen likelihood, and will subsequenly be erminaed in he resampling phase wih high probabiliy. Unforunaely, many real-world robo sysems are characerized by relaively high moion noise. As illusraed in he experimenal resuls secion of his paper, he wase incurred by his inefficien sampling scheme can be significan. 4.1 Sampling The Pose FasSLAM 2.0 implemens a single new idea: Poses are sampled under consideraion of boh he moion u and he measuremen z. This is formally denoed by he following sampling disribuion, which now akes he measuremen z ino consideraion: s p(s s 1,, u, z, n ) (9) In comparison o (6), incorporaing he measuremen only makes sense if we incorporae our curren esimae of he observed landmark obained from he variables s 1,, u 1, z 1, n 1 (which are included of he condiioning variables above). So in essence, he difference o Fas- SLAM is ha he measuremen z is incorporaed. However, his change has imporan ramificaions. The proposal disribuion (9) can be reformulaed as follows: p(s s 1,, u, z, n ) = η p(z θ n, s, n ) N (z ;g(θn,s ),R ) p(s s 1, u) N (s ;h(s 1,u ),P ) p(θ n s 1,, z 1, n 1 ) N (θn ; n, 1,Σ n, 1 ) dθ n Tha is, he proposal disribuion is he produc of wo facors: he familiar nex sae disribuion p(s s 1, u ), and he probabiliy of he measuremen z. Calculaing he laer involves an inegraion over possible landmark locaions θ n. Unforunaely, sampling direcly from his disribuion is impossible in he general case; i does no even possess a closed form. Luckily, a closed form soluion can be aained if g is approximaed by a linear funcion (h may remain non-linear!): g(θ n, s ) ẑ + G θ (θ n n, 1) + Gs (s ŝ ) (10) where ẑ, ŝ ) denoes he prediced measure- = h(s 1, u ) he prediced robo pose, and ˆθ n = men, ŝ = g(ˆθ n n, 1 he prediced landmark locaion. The marices G θ and G s are he Jacobians (firs derivaives) of g wih respec o θ and s, respecively: G θ = θn g(θ n, s ) s =ŝ ;θ n =ˆθ (11) n G s = s g(θ n, s ) s =ŝ ;θ n =ˆθ n (12) Under his EKF-syle approximaion, he proposal disribuion (9) is Gaussian wih he following parameers: [ ] 1 Σ s = G T s Q 1 G s + P 1 (13) s = Σ G T s Q 1 (z ẑ ) + ŝ (14) where he marix Q Q s is defined as follows: = R + G θ Σ n, 1 GT θ (15) 4.2 Updaing The Observed Landmark Esimae The updaing sep remains he same as in FasSLAM (see (7)). As saed in he previous secion, g is linearized o reain Gaussianiy of he poserior. This leads o he following updae equaions, whose derivaion is equivalen o ha of he sandard EKF measuremen updae [13]: K = Σ n, 1 GT θ Q 1 (16) n, = Σ n, 1 + K (z ẑ ) (17) n, = (I K G θ )Σ n, 1 (18) 4.3 The Imporance Weighs Resampling is necessary even in our new version of Fas- SLAM, since he paricles generaed do no ye mach he desired poserior. The culpri is he normalizer η in (10), which may be differen for differen paricles m. This normalizer is he inverse of he probabiliy of he measuremen under he m-h paricle: η = p(z s 1,, u, z 1, n ) 1. To accoun for his mismach, our algorihm resamples in proporion o he following imporance facor: w p(z s 1,, u, z 1, n ) = p(z θ n, s, n ) N (z ;g(θn,s ),R ) p(θ n s 1,, u 1, z 1, n 1 ) N (θn ; n, 1,Σ n, 1 ) dθ n p(s s 1 }{{, u) ds } N (s ;ŝ 1,P ) This expression can once again be approximaed as a Gaussian by linearizing g. The mean of his Gaussian is ẑ, and is covariance is G sp G T s + G θ Σ n, 1 GT θ + R (19) 4.4 Unknown Daa Associaions The approach for handling daa associaion is similar o he one in regular FasSLAM: Again, we selec he daa associaion n ha maximizes he probabiliy of he measuremen z for he m-h paricle: ˆn = argmax n p(z n, ˆn 1,, s,, z 1, u ) (20)

4 (a) Raw vehicle odomery (b) FasSLAM 2.0, M=1 paricle (c) Same w. dynamic feaure managemen Figure 1: FasSLAM 2.0 applied o he Vicoria Park benchmark daa se using only M=1 paricle. The accuracy of he recovered pah and he resuling map is indisinguishable from ha he bes EKF-syle mehods and he original FasSLAM algorihm wih M=100 paricles. A firs glance, one may be emped o subsiue w for he probabiliy on he righ-hand side, as in regular Fas- SLAM. However, w does no consider he sampled pose s, whereas he expression here does. This leads o a slighly differen probabiliy, which is calculaed as follows. p(z n, ˆn 1,, s,, z 1, u ) = p(z θ n, n, s ) N (z ;g(θn,s ),R ) p(θ n ˆn 1,, s 1,, z 1 ) N ( n, 1,Σ n, 1 ) dθ(21) n Linearizaion of g leads o a Gaussian over z wih mean g( n,, 1 s ) and covariance Q. Boh are funcions of he daa associaion variable n. 4.5 Feaure Managemen Finally, in cases wih unknown daa associaions, feaures have o creaed dynamically. As is common for SLAM algorihms [5], our approach creaes new feaures when he measuremen probabiliy in (20) is below a hreshold. However, real-world daa wih frequen ouliers will generae spurious landmarks using his rule. Following [5], our approach removes such spurious landmarks by keeping rack of heir poserior probabiliy of exisence. Our mechanism analyzes measuremen o he presence and absence of feaures. Observing a landmark provides posiive evidence for is exisence, whereas no observing i when n falls wihin he robo s percepual range provides negaive evidence. The poserior probabiliy of landmark exisence is accumulaed by he following Bayes filer, whose log-odds form is familiar from he lieraure on occupancy grid maps [16]: τ n = ln p(i n 1 p(i n s, z, ˆn ) s, z, ˆn ) (22) Here τ n are he log-odds of he physical exisence of landmark θ n in map m, and p(i n s, z, ˆn ) is he probabilisic evidence provided by a measuremen. Under appropriae definiion of he laer, his rule provides for a simple evidence couning rule. If he log odds drops below a predefined hreshold, he corresponding landmark is removed from he map. This mechanism enables paricles o free hemselves of spurious feaures. 5 Convergence A key resul in his paper is he fac ha our new version of FasSLAM converges for M =1 paricle, for a resriced class of linear Gaussian problems (he same for which KFs converge [5; 18]). Specifically, our resul applies o SLAM problems characerized by he following linear form: g(s, θ n ) = θ n s (23) h(u, s 1) = u + s 1 (24) Linear SLAM can be hough of as a robo operaing in a Caresian space equipped wih a noise-free compass, and sensors ha measure disances o feaures along he coordinae axes. The following heorem, whose proof can be found in he appendix, saes he convergence of our new FasSLAM varian: Theorem. For linear SLAM, FasSLAM wih M =1 paricles converges in expecaion o he correc map if all feaures are observed infiniely ofen, and if he locaion of one feaure is known in advance. This heorem parallels a similar resul previously published for he Kalman filer [5; 18]. However, his resul applies o he Kalman filer, whose updae requires ime quadraic in he number of landmarks N. Wih M=1, he resampling sep becomes obsolee and each updae akes consan ime. To our knowledge, our resul is he firs convergence resul for a consan-ime SLAM algorihm. I even holds if all feaures are arranged in a large loop, a siuaion ofen hough of as he wors case for SLAM problems [8]. 6 Experimenal Resuls Sysemaic experimens showed ha FasSLAM 2.0 provides excellen resuls wih surprisingly few paricles, including M=1. Mos of our experimens were carried ou using a benchmark daa se colleced wih an oudoor vehicle in Vicoria Park, Sydney [7]. The vehicle pah is 3.5km long, and he map is 320 meers wide. The vehicle is equipped wih differenial GPS ha is used for evaluaion only. Fig. 1a shows he map of he errain, along wih he pah obained by raw odomery (which is very poor, he average RMS error is 93.6 meers). This daa se is presenly he mos popular benchmark in he SLAM research communiy [3]. Figs. 1b&c show he resul of applying FasSLAM wih M=1 paricle o he daa se, wihou (Fig. 1b) and wih

5 (a) (b) RMS Pose Error (meers) RMS Pose Error (meers) Accuracy of FasSLAM on Vicoria Park Daase Number of Paricles Accuracy of FasSLAM Algorihms On Simulaed Daa FasSLAM 1.0 FasSLAM 2.0 FasSLAM 1.0 FasSLAM Number of Paricles Figure 2: RMS map error for regular FasSLAM (dashed line) versus FasSLAM 2.0 (solid line) on (a) he Vicoria Park daa (b) simulaed daa. FasSLAM 2.0 s resuls even wih a single paricle are excellen. (Fig. 1c) he feaure managemen approach described in Secion 4.5. In boh cases, he esimaed vehicle pah is shown in yellow, and he GPS informaion is shown in blue. Resuls of he same accuracy were previously achieved only wih O(N 2 ) EKF-syle mehods [7] and wih FasSLAM using M=50 paricles. The feaure managemen rule reduces he number of landmarks in he map from 768 (Fig. 1b) o 343 (Fig. 1c). Fig. 2 plos he RMS error of he vehicle posiion esimae as funcion of he number of paricles for he Vicoria daa se (panel a) and for synheic simulaion daa (panel b) aken from [15]. While our new algorihm does approximaely equally well for any number of paricles, regular FasSLAM performs poorly for very small paricle ses. We suspec ha he poor performance of regular FasSLAM is due o he fac ha he vehicle possesses relaively inaccurae odomery (see Fig. 1a), ye uses a low-noise range finder for landmark deecion (a common configuraion in oudoor roboics), leading o he generaion of many paricles of low likelihood. The small number of examples needed o obain sae-ofhe-ar esimaion ranslaes o unprecedened efficiency of he new filer. The following able shows he resuls required o process he Vicoria Park daa se on a 1GHz Penium PC: EKF 7,807 sec regular FasSLAM, M=50 paricles 403 sec FasSLAM 2.0, M=1 paricle 140 sec In comparison, he daa acquisiion required 1,550 seconds. Thus, while EKFs canno be run in real-ime, our new algorihm requires less han 10% of he vehicle s rajecory ime. 7 Discussion This paper describes a modified FasSLAM algorihm ha is uniformly superior o he FasSLAM algorihms proposed in [15]. The new FasSLAM algorihm uilizes a differen proposal disribuion which incorporaes he mos recen measuremen in he pose predicion process. In doing so, i makes more efficien use of he paricles, paricularly in siuaions in which he moion noise is high in relaion o he measuremen noise. A main conribuion of his paper is a convergence proof for FasSLAM wih a single paricle. This proof is an improvemen over previous formal resuls, which applied o algorihms much less efficien han he curren one. In fac, his resul is a firs convergence resul for a consan ime SLAM algorihm. The heoreical finding is complemened by experimenal resuls using a sandard benchmark daa se. The new algorihm is found o ouperform he previous FasSLAM algorihm and he EKF approach o SLAM by a large margin. In fac, a single paricle suffices o generae an accurae map of a challenging benchmark daa se. Despie his surprising resul, he use of muliple paricles is clearly warraned in siuaions wih ambiguous daa associaion. We believe ha our resuls illusrae ha SLAM can be solved robusly by algorihms ha are significanly more efficien han EKF-based algorihms. References [1] T. Bailey. Mobile Robo Localisaion and Mapping in Exensive Oudoor Environmens. PhD hesis, Univ. of Sydney, [2] Y. Bar-Shalom and T. E. Formann. Tracking and Daa Associaion. Academic Press, [3] SLAM summer school See hp:// [4] N. de Freias, M. Niranjan, A. Gee, and A. Douce. Sequenial mone carlo mehods o rain neural nework models. Neural Compuaion, 12(4), [5] G. Dissanayake, P. Newman, S. Clark, H.F. Durran-Whye, and M. Csorba. A soluion o he simulaneous localisaion and map building (SLAM) problem. IEEE Trans. Roboics and Auomaion, [6] A. Douce, J.F.G. de Freias, and N.J. Gordon, ediors. Sequenial Mone Carlo Mehods In Pracice. Springer, [7] J. Guivan and E. Nebo. Opimizaion of he simulaneous localizaion and map building algorihm for real ime implemenaion. Trans. of Roboics and Auomaion, [8] J.-S. Gumann and K. Konolige. Incremenal mapping of large cyclic environmens. Proc. CIRA, [9] S. J. Julier and J. K. Uhlmann. A new exension of he Kalman filer o nonlinear sysems. Proc. AeroSense, [10] K. Kanazawa, D. Koller, and S.J. Russell. Sochasic simulaion algorihms for dynamic probabilisic neworks. UAI-95. [11] Noes ICRA Workshop Concurren Mapping and Localizaion for Auonomous Mobile Robos, 2002 [12] J.J. Leonard and H.J.S. Feder. A compuaionally efficien mehod for large-scale concurren mapping and localizaion. Proc. IRSS, [13] P. Maybeck. Sochasic Models, Esimaion, and Conrol, Volume 1. Academic Press, [14] M. Monemerlo and S. Thrun. Simulaneous localizaion and mapping wih unknown daa associaion using FasSLAM. A hp:// hrun/papers/ monemerlo.fasslam-daaassoc03.hml [15] M. Monemerlo, S. Thrun, D. Koller, and B. Wegbrei. Fas- SLAM: A facored soluion o he simulaneous localizaion and mapping problem. Proc. AAAI, [16] H. P. Moravec. Sensor fusion in cerainy grids for mobile robos. AI Magazine, 9(2), [17] K. Murphy. Bayesian map learning in dynamic environmens. Proc. NIPS, 1999.

6 [18] P. Newman. On he Srucure and Soluion of he Simulaneous Localisaion and Map Building Problem. PhD hesis, Univ. of Sydney, [19] M.A. Paskin. Thin juncion ree filers for simulaneous localizaion and mapping. TR UCB/CSD , UC Berkeley, [20] D.B. Rubin. Using he SIR algorihm o simulae poserior disribuions. In Bayesian Saisics 3. Oxford Univ. Press, [21] Randall C. Smih and Peer Cheeseman. On he represenaion and esimaion of spaial uncerainy. In. J. Roboics Research, 5(4), [22] J.D. Tardós, J. Neira, P. Newman, and J. Leonard. Robus mapping and localizaion in indoor environmens using sonar daa. TR TM , MIT, [23] J.D. Tardós, J. Neira, P.M. Newman, and J.J. Leonard. Robus mapping and localizaion in indoor environmens using sonar daa. In. J. Roboics Research, 21(4), [24] S. Thrun, D. Koller, Z. Ghahramani, H. Durran-Whye, and A.Y. Ng. Simulaneous mapping and localizaion wih sparse exended informaion filers. Proc. WAFR, [25] R. van der Merwe, N. de Freias, A. Douce, and E. Wan. The unscened paricle filer. Proc. NIPS, [26] S.B. Williams, G. Dissanayake, and H. Durran-Whye. An efficien approach o he simulaneous localisaion and mapping problem. Proc. ICRA, Appendix The linear form (23) and (24) implies ŝ n, 1 s 1 u, G θ = I, G s = I, and Q = s 1 + u, ẑ = = R + Σ n, 1. From ha we obain for he mean and covariance (13) and (14) of he proposal disribuion: Σ s = s = Σ [ ] 1 (R + Σ n, 1 ) 1 + P 1 (25) s (R + Σ n, 1 ) 1 (z n, 1 + s 1 + u) +s 1 + u (26) The updae of he landmark mean (Eq. (16) and (17)) resolves o: n, = n, 1 + Σ n, 1 (R + Σ n, 1 ) 1 (z n, 1 + s 1 + u)(27) We define he error in he robo pose and landmark locaions as: α = s s and β n, = n, θ n (28) We firs characerize he effec of map errors β on he pose error α: Lemma 1. If he error β n, of he observed landmark z a ime is smaller in magniude han he robo pose error α, α shrinks in expecaion as a resul of his measuremen. Conversely, if β n, is larger han he pose error α, he laer may increase, bu in expecaion will no exceed β n,. Proof. The expeced error of he robo pose a ime is given by E[α ] = E[s s ] = E[s ] E[s ] (29) The firs erm is obained via he sampling disribuion (26), and he second erm is obained from he linear moion model (24), giving: E[α ] = Σ (R + Σ n, 1 ) 1 (E[z ] s n, 1 + s 1 + u) + α 1 (30) For linear SLAM, he expecaion E[z ] = θ n E[s ] = θ n u s 1. Wih ha, he expression in he brackes becomes E[z ] n, 1 + s 1 + u = θ n u s 1 n, 1 + s 1 + u = α 1 β n, 1 (31) Subsiuing his back ino (30) and subsequenly subsiuing Σ s according o (25) gives us: E[α ] (32) = α 1 + Σ s (R + Σ = α 1 + [ I + (R + Σ n, 1 n, 1 ) 1 (β n, 1 α 1 ) )P 1 ] 1 (β n, 1 α 1 ) The lemma follows from he fac ha R, Σ 1 n, 1, and P are posiive semidefinie, hence he inverse of I + (R + Σ 1 n, 1)P is a conracion marix. E[α depends on he sign of β n, 1 exceed β in his case. qed. n, 1 ] is larger in magniude if and only if α > α 1 1 ; however, E[α ] canno Of paricular ineres is he resul of observing he anchoring landmark, by which we mean he landmark whose locaion is known. Wihou loss of generaliy, we assume ha his landmark is θ 1. Lemma 2. If he robo observes he anchoring landmark θ 1, is pose error will shrink in expecaion. Proof. The anchoring landmark has zero error: β 1, = 0, and is covariance is also zero: Σ 1, = 0. Plugging his ino (32), we ge: 1 + [ ] I + (R + 0)P 1 1 (0 α 1 ) = α 1 [ ] I + R P 1 1 α 1 (33) E[α ] = α qed. Finally, a lemma similar o Lemma 1 can be saed on he effec of pose errors α on map errors β. Is proof is analogous ha of Lemma 1, wih reverse roles of α and β. Lemma 3. If he pose error α 1 is smaller han he error β n, of he observed landmark z in magniude, observing z shrinks he landmark error β n, in expecaion. Conversely, if α 1 is larger han he landmark error β n,, he laer may increase, bu in expecaion will no exceed α 1. Proof of Theorem. Le ˆβ denoe landmark error ha is larges in magniude among all landmark errors a ime. ˆβ = argmax β n, (34) β n, Lemma 3 suggess ha his error may increase in expecaion, bu only if he absolue robo pose error α 1 exceeds his error in magniude. However, in expecaion his will only be he case for a limied number of ieraions. In paricular, Lemma 1 guaranees ha α 1 may only shrink in expecaion. Furhermore, Lemma 2 saes ha every ime he anchoring landmark is observed, his error will shrink by a finie amoun, regardless of he magniude of ˆβ. Hence, α 1 will ulimaely become smaller in magniude (and in expecaion) han he larges landmark error. Once his has happened, Lemma 3 saes ha he laer will shrink in expecaion every ime he landmark is observed whose error is larges. I is now easy o see ha boh ˆβ and α 1 converge o zero: Observing he anchoring landmark induces a finie reducion as saed in (33). To increase α 1 o is old value in expecaion, he oal landmark error mus shrink in expecaion (Lemma 3). This leads o an eernal shrinkage of he oal landmark error down o zero. Since his error is an upper bound for he expeced pose error (see Lemma 1), we also have convergence in expecaion for he robo pose error. qed.