FastSLAM: A Factored Solution to the Simultaneous ...

1 FastSLAM: A FactoredSolutiontotheSimultaneousLocaliz ationandMappingProblemMichaelMontemerloa ndSebastianThrunSchoolofComputerScienceC arnegieMellonUniversityPittsburgh,PA robotandac-curatelymapitssurroundingsis consideredbymany tobea key , few approachestothisproblemscaleuptohandleth everylargenumberoflandmarkspresentin ,forexample,requiretimequadraticin thenumberoflandmarksto ,analgorithmthatrecursivelyestimatesthef ullposteriordistributionoverrobotposeand landmarklocations,yetscaleslogarithmical lywiththenumberoflandmarksin basedonanex-actfactorizationoftheposteri orintoa productofcon-ditionallandmarkdistributio nsanda as50,000landmarks, , alsoknownasSLAM,hasattractedimmenseatten tionin mapofanenvironmentfroma sequenceofland-markmeasurementsobtainedf roma subjectto error, themappingproblemneces-sarilyinducesa robotlocalizationproblem robotandaccuratelymapitsenvironmentis consideredbymany tobea key prerequisiteoftrulyautonomousrobots[3,7, 15].

2 ThedominantapproachtotheSLAM problemwasin-troducedina seminalpaperbySmith,Self,andCheese-man[1 4].ThispaperproposedtheuseoftheextendedK almanfilter(EKF) ,thisapproachhasfoundwidespreadacceptanc einfieldrobotics,asa recenttutorialpaper[2] 2002,AmericanAssociationforArtificialInt elli-gence( ). [6,8,9] andtoalgorithmsforhan-dlingdataassociati onproblems[16].A key limitationofEKF-basedapproachesis theircompu-tationalcomplexity. (K2)elements,allofwhichmustbeupdatedeven if justa singlelandmarkis fewhundred [6, 8, 13]. illustratesa generativeprobabilisticmodel(dynamicBaye snetwork) , therobotposes,denoteds1; s2; : : : ; st, evolve overtimeasa functionoftherobotcontrols,denotedu1; : : : ; ut. Eachoftheland-markmeasurements,denotedz1 ; : : : ; zt, is a functionoftheposition kofthelandmarkmeasuredandoftherobotposea t , knowledgeoftherobot spaths1; s2; : : : ; strenderstheindividuallandmarkmeasure-me ntsindependent.

3 Soforexample,if anoracleprovideduswiththeexactpathofther obot,theproblemofdeterminingthelandmarkl ocationscouldbedecoupledintoKindepen-den testimationproblems, ,thispaperdescribesanefficientSLAM algorithmcalledFastSLAM. FastSLAM decomposestheSLAM problemintoa robotlocalizationproblem, exact,dueto aninstanceoftheRao-Blackwellizedparticle fil-ter[5,12].Anaive implementationofthisidealeadstoanalgorit hmthatrequiresO(M K)time,whereMis thenum-berofparticlesintheparticlefilter andKis developa tree-baseddatastructurethatre- ..Figure 1: TheSLAM problem :Therobotmovesfromposes1througha sequenceofcontrols,u1; u2; : : : ; ut. Asit moves, 1, it observeslandmark 1outoftwo landmarks,f 1; 2g. Themeasurementis denotedz1(rangeandbearing).Attimet= 1, it observestheotherlandmark, 2, andat timet= 3, it observes s pathfromthecontrolsuandthemeasurementsz. Thegrayshadingillustratesa (MlogK), mak-ingit alsoextendtheFastSLAM algorithmtositu-ationswithunknowndataass ociationandunknownnumberoflandmarks, physicalrobotanda alsofindthatincertainsituations,anin-cre asednumberoflandmarksKleadstoa mildreductionofthenumberofparticlesMneed edtogenerateaccuratemaps ,asdefinedintherichbodyoflitera-tureonSL AM,is bestdescribedasa s poseattimetwillbedenotedst.

4 Forrobotsoperatingintheplane whichis thecaseinallofourexperiments posesarecomprisedofa robot accordingtoa probabilisticlaw, oftenre-ferredtoasthemotionmodel:p(stjut ; st 1)(1)Thus,stisa probabilisticfunctionoftherobotcontrolut andthepreviousposest 1. Inmobilerobotics,themotionmodelis usuallya time-invariantprobabilisticgeneralizatio nofrobotkinematics[1].Therobot s ,denoted kfork= 1; : : : ; K. Withoutlossofgen-erality, wewillthinkoflandmarksaspointsintheplane ,sothatlocationsarespecifiedbytwo mapitsenvironment, ,it maybeabletomeasurerangeandbearingtoa landmark,relative timetwillbedenotedzt. Whilerobotscanoftensensemorethanonelandm arkata time,wefollowcom-monplacenotationbyassum ingthatsensormeasurementscorrespondtoexa ctlyonelandmark[2]. posesnorestriction,asmultiplelandmarksig htingsata probabilisticlaw,oftenreferredtoasthemea surementmodel:p(ztjst; ; nt)(2)Here =f 1; : : : ; kgisthesetofalllandmarks,andnt2f1; : : : ; Kgis theindex ofthelandmarkperceivedattimet.

5 Forexample,inFigure1,wehaven1= 1; n2= 2,andn3=1, sincetherobotfirstobserveslandmark 1,thenlandmark 2, andfinallylandmark 1fora measurementmodelsintheliteratureassumeth attherobotcanmeasurerangeandbearingtolan dmarks, Mosttheoreticalworkintheliteratureassume sknowledgeofthecorrespondenceor, putdifferently, , whichworkwellif , arenow , SLAMis theproblemofdeterminingthelocationofalll andmarks androbotposesstfrommeasurementszt=z1; : : : ; ztandcontrolsut=u1; : : : ; ut. Inprobabilis-ticterms,thisis expressedbythefollowingposterior:p(st; jzt; ut)(3)Hereweusethesuperscriptttorefertoa setofvariablefromtime1 totimet. If thecorrespondencesareknown,theSLAM problemis simpler:p(st; jzt; ut; nt)(4)Asarguedintheintroductiontothisart icle,allindividuallandmarkestimationprob lemsareindependentif oneknewtherobot s pathstandthecorrespondencevariablesnt. Thisconditionalindependenceis beginourconsiderationwiththeimportantcas ewherethecorrespondencesnt=n1; : : : ; ntareknown, (4)canbefactoredasfollows:p(st; jzt; ut; nt)=p(stjzt; ut; nt)Ykp( kjst; zt; ut; nt)(5)Putverbally, theproblemcanbedecomposedintoK+1esti-mat ionproblems,oneproblemofestimatinga posterioroverrobotpathsst, (stjzt; ut; nt)usinga modifiedparticlefilter[4].

6 Aswearguefurtherbelow, thisfiltercansampleefficientlyfromthissp ace,providinga ( kjst; zt; ut; nt)arerealizedbyKalmanfilters, ,eachparticleintheparticlefilterhasitsow n, ,forMparticlesandKland-marks,therewillbe a totalofKMKalmanfilters,eachofdimension2 (forthetwo landmarkcoordinates).Thisrepre-sentation willnow particlefilterforestimatingthepathposter iorp(stjzt; ut; nt)in(5),usinga filterthatis similar(butnotidentical)totheMonteCarlol ocalization(MCL)algorithm[1].MCLisanappl icationofparticlefiltertotheproblemofrob otposeestimation( localization ).Ateachpoi ntin time,bothalgorithmsmaintaina setofparticlesrep-resentingtheposteriorp (stjzt; ut; nt), denotedSt. Eachparticlest;[m]2 Strepresentsa guess oftherobot s path:St=fst;[m]gm=fs[m]1; s[m]2; : : : ; s[m]tgm(6)We usethesuperscriptnotation[m] , fromthesetSt 1at timet 1, a robotcontrolut, anda measurementzt.

7 First,eachparticlest;[m]inSt 1is usedtogenerateaprobabilisticguessofthero bot s poseat timet:s[m]t p(stjut; s[m]t 1)(7) thenaddedto a temporarysetofparticles,alongwiththepath st 1;[m]. Undertheassump-tionthatthesetofparticles inSt 1is distributedaccordingtop(st 1jzt 1; ut 1; nt 1)(whichisanasymptoticallycorrectapproxi mation),thenewparticleisdistributedac-co rdingto:p(stjzt 1; ut; nt 1)(8) , thenew ;[m]is drawn(withreplacement)witha probabilityproportionaltoa so-calledimportancefactorw[m]t, whichiscalculatedasfollows[10]:w[m]t=tar getdistributionproposaldistribution=p(st ;[m]jzt; ut; nt)p(st;[m]jzt 1; ut; nt 1)(9)Theexactcalculationof(9) distributedaccordingto anap-proximationtothedesiredposeposterio rp(stjzt; ut; nt),anapproximationwhichis correctasthenumberofparticlesMgoestoinfi nity. We alsonoticethatonlythemostrecentrobotpose estimates[m]t 1is usedwhengeneratingtheparti-clesetSt.

8 Thiswillallowsustosilently forget allotherposeestimates, ( kjst; zt; ut; nt)in(5) conditionedontherobotpose,theKalmanfilte rsareattachedtoindividualposeparticlesin St. Morespecifi-cally, thefullposterioroverpathsandlandmarkposi tionsintheFastSLAM algorithmis representedbythesamplesetSt=fst;[m]; [m]1; [m]1; : : : ; [m]K; [m]Kgm(10)Here [m]kand [m]karemeanandcovarianceoftheGaus-sianre presentingthek-thlandmark k, ,eachmean [m]kis a two-elementvector, and [m]kis a 2 by2 kis ,thatis,whetherornot kwasobservedat timet. Fornt=k,weobtainp( kjst; zt; ut; nt)(11)Bayes/p(ztj k; st; zt 1; ut; nt)p( kjst; zt 1; ut; nt)Markov=p(ztj k; st; nt)p( kjst 1; zt 1; ut 1; nt 1)Fornt6=k, wesimplyleave theGaussianunchanged:p( kjst; zt; ut; nt) =p( kjst 1; zt 1; ut 1; nt 1)(12)TheFastSLAM algorithmimplementstheupdateequation(11) usingtheextendedKalmanfilter(EKF).Asinex -istingEKFapproachestoSLAM,thisfilteruse sa lin-earizedversionoftheperceptualmodelp( ztjst; ; nt)[2].

9 Thus,FastSLAM s EKFis similartothetraditionalEKFforSLAM[14] inthatit approximatesthemeasurementmodelusinga notethat,withanac-tuallinearGaussianobse rvationmodel,theresultingdistri-butionp( kjst; zt; ut; nt)is exactlya Gaussian,evenif themotionmodelis notlinear. Thisis a consequenceoftheuseofsamplingtoapproxima tethedistributionovertherobot s useofKalmanfiltersandthatofthetraditiona lSLAM algorithmis thattheupdatesintheFastSLAM algo-rithminvolve onlya Gaussianofdimensiontwo (forthetwolandmarklocationparameters),wh ereasintheEKF-basedSLAM approacha Gaussianofsize2K+3hasto beupdated(withKlandmarksand3 robotposeparameters).Thiscal-culationcan bedonein constanttimein FastSLAM,whereasit [m]tneededforparticlefilterresampling,as 8, 8 7, 7k 7 ?FT 6, 6 5, 5k 5 ?FT 4, 4 3, 3k 3 ?FT 2, 2 1, 1k 1 ?FTk 6 ?FTk 2 ?FTk 4 ?FT[m][m][m][m][m][m][m][m][m][m][m][m][ m][m][m][m] 8, 8 7, 7k 7 ?

10 FT 6, 6 5, 5k 5 ?FT 4, 4 3, 3k 3 ?FT 2, 2 1, 1k 1 ?FTk 6 ?FTk 2 ?FTk 4 ?FT[m][m][m][m][m][m][m][m][m][m][m][m][ m][m][m][m]Figure 2: A treerepresentingK= (9):w[m]t/p(st;[m]jzt; ut; nt)p(st;[m]jzt 1; ut; nt 1)Bayes=p(zt; ntjst;[m]; zt 1; ut; nt 1)p(zt; ntjzt 1; ut; nt 1)p(st;[m]jzt 1; ut; nt)p(st;[m]jzt 1; ut; nt)=p(zt; ntjst;[m]; zt 1; ut; nt 1)p(zt; ntjzt 1; ut; nt 1)/p(zt; ntjst;[m]; zt 1; ut; nt 1)=Zp(zt; ntj ; st;[m]; zt 1; ut; nt 1)p( jst;[m]; zt 1; ut; nt)d Markov=Zp(zt; ntj ; s[m]t)p( jst 1;[m]; zt 1; ut 1; nt 1)d =Zp(ztj ; s[m]t; nt)p(ntj ; s[m]t)p( jst 1;[m]; zt 1; ut 1; nt 1)d /Zp(ztj ; s[m]t; nt)p( jst 1;[m]; zt 1; ut 1; nt 1)d EKF Zp(ztj [m]nt; s[m]t; nt)p( [m]nt)d nt(13)Hereweassumethatthedistributionp(n tj ; s[m]t)isuniform , EKF makesexplicittheuseofa linearizedmodelasanap-proximationtotheob servationmodelp(ztj [m]nt; s[m]t), andtheresultingGaussianposteriorp( [m]nt).