Simultaneous Localization and Mapping with Unknown Data Association Using FastSLAM

Transcription

1 Simulaneous Localizaion and Mapping wih Unknown Daa Associaion Using FasSLAM Michael Monemerlo, Sebasian Thrun Absrac The Exended Kalman Filer (EKF has been he de faco approach o he Simulaneous Localizaion and Mapping (SLAM problem for nearly fifeen years. However, he EKF has wo serious deficiencies ha preven i from being applied o large, realword environmens: quadraic complexiy and sensiiviy o failures in daa associaion. FasSLAM, an alernaive approach based on he Rao-Blackwellized Paricle Filer, has been shown o scale logarihmically wih he number of landmarks in he map [10]. This efficiency enables FasSLAM o be applied o environmens far larger han could be handled by he EKF. In his paper, we will show ha FasSLAM also subsanially ouperforms he EKF in environmens wih ambiguous daa associaion. The performance of he wo algorihms is compared on a real-world daa se wih various levels of odomeric noise. In addiion, we will show how negaive informaion can be incorporaed ino FasSLAM in order o improve he accuracy of he esimaed map. I. INTRODUCTION The problem of simulaneous localizaion and mapping, also known as SLAM, has araced immense aenion in he mobile roboics lieraure. SLAM addresses he problem of building a map of an unknown environmen from a sequence of noisy landmark measuremens obained from a moving robo. Since robo moion is also subjec o error, he mapping problem necessarily induces a robo localizaion problem hence he name SLAM. SLAM is considered by many o be an essenial capabiliy for auonomous robos operaing in environmens where precise maps and posiioning are no available [3], [7], [14]. The dominan approach o he SLAM problem was inroduced in a seminal paper by Smih, Self, and Cheeseman [13]. This paper proposed he use of he Exended Kalman Filer (EKF for incremenally esimaing he join poserior disribuion over robo pose and landmark posiions. In he las decade, his approach has found widespread accepance in field roboics, as a recen uorial paper documens []. EKF-based approaches o SLAM suffer from wo imporan limiaions. Firs, sensor updaes require ime quadraic in he oal number of landmarks K in he map. This complexiy sems from he fac ha he covariance marix mainained by he Kalman filer has O(K elemens, all of which mus be updaed even if jus a single landmark is observed. Quadraic complexiy limis he number of landmarks ha can be handled by his approach Michael Monemerlo and Sebasian Thrun are wih he Roboics Insiue a Carnegie Mellon Universiy, Pisburgh, Pennsylvania. {mmde, hrun}@cs.cmu.edu o only a few hundred whereas naural environmen models frequenly conain millions of feaures. Second, EKF-based SLAM algorihms rely heavily on he assumpion ha he mapping beween observaions and landmarks is known. Associaing a small number of observaions wih incorrec landmarks in he EKF can cause he filer o diverge. The problem of deermining he correc mapping of observaions o landmarks is commonly referred o as he daa associaion, or correspondence problem. An alernaive approach o he SLAM problem has been inroduced ha facors he SLAM poserior ino a localizaion problem and K independen landmark esimaion problems condiioned on he robo pose esimae. This algorihm, called FasSLAM [10], uses a modified paricle filer for esimaing he poserior over robo pahs. Each paricle possesses K independen Kalman filers ha esimae he landmark locaions condiioned on he paricle s pah. The resuling algorihm is an insance of he Rao- Blackwellized paricle filer [5], [11]. By represening paricles as binary rees of Kalman Filers, observaions can be incorporaed ino FasSLAM in O(M logk ime, where M is he number of paricles, and K is he number of landmarks. FasSLAM has been demonsraed wih up o 100,000 landmarks, problems far beyond he reach of he EKF. Since each paricle represens a differen robo pose hypohesis, daa associaion can be considered separaely for every paricle. This has wo advanages. Firs, he noise of robo moion does no affec he accuracy of daa associaion. Second, if observaions are associaed correcly in some paricles and incorrecly in ohers, he incorrec paricles will receive lower probabiliies and will be removed in fuure resampling seps. In his way, FasSLAM can forge incorrec associaions from he pas, when oher correc associaions beer explain he daa. We will demonsrae ha FasSLAM subsanially ouperforms he Kalman Filer on real-world daa wih ambiguous daa associaion. By adding exra odomeric noise, we will show ha FasSLAM coninues o perform well in siuaions in which he Kalman Filer ineviably diverges. In fac, FasSLAM can esimae an accurae map in his environmen wihou any odomery a all. Finally, we will show how o incorporae negaive informaion ino Fas- SLAM. The consideraion of negaive evidence resuls in a measurable increase in he accuracy of he resuling map.

2 II. SLAM PROBLEM DEFINITION The SLAM problem is bes described as a probabilisic Markov chain. The robo s pose a ime will be denoed s. If he robo is operaing in a planar environmen, his pose is he robo s x, y posiion and is heading orienaion. The robo s environmen is assumed o be comprised of K immobile, poin landmarks. Each landmark is characerized by is locaion in space, denoed θ i for i 1,..., K. The se of all landmarks will be denoed as θ. Robo poses evolve according o a probabilisic law, referred o as he moion model: p(s u, s 1 (1 The curren pose s is a probabilisic funcion of he robo conrol u and he previous pose s 1. To map is environmen, he robo can sense landmarks. I may be able o measure range and bearing o landmarks, relaive o is local coordinae frame. The measuremen a ime will be denoed z. Sensor measuremens are also governed by a probabilisic law, referred o as he measuremen model: p(z s, θ n, n ( where n is he index of he landmark currenly being perceived. The observaion z is a probabilisic funcion of he curren pose of he robo s and he landmark being observed θ n. While robos ofen can sense more han one landmark a a ime, we follow he common pracice of assuming ha each observaion corresponds o a measuremen of exacly one landmark []. This convenion is adoped solely for mahemaical convenience. I poses no resricion, as muliple landmark sighings a a single ime sep can be processed sequenially. In shor, SLAM is he problem of deermining he locaions of all landmarks θ and robo poses s from measuremens z z 1,..., z and conrols u u 1,..., u. In probabilisic erms, his is expressed by he following poserior: p(s, θ z, u (3 Here we use he superscrip o refer o a se of variables from ime 1 o ime. If he associaions n are known, he SLAM problem is simpler. The poserior becomes: p(s, θ z, u, n (4 III. DATA ASSOCIATION In real-world SLAM problems, he mapping n beween observaions and landmarks is rarely known. The oal number of landmarks K is also unknown. Every ime he robo makes an observaion, ha reading mus be corresponded wih an exising landmark or considered as coming from a previously unseen landmark. If his mapping is no obvious, picking he wrong associaion can cause Landmark posiion uncerainy Fig. 1. Measuremen Ambiguiy: Two landmarks (shown as black circles are close enough ha he observaion (shown as an x plausibly could have come from eiher one. a filer o diverge. A beer undersanding of how uncerainy in he SLAM poserior generaes ambiguiy in daa associaion will demonsrae how simple daa associaion heurisics ofen fail. Two facors conribue o uncerainy in he SLAM poserior: measuremen noise and moion noise. Each leads o a differen ype of daa associaion ambiguiy. Noise in he measuremen model will resul in higher uncerainy in he landmark posiions. Uncerain landmark posiions will lead o measuremen ambiguiy, or confusion beween nearby landmarks. (See Figure 1. A misake due o measuremen ambiguiy will have a relaively small effec on esimaion error because he observaion plausibly could have come from eiher landmark.? Pose uncerainy Fig.. Moion Ambiguiy: Observaions may be associaed wih compleely differen landmarks if he orienaion of he robo changes a small amoun. Ambiguiy due o moion noise can have much more severe consequences. Higher moion noise will lead o higher robo pose uncerainy afer incorporaing a conrol. If his uncerainy is high enough, differen plausible poses of he robo will lead o drasically differen daa associaion hypoheses for he subsequen observaions. Moion ambiguiy is easily induced if here is significan angular uncerainy in he robo pose esimae. (See Figure. If muliple observaions are incorporaed per imesep, he moion of he robo will correlae he associaions of all of he landmarks. If a SLAM algorihm chooses he wrong associaion for a single landmark due o moion ambiguiy, wih high probabiliy he res of he associaions will also be wrong. This will add a large amoun of error o he robo s pose, and ofen cause a filer o diverge. EKF SLAM algorihms commonly deermine daa associaion using a maximum likelihood approach. Each observaion is associaed wih he landmark ha was mos likely o have generaed i. A every ime sep, only he single mos probable daa associaion hypohesis is considered. More sophisicaed EKF daa associaion algorihms have

3 Θ 1 s1 Θ z s s 3 z z... u u u Fig. 3. The SLAM problem: The robo moves hrough poses s 1... s based on a sequence of conrols, u 1... u. As i moves, i observes nearby landmarks. A ime 1, i observes landmark θ 1. The measuremen is denoed z 1. A ime, i observes he oher landmark, θ, and a ime 3, i observes θ 1 again. The SLAM problem is concerned wih esimaing he locaions of he landmarks and he robo s pah from he conrols u and he measuremens z. The gray shading illusraes he fac ha knowledge of he robo s pah renders he landmark posiions θ 1 and θ condiionally independen. been developed o consider he bes associaion of all observaions simulaneously [1], however hese approaches sill rely on a single daa associaion hypohesis a every imesep. In a scenario wih ambiguous daa associaion, an algorihm ha mainains a single daa associaion will someimes pick he wrong associaion. If he ambiguiy is due o he robo s moion, his will lead o divergence of he EKF. The following secions of his paper will describe Fas- SLAM, an alernaive approach o he SLAM problem ha can sample over muliple daa associaion hypoheses. Experimenal daa will show ha his resuls in beer performance in siuaions wih significan moion ambiguiy. IV. FASTSLAM WITH KNOWN DATA ASSOCIATION Figure 3 illusraes a generaive probabilisic model (dynamic Bayes nework ha describes he SLAM problem. From his diagram i is clear ha he SLAM problem conains imporan condiional independences. In paricular, knowledge of he robo s pah s 1,..., s renders he individual landmark measuremens independen. So for example, if an oracle provided us wih he exac pah of he robo, he problem of deermining he landmark locaions could be decoupled ino K independen esimaion problems, one for each landmark. This condiional independence is he basis for he FasSLAM algorihm. This condiional independence implies ha he poserior (4 can be facored as follows ino a robo pah poserior and a produc of individual landmark poseriors condiioned on he robo s pah: p(s, θ z, u, n p(s z, u, n p(θ i s, z, u, n (5 s z A derivaion of his facorizaion is given in he Appendix. FasSLAM esimaes he facored SLAM poserior using a modified paricle filer, wih K independen Kalman Filers for each paricle o esimae he landmark posiions condiioned on he hypohesized robo pahs. The resuling algorihm is an insance of he Rao-Blackwellized paricle filer [5], [11]. A. Paricle Filer Pah Esimaion FasSLAM esimaes he robo pah poserior in (5 using a paricle filer, in a way ha is similar (bu no idenical o he Mone Carlo Localizaion (MCL algorihm [1]. A each poin in ime, FasSLAM mainains a se of paricles represening he poserior p(s z, u, n, denoed S. Each paricle s,[m] S represens a guess of he robo s pah: S {s,[m] } m {s [m] 1, s [m],..., s [m] } m (6 We use he superscrip noaion [m] o refer o he m-h paricle in he se. The paricle se S is calculaed incremenally, from he se S 1 a ime 1, a robo conrol u, and a measuremen z. Firs, each paricle s,[m] in S 1 is used o generae a probabilisic guess of he robo s pose a ime : s [m] p(s u, s [m] 1 (7 This guess is obained by sampling from he probabilisic moion model. This esimae is hen added o a emporary se of paricles, along wih he pah s 1,[m]. Under he assumpion ha he se of paricles in S 1 is disribued according o p(s 1 z 1, u 1, n 1 (which is an asympoically correc approximaion, he new paricle is disribued according o: p(s z 1, u, n 1 (8 This disribuion is commonly referred o as he proposal disribuion of paricle filering. Afer generaing M paricles in his way, he new se S is obained by sampling from he emporary paricle se. Each paricle s,[m] is drawn (wih replacemen wih a probabiliy proporional o a so-called imporance facor w [m], which is calculaed as follows [9]: w [m] arge dis. proposal dis. p(s,[m] z, u, n p(s,[m] z 1, u, n 1 The exac calculaion of (9 will be discussed furher below. The resuling sample se S is disribued according o an approximaion o he desired pah poserior p(s z, u, n, an approximaion which is correc as he number of paricles M goes o infiniy. We also noice ha only he mos recen robo pose esimae s [m] 1 is used when generaing he paricle se S. This will allows us o silenly forge all oher pose esimaes, rendering he size of each paricle independen of he ime index. (9

4 B. Landmark Locaion Esimaion FasSLAM represens he condiional landmark esimaes p(θ i s, z, u, n in (5 using Kalman filers. Since his esimae is condiioned on he robo pose, he Kalman filers are aached o individual pose paricles in S. More specifically, he full poserior over pahs and landmark posiions in he FasSLAM algorihm is represened by he sample se S {s,[m], µ [m] 1, Σ [m] 1,..., µ [m] K, Σ[m] K } m (10 Here µ [m] i and Σ [m] i are he mean and covariance of he Gaussian represening he i-h landmark θ i, aached o he m-h paricle. In he planar robo navigaion scenario, each mean µ [m] i is a wo-elemen vecor, and Σ [m] i is a by marix. The poserior over he i-h landmark pose θ i is easily obained. Is compuaion depends on wheher or no he landmark was observed. If he landmark is observed, we obain: p(θ in s, z, u, n (11 p(z θ n, s, n p(θ n s 1, z 1, u 1, n 1 If landmark i is no observed, we simply leave he Gaussian unchanged: p(θ i n s, z, u, n p(θ i s 1, z 1, u 1, n 1 (1 The FasSLAM algorihm implemens he updae equaion (11 using he exended Kalman filer (EKF. As in exising EKF approaches o SLAM, his filer uses a linearized version of he percepual model p(z s, θ n, n []. One significan difference beween FasSLAM s use of Kalman filers and ha of he radiional SLAM algorihm is ha he updaes in he FasSLAM algorihm involve only a Gaussian of dimension wo (for he wo landmark locaion parameers. In he EKF-based SLAM approach a Gaussian of size K + 3 has o be updaed (wih K landmarks and 3 robo pose parameers. This calculaion can be done in consan ime in FasSLAM, whereas i requires ime quadraic in K in he EKF. C. Imporance Weighs Before he robo pah paricles can be resampled, he imporance weighs (9 mus be calculaed. For he sake of breviy, he derivaion of hese imporance weighs has been omied. The weigh w is equal o: w [m] p(s,[m] z, u, n p(s,[m] z 1, u, n 1 p(z θ n [m], s [m] p(θ n [m] dθ n (13 This quaniy can be compued in closed form because he landmark esimaors are Kalman filers. For a complee derivaion of he imporance weighs, see [10]. V. FASTSLAM WITH UNKNOWN DATA ASSOCIATION If he mapping beween observaions and landmarks is known, he FasSLAM algorihm samples over robo pahs, and compues he condiional landmark esimaes analyically for every pah sample. When his mapping is unknown, FasSLAM can be exended o sample over possible daa associaions as well as robo pahs. There are several ways ha his sampling can be done. A. Per-Paricle Maximum Likelihood Daa Associaion The simples approach o sampling over daa associaions is o adop he maximum likelihood assignmen procedure used by EKFs, bu on a per-paricle basis. Paricles ha pick he correc daa associaion will receive high probabiliies because hey explain he measuremens well. Paricles ha assign observaions incorrecly will receive lower probabiliies and be removed in fuure resampling seps. This procedure can be wrien as: n [m] argmax n p(z s [m], θ n, n (14 Per-paricle daa associaion has wo clear consequences. Firs, robo moion noise does no affec he accuracy of daa associaion, given an appropriae number of paricles. This fac alone will resul in significanly improved performance in siuaions wih subsanial moion ambiguiy. If applied o he scenario shown in Figure, some of he paricles will represen he pose on he lef and pick he firs daa assoociaion hypohesis, and oher paricles will pick he second hypohesis. The second consequence of per-paricle daa associaion is buil-in, delayed decision-making. A any given ime, some fracion of he paricles will receive plausible, bu wrong daa associaions. In he fuure, new observaions may be received ha clearly refue hese prior assignmens. These paricles will receive low probabiliy and be removed from he filer. As a resul, he effec of wrong associaions made in he pas can be removed from he filer a a laer ime. This is in sark conras o he EKF, in which he effec of an incorrec daa associaion can never be removed once i is incorporaed. Moreover, no heurisics are needed o manage he removal of old associaions. This is done in a saisically valid manner, simply as a consequence of he resampling sep of he paricle filer. Naurally, sampling over robo pahs and daa associaions will require more paricles han sampling over robo pahs alone. Resuls in he nex secion will show ha even a modes number of paricles (M 100 will resul in subsanially improved daa associaion over he EKF. B. Mone-Carlo Daa Associaion Per-paricle daa associaion can be aken a sep furher. Insead of assigning each paricle he mos likely daa associaion, each paricle can draw a random associaion weighed by he probabiliies of each landmark

5 having generaed he observaion. Using his approach FasSLAM will also generae correc daa associaion hypoheses given measuremen ambiguiy. If a small number of landmarks exhibi measuremen ambiguiy, his procedure can have a small posiive effec on esimaion accuracy. However, uniformly high measuremen error induces a combinaorial number of plausible daa associaions for every se of observaions. This, in urn, would require exponenially more paricles han he same scenario wih known daa associaion. C. Muual Exclusion If more han one observaion is received per imesep, muual exclusion can be used o eliminae daa associaion hypoheses ha assign muliple measuremens o he same landmark. Muual exclusion can be applied in EKFs, bu only if he daa associaions of all observaions are considered simulaneously. This consideraion is, in general, compuaionally difficul wih a large number of observaions. Since FasSLAM mainains a se of daa associaion hypoheses, he muual exclusion consrain can be applied in a greedy fashion. Each observaion is associaed wih he mos likely landmark in each paricle ha has no received an observaion ye. Since he greedy heurisic will someimes apply he muual exclusion consrain incorrecly, more paricles will be needed when applying his echnique. However, muual exclusion makes he process of deciding when o add new landmarks a much simpler problem. Insead of incorrecly assigning an observaion from an unseen landmark o a nearby, previously seen landmark, muual exclusion will force he creaion of a new landmark. D. Negaive Informaion The poin landmark represenaion is accepable for making inferences abou where objec are in he world. In general, i does no allow us o say where objecs are no. Simply deciding ha areas ha do no conain landmarks are empy is no correc. However, one ype of inference can be made using negaive informaion. If a robo expecs o see a paricular landmark and does no, he robo should become less confiden ha his landmark acually exiss. In order o exploi his kind of negaive informaion in SLAM, we will borrow a echnique normally used for making evidence grids. For each landmark in every paricle, we will esimae a single binary variable r indicaing wheher his landmark represens a real landmark in he world. Insead of keeping rack of he probabiliy p(r z, we will insead esimae he log odds raio: p(r z log 1 p(r z (15 The log-odds formulaion of he binary Bayes filer is exremely easy o updae. A complee descripion of his procedure is given in [15]. In shor, every ime he landmark Fig. 4. Typical FasSLAM run. The yellow pah is he esimaed pah of he vehicle. The blue dashed line is he GPS ground ruh daa. The yellow circles are he esimaed posiions of he landmarks. is observed, a consan value is added o he log odds raio. Every ime he landmark is no observed when i should have been, a consan value is subraced from he log odds raio. If his raio falls below a given hreshold, he landmark is considered o be spurious and removed. Negaive informaion is paricularly useful for removing spurious feaures from he map. These feaures may be he resul of false posiives generaed by he feaure deecion algorihm, or hey may correspond o moving objecs. Resuls in he nex secion will show ha using negaive informaion dramaically reduces he number of spurious landmarks in he esimaed map. VI. EXPERIMENTAL RESULTS The FasSLAM and EKF algorihms were compared using he Universiy of Sydney s Vicoria Park daa se. An insrumened vehicle wih a laser rangefinder was driven hrough Vicoria Park. Encoders on he vehicle recorded velociy and seering angle. Ranges and bearings o nearby rees were exraced from he laser daa using a local minima deecor. The vehicle was driven around he park for approximaely 30 minues, covering a disance of over 4 km. Filer accuracy was calculaed by comparing he esimaed vehicle pah wih GPS. An example of a complee run of FasSLAM is shown in Figure 4. Figure 5(a shows he esimaed pah of he vehicle based solely on odomery. Even hough his demonsraes ha he vehicle s odomery is quie inaccurae, daa associaion is his daa se is generally sraighforward. The accuracy of he laser sensor, and he widely spaced fea-

6 (a (b (c (d (e Fig. 5. (a raw odomery (b EKF wih low odomeric error (c FasSLAM wih low odomeric error (d EKF wih high odomeric error (e FasSLAM wih high odomeric error - In Figures (b hrough (e he red solid pah is he esimaed pah. The blue dashed pah is he GPS daa. ures rarely generae any kind of daa associaion ambiguiy. I comes as no surprise ha he performance of Fas- SLAM and he EKF are comparable. Example pah esimaes for he EKF and FasSLAM wih low odomeric noise are shown in Figures 5(b and 5(c. The performance of he wo algorihms was also compared afer adding various amouns of noise o he observed conrols. The resuls of his comparison are shown in Figure 6. The increased moion noise had no measurable effec on he accuracy of FasSLAM. Addiional moion noise caused he EKF o diverge, resuling in very high posiion error on average. In all experimens, FasSLAM was run wih 100 paricles. Example pah esimaes for he EKF and FasSLAM wih high odomeric noise are shown in Figures 5(d and 5(e. In Figure 5(d, he EKF has clearly diverged. FasSLAM was also run on he Vicoria Park daa se wihou any odomery esimaes a all. This was accomplished by adding velociy o he vehicle s sae, and assuming a overly conservaive moion model. The vehicle s ranslaional and roaional velociy were assumed o vary as a coninuous random walk. Figure 7 shows he esimaed pah of he vehicle. Robo RMS Posiion Error (m EKF SLAM FasSLAM No all of he feaures deeced by he feaure exracor belonged o saic objecs. Some feaures were generaed by cars, and oher moving objecs. Feaures from moving objecs frequenly resuled in spurious landmarks being added o he map. In general, i is difficul o measure he accuracy of he esimaed map wih his daa se because here is no ground ruh daa available for he landmarks. However, incorporaing negaive informaion did resul in 44 percen fewer landmarks on average, and many fewer landmarks in dynamic areas (e.g. he sree. VII. CONCLUSIONS We have presened an exension of he FasSLAM algorihm o he case of unknown daa associaion. In addiion o sampling over robo pahs, his formulaion of FasSLAM also samples over poenial daa associaions. The resuling algorihm consisenly ouperformed he Exended Kalman Filer on a real world daa se wih various levels of odomeric noise. In addiion, we have shown how o incorporae negaive informaion ino FasSLAM. This echnique is no specific o FasSLAM and can also be applied o oher SLAM algorihms, including he EKF. Use of negaive evidence resuls in a measurable decrease in he number of false landmarks, especially if he feaure deecor being used generaes a large number of spurious feaures Error Added o Roaional Velociy (sd. Fig. 6. Accuracy of he vehicle pah wih varying levels of odomery noise Fig. 7. Robo pah esimaed wihou odomery

7 VIII. APPENDIX: DERIVATION OF FACTORIZATION The SLAM poserior (4 can be rewrien as: p(s z, u, n p(θ s, z, u, n (16 Thus, o derive he facored version (5 i suffices o show ha: p(θ s, z, u, n p(θ i s, z, u, n (17 This will be shown using inducion. To do his we will need wo inermediae resuls. The firs is he probabiliy of he landmark being observed θ n given he robo pah, he observaions, and he conrols. p(θ n s, z, u, n (18 Bayes p(z θ n, s, z 1, u, n p(z s, z 1, u, n p(θ n s, z 1, u, n Markov p(z θ n, s, n p(z s, z 1, u, n p(θ n s 1, z 1, u 1, n 1 Nex we solve for he righmos erm. p(θ n s 1, z 1, u 1, n 1 p(z s, z 1, u, n p(θ n s, z, u, n (19 p(z θ n, s, n For our second inermediae resul, we will compue he probabiliy of any landmark ha is no being observed given he robo pah, he observaions, and he conrols. p(θ i n s, z, u, n Markov p(θ i n s 1, z 1, u 1, n 1 (0 We will assume he following inducion hypohesis: p(θ s 1, z 1, u 1, n 1 p(θ i s 1, z 1, u 1, n 1 (1 For he base case of K 1 equaion (17 is rivially rue. In general, when K > 1: p(θ s, z, u, n ( Bayes Markov Inducion p(z θ, s, z 1, u, n p(z s, z 1, u, n p(θ s, z 1, u, n p(z θ n, s, n p(z s, z 1, u, n p(θ s 1, z 1, u 1, n 1 p(z θ n, s, n p(z s, z 1, u, n p(θ i s 1, z 1, u 1, n 1 By replacing he erms of he produc wih (19 and (0: p(θ s, z, u, n p(θ n s, z, u, n p(θ i s, z, u, n i n p(θ i s, z, u, n (3 ACKNOWLEDGMENTS This research has been sponsored by DARPA s MARS Program (conrac N C-6018, DARPA s CoABS Program (conrac F , and DARPA s MICA Program (conrac F C-019, all of which is graefully acknowledged. Special hanks o Eduardo Nebo and Juan Nieno from he Universiy of Sydney for giving us access o he Vicoria Park daase. We also graefully acknowledge he Fannie and John Herz Foundaion for heir suppor of Michael Monemerlo s graduae research. REFERENCES [1] F. Dellaer, D. Fox, W. Burgard, and S. Thrun. Mone Carlo localizaion for mobile robos. ICRA-99. [] G. Dissanayake, P. Newman, S. Clark, H.F. Durran-Whye, and M. Csorba. An experimenal and heoreical invesigaion ino simulaneous localisaion and map building (SLAM. Lecure Noes in Conrol and Informaion Sciences: Experimenal Roboics VI, Springer, 000. [3] 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 Transacions of Roboics and Auomaion, 001. [4] A. Douce, J.F.G. de Freias, and N.J. Gordon, ediors. Sequenial Mone Carlo Mehods In Pracice. Springer, 001. [5] A Douce, N. de Freias, K. Murphy, and S. Russell. Rao- Blackwellised paricle filering for dynamic Bayesian neworks. UAI-000. [6] J. Guivan and E. Nebo. Opimizaion of he simulaneous localizaion and map building algorihm for real ime implemenaion. IEEE Transacion of Roboic and Auomaion, May 001. [7] D. Korenkamp, R.P. Bonasso, and R. Murphy, ediors. AI-based Mobile Robos: Case sudies of successful robo sysems, MIT Press, [8] J.J. Leonard and H.J.S. Feder. A compuaionally efficien mehod for large-scale concurren mapping and localizaion. ISRR-99. [9] N. Meropolis, A.W. Rosenbluh, M.N. Rosenbluh, A.H. Teller, and E. Teller. Equaions of sae calculaions by fas compuing machine. Journal of Chemical Physics, 1, [10] M. Monemerlo, S. Thrun, D. Koller, and B. Wegbrei. FasSLAM: A Facored Soluion o he Simulaneous Localizaion and Mapping Problem. AAAI-0 [11] K. Murphy and S. Russell. Rao-blackwellized paricle filering for dynamic bayesian neworks. In Sequenial Mone Carlo Mehods in Pracice, Springer, 001. [1] Neira, J., Tardos, J.D., Daa Associaion in Sochasic Mapping Using he Join Compaibiliy Tes. In IEEE Trans. on Roboics and Auomaion vol. 17, no. 6, pp , Dec 001. [13] R. Smih, M. Self, and P. Cheeseman. Esimaing uncerain spaial relaionships in roboics. In Auonomous Robo Vehicles, Springer, [14] C. Thorpe and H. Durran-Whye. Field robos. ISRR-001. [15] S. Thrun. Learning meric-opological maps for indoor mobile robo navigaion. Arificial Inelligence, 99(1:1-71, 1998.