From Bayesian Filtering to Particle Filters

Dieter FoxUniversity of Washington

Joint work withW. Burgard, F. Dellaert, C. Kwok, S.

Thrun

Bayes Filters: Framework

• Given:• Stream of observations z and action data u:

• Sensor model P(z|x).• Action model P(x|u,x’).• Prior probability of the system state P(x).

• Wanted: • Estimate of the state X of a dynamical system.• The posterior of the state is also called Belief:

),,,|()( 121 tttt zuzuxPxBel

},,,{ 121 ttt zuzud

Graphical Model Representation

Underlying Assumptions• Static world• Independent noise• Perfect model, no approximation errors

),|(),,|( 1:11:11:1 ttttttt uxxpuzxxp )|(),,|( :11:1:0 tttttt xzpuzxzp

Bayes Filters

),,,|(),,,,|( 121121 ttttt uzuxPuzuxzP Bayes

z = observationu = actionx = state

),,,|()( 121 tttt zuzuxPxBel

Markov ),,,|()|( 121 tttt uzuxPxzP

1111 )(),|()|( ttttttt dxxBelxuxPxzP

Markov1121111 ),,,|(),|()|( tttttttt dxuzuxPxuxPxzP

11211

1121

),,,|(

),,,,|()|(

ttt

ttttt

dxuzuxP

xuzuxPxzP

Total prob.

Bayes Filters are Familiar!

• Kalman filters (linear, extended, unscented)

• Particle filters (Rao-Blackwellized)

• Hidden Markov models

• Dynamic Bayesian networks

• Estimator in Partially Observable Markov Decision Processes (POMDPs)

111 )(),|()|()( tttttttt dxxBelxuxPxzPxBel

Robot Localization

• Given • Map of the environment.• Sequence of sensor measurements.

• Wanted• Estimate of the robot’s position.

• Problem classes• Position tracking• Global localization• Kidnapped robot problem (recovery)

“Using sensory information to locate the robot in its environment is the most fundamental problem to providing a mobile robot with autonomous capabilities.” [Cox ’91]

Bayes Filters for Robot Localization

Representations for Bayesian Robot Localization

Discrete approaches (’95)• Topological representation (’95)

• uncertainty handling (POMDPs)• occas. global localization, recovery

• Grid-based, metric representation (’96)• global localization, recovery

Multi-hypothesis (’00)• multiple Kalman filters• global localization, recovery

Particle filters (’99)• sample-based representation• global localization, recovery

Kalman filters (late-80s)• Gaussians• approximately linear models• position tracking

Discrete

Continuous

Particle Filters

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]

Particle Filters

Sample-based Representation

Weight samples: w = f / g

Importance Sampling

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

),...,,(

)()|(),...,,|( :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

zxp

zzzxp

g

f

Importance Sampling with Resampling

Weighted samples After resampling

Particle Filters

)|()(

)()|()()|()(

xzpxBel

xBelxzpw

xBelxzpxBel

Sensor Information: Importance Sampling

'd)'()'|()( , xxBelxuxpxBel

Robot Motion

)|()(

)()|()()|()(

xzpxBel

xBelxzpw

xBelxzpxBel

Sensor Information: Importance Sampling

Robot Motion

'd)'()'|()( , xxBelxuxpxBel

draw xit1 from Bel(xt1)

draw xit from p(xt | xi

t1,ut1)

Importance factor for xit:

)|()(),|(

)(),|()|(ondistributi proposal

ondistributitarget

111

111

tt

tttt

tttttt

it

xzpxBeluxxp

xBeluxxpxzp

w

1111 )(),|()|()( tttttttt dxxBeluxxpxzpxBel

Particle Filter Algorithm

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’.

w2

w3

w1wn

Wn-1

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

Start

Motion Model Example

Proximity Sensor Model Example

Laser sensor Sonar sensor

Sample-based Localization (sonar)

Localization for AIBO robots

How Many Samples?

• Idea: • Assume we know the true belief.

• Represent this belief as a multinomial distribution.

• Determine number of samples such that we can guarantee that, with probability (1- ), the KL-distance between the true posterior and the sample-based approximation is less than .

• Observation: • For fixed and , number of samples only depends on

number k of bins with support:

KLD-sampling

3

12

)1(9

2

)1(9

21

2

1)1,1(

2

1

zkk

kkn

[Fox NIPS-01, Fox IJRR-03]

Evaluation

KLD-Sampling: Sonar

[Fox NIPS-01, Fox IJRR-03]

KLD-Sampling: Laser

Practical Considerations

• Dealing with highly peaked observations• Add noise to observation model• Better proposal distributions: e.g., perform Kalman filter

step to determine proposal

• Recover from failure by selectively adding samples from observations

• Overestimating noise often reduces number of required samples

• Resample only when necessary (efficiency of representation measured by variance of weights)

• Try to avoid sampling whenever possible!

Rao-Blackwellised Particle Filter

Example

Ball Tracking in RoboCup

Extremely noisy (nonlinear) motion of observer Inaccurate sensing, limited processing power Interactions between target and environment Interactions between robot(s) and target

Goal: Unified framework for modeling the ball and its interactions.

Tracking Techniques

Kalman Filter Highly efficient, robust (even for nonlinear) Uni-modal, limited handling of nonlinearities

Particle Filter Less efficient, highly robust Multi-modal, nonlinear, non-Gaussian

Rao-Blackwellised Particle Filter, MHT Combines PF with KF Multi-modal, highly efficient

Dynamic Bayesian Network for Ball Tracking

k-2b k-1b kb

r k-2 r k-1 r k

z k-2 z k-1 z k

u k-2 u k-1

z k-1 z kz k-2

kmk-1mk-2m

Ball observation

Ball location and velocity

Ball motion mode

Map and robot location

Robot control

Landmark detection

Bal

l tra

ckin

gR

obot

loca

liza

tion l l l

b b b

Robot Location

k-2b k-1b kb

r k-2 r k-1 r k

z k-2 z k-1 z k

u k-2 u k-1

z k-1 z kz k-2

kmk-1mk-2m

Ball observation

Ball location and velocity

Ball motion mode

Map and robot location

Robot control

Landmark detection

Bal

l tra

ckin

gR

obot

loca

liza

tion l l l

b b b

Robot and Ball Location (and velocity)

k-2b k-1b kb

r k-2 r k-1 r k

z k-2 z k-1 z k

u k-2 u k-1

z k-1 z kz k-2

kmk-1mk-2m

Ball observation

Ball location and velocity

Ball motion mode

Map and robot location

Robot control

Landmark detection

Bal

l tra

ckin

gR

obot

loca

liza

tion l l l

b b b

Ball-Environment Interactions

None Grabbed

BouncedKicked

(0.8)

Robot loses grab (0.2)

(1.0)

Ro

bo

t kicks b

all (0.9)

(1.0

)

Within grab range and robot grabs

(prob. from model )

(0.1

)

Kick fails (0.1)

Deflected

(residual prob .) (1

.0)

Col

lisio

n w

ith

obje

cts

on m

ap (1

.0)

Integrating Discrete Ball Motion Mode

k-2b k-1b kb

r k-2 r k-1 r k

z k-2 z k-1 z k

u k-2 u k-1

z k-1 z kz k-2

kmk-1mk-2m

Ball observation

Ball location and velocity

Ball motion mode

Map and robot location

Robot control

Landmark detection

Bal

l tra

ckin

gR

obot

loca

liza

tion l l l

b b b

Grab Example (1)

k-2b k-1b kb

r k-2 r k-1 r k

z k-2 z k-1 z k

u k-2 u k-1

z k-1 z kz k-2

kmk-1mk-2m

Ball observation

Ball location and velocity

Ball motion mode

Map and robot location

Robot control

Landmark detection

Bal

l tra

ckin

gR

obot

loca

liza

tion l l l

b b b

Grab Example (2)

k-2b k-1b kb

r k-2 r k-1 r k

z k-2 z k-1 z k

u k-2 u k-1

z k-1 z kz k-2

kmk-1mk-2m

Ball observation

Ball location and velocity

Ball motion mode

Map and robot location

Robot control

Landmark detection

Bal

l tra

ckin

gR

obot

loca

liza

tion l l l

b b b

Inference: Posterior Estimation

k-2b k-1b kb

r k-2 r k-1 r k

z k-2 z k-1 z k

u k-2 u k-1

z k-1 z kz k-2

kmk-1mk-2m

Ball observation

Ball location and velocity

Ball motion mode

Map and robot location

Robot control

Landmark detection

Bal

l tra

ckin

gR

obot

loca

liza

tion l l l

b b b

),,|,,( 1:1:1:1 klk

bkkkk uzzrmbp

Particle Filter for Robot Localization

Rao-Blackwellised PF for Inference Factorize state space into different components

Represent some part(s) by samples and remaining part analytically, conditioned on samples

Each sample

contains robot location, ball mode, ball location

iiiiiiiiii myxbmrbmrskkk

,,,,,,,,,:1:1

),|(),,|(),,,|(

),|,,(

1:1:1:11:1:1:1:11:1:1:1:1

1:1:1:1:1

kkkkkkkkkkkk

kkkkk

uzrpuzrmpuzrmbp

uzrmbp

Rao-Blackwellised Particle Filter for Inference

k-1b kb

r k-1 r k

kmk-1m

Ball location and velocity

Ball motion mode

Map and robot location

Bal

l tra

ckin

gR

obot

loca

liza

tion

)(1

)(1

)(1 ,, i

ki

ki

k bmr

Draw a sample from the previous sample set:

Generate Robot Location

r k-1 r k

u k-1

z k

Map and robot location

Robot control

Landmark detection

Rob

ot lo

cali

zati

on l

k-1b

k-1m

Ball location and velocity

Ball motion mode

Bal

l tra

ckin

g

kb

km

__,,),,,,|(~ )(1

)(1

)(1

)(1

)( ikkk

ik

ik

ikk

ik ruzbmrrpr

Generate Ball Motion Model

k-1b

r k-1 r k

u k-1

z k

kmk-1m

Ball location and velocity

Ball motion mode

Map and robot location

Robot control

Landmark detection

Bal

l tra

ckin

gR

obot

loca

liza

tion l

kb

_,,),,,,|(~ )()(1

)(1

)(1

)()( ik

ikkk

ik

ik

ikk

ik mruzbmrmpm

Update Ball Location and Velocity

k-1b

r k-1 r k

u k-1

z k

kmk-1m

Ball location and velocity

Ball motion mode

Map and robot location

Robot control

Landmark detection

Bal

l tra

ckin

gR

obot

loca

liza

tion l

kb

z k b

)()()()(1

)()()( ,,),,,|(~ ik

ik

ikk

ik

ik

ikk

ik bmrzbmrbpb

Importance Resampling

Weight sample by

if observation is landmark detection and by

if observation is ball detection.

Resample

)|( )()( ik

lk

ik rzpw

ki

ki

ki

ki

ki

ki

ki

kbk

ik

ik

ik

bk

ik

bbrmbpbrmzp

brmzpw

d),,|(),,|(

),,|()(1

)()()()()()(

)(1

)()()(

Ball-Environment Interaction

Ball-Environment Interaction

Tracking and Finding the Ball

Cluster ball samples by discretizing pan / tilt angles

Uses negative information

Experiment: Real Robot

0

0.05

0.1

0.15

0.2

0.25

0.3

0.35

0.4

0.45

0.5

0 5 10 15 20 25 30 35 40 45 50

Pe

rce

nta

ge

of

ba

ll lo

st

Number of ball samples

With MapWithout Map

Robot kicks ball 100 times, tries to find it afterwards

Finds ball in 1.5 seconds on average

Error vs. Prediction Time

0

10

20

30

40

50

60

70

80

0 0.5 1 1.5 2

RM

S E

rror

[cm

]

Prediction time [sec]

RBPFKF'KF*

Reference* Observations

Reference* Observations

Simulation Runs

Comparison to KF* (optimized for straight motion)

RBPFKF*

Reference* Observations

RBPFKF*

Reference* Observations

RBPFKF’

Reference* Observations

RBPFKF’

Reference* Observations

Comparison to KF’ (inflated prediction noise)

Orientation Errors

0

20

40

60

80

100

120

140

160

180

2 3 4 5 6 7 8 9 10 11

Orie

nta

tion

Err

or [

deg

rees

]

Time [sec]

RBPFKF*KF'

Discussion

Particle filters are intuitive and “simple”

Support point-wise thinking (reduced uncertainty)

It’s an art to make them work

Good for test implementation if system behavior is not well known

Inefficient compared to Kalman filters

Rao-Blackwellization for complex problems

Only sample discrete / highly non-linear parts of state space

Solve remaining part analytically (KF,discrete)