Particle Filters

ocelotgiantAI and Robotics

Nov 7, 2013 (3 years and 9 months ago)

82 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
)

by adding
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


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