City College of New York
1
Dr. Jizhong XiaoDepartment of Electrical Engineering
CUNY City [email protected]
Particle Filter/Monte Carlo Localization
Advanced Mobile Robotics
Probabilistic Robotics:
City College of New York
Probabilistic Robotics
Bayes Filter Implementations
Particle filtersMonte Carlo Localization
City College of New York
Sample-based Localization (sonar)
City College of New York
Particle Filter
Definition:Particle filter is a Bayesian based filter that
sample the whole robot work space by a weight function derived from the belief distribution of previous stage.
Basic principle:Set of state hypotheses (“particles”)Survival-of-the-fittest
4
City College of New York
Represent belief by random samplesEstimation of non-Gaussian, nonlinear
processes
Particle filtering non-parametric inference algorithm - suited to track non-linear dynamics.- efficiently represent non-Gaussian distributions
Why Particle Filters
City College of New York
Function Approximation Particle sets can be used to approximate functions
The more particles fall into an interval, the higher the probability of that interval
6
City College of New York
Particle Filter Projection
7
City College of New York
Rejection Sampling Let us assume that f(x)<1 for all x Sample x from a uniform distribution Sample c from [0,1] if f(x) > c keep the sample otherwise reject the sample
8
City College of New York
Importance Sampling Principle
We can even use a different distribution g togenerate samples from f
By introducing an importance weight w, we canaccount for the “differences between g and f ”
w = f / g f is often called target g is often called proposal
9
City College of New York
10
Importance Sampling
Samples cannot be drawn conveniently from the target distribution f.
A sample of f is obtained by attaching the weight f/g to each sample x.
Instead, the importance sampler draws samples from the proposal distribution g, which has a simpler form.
Importance factor w
ondistributi proposalondistributitarget
w
City College of New York
Particle Filter Basics
Sample: Randomly select M particles based on weights (same particle may be picked multiple times)
Predict: Move particles according to deterministic dynamics
Measure: Get a likelihood for each new sample by making a prediction about the image’s local appearance and comparing; then update weight on particle accordingly
11
City College of New York
Particle Filter Basics Particle filters represent a distribution by a set of samples drawn from the
posterior distribution. The denser a sub-region of the state space is populated by samples, the
more likely it is that true state falls into this region. Such a representation is approximate, but it is nonparametric, and therefore
can represent a much broader space of distributions than Gaussians. Weight of particle are given through the measurement model. Re-sampling allows to redistribute particles approximately according to the
posterior . The re-sampling step is a probabilistic implementation of the Darwinian idea
of survival of the fittest: it refocuses the particle set to regions in state space with high posterior probability
By doing so, it focuses the computational resources of the filter algorithm to regions in the state space where they matter the most.
12
City College of New York
Importance Sampling with Resampling:Landmark Detection Example
City College of New York
Distributions
City College of New York
15
Distributions
Wanted: samples distributed according to p(x| z1, z2, z3)
City College of New York
This is Easy!We can draw samples from p(x|zl) by adding noise to the detection parameters.
City College of New York
Importance Sampling with Resampling
),...,,(
)()|(),...,,|( :fon distributiTarget
2121
n
kk
n zzzp
xpxzpzzzxp
)()()|()|( :gon distributi Sampling
l
ll zp
xpxzpzxp
),...,,(
)|()(
)|(),...,,|( : w weightsImportance
21
21
n
lkkl
l
n
zzzp
xzpzp
zxpzzzxp
gf
City College of New York
Importance Sampling with Resampling
Weighted samples After resampling
City College of New York
19
draw xit1 from Bel(xt1)
draw xit from p(xt | xi
t1,ut1)Importance factor for xi
t:
)|()(),|(
)(),|()|()()(
ondistributi proposalondistributitarget
111
111
tt
tttt
tttttt
t
tit
xzpxBeluxxp
xBeluxxpxzpxBelxBel
w
1111 )(),|()|()( tttttttt dxxBeluxxpxzpxBel
Particle Filter Algorithm
• Prediction (Action)
• Correction (Measurement)111 )(),|()( tttttt dxxbelxuxpxbel
)()|()( tttt xbelxzpxbel
City College of New York
Particle Filter Algorithm
20
Each particle is a hypothesis as to what the true world state may be at time tSampling from the state transition distributionImportance factor, incorporates the measurement into particle set Re-sampling, importance sampling
City College of New York
Particle Filter Algorithm Line 4 – hypothetical state
- sampling from the state transition distribution- set of particles obtained after M iterations is the filter’s representation of the posterior
Line 5 – importance factor- incorporate measurement into the particle set- set of weighted particles represents Bayes filter posterior
Line 8 to 11 – Importance samplingParticle distribution changes - incorporating importance weights
into the re-sampling process. Survival of the fittest: After resampling step, refocuses particle
set to the regions in state space with higher posterior probability, distributed according to 21
)( txbel
)( txbel
)()()( ][t
mttt xbelxzpxbel
City College of New York
1. Algorithm particle_filter( St-1, ut-1 zt):2.
3. For Generate new samples
4. Sample index j(i) from the discrete distribution given by wt-
1
5. Sample from using and
6. Compute importance weight
7. Update normalization factor
8. Insert
9. For
10. Normalize weights
Particle Filter Algorithm(from Fox’s slides)
0, tS
ni 1
},{ it
ittt wxSS
itw
itx ),|( 11 ttt uxxp )(
1ij
tx 1tu
)|( itt
it xzpw
ni 1
/it
it ww
City College of New York
Resampling
• Given: Set S of weighted samples.
• Wanted : Random sample, where the probability of drawing xi is given by wi.
• Typically done n times with replacement to generate new sample set S’.
City College of New York
1. Algorithm systematic_resampling(S,n):
2. 3. For Generate cdf4. 5. Initialize threshold
6. For Draw samples …7. While ( ) Skip until next threshold reached8. 9. Insert10. Increment threshold
11. Return S’
Resampling Algorithm
11,' wcS
ni 2i
ii wcc 1
1],,0[~ 11 inUu
nj 1
11
nuu jj
ij cu
1,'' nxSS i
1ii
Also called stochastic universal sampling
City College of New York
Particle Filters
Pose particles drawn at random and uniformly over the entire pose space
City College of New York
)|()(
)()|(
)()|()(
xzpxBel
xBelxzpw
xBelxzpxBel
Sensor Information: Importance Sampling
After robot senses the door, MCL Assigns importance factors to each particle
City College of New York
'd)'()'|()( , xxBelxuxpxBel
Robot Motion
After incorporating the robot motion and after resampling, leads to new particle set with uniform importance weights, but with an increased number of particles near the three likely places
City College of New York
)|()(
)()|(
)()|()(
xzpxBel
xBelxzpw
xBelxzpxBel
Sensor Information: Importance Sampling
New measurement assigns non-uniform importance weights to the particle sets, most of the cumulative probability mass is centered on the second door
City College of New York
Robot Motion
'd)'()'|()( , xxBelxuxpxBel
Further motion leads to another re-sampling step, and a step in which a new particle set is generated according to the motion model
City College of New York
Practical Considerations Particles represent discrete approximation of continuous beliefs
1. Density Extraction (Estimation)extracting a continuous density from samplesDensity Estimation MethodsGaussian approximation – unimodal distributionHistogram - Multi-model distributionsThe probability of each bin is by summing the weights of the particles that fall in its range Kernel Density Estimation – multi-modal, smooth
each particle is used as the kernelplacing a Gaussian kernel at each particleoverall density is a mixture of the kernel density
30
City College of New York
Different Ways of Extracting Densities from Particles
31
Gaussian Approximation
Histogram Approximation
Kernel density Estimate
City College of New York
Properties of Particle Filters
Sampling VarianceVariation due to random samplingThe sampling variance decreases with the
number of samplesHigher number of samples result in accurate
approximations with less variability If enough samples are chosen, the observations
made by a robot – sample based belief “close enough” to the true belief.
32
City College of New York
Drawbacks In order to explore a significant part of the state space, the
number of particles should be very large which induces complexity problems not adapted to a real-time implementation.
PF methods are very sensitive to non consistent measures or high measurement errors.
33
City College of New York
34
City College of New York
Importance Sampling with Resampling
Reducing sampling error1. Variance reduction Reduce the frequency of resampling Maintains importance weight in memory & updates them as
follows:
2. Low variance sampling Computes a single random number Any number points to exactly only one particle,
35
Instead of choosing M random numbers and selecting those particles accordingly, this procedure computes a single random number and selects samples according to this number but still with a probability proportional to the sample weight
Resampling too often increases the risk of losing diversity. Too infrequently many samples might be wasted in regions of low probability,
City College of New York
Low variance resampling
36
Draw a random number r in the interval (0; M-1 )
Select particles by repeatedly adding the fixed amount of M-1 to r and by choosing the particle that corresponds to the resulting number
City College of New York
Low Variance Resampling ProcedurePrinciple:Selection involves sequential stochastic
processSelect samples w.r.t. a single random number,r
but with a probability proportional to sample weight
37
City College of New York
Advantages of Particle Filters:
can deal with non-linearitiescan deal with non-Gaussian noisemostly parallelizableeasy to implementPFs focus adaptively on probable regions of
state-space
38
City College of New York
Summary Particle filters are an implementation of recursive
Bayesian filtering They represent the posterior by a set of weighted
samples. In the context of localization, the particles are
propagated according to the motion model. They are then weighted according to the likelihood of
the observations. In a re-sampling step, new particles are drawn with a
probability proportional to the likelihood of the observation
39
City College of New York
40
City College of New York
41
Monte Carlo Localization
• One of the most popular particle filter methods for robot localization
• Reference: – Dieter Fox, Wolfram Burgard, Frank Dellaert,
Sebastian Thrun, “Monte Carlo Localization: Efficient Position Estimation for Mobile Robots”, Proc. 16th National Conference on Artificial Intelligence, AAAI’99, July 1999
City College of New York
42
MCL in action“Monte Carlo” Localization -- refers to the resampling of the
distribution each time a new observation is integrated
City College of New York
43
Monte Carlo Localization• the probability density function is represented by
samples• randomly drawn from it• it is also able to represent multi-modal distributions,
and thus localize the robot globally• considerably reduces the amount of memory required
and can integrate measurements at a higher rate• state is not discretized and the method is more
accurate than the grid-based methods• easy to implement
City College of New York
44
Robot modeling
p( z | x, m ) sensor modelmap m and location x
p( | x, m ) = .75
p( | x, m ) = .05
potential observations z
City College of New York
45
Robot modeling
p( z | x, m ) sensor model
p( xnew | xold, u, m ) action model
“probabilistic kinematics” -- encoder uncertainty
• red lines indicate commanded action• the cloud indicates the likelihood of various final states
map m and location r
p( | x, m ) = .75
p( | x, m ) = .05
potential observations o
City College of New York
46
Robot modeling: how-to
p(z | x, m ) sensor model
p( xnew | xold, u, m ) action model
(0) Model the physics of the sensor/actuators(with error estimates)
(1) Measure lots of sensing/action results and create a model from them
theoretical modeling
empirical modeling
• take N measurements, find mean (m) and st. dev. () and then use a Gaussian model• or, some other easily-manipulated model...
(2) Make something up...
p( x ) = p( x ) = 0 if |x-m| > 1 otherwise 1- |x-m|/
otherwise
0 if |x-m| >
City College of New York
Start
Motion Model Reminder
City College of New York
Proximity Sensor Model Reminder
Laser sensor Sonar sensor
City College of New York
49
Monte Carlo Localization
Start by assuming p( x0 ) is the uniform distribution.take K samples of x0 and weight each with an importance factor of 1/K
City College of New York
50
Monte Carlo Localization
Start by assuming p( x ) is the uniform distribution.
Get the current sensor observation, z1
For each sample point x0 multiply the importance factor by p(z1 | x0, m)
take K samples of x0 and weight each with an importance factor of 1/K
City College of New York
51
Monte Carlo Localization
Start by assuming p( x0 ) is the uniform distribution.
Get the current sensor observation, z1
For each sample point x0 multiply the importance factor by p(z1 | x0, m)
take K samples of x0 and weight each with an importance factor of 1/K
Normalize (make sure the importance factors add to 1)
You now have an approximation of p(x1 | z1, …, m) and the distribution is no longer uniform
City College of New York
52
Monte Carlo Localization
Start by assuming p( x0 ) is the uniform distribution.
Get the current sensor observation, z1
For each sample point x0 multiply the importance factor by p(z1 | x0, m)
take K samples of x0 and weight each with an importance factor of 1/K
Normalize (make sure the importance factors add to 1)
You now have an approximation of p(x1 | z1, …, m)
Create x1 samples by dividing up large clumps
and the distribution is no longer uniform
each point spawns new ones in proportion to its importance factor
City College of New York
53
Monte Carlo Localization
Start by assuming p( x0 ) is the uniform distribution.
Get the current sensor observation, z1
For each sample point x0 multiply the importance factor by p(z1 | x0, m)
take K samples of x0 and weight each with an importance factor of 1/K
Normalize (make sure the importance factors add to 1)
You now have an approximation of p(x1 | z1, …, m)
Create x1 samples by dividing up large clumps
and the distribution is no longer uniform
The robot moves, u1
each point spawns new ones in proportion to its importance factor
For each sample x1, move it according to the model p(x2 | u1, x1, m)
City College of New York
54
Monte Carlo Localization
Start by assuming p( x0 ) is the uniform distribution.
Get the current sensor observation, z1
For each sample point x0 multiply the importance factor by p(z1 | x0, m)
take K samples of x0 and weight each with an importance factor of 1/K
Normalize (make sure the importance factors add to 1)
You now have an approximation of p(x1 | z1, …, m)
Create x1 samples by dividing up large clumps
and the distribution is no longer uniform
The robot moves, u1
each point spawns new ones in proportion to its importance factor
For each sample x1, move it according to the model p(x2 | u1, x1, m)
City College of New York
55
MCL in action“Monte Carlo” Localization -- refers to the resampling of the
distribution each time a new observation is integrated
City College of New York
56
Initial Distribution
City College of New York
57
After Incorporating Ten Ultrasound Scans
City College of New York
58
After Incorporating 65 Ultrasound Scans
City College of New York
59
Estimated Path
City College of New York
Using Ceiling Maps for Localization
City College of New York
Vision-based Localization
P(z|x)
h(x)z
City College of New York
Under a LightMeasurement z: P(z|x):
City College of New York
Next to a LightMeasurement z: P(z|x):
City College of New York
ElsewhereMeasurement z: P(z|x):
City College of New York
Global Localization Using Vision
City College of New York
66
Robots in Action: Albert
City College of New York
67
Application: Rhino and Albert Synchronized in Munich and Bonn
[Robotics And Automation Magazine, to appear]
City College of New York
Localization for AIBO robots
City College of New York
69
Limitations
• The approach described so far is able to – track the pose of a mobile robot and to– globally localize the robot.
• How can we deal with localization errors (i.e., the kidnapped robot problem)?
City College of New York
70
Approaches
• Randomly insert samples (the robot can be teleported at any point in time).
• Insert random samples proportional to the average likelihood of the particles (the robot has been teleported with higher probability when the likelihood of its observations drops).
City College of New York
71
Random Samples Vision-Based Localization
936 Images, 4MB, .6secs/imageTrajectory of the robot:
City College of New York
72
Odometry Information
City College of New York
73
Image Sequence
City College of New York
74
Resulting TrajectoriesPosition tracking:
City College of New York
75
Resulting TrajectoriesGlobal localization:
City College of New York
76
Global Localization
City College of New York
77
Kidnapping the Robot
City College of New York
78
Summary• Particle filters are an implementation of recursive
Bayesian filtering• They represent the posterior by a set of
weighted samples.• In the context of localization, the particles are
propagated according to the motion model.• They are then weighted according to the
likelihood of the observations.• In a re-sampling step, new particles are drawn
with a probability proportional to the likelihood of the observation.
City College of New York
79
References
1. Dieter Fox, Wolfram Burgard, Frank Dellaert, Sebastian Thrun, “Monte Carlo Localization: Efficient Position Estimation for Mobile Robots”, Proc. 16th National Conference on Artificial Intelligence, AAAI’99, July 1999
2. Dieter Fox, Wolfram Burgard, Sebastian Thrun, “Markov Localization for Mobile Robots in Dynamic Environments”, J. of Artificial Intelligence Research 11 (1999) 391-427
3. Sebastian Thrun, “Probabilistic Algorithms in Robotics”, Technical Report CMU-CS-00-126, School of Computer Science, Carnegie Mellon University, Pittsburgh, USA, 2000
City College of New York
80
Thank You