# Particle Filters

AI and Robotics

Nov 7, 2013 (4 years and 6 months ago)

93 views

From Bayesian Filtering to

Particle Filters

Dieter Fox

University of Washington

Joint work with

W. 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
:

)
,
,
,
|
(
)
(
1
2
1
t
t
t
t
z
u
z
u
x
P
x
Bel

}
,
,
,
{
1
2
1
t
t
t
z
u
z
u
d

Graphical Model Representation

Underlying Assumptions

Static world

Independent noise

Perfect model, no approximation errors

)
,
|
(
)
,
,
|
(
1
:
1
1
:
1
1
:
1
t
t
t
t
t
t
t
u
x
x
p
u
z
x
x
p

)
|
(
)
,
,
|
(
:
1
1
:
1
:
0
t
t
t
t
t
t
x
z
p
u
z
x
z
p

Bayes Filters

)
,
,
,
|
(
)
,
,
,
,
|
(
1
2
1
1
2
1

t
t
t
t
t
u
z
u
x
P
u
z
u
x
z
P

Bayes

z

= observation

u

= action

x

= state

)
,
,
,
|
(
)
(
1
2
1
t
t
t
t
z
u
z
u
x
P
x
Bel

Markov

)
,
,
,
|
(
)
|
(
1
2
1

t
t
t
t
u
z
u
x
P
x
z
P

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

t
t
t
t
t
t
t
dx
x
Bel
x
u
x
P
x
z
P

Markov

1
1
2
1
1
1
1
)
,
,
,
|
(
)
,
|
(
)
|
(

t
t
t
t
t
t
t
t
dx
u
z
u
x
P
x
u
x
P
x
z
P

1
1
2
1
1
1
1
2
1
)
,
,
,
|
(
)
,
,
,
,
|
(
)
|
(

t
t
t
t
t
t
t
t
dx
u
z
u
x
P
x
u
z
u
x
P
x
z
P

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)

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

t
t
t
t
t
t
t
t
dx
x
Bel
x
u
x
P
x
z
P
x
Bel

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| z
1
, z
2
, z
3
)

This is Easy!

We can draw samples from
p(x|z
l
)

noise to the detection parameters.

Importance Sampling with
Resampling

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

:
f
on
distributi
Target
2
1
2
1
n
k
k
n
z
z
z
p
x
p
x
z
p
z
z
z
x
p

)
(
)
(
)
|
(
)
|
(

:
g
on
distributi

Sampling
l
l
l
z
p
x
p
x
z
p
z
x
p

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

:
w
weights
Importance
2
1
2
1
n
l
k
k
l
l
n
z
z
z
p
x
z
p
z
p
z
x
p
z
z
z
x
p
g
f

Importance Sampling with
Resampling

Weighted samples

After resampling

Particle Filters

)
|
(
)
(
)
(
)
|
(
)
(
)
|
(
)
(
x
z
p
x
Bel
x
Bel
x
z
p
w
x
Bel
x
z
p
x
Bel

Sensor Information: Importance Sampling

'
d
)
'
(
)
'
|
(
)
(
,
x
x
Bel
x
u
x
p
x
Bel
Robot Motion

)
|
(
)
(
)
(
)
|
(
)
(
)
|
(
)
(
x
z
p
x
Bel
x
Bel
x
z
p
w
x
Bel
x
z
p
x
Bel

Sensor Information: Importance Sampling

Robot Motion

'
d
)
'
(
)
'
|
(
)
(
,
x
x
Bel
x
u
x
p
x
Bel
draw
x
i
t

1

from
Bel
(x
t

1
)

draw
x
i
t

from
p
(
x
t
|
x
i
t

1
,
u
t

1
)

Importance factor for
x
i
t
:

)
|
(
)
(
)
,
|
(
)
(
)
,
|
(
)
|
(
on
distributi

proposal
on
distributi
target
1
1
1
1
1
1
t
t
t
t
t
t
t
t
t
t
t
t
i
t
x
z
p
x
Bel
u
x
x
p
x
Bel
u
x
x
p
x
z
p
w

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

t
t
t
t
t
t
t
t
dx
x
Bel
u
x
x
p
x
z
p
x
Bel

Particle Filter Algorithm

Resampling

Given
: Set
S

of weighted samples.

Wanted
: Random sample, where the
probability of drawing
x
i

is given by
w
i
.

Typically done
n

times with replacement to
generate new sample set
S’
.

w
2

w
3

w
1

w
n

W
n
-
1

Resampling

w
2

w
3

w
1

w
n

W
n
-
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
-

d
)
, the KL
-
distance between the true
posterior and the sample
-
based approximation is less than
e
.

Observation
:

For fixed
d

and
e
, number of samples only depends on
number
k
of bins with support:

KLD
-
sampling

3
1
2
)
1
(
9
2
)
1
(
9
2
1
2
1
)
1
,
1
(
2
1

d
e
d
e
z
k
k
k
k
n
[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

Better proposal distributions: e.g., perform Kalman filter
step to determine proposal

Recover from failure

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
-
2

b

k
-
1

b

k

b

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

k

z

k
-
2

k

m

k
-
1

m

k
-
2

m

Ball observation

Ball location and velocity

Ball motion mode

Map and robot location

Robot control

Landmark detection

Ball tracking

Robot localization

l

l

l

b

b

b

Robot Location

k
-
2

b

k
-
1

b

k

b

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

k

z

k
-
2

k

m

k
-
1

m

k
-
2

m

Ball observation

Ball location and velocity

Ball motion mode

Map and robot location

Robot control

Landmark detection

Ball tracking

Robot localization

l

l

l

b

b

b

Robot and Ball Location (and
velocity)

k
-
2

b

k
-
1

b

k

b

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

k

z

k
-
2

k

m

k
-
1

m

k
-
2

m

Ball observation

Ball location and velocity

Ball motion mode

Map and robot location

Robot control

Landmark detection

Ball tracking

Robot localization

l

l

l

b

b

b

Ball
-
Environment Interactions

None

Grabbed

Bounced

Kicked

(

0

.

8

)

Robot loses grab

(

0

.

2

)

R

o

b

o

t

k

i

c

k

s

b

a

l

l

(

0

.

9

)

Within grab range

and robot grabs

(

prob

.

from model

)

Kick fails

(

0

.

1

)

Deflected

(

residual prob

.)

Integrating Discrete Ball
Motion Mode

k
-
2

b

k
-
1

b

k

b

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

k

z

k
-
2

k

m

k
-
1

m

k
-
2

m

Ball observation

Ball location and velocity

Ball motion mode

Map and robot location

Robot control

Landmark detection

Ball tracking

Robot localization

l

l

l

b

b

b

Grab Example (1)

k
-
2

b

k
-
1

b

k

b

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

k

z

k
-
2

k

m

k
-
1

m

k
-
2

m

Ball observation

Ball location and velocity

Ball motion mode

Map and robot location

Robot control

Landmark detection

Ball tracking

Robot localization

l

l

l

b

b

b

Grab Example (2)

k
-
2

b

k
-
1

b

k

b

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

k

z

k
-
2

k

m

k
-
1

m

k
-
2

m

Ball observation

Ball location and velocity

Ball motion mode

Map and robot location

Robot control

Landmark detection

Ball tracking

Robot localization

l

l

l

b

b

b

Inference: Posterior
Estimation

k
-
2

b

k
-
1

b

k

b

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

k

z

k
-
2

k

m

k
-
1

m

k
-
2

m

Ball observation

Ball location and velocity

Ball motion mode

Map and robot location

Robot control

Landmark detection

Ball tracking

Robot localization

l

l

l

b

b

b

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

k
l
k
b
k
k
k
k
u
z
z
r
m
b
p
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

i
i
i
i
i
i
i
i
i
i
m
y
x
b
m
r
b
m
r
s
k
k
k

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

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

k
k
k
k
k
k
k
k
k
k
k
k
k
k
k
k
k
u
z
r
p
u
z
r
m
p
u
z
r
m
b
p
u
z
r
m
b
p
Rao
-
Blackwellised Particle Filter for
Inference

k
-
1

b

k

b

r

k
-
1

r

k

k

m

k
-
1

m

Ball location and velocity

Ball motion mode

Map and robot location

Ball tracking

Robot localization

)
(
1
)
(
1
)
(
1
,
,
i
k
i
k
i
k
b
m
r

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

Robot localization

l

k
-
1

b

k
-
1

m

Ball location and velocity

Ball motion mode

Ball tracking

k

b

k

m

_
_,
,
)
,
,
,
,
|
(
~
)
(
1
)
(
1
)
(
1
)
(
1
)
(
i
k
k
k
i
k
i
k
i
k
k
i
k
r
u
z
b
m
r
r
p
r

Generate Ball Motion Model

k
-
1

b

r

k
-
1

r

k

u

k
-
1

z

k

k

m

k
-
1

m

Ball location and velocity

Ball motion mode

Map and robot location

Robot control

Landmark detection

Ball tracking

Robot localization

l

k

b

_
,
,
)
,
,
,
,
|
(
~
)
(
)
(
1
)
(
1
)
(
1
)
(
)
(
i
k
i
k
k
k
i
k
i
k
i
k
k
i
k
m
r
u
z
b
m
r
m
p
m

Update Ball Location and
Velocity

k
-
1

b

r

k
-
1

r

k

u

k
-
1

z

k

k

m

k
-
1

m

Ball location and velocity

Ball motion mode

Map and robot location

Robot control

Landmark detection

Ball tracking

Robot localization

l

k

b

z

k

b

)
(
)
(
)
(
)
(
1
)
(
)
(
)
(
,
,
)
,
,
,
|
(
~
i
k
i
k
i
k
k
i
k
i
k
i
k
k
i
k
b
m
r
z
b
m
r
b
p
b

Importance Resampling

Weight sample by

if observation is
landmark detection

and by

if observation is
ball detection
.

Resample

)
|
(
)
(
)
(
i
k
l
k
i
k
r
z
p
w

k
i
k
i
k
i
k
i
k
i
k
i
k
i
k
b
k
i
k
i
k
i
k
b
k
i
k
b
b
r
m
b
p
b
r
m
z
p
b
r
m
z
p
w
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

Percentage of ball lost

Number of ball samples

With Map

Without 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

RMS Error [cm]

Prediction time [sec]

RBPF

KF'

KF*

Reference

*

Observations

Simulation Runs

Comparison to KF*
(optimized for straight
motion)

RBPF

KF*

Reference

*

Observations

RBPF

KF’

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

Orientation Error [degrees]

Time [sec]

RBPF

KF*

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)