sign in sign up
<<
Home , Particle filter

Frank Dellaert, Fall 07

Particle Filtering for Tracking and Localization

Tutorial : Monte Carlo Methods Frank Dellaert October ‘07

Data

Belief before Belief after

Tutorial : Monte Carlo Methods Frank Dellaert October ‘07 World Knowledge

z

d x Sensor Model P(z|x)

d MTutoriasl : tM oontef Ctaerlo nMe thaodns alytic expFrraenks Dsellaieort n , c a n b e l e a r n e d October ‘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 • 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 x,y • Mixture of Gaussians • Finite element i.e. • Does not scale to large state spaces encountered in computer vision &

Tutorial : Monte Carlo Methods Frank Dellaert October ‘07 Hidden Markov Models

A B C D

E

A B C D E

Tutorial : Monte Carlo Methods Frank Dellaert October ‘07 = 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 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 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 can’t 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… • •

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

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

Tutorial : Monte Carlo Methods Frank Dellaert October ‘07 Importance Sampling • Sample x(r) from q(x) (r) (r) • wr = π(x )/q(x )

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

R

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)

FirstTutor iaappearedl : Monte Carlo Meth oinds 70’s, re-discoveredFrank Dellaert by Kitagawa, I sard Octob,e r …‘07 Monte Carlo Localization

’ ’ Sk-1 S k weighted S k 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

• 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

B G

R

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

!