Particle Filters

Pieter Abbeel
UC Berkeley EECS

Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics
 Motivation
§  For continuous spaces: often no analytical formulas for Bayes filter updates

§  Solution 1: Histogram Filters: (not studied in this lecture)

§  Partition the state space
§  Keep track of probability for each partition
§  Challenges:
§  What is the dynamics for the partitioned model?
§  What is the measurement model?
§  Often very fine resolution required to get reasonable results

§  Solution 2: Particle Filters:

§  Represent belief by random samples
§  Can use actual dynamics and measurement models
§  Naturally allocates computational resources where required (~ adaptive
resolution)
§  Aka Monte Carlo filter, Survival of the fittest, Condensation, Bootstrap filter
2
Sample-based Localization (sonar)
 Problem to be Solved

n  Given a sample-based representation St = {x1t , xt2 ,..., xtN }

of Bel(xt) = P(xt | z1, …, zt, u1, …, ut)

Find a sample-based representation St+1 = {x1t+1, xt+1

2 N
,..., xt+1 }

of Bel(xt+1) = P(xt+1 | z1, …, zt, zt+1 , u1, …, ut+1)

 Dynamics Update

n  Given a sample-based representation St = {x1t , xt2 ,..., xtN }

of Bel(xt) = P(xt | z1, …, zt, u1, …, ut)

Find a sample-based representation

of P(xt+1 | z1, …, zt, u1, …, ut+1)

n  Solution:
n  For i=1, 2, …, N
n  Sample xit+1 from P(Xt+1 | Xt = xit, ut+1)
 Observation update

n  Given a sample-based representation of {x1t+1, xt+1

2 N
,..., xt+1 }
P(xt+1 | z1, …, zt)

Find a sample-based representation of

P(xt+1 | z1, …, zt, zt+1) = C * P(xt+1 | z1, …, zt) * P(zt+1 | xt+1)

n  Solution:
n  For i=1, 2, …, N
n  w(i)t+1 = w(i)t* P(zt+1 | Xt+1 = x(i)t+1)
n  the distribution is represented by the weighted set of samples

{< x1t+1, w1t+1 >, < xt+1

2 2
, wt+1 N
>,..., < xt+1 N
, wt+1 >}
Sequential Importance Sampling (SIS) Particle Filter
n  Sample x11, x21, …, xN1 from P(X1)

n  Set wi1= 1 for all i=1,…,N

n  For t=1, 2, …

n  Dynamics update:
n  For i=1, 2, …, N

n  Sample x t+1 from P(Xt+1 | Xt = x t , ut+1)

i i

n  Observation update:

n  For i=1, 2, …, N

n  w t+1 = w t* P(zt+1 | Xt+1 = x t+1)

i i i

n  At any time t, the distribution is represented by the weighted set of samples

{ <xit, wit> ; i=1,…,N}

SIS particle filter major issue
n  The resulting samples are only weighted by the evidence
n  The samples themselves are never affected by the evidence
à Fails to concentrate particles/computation in the high
probability areas of the distribution P(xt | z1, …, zt)
Sequential Importance Resampling (SIR)
n  At any time t, the distribution is represented by the weighted
set of samples
{ <xit, wit> ; i=1,…,N}
à  Sample N times from the set of particles
à  The probability of drawing each particle is given by its
importance weight

à More particles/computation focused on the parts of the state

space with high probability mass
 Sequential Importance Resampling (SIR)
Particle Filter
1.  Algorithm particle_filter( St-1, ut , zt):
2.  St = ∅, η =0
3.  For i = 1… n Generate new samples
4.  Sample index j(i) from the discrete distribution given by wt-1
i j (i )
x
5.  Sample t from p(xt | xt!1, ut ) x
using t −1 and ut
6.  wti = p ( zt | xti ) Compute importance weight
7.  η = η + wti Update normalization factor
8.  S t = S t ∪ {< xti , wti >} Insert
9.  For i = 1… n
10.  wti = wti / η Normalize weights
11.  Return St
Particle Filters
Sensor Information: Importance Sampling
Robot Motion
Sensor Information: Importance Sampling
Robot Motion
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
 Noise Dominated by Motion Model

[Grisetti, Stachniss, Burgard, T-RO2006]

à Most particles get (near) zero weights and are lost.

 Importance Sampling

n  Theoretical justification: for any function f we have:

n  f could be: whether a grid cell is occupied or not, whether

the position of a robot is within 5cm of some (x,y), etc.
 Importance Sampling

n  Task: sample from density p(.)

n  Solution:
n  sample from “proposal density” ¼(.)
n  Weight each sample x(i) by p(x(i)) / ¼(x(i))
n  E.g.:
p ¼

n  Requirement: if ¼(x) = 0 then p(x) = 0.

 Particle Filters Revisited
1.  Algorithm particle_filter( St-1, ut , zt):
2.  St = ∅, η =0
3.  For i = 1… n Generate new samples
4.  Sample index j(i) from the discrete distribution given by wt-1
i
5.  Sample xt from ! (xt | x t!1
j (i)
, ut , zt )
i p(zt | xti )p(xti | xt!1
i
, ut )
6.  w =
t
! (xti | xt!1
i
, ut , zt )
Compute importance weight

7.  η = η + wti Update normalization factor

8.  S t = S t ∪ {< xti , wti >} Insert
9.  For i = 1… n
10.  wti = wti / η Normalize weights
11.  Return St
Optimal Sequential Proposal ¼(.)
n  Optimal ! (xt | x it!1, ut , zt ) = p(xt | x it!1, ut , zt )
i i i
i p(z t |x t )p(x t |xt−1 , ut )
à wt =
π(xit |xit−1 , ut , zt )
p(zt |xit )p(xit |xit−1 , ut )
=
p(xit |xit−1 , ut , zt )

n  Applying Bayes rule to the denominator gives:

i i i i
i i p(z t |x t , u t , x t−1 )p(x t |xt−1 , ut )
p(xt |xt−1 , ut , zt ) =
p(zt |xit−1 , ut )

n  Substitution and simplification gives

Optimal Sequential Proposal ¼(.)
n  Optimal ! (xt | x it!1, ut , zt ) = p(xt | x it!1, ut , zt )

n  à
n  Challenges:
Typically difficult to sample from p(xt | x t!1, ut , zt )
i
n 

n  Importance weight: typically expensive to compute integral

Example 1: ¼(.) = Optimal Proposal
Nonlinear Gaussian State Space Model

n  Nonlinear Gaussian State Space Model:

n  Then:
with

n  And:
Example 2: ¼(.) = Motion Model

n  à the “standard” particle filter

Example 3: Approximating Optimal ¼ for Localization

[Grisetti, Stachniss, Burgard, T-RO2006]

n  One (not so desirable solution): use smoothed likelihood

such that more particles retain a meaningful weight --- BUT
information is lost
n  Better: integrate latest observation z into proposal ¼
 Example 3: Approximating Optimal ¼ for
Localization: Generating One Weighted Sample
Build Gaussian Approximation to Optimal Sequential Proposal
1.  Initial guess

2.  Execute scan matching starting from the initial guess , resulting in
pose estimate .

3.  Sample K points in region around .

4.  Proposal distribution is Gaussian with mean and covariance:

5.  Sample from (approximately optimal) sequential proposal distribution.

�
6.  Weight = p(zt |x� , m)p(x� |xit−1 , ut )dx� ≈ η i
x�
 Example 3: Example Particle Distributions
[Grisetti, Stachniss, Burgard, T-RO2006]

Particles generated from the approximately optimal proposal

distribution. If using the standard motion model, in all three
cases the particle set would have been similar to (c).
Resampling
n  Consider running a particle filter for a system with
deterministic dynamics and no sensors
n  Problem:
n  While no information is obtained that favors one particle
over another, due to resampling some particles will
disappear and after running sufficiently long with very high
probability all particles will have become identical.
n  On the surface it might look like the particle filter has
uniquely determined the state.

n  Resampling induces loss of diversity. The variance of the

particles decreases, the variance of the particle set as an
estimator of the true belief increases.
Resampling Solution I
n  Effective sample size:

Normalized weights
n  Example:
n  All weights = 1/N à Effective sample size = N
n  All weights = 0, except for one weight = 1 à Effective
sample size = 1

n  Idea: resample only when effective sampling size is low

Resampling Solution I (ctd)
 Resampling Solution II: Low Variance
Sampling
n  M = number of particles

n  r 2 [0, 1/M]

n  Advantages:
n  More systematic coverage of space of samples
n  If all samples have same importance weight, no samples are lost
n  Lower computational complexity
Resampling Solution III
n  Loss of diversity caused by resampling from a discrete
distribution

n  Solution: “regularization”

n  Consider the particles to represent a continuous density
n  Sample from the continuous density
n  E.g., given (1-D) particles
sample from the density:
 Particle Deprivation
n  = when there are no particles in the vicinity of the correct state

n  Occurs as the result of the variance in random sampling. An unlucky

series of random numbers can wipe out all particles near the true state.
This has non-zero probability to happen at each time à will happen
eventually.

n  Popular solution: add a small number of randomly generated particles

when resampling.
n  Advantages: reduces particle deprivation, simplicity.
n  Con: incorrect posterior estimate even in the limit of infinitely many
particles.
n  Other benefit: initialization at time 0 might not have gotten anything near the true state, and not
even near a state that over time could have evolved to be close to true state now; adding random
samples will cut out particles that were not very consistent with past evidence anyway, and instead
gives a new chance at getting close the true state.
Particle Deprivation:
How Many Particles to Add?
n  Simplest: Fixed number.
n  Better way:
n  Monitor the probability of sensor measurements

which can be approximated by:

n  Average estimate over multiple time-steps and compare

to typical values when having reasonable state estimates.
If low, inject random particles.
Noise-free Sensors
n  Consider a measurement obtained with a noise-free sensor,
e.g., a noise-free laser-range finder---issue?
n  All particles would end up with weight zero, as it is very
unlikely to have had a particle matching the measurement
exactly.
n  Solutions:
n  Artificially inflate amount of noise in sensors
n  Better proposal distribution (e.g., optimal sequential
proposal)
Adapting Number of Particles: KLD-Sampling

n  E.g., typically more particles need at the beginning of

localization run
n  Idea:
n  Partition the state-space
n  When sampling, keep track of number of bins occupied
n  Stop sampling when a threshold that depends on the
number of occupied bins is reached
n  If all samples fall in a small number of bins à lower threshold
n  z_{1-±}: the upper 1-±
quantile of the standard
normal distribution

n  ± = 0.01 and ² = 0.05

works well in practice
KLD-sampling
KLD-sampling