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

MostMonte Carlo Methods Tutorial : often analytic expression, can be learned Frank Dellaert October 07

Recap: Bayes Law

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

model L(x;z)
Belief before = P(x) Prior Distribution of x
Tutorial : Monte Carlo Methods

Belief after = P(x|z) Likelihood of x given Z

Frank Dellaert

Posterior Distribution of x given Z

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

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) and q need only be known up to a factor

(x)

q(x)

Image by MacKay
Tutorial : Monte Carlo Methods Frank Dellaert October 07

The Good

Tutorial : Monte Carlo Methods

9% Rejection Rate
Frank Dellaert

October 07

the Bad

Tutorial : Monte Carlo Methods

50% Rejection Rate

Frank Dellaert

October 07

and the Ugly.

Tutorial : Monte Carlo Methods

70% Rejection Rate

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

Tutorial : Monte Carlo Methods

Frank Dellaert

October 07

Likelihood
x

P(x, y | color) " P(color | x, y)P(x, y) P(color | x, y) =

Tutorial : Monte Carlo Methods Frank Dellaert

$ N( ,#
c c= r,g,b

^2)
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 RaoBlackwellized 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:

Monte Carlo Approximation:

Predictive Density

Tutorial : Monte Carlo Methods

Frank Dellaert

October 07

Particle Filter
Empirical predictive density = Mixture Model
(1) (2)

(3)

Tutorial : Monte Carlo Methods Frank Dellaert October 07 First appeared in 70s, re-discovered by Kitagawa, Isard,

Monte Carlo Localization

Sk-1

weighted

Sk

Sk

Predict
Tutorial : Monte Carlo Methods

Weight
Frank Dellaert

Resample
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

u 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

Tutorial : Monte Carlo Methods

Frank Dellaert

October 07

Likelihood
x

P(x, y | color) " P(color | x, y)P(x, y) P(color | x, y) =

Tutorial : Monte Carlo Methods Frank Dellaert

$ N( ,#
c c= r,g,b

^2)
October 07