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 ideal noise

free environment is computed For laser range nders this is easily done

using raytracing in a geometric map of the environment such as the one

shown in Figure a Second the desired density py j x is obtained as a mix

ture of random variables comp osed of one that mo dels the event of getting

the correct reading convolved with small Gaussiandistributed measurement

noise one for receiving a maxrange reading which o ccurs frequently and

one that mo dels random noise and is exp onentially distributed Figure b

shows a picture of py j x and Figure c plots py j x for the sp ecic sensor

scan y shown in Figure a

Implementation as Particle Filters

If the state space is continuous as is the case in mobile rob ot lo caliza

tion implementing the b elief up date equation is not a trivial matter

particularly if one is concerned ab out eciency The idea of MCL and other

particle lter algorithms is to represent the b elief Belxby a set of m

Belx weighted samples distributed according to

i i

Belx fx p g

i
m

i i

Here each x is a sample a state and p are nonnegativenumerical factors

called importance factorswhich sum up to one As the name suggests the

imp ortance factors determine the weight imp ortance of each sample

In global mobile rob ot lo calization the initial b elief is a set of p oses drawn

according to a uniform distribution over the rob ots universe annotated by

the uniform imp ortance factor

m

The recursive up date is realized in three steps computing the expression

in from the right to the left

i

from the Sample a state x from Belx by drawing a random x

t
t

t

Particle Filters for Mobile Rob ot Lo calization

a laser scan and map

b sensor mo del py j x

0.125 Approximated Measured 0.1 i

p(y | x) expected distance 0.075

0.05 probability

0.025

0 100 200 300 400 500

measured distance y [cm]

c probability distribution for dierent p oses

Figure a Laser range scan pro jected into a map b The density py j x

c py j x for the scan shown in a Based on a single sensor scan the rob ot

assigns high likeliho o d for b eing somewhere in the main corridor

Fox Thrun Burgard Dellaert

sample set representing Belx according to the discrete distribu

t

i

tion dened through the imp ortance factors p

t

i j 

Use the sample x and the action u to sample x from the distri

t

t

t

j 

bution px j x u The predictive densityof x is now given by

t t
t

t

the pro duct px j x u Belx

t t
t
t

j 

Finallyweight the sample x by the nonnormalized imp ortance fac

t

j  j 

tor py j x the likeliho o d of the sample x given the measurement

t

t t

y

t

After the generation of m samples the new imp ortance factors are normal

ized so that they sum up to hence dene a probability distribution The

reader should quickly see that this pro cedure in fact implements us

ing an approximate samplebased representation Obviously our algorithm

constitutes just one p ossible implementation of the particle ltering idea

other sampling schemes exist that further reduce variance Kitagawa

Detailed convergence results can b e found in Chapters and of this b o ok

Further b elow it will b e convenient to notice that in this version of MCL

the prop osal distribution for approximating Belx via imp ortance sampling

t

is given by

q px j x u Belx

t t
t
t

which is used to approximate the desired p osterior

px j u x Belx py j x

t t
t
t
t t

py j d u

t t
t

Consequently the imp ortance factors are given by the quotient

py j x px j u x Belx

t t t t
t
t

px j x u Belx

t t
t
t

py j d u

t t
t

py j x

t t

Rob ot Results

MCL has b een at the core of our rob ot navigation software It is more ecient

and accurate than any of our previous algorithms We thoroughly tested MCL

in a range of realworld environments applying it to at least three dierent

typ es of sensors cameras sonar and laser proximity data Our exp eriments

have b een carried out using several B B Pioneer Scout and XR

rob ots twoofwhich are shown in Figure These rob ots were equipp ed with

arrays of sonar sensors from to one or two laser range nders and in

Particle Filters for Mobile Rob ot Lo calization

Figure Two of the rob ots used for testing RHINO left and MINERVA

center and right which successfully guided thousands of p eople through crowded

museums

the case of Minerva the rob ot shown in center and right of Figure a BW

camera p ointed at the ceiling

Atypical example of MCL is shown in Figure This example illustrates

MCL in the context of lo calizing a mobile rob ot globally in an oce environ

ment This rob ot is equipp ed with sonar range nders and it is also given a

map of the environment In Figure a the rob ot is globally uncertain hence

the samples are spread uniformly trough the freespace pro jected into D

Figure b shows the sample set after approximately meter of rob ot motion

at whichpoint MCL has disambiguated the rob ots p osition up to a single

symmetry Finally after another meters of rob ot motion the ambiguityis

resolved and the rob ot knows where it is The ma jority of samples is now

centered tightly around the correct p osition as shown in Figure c

Comparison to GridBased Lo calization

To elucidate the advantage of particle lters over alternative representations

we are particularly interested in gridbased representations which are at the

core of an alternative family of Markov lo calization algorithms Fox et al

The algorithm describ ed in Fox et al relies on a negrained

grid approximation of the b elief Bel using otherwise identical sensor and

motion mo dels Figure plots the lo calization accuracy for gridbased lo cal

ization as a function of the grid resolution Note that the results in Figure

were not generated in realtime As shown there the accuracy increases with

the resolution of the grid b oth for sonar solid line and for laser data dashed

line However grid sizes b eyond cm do not p ermit up dating in realtime

even when highly ecient selectiveupdateschemes are used Fox et al

Fox Thrun Burgard Dellaert

Robot position

Robot position

Robot position

Figure Global lo calization of a mobile rob ot using MCL samples

Particle Filters for Mobile Rob ot Lo calization

30 Sonar Laser 25

20

15

10

Average estimation error [cm] 5

0 0 10 20 30 40 50 60 70

Cell size [cm]

Figure Accuracy of gridbased Markov lo calization using dierent spatial res

olutions

Results for MCL with xed sample set sizes are shown in Figure These

results have b een generated using realtime conditions where large sample

sizes samples result in loss of sensor data due to time constraints

Here very small sample sets are disadvantageous since they infer to o large

an error in the approximation Large sample sets are also disadvantageous

since pro cessing them requires to o much time and fewer sensor items can b e

pro cessed in realtime The optimal sample set size according to Figure

is somewhere b etween and samples Gridbased lo calization to

reach the same level of accuracy has to use grids with cm resolutionwhich

is infeasible given even our fastest computers we currently have

In comparison the gridbased approach with a resolution of cm re

quires almost exactly ten times as much memory when compared to MCL

with samples During global lo calization integrating a single sensor

scan requires up to seconds using the gridbased approach whereas MCL

consumes consistently less than seconds under otherwise equal conditions

This illustrates that particle lters are clearly sup erior over gridbased rep

resentations which previously was among the b est known algorithms for the

global lo calization problem

Similar results were obtained using a camera as the primary sensor for lo

calization Dellaert et al a To test MCL under extreme circumstances

weevaluated it using data collected in a p opulated museum During a two

week exhibition our rob ot Minerva Figure was employed as a tourguide

in the Smithsonians Museum of Natural History during whichittraversed

more than km Thrun et al To aid lo calization Minerva is equipp ed

with a camera p ointed towards the ceiling Using this camera the brightness

Fox Thrun Burgard Dellaert

30 Sonar Laser 25

20

15

10

Average estimation error [cm] 5

0 10 100 1000 10000 100000

Number of samples

Figure Accuracy of MCL for dierentnumb ers of samples log scale

of a small patch of the ceiling directly ab ove the rob ot is measured and com

pared to a largescale mosaic of the museums ceiling obtained b eforehand

Dellaert et al c shown in Figure This constitutes the likelihood

mo del The data used here is among the most dicult data sets in our p os

session as the rob ot traveled with sp eeds of up to cmsec Whenever

it entered or left the carp eted area in the center of the museum it crosses a

cm bump whichintro duced signicant errors in the rob ots o dometry

When only using vision information gridbased lo calization fatally failed

to track the rob ot This is b ecause the enormous computational overhead

makes it imp ossible to incorp orate suciently many images MCL however

succeeded in globally lo calizing the rob ot and tracking the rob ots p osition

Figure shows an example of global lo calization with MCL In the b eginning

the rob ot starts with uniformly distributed samples representing the

absolute uncertainty ab out the rob ots p osition After incorp orating images

rst diagram the samples are still scattered over the whole area but already

started to concentrate on several lo cations After incorp orating images

most of the ambiguities are resolved and the samples are concentrated on

a small numb er of p eaks second diagram Finally after iterations the

rob ot is highly certain ab out its p osition third diagram which is represented

by a concentration of the samples of the true lo cation of the rob ot

Particle Filters for Mobile Rob ot Lo calization

Figure Ceiling map of the National Museum of American History whichwas

used as the p erceptual mo del in navigating with a vision sensor

MCL with Mixture Prop osal Distributions

The Need For Better Sampling

As noticed by several authors Doucet Lenser and Veloso Liu and

Chen Pitt and Shephard the basic particle lter p erforms p o orly

if the prop osal distribution which is used to generate samples places to o

little samples in regions where the desired p osterior Belx is large

t

This problem has indeed great practical imp ortance in the context of MCL

as the following example illustrates The solid curve in Figure shows the

accuracy MCL achieves after steps using m samples These

results were obtained in simulation enabling us to vary the amount of p er

ceptual noise from on the right to on the left in particular we

simulated a mobile rob ot lo calizing an ob ject in D space from monocamera

imagery It app ears that MCL works b est for to p erceptual noise

The degradation of p erformance towards the right when there is high noise

barely surprises The less accurate a sensor the larger an error one should

exp ect However MCL also p erforms p o orly when the noise level is to o small

In other words MCL with accurate sensors may p erform worse than MCL

with inaccurate sensors This nding is a bit counterintuitive in that it sug

gests that MCL only works well in sp ecic situations namely those where

the sensors p ossess the right amount of noise

At rst glance one might attempt to x the problem by using a p erceptual

Fox Thrun Burgard Dellaert

Figure Global lo calization of a mobile rob ot using a camera p ointed at the ceiling

Particle Filters for Mobile Rob ot Lo calization

1. 2. 3. 4. 5. 7. 10. 20. 30. 40. 50. 500

400

300 MCL

200 error (in centimeter)

100 dashed high error mo del

1. 2. 3. 4. 5. 7. 10. 20. 30. 40. 50.

sensor noise level (in %)

Figure Solid curve error of MCL after steps as a function of the sensor

noise condence intervals are indicated by the bars Note that this function

not is monotonic as one might exp ect Dashed curve Same exp eriment with

higherror mo del

likeliho o d py j x that overestimates the sensor noise In fact sucha

t t

strategy partially alleviates the problem The dashed curve in Figure b

shows the accuracy if the error mo del assumes a xed noise shown

there only for smaller true error rates While the p erformance is b etter

this is hardly a principled way of xing the problem The overly p essimistic

sensor mo del is inaccurate throwing away precious information in the sensor

readings In fact the resulting b elief is not any longer a p osterior even if

innitely many samples were used As we will see b elow a mathematically

sound metho d exists that pro duces much b etter results

To analyze the problem more thoroughlywe rst notice that the true

goal of Bayes ltering is to calculate the pro duct distribution sp ecied in

Equation Thus the optimal prop osal distribution would b e this pro duct

distribution However sampling from this distribution directly is to o dicult

As noticed ab ove MCL samples instead from the prop osal distribution q

dened in Equation and uses the imp ortance factors to account

for the dierence It is wellknown from the statistical literature Doucet

Pitt and Shephard Liu and Chen Tanner that the

divergence b etween and determines the convergence sp eed This

j x If the sensors dierence is accounted by the p erceptual density py

t t

are entirely uninformative this distribution is at and equals

For lownoise sensors however py j x istypically quite narrow hence

t t

MCL converges slowlyThus the error in Figure is in fact caused bytwo

dierenttyp es of errors one arising from the limitation of the sensor data

noise and one that arises from the mismatch of and in MCL

Fox Thrun Burgard Dellaert

This suggests to use dierent prop osal distributions for sampling that can

accommo date highly accurate sensors

An Alternative Prop osal Distribution

To alleviate this problem one can use a dierent prop osal distribution one

that samples according to the most recent sensor measurement y see also Lenser

t

and Veloso Thrun et al The key idea is to sample x directly

t

from a distribution that is prop ortional to the p erceptual likeliho o d py j x

t t

Z

py j x

t t

q with y py j x dx

t t t t

y

t

This new prop osal distribution p ossesses orthogonal limitations from the one

describ ed ab ove in that it generates samples that are highly consistent with

the most recent sensor measurement but ignorant of the b elief Belx and

t

the control u

t

The imp ortance factors for these samples can b e calculated in three ways

Recall that our goal is to sample from the pro duct distribution

py j x px j d u py j x px j u x Belx

t t t t
t
t t t t
t
t

py j d u py j d u

t t
t
t t
t

Approach prop osed by Arnaud Doucet p ersonal communication The

i i i

idea is to draw random pairs hx x i by sampling x as describ ed ab ove

t t

t

i

and x bydrawing from Belx Obviously the combined prop osal dis

t

t

tribution is then given by

i

py j x

t

i

t

Belx

t

y

t

and hence the imp ortance factors are given by the quotient

i i i i i

py j x px j u x Belx py j x

t t
t

i

t t t

t
t

Belx

t

y py j d u

t t t
t

i i

px j u x y

t
t

t

t

py j d u

t t
t

i i

px j u x

t

t

t

This approach is mathematically more elegantthanthetwo alternatives de

scrib ed b elow in that it avoids the need to transform sample sets into densities

which will b e the case b elow Wehavenotyet implemented this approach

hence are unable to commentonhowwell it works in practice However in

the context of global mobile rob ot lo calization we susp ect the imp ortance

i i i i 

will b e zero for many p ose pairs hx x i factor px j u x

t

t t

t
t

Particle Filters for Mobile Rob ot Lo calization

Approach An alternative approach uses forward sampling and kdtrees

to generate an approximate densityof px j d u This densityis

t t
t

then used in a second phase to calculate the desired imp ortance factor More

sp ecically Equations and suggest that the imp ortance factors of

i

a sample x can b e written as

t

i i i

py j x px j d u py j x

t t
t
t

t t t

y py j d u

t t t
t

i

px j d u

t
t

t

Computing these imp ortance factors is not trivial since Belx isrep

t

resented by a set of samples The trick here is to employa twostaged

approach which rst approximates px j d u and then uses this

t t
t

approximate density to calculate the desired imp ortance factors

The following algorithm implements this alternative imp ortance sampler

j  j 

by rst sampling from Belx Generate a set of samples x and

t

t

j  j 

as describ ed ab ove Obviously j u x then sampling from px

t

t

t

j 

j d u these samples approximate px

t
t

t

Transform the resulting sample set into a kdtree Bentley Mo ore

j 

The tree generalizes samples to arbitrary p oses x in p ose space

t

which is necessary to calculate the desired imp ortance factors

i

 py jx i

t

t

Weight from our prop osal distribution Finally sample x

t

y 

t

eachsuch sample by an imp ortance factor that is prop ortional to its

probability under the previously generated density tree

i i

This approachavoids the danger of generating pairs of p oses hx x i with

t

t

i i

zero probabilityunder px j u x However it involves an explicit

t

t

t

forward sampling phase

Approach The third approach combines the b est of b oth worlds in that

it avoids the explicit forwardsampling phase of the second approach but

also generates imp ortance factors that are large In particular this approach

transforms the initial b elief Belx into a kdtree It then generates sam

t

i

ples x according to

t

py j x

t t

y

t

i i

For eachsuch sample x according to it generates a sample x

t

t

i

px j u x

t
t

t

i

j u x

t
t

Fox Thrun Burgard Dellaert

where

Z

i i

x j u px j u x dx

t
t
t
t

t t

i i

Each of these combined samples hx x i is thus sampled from the joint

t

t

distribution

i i i

j u x px py j x

t
t

t t

t

i

y

t

x j u

t

t

The imp ortance factor is calculated as follows

i i i i i i i

py j x px j x u Belx px j u x py j x

t t
t
t

t t t t

t
t
t

i

y py j d

t t t

j u x

t

t

i i

j u Belx y x

t
t

t

t

py j d

t t

i i

j u Belx x

t

t

t

i

is calculated using the kdtree representing this b elief density where Belx

t

i

The only complication arises from the need to calculate x j u which

t

t

i i

dep ends on b oth x and u Luckily in mobile rob ot lo calization x j

t

t t

u can safely b e assumed to b e a constanteven though this assumption

t

is not valid in general This leads to the following Monte Carlo algorithm

i

Sample a p ose x from a prop osal distribution that is prop ortional to

t

P y j x

t t

i i

For this x sample a p ose x from a distribution that is prop ortional

t

t

i

to P x j u x

t
t

t

Set the imp ortance factor to a value prop ortional to the p osterior prob

i

abilityof x under the density tree that represents Belx

t

t

The Mixture Prop osal Distribution

Neither prop osal distribution alonethe original distribution q describ ed in

and the alternative distributionq given in is satisfactory The

original MCL prop osal distribution fails if the p erceptual likeliho o d is to o

peaked The alternative prop osal distribution however only considers the

most recent sensor measurement hence is prone to failure when the sensors err

Particle Filters for Mobile Rob ot Lo calization

1. 2. 3. 4. 5. 7. 10. 20. 30. 40. 50. 500

400

300

200 error (in centimeter)

100

1. 2. 3. 4. 5. 7. 10. 20. 30. 40. 50.

sensor noise level (in %)

Figure Error of MCL with mixture prop osal distribution as a function of the

sensor noise Compare this curve with Figure

A mixture of b oth prop osal distributions gives excellent results

q q

Here with denotes the mixing ratio between regular and

dual MCL Figure shows p erformance results of MCL using this mixture

prop osal distribution using a xed mixing ratio All data p oints

are averaged over indep endent exp eriments Comparison with Figure

suggests that this prop osal distribution is uniformly sup erior to regular MCL

and in certain cases reduces the error by more than an order of magnitude

These results have b een obtained with the third metho d for calculating

imp ortance factors describ ed in the previous section In our simulation ex

p eriments we found that the second approach yields slightly worse results

but the dierence was not signicant at the condence level As noted

ab ove wehavenotyet implemented the rst approach In our rob ot results

b elow we use the second metho d for calculating imp ortance factors

Rob ot Results

A series of exp eriments was conducted carried out b oth in simulation and

using physical rob ot hardware to elucidate the dierence b etween MCL with

the standard and the mixture prop osal distribution We found that the mo d

ied prop osal distribution scales much b etter to small sample set sizes than

conventional MCL Figure plots the error of b oth MCL algorithms for dif

With samples the compu ferent error levels using m samples only

tational load is on a MHz Pentium Computermeaning that the

Fox Thrun Burgard Dellaert

1. 2. 3. 4. 5. 7. 10. 20. 30. 40. 50. 500

400

300

200 error (in centimeter)

100

1. 2. 3. 4. 5. 7. 10. 20. 30. 40. 50.

sensor noise level (in %)

Figure Error of MCL top curve and hybrid MCL b ottom curve with

samples instead of for each b elief state

algorithm is approximately times faster than realtime While MCL with

the standard prop osal distribution basically fails under this circumstances

to track the rob ots p osition our extended approachgives excellent results

which are only slightly inferior to those obtained with sample

The following exp erimentevaluates MCL with mixture prop osal distribu

tion in the context of the kidnapp ed rob ot problem This MCL algorithm

addresses the issue of recovery from a kidnapping in that it generates sam

ples that are consistentwithmomentary sensor readings Our approachwas

tested using laser range data recorded during the twoweek deploymentofthe

rob ot Minerva Figure shows part of the map of the museum and the path

of the rob ot used for this exp eriment To enforce the kidnapp ed rob ot prob

lem we rep eatedly intro duced errors into the o dometry information These

errors made the rob ot lose track of its p osition with probability of when

advancing one meter These errors where synthetic however they accurately

mo deled the eect of kidnapping a rob ot to a random lo cation

Figure shows comparative results for three dierent approaches The

error is measured by the p ercentage of time during which the estimated p osi

tion deviates more than meters from the reference p osition Obviously using

the mixture prop osal distribution yields signicantly b etter results even if the

basic prop osal distribution is mixed with random samples as suggested

in Fox et al b to alleviate the kidnapp ed rob ot problem The mix

ture prop osal distribution reduces the error rate of lo calization byasmuchas

more than MCL if the standard prop osal distribution is employed and

when compared to the case where the standard prop osal distribution is

mixed with a uniform distribution These results are signicant at the

condence level evaluated over actual rob ot data

Particle Filters for Mobile Rob ot Lo calization

Figure Part of the map of the Smithsonians Museum of National History

and path of the rob ot

We also compared MCL with dierent prop osal distributions in the con

text of visual lo calization using only camera imagery obtained with the rob ot

Minerva during public museum hours The sp ecic image sequence is of ex

tremely p o or quality as p eople often intentionally covered the camera with

their hand and placed dirt on the lens Figure shows the lo calization error

obtained when using vision only calculated using the lo calization results from

the laser as ground truth The data covers a p erio d of approximately

seconds during which MCL pro cesses a total of images After approx

imately seconds a drastic error in the rob ots o dometry leads to a loss of

the p osition which is an instance of the kidnapp ed rob ot problem As the

two curves in Figure illustrate the regular MCL sampler dashed curve

is unable to recover from this event whereas MCL with mixture prop osal

distribution solid curve recovers quickly These result are not statistically

signicant in that only a single run is considered but they conrm our nd

ings with laser range nders Together our result suggest that the mixture

distribution drastically increases the robustness of the statistical estimator

for mobile rob ot lo calization

MultiRob ot MCL

Basic Considerations

The nal section of this chapter briey addresses the multirob ot lo calization

problem As mentioned in the intro duction multirob ot lo calization involves

a team of rob ots whichsimultaneously seek to determine their p oses in a

known environment This problem is particularly interesting if rob ots can

Fox Thrun Burgard Dellaert

1

MCL without random samples 0.8

0.6

0.4 MCL with random samples

0.2 Mixture-MCL error rate (in percentage of lost positions)

0 250 500 1000 2000 4000

number of samples

Figure Performance of MCL with the conventional top curve and mixture

prop osal distribution b ottom curve evaluated for the kidnapp ed rob ot problem

in the Smithsonian museum The middle curve reects the p erformance of MCL

with a small numb er of random samples added in the resampling step as suggested

in Fox et al as a means to recover from lo calization failures The error rate

is measured in p ercentage of time during which the rob ot lost track of its p osition

sense each other during lo calization The ability to detect each other can

signicantly sp eed up learning however it also creates dep endencies in the

p ose estimates of individual rob ots that p ose ma jor challenges for the design

of the estimator

Formally sp eaking the multirob ot lo calization problem is the problem of

N

N

i i

estimating a p osterior densityover a pro duct space X X where X

i

describ es the p osition of the ith rob ot Every time a rob ot senses it obtains

information ab out the relative p oses of all other rob ots either by detecting

nearby rob ots or by not detecting them which also provides information

ij

ab out other rob ots p oses Let r denote the random variable that mo dels

t

the detection of rob ot j by rob ot i at time t i j Thus the variable

ij

r either takes the value not detected or it contains a relative distance and

t

b earing of rob ot j relative to rob ot iThemultirob ot lo calization problem

thus extends the single rob ot lo calization problem by additional observations

ij

i ij j

r which are mo deled using a timeinvariant sensor mo del px j r x

t

time index omitted as ab ove

The rst and most imp ortant thing to notice is that the multirob ot lo

calization problem is very hard and in fact weonlyknow of a rudimentary

solution which while exhibiting reasonable p erformance in practice p ossesses

clear limitations What makes this problem hard is the fact that the random

ij

intro duce dep endencies in the rob ots b eliefs Thus ideally one variables r

t

N

N

i

X would like to estimate the p osterior over the joint distribution X

i

However such calculations cannot b e carried out lo cally a desirable prop

Particle Filters for Mobile Rob ot Lo calization

4500 Standard MCL 4000 Mixture MCL 3500 3000 2500 2000

Distance [cm] 1500 1000 500 0 0 500 1000 1500 2000 2500 3000 3500 4000

Time [sec]

Figure MCL with the standard prop osal distribution dashed curve compared

to MCL with the new mixture distribution solid line Shown here is the error for

a second episo de of camerabased lo calization in the Smithsonian museum

erty of autonomous rob ots and more imp ortantly the size of X increases

exp onentially with the numb er of rob ots N The latter is not muchofa prob

lem if all rob ots are welllo calized however during global lo calization large

subspaces of X would have to b e p opulated with samples rendering particle

lters hop efully inecient for this dicult problem

Our approach basically ignores these nontrivial interdep endencies and in

stead represents the b elief at time t by the pro duct of its marginals

N

Y

i

Belx Belx

t

t

i

Thus our representation eectively makes a false indep endence assumption

see Boyen and Koller for an idea howtoovercome this indep endence as

sumption while still avoiding the exp onential death of the full pro duct space

When a rob ot detects another rob ot the observation is folded into a rob ots

current b elief and the result is used to up date the b elief of the other rob ots

More sp ecically supp ose rob ot i detects rob ot j at time tThenj s b elief

is up dated according to

Z

j ij j j

i i i

px Belx j x r Belx dx Belx

t t

t
t

t
t
t

The derivation of this formula is analogous to the derivation of Bayes lters

ab ove and can b e found in Fox et al By symmetry the same detection

is b e used to constrain the ith rob ots p osition based on the b elief of the j the rob ot

Fox Thrun Burgard Dellaert

Figure Sample set representing a rob ots b elief

Clearly our up date rule assumes indep endence Hence when applied more

than once it can lead to rep etitive use of the same evidence which will make

our rob ots more condent than warranted by the data Unfortunatelywe

are not aware of a go o d x to this problem that would maintain the same

computational eciency as our approach To reduce this eect our current

algorithm only pro cesses p ositivesightings that is the eventof not seeing

another rob ot has no eect Additionally rep etitivesightings in short time

intervals are ignored Nevertheless the o ccasional transfer from one rob ot to

another can have a substantial eect on each rob ots ability to lo calize

The implementation of the multirob ot lo calization algorithm as a dis

tributed particle lter requires some thought This is b ecause under our fac

torial representation each rob ot maintains its own lo cal sample set When

one rob ot detects another b oth sample sets havetobesynchronized accord

ing to Equation Note that this equation requires the multiplication of

two densities which means that wehave to establish a corresp ondence b e

tween the individual samples of rob ot j and the density representing rob ot

is b elief ab out the p osition of rob ot j However b oth of these densities

are themselves represented by sample sets and with probabilityonenotwo

samples in these sets are the same Tosolve this problem our approach

transforms sample sets into density functions using density trees Koller and

Fratkina Mo ore et al Omohundro Density trees are con

tinuations of sample sets which approximate the underlying density using a

variableresolution piecewise constant density

Figure shows such a tree which corresp onds to a rob ots estimate of

another rob ots lo cation Together with Figure it shows a map of our

testing environment along with a sample set obtained during global lo caliza

tion The resolution of the tree is a function of the densities of the samples

the more samples exist in a region of space the more negrained the tree

representation The tree enables us to integrate the detection into the sam

ple set of the detected rob ot using imp ortance sampling for each individual

Particle Filters for Mobile Rob ot Lo calization

Figure Tree representation extracted from the sample set

sample hx w i

Z

j

i i i i

w px j x r Belx dx

t

t
t
t
t

Rob ot Results

MultiRob ot MCL has b een tested using twoRWI Pioneer rob ots equipp ed

with a camera and a laser range nder for detection see Fox et al

for details In particular our implementation detects rob ots visuallyand

uses a laser range nder to determine the relative distance and b earing The

i ij j

p erceptual mo dels px j r x were estimated from data collected in a

separate training phase where the exact lo cation of eachrobotwas known

After training the mean error of the distance estimation was cm and

the mean angular error was degree Additionally there was a chance

of erroneously detecting a rob ot false p ositive

Figure plots the lo calization error as a function of time averaged over

ten exp eriments involving physical rob ots in the environment shown in Fig

ure The ability of the rob ots to detect eac h other clearly reduces the

time required for global lo calization Obviouslytheoveruse of evidence

while theoretically present app ears not to harm the rob ots abilitytolocal

ize themselves We attribute this nding to the fact that our multirob ot

MCL is highly selective when incorp orating relative information These nd

ings were conrmed in systematic simulation exp eriments Fox et al

involving larger groups of rob ots in a range of dierentenvironments

Conclusion

This chapter has surveyed a family of particle lters for mobile rob ot lo caliza

tion commonly known as Monte Carlo localization MCL MCL algorithms

provide ecient and robust solutions for a range of mobile rob ot lo calization

Fox Thrun Burgard Dellaert

2500 Single robot Multi-robot 2000

1500

1000 Estimation error [cm] 500

0 0 20 40 60 80 100 120

Time [sec]

Figure Empirical results comparing single rob ot MCL and multi rob ot MCL

problems such as p osition tracking global lo calization rob ot kidnapping

and multirob ot lo calization

This chapter investigated three variants of the basic algorithm The basic

MCL algorithm which has b een applied with great success to global lo caliza

tion and tracking followed by an extension that uses a more sensible prop osal

distribution whichovercomes certain limitations of MCL suchaspoorper

formance when sensors are to o accurate and sub optimal recovery from rob ot

kidnapping Finally the pap er prop osed an extension to multirob ot lo caliza

tion where a distributed factorial representation was employed to estimate

the joint p osterior

For all these algorithms we obtained favorable results in practice In

fact an elab orate exp erimental comparison with our previous b est metho d a

version of Markov lo calization that uses negrained grid representations Fox

at al a showed that MCL is an order of magnitude more ecient and

accurate than the gridbased approach

The derivation of all these algorithms is based on a collection of indep en

dence assumptions ranging from a static world assumption to the assumption

that the joint b elief space of multiple rob ots can b e factorized into indep en

dent comp onents that are up dated lo cally on each rob ot Clearly in most ap

plication domains all of these indep endence assumptions are violated Rob ot

environments for example are rarely static Relaxing those assumptions is

akey goal of current research with enormous p otential b enets for rob ot practitioners

Particle Filters for Mobile Rob ot Lo calization

Acknowledgments

The authors like to thank Hannes Kruppa for his contributions to the multi

rob ot MCL approach and him and the memb ers of CMUs Rob ot Learning

Lab for stimulating discussions that have inuenced this research We are

also indebted to Nando de Freitas and Arnaud Doucet for their insightful

comments on an earlier draft of this pap er that substantially improved the

presentation of the material

This research is sp onsored by the National Science Foundation CAREER

grantnumb er I IS and regular grantnumb er I IS and by

DARPAATO via TACOM contract number DAAECL and DARPA

ISO via Rome Labs contract numb er F which is gratefully

acknowledged The views and conclusions contained in this do cument are

those of the author and should not b e interpreted as necessarily representing

ocial p olicies or endorsements either expressed or implied of the United

States Governmentorany of the sp onsoring institutions

References

Bentley J Multidimensional divide and conquer Communications of

the ACM

Borenstein J Everett B and Feng L Navigating Mobile Robots

Systems and TechniquesAKPeters Ltd Wellesley MA

en X and Koller D Tractable inference for complex sto chastic Boy

pro cesses Proceedings of Uncertainty in Articial Intel ligen ce Madison

WI pp

Burgard W Fox D Hennig D and Schmidt T Estimating the

absolute p osition of a mobile rob ot using p osition probability grids Pro

ceedings of the Thirteenth National ConferenceonArticial Intel ligence

AAAI AAAI PressMIT Press Menlo Park

Cox I Blanchean exp eriment in guidance and navigation of an au

tonomous rob ot vehicle IEEE Transactions on Robotics and Automation

Cox I and Wilfong G eds Autonomous Robot Vehicles Springer

Verlag

Dean T L and Bo ddy M An analysis of timedep endent plan

ning Proceeding of Seventh National ConferenceonArticial Intel li

gence AAAI AAAI AAAI PressThe MIT Press Menlo Park CA pp

Fox Thrun Burgard Dellaert

Dellaert F Burgard W Fox D and Thrun S a Using the con

densation algorithm for robust visionbased mobile rob ot lo calization

Proceedings of the IEEE International Conference on Computer Vision

and Pattern Recognition IEEE Fort Collins CO

Dellaert F Fox D Burgard W and Thrun S b Monte carlo

lo calization for mobile rob ots Proceedings of the IEEE International

ConferenceonRobotics and Automation ICRA

Dellaert F Thorp e C and Thrun S c Mosaicing a large number

of widely disp ersed noisy and distorted images A bayesian approach

Technical Report CMURITR Carnegie Mellon University Pitts

burgh PA

Doucet A On sequential simulationbased metho ds for bayesian

ltering Technical Report CUEDFINFENGTR Cambridge Uni

versity Department of Engineering Cambridge UK

Engelson S and McDermott D Error correction in mobile rob ot

map learning Proceedings of the IEEE International Conference

on Robotics and Automation Nice France pp

Fox D Burgard W and Thrun S ActiveMarkov lo calization for

mobile rob ots Robotics and Autonomous Systems

Fox D Burgard W and Thrun S a Markov lo calization for mo

bile rob ots in dynamic environments Journal of Articial Intel ligence

Research

Fox D Burgard W Dellaert F and Thrun S b Monte carlo lo

calization Ecient p osition estimation for mobile rob ots Proceedings

of the AAAI National ConferenceonArticial Intel ligen ce AAAI Or

lando FL

Fox D Burgard W Kruppa H and Thrun S A probabilistic

approach to collab orativemultirob ot lo calization Autonomous Robots

Fukuda T Ito S Oota N Arai F Ab e Y Tanake K and Tanaka Y

Navigation system based on ceiling landmark recognition for au

tonomous mobile rob ot Proc Intl Conference on Industrial Electronics

Control and Instrumentation IECONVol pp

Fung R and Del Favero B Backward simulation in bayesian net

works Proce edings of the ConferenceonUncertainty in AI UAI

Particle Filters for Mobile Rob ot Lo calization

Hinkel R and Knieriemen T Environment p erception with a laser

radar in a fast moving rob ot Proceedings of Symposium on Robot Con

trol Karlsruhe Germany pp

Kaelbling L Cassandra A and Kurien J Acting under uncertainty

Discrete bayesian mo dels for mobilerob ot navigation Proceedings of the

IEEERSJ International Conference on Intel ligent Robots and Systems

Kitagawa G Monte carlo lter and smo other for nongaussian nonlin

ear state space mo dels Journal of Computational and Graphical Statis

tics

Ko enig S and Simmons R Passive distance learning for rob ot nav

igation in L Saitta ed Proceedings of the Thirteenth International

Conference on Machine Learning

Koller D and Fratkina R Using learning for approximation in

sto chastic pro cesses Proc of the International Conference on Machine

Learning ICML

Kortenkamp D Bonasso R and Murphy R eds AIbased Mobile

ot systems MIT Press Cambridge Robots Case studies of successful rob

MA

Lenser S and Veloso M Sensor resetting lo calization for p o orly mo d

elled mobile rob ots Proceedings of the IEEE International Conference

on Robotics and Automation ICRA IEEE San Francisco CA

Leonard J DurrantWhyte H and Cox I Dynamic map building for

an autonomous mobile rob ot International Journal of Robotics Research

Liu J and Chen R Sequential monte carlo metho ds for dynamic

systems Journal of the American Statistical Association

Mo ore A Schneider J and Deng K Ecient lo cally weighted

p olynomial regression predictions Proc of the International Conference

on Machine Learning ICML

Mo ore A W Ecient MemorybasedLearning for Robot Control

PhD thesis Trinity Hall UniversityofCambridge England

Nourbakhsh I Powers R and Bircheld S DERVISH an oce

navigating rob ot AI Magazine

Omohundro S M Bumptrees for ecient function constraint and

classication learning in R P Lippmann J E Mo o dy and D S Touret

zky eds Advanc es in Neural Information Processing Systems Mor gan Kaufmann

Fox Thrun Burgard Dellaert

Pitt M and Shephard N Filtering via simulation auxiliary particle

lter Journal of the American Statistical AssociationForthcoming

Rencken W Concurrent lo calisation and map building for mobile

rob ots using ultrasonic sensors Proceedings of the IEEERSJ Interna

tional Conference on Intel ligen t Robots and SystemsYokohama Japan

pp

Simmons R and Ko enig S Probabilistic rob ot navigation in par

tially observable environments Proceedings of IJCAI IJCAI Inc

Montreal Canada pp

Simmons R Go o dwin R Haigh K Ko enig S and OSullivan J

Alayered architecture for oce delivery rob ots Proceedings of the First

International ConferenceonAutonomous Agents Marina del ReyCA

Tanner M Tools for Statistical Inference Springer Verlag New York

nd edition

Thrun S Bayesian landmark learning for mobile rob ot lo calization

Machine Learning

Thrun S Bennewitz M Burgard W Cremers A Dellaert F Fox

hulz D D Hahnel D Rosenb erg C RoyNSchulte J and Sc

MINERVA A second generation mobile tourguide rob ot Pro

ceedings of the IEEE International ConferenceonRobotics and Automa

tion ICRA

Thrun S Fox D and Burgard W Monte carlo lo calization with

mixture prop osal distributions Proceedings of the AAAI National Con

ferenceonArticial Intel ligence AAAI Austin TX

Wei G Wetzler C and von Puttkamer E Keeping track of p osition

and orientation of moving indo or systems by correlation of rangender

scans Proceedings of the International Conference on Intel ligen t Robots

and Systems pp

Zilb erstein S and Russell S Approximate reasoning using anytime

algorithms in S Natara jan ed Imprecise and Approximate Compu

tation Kluwer Academic Publishers Dordrecht