Particle Filters for Mobile Rob ot Lo calization Dieter Fox Sebastian Thrun Wolfram Bur gard and Frank Del laert Intro duction This chapter investigates the utility of particle lters in the context of mobile rob otics In particular we rep ort results of applying particle lters to the problem of mobile rob ot lo calization which is the problem of estimating a rob ots p ose relative to a map of its environment The lo calization problem is akey problem in mobile rob otics as it plays a fundamental role in various suc cessful mobile rob ot systems see eg Cox and Wilfong Fukuda et al Hinkel and Knieriemen Leonard et al Rencken Sim mons et al Wei et al and various chapters in Borenstein et al and Kortenkamp et al Occasionally it has b een referred to as the most fundamental problem to providing a mobile rob ot with autonomous capabilities Cox The mobile rob ot lo calization problem comes in dierentavors The sim plest lo calization problemwhich has received by far the most attention in the literatureis position tracking Here the initial rob ot p ose is known and lo calization seeks to correct small incremental errors in a rob ots o dometry More challenging is the global localization problem where a rob ot is not told its initial p ose but instead has to determine it from scratch The global lo calization problem is more dicult since the rob ots lo calization error can b e arbitrarily large Even more dicult is the kidnappedrobot problem En gelson and McDermott in whicha welllo calized rob ot is telep orted to some other p osition without b eing told This problem diers from the global e to b e somewhere lo calization problem in that the rob ot might rmly b eliev else at the time of the kidnapping The kidnapp ed rob ot problem is often used to test a rob ots abilitytorecover autonomously from catastrophic lo cal ization failures Finally there also exists the multirobot localization problem in which a team of rob ots seeks to lo calize themselves The multirob ot lo cal ization problem is particularly interesting if rob ots are able to p erceiveeach other whichintro duces nontrivial statistical dep endencies in the individual rob ots estimates The b eauty of particle lters is that they provide solutions to all of the problems ab ove Even the most straightforward implementation of particle Particle Filters for Mobile Rob ot Lo calization lters exhibits excellent results for the p osition tracking and the global lo calization problem Extensions of the basic algorithm have led to excellent results on the kidnapp ed rob ot and the multirob ot lo calization problem The p ower of particle lters relative to these problems stems from mul tiple asp ects in contrast to the widely used Kalman lters particle lters can approximate a large range of probability distributions not just normal distributions Once a rob ots b elief is fo cused on a subspace of the space of all p oses particle lters are computationally ecient since they fo cus their resources on regions in state space with high likelihood Particle lters are also easily implementedasanytime lters Dean and Bo ddy Zilb erstein and Russell by dynamically adapting the numb er of samples based on the available computational resources Finally particle lters for lo calization are remarkably easy to implement which also contributes to their p opularity This article describ es a family of metho ds known as Monte Carlo localiza tion MCL Dellaert at al b Fox et al b The MCL algorithm is a particle lter combined with probabilistic mo dels of rob ot p erception and motion Building on this we will describ e a variation of MCL which uses a dierent prop osal distribution a mixture distribution that facilitates fast recovery from global lo calization failures As we will see this prop osal dis tribution has a range of advantages over that used in standard MCL but it comes at the price that it is more dicult to implement and it requires an algorithm for sampling p oses from sensor measurements whichmightbe dicult to obtain Finallywe will present an extension of MCL to co op er ativemultirob ot lo calization of rob ots that can p erceiveeach other during lo calization All these approaches have b een tested thoroughly in practice Exp erimental results are provided to demonstrate their relative strengths and eaknesses in practical rob ot applications w Monte Carlo Lo calization Bayes Filtering Particle lters have already b een discussed in the intro ductory chapters of this b o ok For the sake of consistency let us briey derive the basics b e ginning with Bayes lters Bayes lters address the problem of estimating the state x of a dynamical system from sensor measurements For example in mobile rob ot lo calization the dynamical system is a mobile rob ot and its environment the state is the rob ots p ose therein often sp ecied byaposi tion in a twodimensional Cartesian space and the rob ots heading direction and measurements may include range measurements camera images and o dometry readings Bayes lters assume that the environmentis Markov that is past and future data are conditionally indep endent if one knows the current state Fox Thrun Burgard Dellaert The key idea of Bayes ltering is to estimate the p osterior probability densityover the state space conditioned on the data In the rob otics and AI literature this p osterior is typically called the belief Throughout this chapter we will use the following notation Belx px j d t t t Here x denotes the state x is the state at time t and d denotes the data t t starting at time up to time tFor mobile rob ots we distinguish twotyp es of data perceptual data such as laser range measurements and odometry data or controlswhich carries information ab out rob ot motion Denoting the former by y and the latter by uwehave Belx px j y u y u u y t t t t t t Without loss of generalitywe assume that observations and actions o ccur in an alternating sequence Note that the most recent p erception in Belx is t y whereas the most recentcontrolso dometry reading is u t t Bayes lters estimate the b elief recursively The initial b elief character izes the initial knowledge ab out the system state In the absence of such knowledge eg global lo calization it is typically initialized byauniform distribution over the state space To derive a recursive up date equation weobserve that Expression can b e transformed byBayes rule to py j x u y px j u y t t t t t Belx t py j u y t t py j x u y px j u y t t t t t py j u d t t t The Markov assumption states that measurements y are conditionally inde t p endent of past measurements and o dometry readings given knowledge of the state x t py j x u y py j x t t t t t This allows us to conveniently simplify Equation py j x px j u y t t t t Belx t py j u d t t t To obtain our nal recursiveformwenowhavetointegrate out the p ose x t at time t which yields Z py j x t t px j x u y px j u y dx t t t t t t py j u d t t t Particle Filters for Mobile Rob ot Lo calization The Markov assumption also implies that given knowledge of x and u t t the state x is conditionally indep endent of past measurements y y t t and o dometry readings u u up to time t that is t px j x u y px j x u t t t t t t Using the denition of the b elief Belwe obtain a recursive estimator known as Bayes lter Z py j x t t px j x u Belx dx Belx t t t t t t py j u d t t t Z py j x px j x u Belx dx t t t t t t t where is a normalizing constant This equation is of central imp ortance as it is the basis for various MCL algorithms studied here Mo dels of Rob ot Motion and Perception In the context of mobile rob ot lo calization Bayes lters are also known as x at al Markov localization Burgard Fox Hennig and Schmidt Fo a Kaelbling et al Ko enig and Simmons Nourbakhsh et al Simmons and Ko enig Thrun ToimplementMarkov lo calization one needs to know three distributions the initial b elief Belx eg uniform the next state probability px j x u called the motion t t t model and the p erceptual likeliho o d py j x called the perceptual model t t The sp ecic shap e of these probabilities dep ends on the rob ots o dometry and the typ e of sensors used for lo calization Both of these mo dels are time invariant we will henceforth omit the time index t A sp ecic motion mo del for an RWI B rob ot is shown in Figure This gure shows the probabilistic outcome of two example motion commands indicated by the lines The greyscale corresp onds to px j x a pro jected into D This sp ecic mo del is the result of convolving conventional rob ot kinematics with two indep endent zeromean random variables one of which mo dels noise in rotation and one mo dels translational noise The mo del is easily co ded in lines of C co de The p erceptual mo del py j x dep ends on the sp ecic sensor If y are raw camera images computing py j x is related to the computer graphics problem in that the app earance of an image y at p ose x has to b e predicted However py j x is considerably simpler if one uses range nders for p er ception Such sensors measure the distance of the rob ot to nearby obstacles using sound or structured laser light Figure illustrates the mo del of rob ot p erception for a planar D laser range nder which is commonly used in mo bile rob otics Figure a shows a laser scan and a map The sp ecic density Fox Thrun Burgard Dellaert Figure The density py j x after moving meters left diagram and meters right diagram The darker a p ose the more likely it is py j x is computed in two stages First the measurement in an