HCI / CprE / ComS 575:
Computational Perception
Instructor: Alexander Stoytchevhttp://www.ece.iastate.edu/~alexs/classes/2010_Spring_575/
Particle Filters
HCI/ComS 575X: Computational PerceptionIowa State UniversityCopyright © Alexander Stoytchev
Sebastian Thrun, Wolfram Burgard and Dieter Fox (2005).
Probabilistic Robotics
MIT Press.
F. Dellaert, D. Fox, W. Burgard, and S. Thrun (1999).
"Monte Carlo Localization for Mobile Robots", IEEE International Conference on Robotics
and Automation (ICRA99), May, 1999.
Ioannis Rekleitis (2004).
A Particle Filter Tutorial for Mobile Robot Localization.
Technical Report TR-CIM-04-02, Centre for Intelligent Machines, McGill University,
Montreal, Quebec, Canada.
Wednesday
Next Week
• Preliminary Project Presentatons
Represent belief by random samples
Estimation of non-Gaussian, nonlinear processes
Monte Carlo filter, Survival of the fittest, Condensation, Bootstrap filter, Particle filter
Filtering: [Rubin, 88], [Gordon et al., 93], [Kitagawa 96]
Computer vision: [Isard and Blake 96, 98] Dynamic Bayesian Networks: [Kanazawa et al., 95]d
Particle Filters
Example
Using Ceiling Maps for Localization
Vision-based Localization
P(z|x)
h(x)z
Under a LightMeasurement z: P(z|x):
Next to a LightMeasurement z: P(z|x):
ElsewhereMeasurement z: P(z|x):
Global Localization Using Vision
Sample-based Localization (sonar)
Example
Importance Sampling with Resampling:Landmark Detection Example
Distributions
Distributions
Wanted: samples distributed according to p(x| z1, z2, z3)
This is Easy!We can draw samples from p(x|zl) by adding noise to the detection parameters.
Importance Sampling with Resampling
Weighted samples After resampling
Quick review of Kalman Filters
Conditional density of position based on measured value of z1
[Maybeck (1979)]
Conditional density of position based on measured value of z1
[Maybeck (1979)]
position
measured position
uncertainty
Conditional density of position based on measurement of z2 alone
[Maybeck (1979)]
Conditional density of position based on measurement of z2 alone
[Maybeck (1979)]measured position 2
uncertainty 2
Conditional density of position based on data z1 and z2
[Maybeck (1979)]position estimate
uncertainty estimate
Propagation of the conditional density
[Maybeck (1979)]
Propagation of the conditional density
[Maybeck (1979)]
movement vector
expected position just prior to taking measurement 3
Propagation of the conditional density
[Maybeck (1979)]
movement vector
expected position just prior to taking measurement 3
Propagation of the conditional density
z3
σx(t3)
measured position 3
uncertainty 3
Updating the conditional density after the third measurement
z3
σx(t3)
position uncertainty
position estimate
x(t3)
Some Questions
• What if we don’t know the start position of the robot?
• What if somebody moves the robot without the robot’s knowledge?
Robot Odometry Errors
Raw range data, position indexed by odometry
[Thrun, Burgard & Fox (2005)]
Resulting Occupancy Grid Map
[Thrun, Burgard & Fox (2005)]
Basic Idea Behind Particle Filters
x
In 2D it looks like this
[http://www.ite.uni-karlsruhe.de/METZGER/DIPLOMARBEITEN/dipl2.html]
Robot Pose
Odometry Motion Model
Sampling From the Odometry Model
Motion Model
Motion Model
Velocity model for different noise parameters
Sampling from the velocity model
In Class Demoof Particle Filters
Example
[Thrun, Burgard & Fox (2005)]
Initially we don’t know the location of the robot so we have particles everywhere
Next, the robot senses that it is near a door
Since there are 3 identical doors the robot can be next any one of them
Therefore, we inflate balls (particles) that are next to doors and shrink all others
Therefore, we grow balls (particles) that are next to doors and shrink all others
Before we continue we have to make all ball to be of equal size. We need to resample.
Before we continue we have to make all ball to be of equal size. We need to resample.
Resampling Rules
=
=
=
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’.
[From Thrun’s book “Probabilistik Robotics”]
w2
w3
w1wn
Wn-1
Roulette wheel Resampling
w2
w3
w1wn
Wn-1
• Roulette wheel
• Binary search, n log n
• Stochastic universal sampling
• Systematic resampling
• Linear time complexity
• Easy to implement, low variance
[From Thrun’s book “Probabilistik Robotics”]
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
[From Thrun’s book “Probabilistik Robotics”]
Next, The robot moves to the right
… thus, we have to shift all balls (particles) to the right
… thus, we have to shift all balls (particles) to the right
… and add some position noise
… and add some position noise
Next, the robot senses that it is next to one of the three doors
Next, the robot senses that it is next to one of the three doors
Now we have to resample again
The robot moves again
… so we must move all balls (particles) to the right again
… and add some position noise
And so on …
Now Let’s Compare that With Some of the Other Methods
Grid Localization
[Thrun, Burgard & Fox (2005)]
Grid Localization
[Thrun, Burgard & Fox (2005)]
Grid Localization
[Thrun, Burgard & Fox (2005)]
Grid Localization
[Thrun, Burgard & Fox (2005)]
Grid Localization
[Thrun, Burgard & Fox (2005)]
Markov Localization
[Thrun, Burgard & Fox (2005)]
Kalman Filter
[Thrun, Burgard & Fox (2005)]
Particle Filter
[Thrun, Burgard & Fox (2005)]
Importance Sampling
• Ideally, the particles would represent samples drawn from the distribution p(x|z).– In practice, we usually cannot get p(x|z) in
closed form; in any case, it would usually be difficult to draw samples from p(x|z).
• We use importance sampling:– Particles are drawn from an importance
distribution.– Particles are weighted by importance weights.
[ http://www.fulton.asu.edu/~morrell/581/ ]
Monte Carlo Samples (Particles)
• The posterior distribution p(x|z) may be difficult or impossible to compute in closed form.
• An alternative is to represent p(x|z) using Monte Carlo samples (particles):– Each particle has a value and a weight
x
x
[ http://www.fulton.asu.edu/~morrell/581/ ]
In 2D it looks like this
[http://www.ite.uni-karlsruhe.de/METZGER/DIPLOMARBEITEN/dipl2.html]
Objective-Find p(xk|zk,…,z1)
• The objective of the particle filter is to compute the conditional distribution
p(xk|zk,…,z1)
• To do this analytically, we would use the Chapman-Kolmogorov equation and Bayes Theorem along with Markov model assumptions.
• The particle filter gives us an approximate computational technique.
[ http://www.fulton.asu.edu/~morrell/581/ ]
Initial State Distribution
x0
x0
[ http://www.fulton.asu.edu/~morrell/581/ ]
State Update
x0
x1 = f0 (x0, w0)
x1
[ http://www.fulton.asu.edu/~morrell/581/ ]
Compute Weights
x1
x1
p(z1|x1)
x1
Before
After
[ http://www.fulton.asu.edu/~morrell/581/ ]
Resample
x1
x1
[ http://www.fulton.asu.edu/~morrell/581/ ]
THE END