Frank Dellaert, Fall 07

Particle Filtering
for Tracking and Localization

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 Bayesian Inference

Data

Belief before Belief after

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 World Knowledge

z
d

x
Sensor Model P(z|x)

d
Most often
Tutorial : Monte analytic expression,
Carlo Methods Frank Dellaert can be learnedOctober 07
 Recap: Bayes Law

P(x|z) ~ L(x;z)P(x)

data = z

model L(x;z)

Belief before = P(x) Belief after = P(x|z)

Prior Distribution Likelihood Posterior Distribution
of x of x given Z of x given Z
Tutorial : Monte Carlo Methods Frank Dellaert October 07
 Example: 1D Robot Localization

Prior P(x)

Likelihood
L(x;z)

Posterior
P(x|z)
Tutorial : Monte Carlo Methods Frank Dellaert October 07
 Problem: Large Open Spaces

Walls and obstacles out of range

Sonar and laser have problems
Horizontally mounted sensors have problems

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 Problem: Large Crowds

One solution: Robust filtering

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 Solution: Ceiling Camera

Upward looking camera

Model of the world = Ceiling Mosaic

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 Global Alignment (other talk)

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 Ceiling Mosaic
Tutorial : Monte Carlo Methods Frank Dellaert October 07
 Vision based Sensor

P(z|x)
z
h(x)

Tutorial : Monte Carlo Methods Frank Dellaert October 07

Under Light
Next to Light
Elsewhere
 Various Density Representations

Gaussian centered around mean x,y

Mixture of Gaussians
Finite element i.e. histogram
Does not scale to large state spaces
encountered in computer vision & robotics

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 Hidden Markov Models

A B C D

ABCDE

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 Kalman Filter = Very Easy

Think adding quadratics

Then Minimize
Dynamics = Enlarge Quadratic

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 Kalman Filter

Very powerful
Gaussian, unimodal
sensor

motion motion

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 Example: 2D Robot Location

p(x)

x2

State space = 2D, infinite #states

x1
Tutorial : Monte Carlo Methods Frank Dellaert October 07
 Markov Localization

Fine discretization over {x,y,theta}

Very successful: Rhino, Minerva, Xavier

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 Dynamic Markov Localization

Burgard et al., IROS 98

Idea: use Oct-trees

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 Sampling as Representation

p(x)

x2

x1
Tutorial : Monte Carlo Methods Frank Dellaert October 07
 Sampling Advantages

Arbitrary densities
Memory = O(#samples)
Only in Typical Set
Great visualization tool !

minus: Approximate

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 Mean and Variance of a Sample

Mean

Variance (1D)

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 Inference = Monte Carlo Estimates

Estimate expectation of any function f:

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 Monte Carlo Expected Value

Expected angle = 30o
Tutorial : Monte Carlo Methods Frank Dellaert October 07
 How to Sample ?

Target Density (x)

Assumption: we can evaluate (x) up to an
arbitrary multiplicative constant

Why cant we just sample from (x) ??

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 How to Sample ?

Numerical Recipes in C, Chapter 7

Transformation method: Gaussians etc
Rejection sampling
Importance sampling

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 Rejection Sampling

Target Density (x)

Proposal Density q(x) (x) q(x)

and q need only be

known up to a factor

Image by MacKay

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 The Good

9% Rejection Rate
Tutorial : Monte Carlo Methods Frank Dellaert October 07
 the Bad

50% Rejection Rate

Tutorial : Monte Carlo Methods Frank Dellaert October 07
 and the Ugly.

70% Rejection Rate

Tutorial : Monte Carlo Methods Frank Dellaert October 07
 Inference by Rejection Sampling
P(measured_angle|x,y) = N(predicted_angle,3 degrees)

Prior(x,y)
Posterior(x,y|measured_angle=20o)

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 Importance Sampling

Good Proposal Density would be: prior !

Problem:
No guaranteed c s.t. c P(x)>=P(x|z) for all x

Idea:
sample from P(x)
give each sample x(r) a importance weight equal
to P(Z|x (r))
Tutorial : Monte Carlo Methods Frank Dellaert October 07
 Example Importance Sampling

{x(r),y(r)~Prior(x,y), wr=P(Z|x(r),y(r)) }

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 Importance Sampling

Sample x(r) from q(x)

wr = (x(r))/q(x(r))

q(x)
(x)

Image by MacKay

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 1D Importance Sampling

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 Example 1

Learn Color Model

Implement Rejection Sampling
Implement Importance Sampling
Add a spatial prior

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 A Simple Color Model

B
G

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 Likelihood
x

B
G

y
R
P(x, y | color) " P(color | x, y)P(x, y)
P(color | x, y) = $ N( ,# c c ^2)
c= r,g,b
Tutorial : Monte Carlo Methods Frank Dellaert October 07
 References

Isard & Blake 98, Condensation -- conditional

density propagation for visual tracking
Dellaert, Fox, Burgard & Thrun 99, Monte
Carlo Methods Localization for Mobile Robots
Khan, Balch & Dellaert 04 A Rao-
Blackwellized Particle Filter for EigenTracking

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 Example: 1D Robot Localization

Prior P(x)

Likelihood
L(x;z)

Posterior
P(x|z)
Tutorial : Monte Carlo Methods Frank Dellaert October 07
 1D Importance Sampling

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 Particle Filter Tracking

State Xt-2 Xt-1 Xt

Measurement Zt-2 Zt-1 Zt

Monte Carlo Approximation of Posterior:

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 Bayes Filter and Particle Filter

Motion Model
Recursive Bayes Filter Equation:

Predictive Density
Monte Carlo Approximation:

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 Particle Filter

Empirical predictive density = Mixture Model

(2)
(1) (3)

First appeared in 70s, re-discovered

Tutorial : Monte Carlo Methods Frank Dellaert
by Kitagawa, Isard,
October 07
 Monte Carlo Localization

Sk-1 S k
weighted Sk Sk

Predict Weight Resample

Tutorial : Monte Carlo Methods Frank Dellaert October 07
 3D Particle filter for robot pose:
Monte Carlo Localization
Dellaert, Fox, Burgard & Thrun ICRA 99

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 Resampling

Importance Sampling => weighted

To get back a fair sample:
Resample from the weighted samples according
to the importance weights
efficient O(N) algorithms exist

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 Condensation Algorithm

Sequential Estimation
Iterates over:
Prediction with motion model
Importance Sampling for Inference
Resampling

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 1. Prediction Phase

P(xt| ,u)
Motion Model

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 2. Measurement Phase

P(Z|xt)

Sensor Model

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 3. Resampling Step

O(N)

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 Example 1

Start from Importance Sampling w Prior

Implement Sample Mean
Try increasing nrSamples
Implement Resampling Step
Implement Particle Motion Model

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 A Simple Color Model

B
G

Tutorial : Monte Carlo Methods Frank Dellaert October 07

 Likelihood
x

B
G

y
R
P(x, y | color) " P(color | x, y)P(x, y)
P(color | x, y) = $ N( ,# c c ^2)
c= r,g,b
Tutorial : Monte Carlo Methods Frank Dellaert October 07