Particle
Filters
Importance
Sampling
Importance Sampling
•
Unfortunately it is often not possible to sample directly from the posterior
distribution, but we can use importance sampling.
•
Let p(x) be a pdf from which it is difficult to draw samples.
•
Let xi ~ q(x), i=1, …, N, be samples that are easily generated from a proposal pdf
q, which is called an importance density.
Then approximation to the density p is given by
•
n
p ( x )   wi  ( x  x i )
i 1
•where
i
p( x )
w 
i
q( x )
i
Bayesian Importance Sampling
•
i
By drawing samples x0:k from a known easy to sample proposal
q( x0:k | Dk )we obtain:
distribution
N

p( x0:k | Dk )   wki  ( x0:k  x0i :k )
i 1
•where
i
p
(
x
i
0:k | Dk )
wk 
q( x0i :k | Dk )
•are normalized weights.
Sensor Information: Importance Sampling
Sequential Importance Sampling (I)
•
Factorizing the proposal distribution:
k
q( x0:k | Dk )  q( x0 )  q( x j | x0: j 1, D j )
j 1
•
and remembering that the state evolution is modeled as a Markov
process
•
we obtain a recursive estimate of the importance weights:
wk  wk 1
•
p( yk | xk ) p( xk | xk 1 )
q( xk | x0:k 1 , Dk )
Factorizing is obtained by recursively applying
q( x0:k | Dk )  q( xk | x0:k 1, Dk )q( x0:k 1 | Dk 1 )
•Sequential Importance Sampling (SIS)
Particle Filter
•SIS Particle Filter Algorithm
[{xki , wki }iN1 ]  SIS [{xki 1 , wki 1}iN1 , zk ]
•for i=1:N
•Draw a particle
xki ~ q( xki | xki 1 , zk )
i
i
•Assign a weight wk  wk 1
p( zk | xki ) p( xki | xki 1 )
q( xki | x0i :k 1 , Dk )
•(k is index over time and i is the particle index)
•end
Rejection
Sampling
Rejection Sampling




Let us assume that f(x)<1 for all x
Sample x from a uniform distribution
Sample c from [0,1]
if f(x) > c
otherwise
keep the sample
reject the sample
•f(x’)
•c’
•c
•f(x)
•x
•OK
•x
’
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
•After
Resampling
Particle Filter
Algorithm
weight =
target distribution / proposal distribution
Particle Filter Algorithm
Bel ( xt )   p( zt | xt )  p( xt | xt 1 , ut 1 ) Bel ( xt 1 ) dxt 1
•draw xit1 from Bel(xt1)
•draw xit from p(xt | xit1,ut1)
•Importance factor for xit:
target distributi on
proposal distributi on
 p( zt | xt ) p( xt | xt 1 , ut 1 ) Bel ( xt 1 )

p( xt | xt 1 , ut 1 ) Bel ( xt 1 )
wti 
 p( zt | xt )
Particle Filter Algorithm
1. Algorithm particle_filter( St-1, ut-1 zt):
2.
3. For
St  ,
 0
i  1n
Generate new samples
4.
Sample index j(i) from the discrete distribution given by wt-1
5.
Sample xti from p( xt | xt 1 , ut 1 ) using xtj(1i ) and ut 1
6.
wti  p( zt | xti )
Compute importance weight
7.
    wti
Update normalization factor
8.
St  St  { xti , wti }
Insert
9. For
i  1n
wti  wti / 
Normalize weights
10.
Particle Filter
Algorithm
Particle Filter for Localization
Particle Filter in
Matlab
•Matlab code: truex is a vector of 100 positions to be tracked.
Application: Particle Filter for Localization
(Known Map)
Resampling
Resampling
Resampling
Resampling Algorithm
1. Algorithm systematic_resampling(S,n):
2. S '  , c1  w1
3. For i  2n
Generate cdf
ci  ci 1  w
4.
5. u1 ~ U ]0, n 1 ], i  1
i
Initialize threshold
6. For j  1 n
7.
8.
9.
10.
While ( u j  ci )
i  i 1

Draw samples …
Skip until next threshold reached

S '  S '  x i , n 1 
u j 1  u j  n1
Insert
Increment threshold
11. Return S’
•Also called stochastic universal sampling
Low Variance Resampling
SIS
weights
Derivation of SIS weights (I)
•
The main idea is Factorizing :
k
k
j 1
j 1
p( x0:k )  p( x0 )  p( x j | x j 1) •and p( Dk | x0:k )   p( y j | x j )
•Our goal is to expand p and q in time t
i
p
(
x
0:k | Dk )
wki 
q( x0i :k | Dk )
q( x0:k | Dk )  q( xk | x0:k 1 , Dk )q( x0:k 1 | Dk 1 )
Derivation of SIS weights (II)
p( x0:k | Dk ) 
p( Dk | x0:k ) p( x0:k ) p( zk | Dk 1 , x0:k ) p( Dk 1 | x0:k ) p( xk | x0:k 1 ) p( x0:k 1 )

p( Dk )
p( zk | Dk 1 ) p( Dk 1 )
p( zk | Dk 1 , x0:k ) p( xk | x0:k 1 ) p( Dk 1 | x0:k 1 ) p( x0:k 1 )

p( zk | Dk 1 ) p( Dk 1 )

p( zk | Dk 1 , x0:k ) p( xk | x0:k 1 ) p( x0:k 1 | Dk 1 )
p( zk | Dk 1 )
 p( zk | Dk 1 , x0:k ) p( xk | x0:k 1 ) p( x0:k 1 | Dk 1 )
•Derivation of SIS weights (II)
i
i
i
i
i
p
(
x
|
D
)
p
(
z
|
D
,
x
)
p
(
x
|
x
)
p
(
x
i
0:k
k
k
k 1 0:k
k
0:k 1
0:k 1 | Dk 1 )
wk 

i
q( x0:k | Dk )
q( xki | x0i :k 1 , Dk )
q( x0i :k 1 | Dk 1 )
 wki 1
p( zk | Dk 1 , x0i :k ) p( xki | x0i :k 1 )
q( xki | x0i :k 1 , Dk )
•and under Markov assumptions
wki  wki 1
p( zk | xki ) p( xki | xki 1 )
q( xki | xki 1 , zk )
SIS Particle Filter Foundation
•
•
At each time step k i
Random samples x k are drawn from the proposal distribution for i=1, …,
N
q( xk | x0:k 1 , Dk )
•
They represent posterior distribution using a set of samples or particles
N

p( xk | Dk )   wki  ( xk  xki )
i 1
•
Since the weights are given by
•
and q factorizes as
k
i
p
(
x
i
0:k | Dk )
wk 
q( x0i :k | Dk )
q( x0:k | Dk )  q( x0 )  q( x j | x0: j 1, D j )
j 1
Sequential Importance
Sampling (II)
•
Choice of the proposal distribution:
q( xk | x0:k 1, Dk )
•
Choose proposal function to minimize variance of
1999):
wk (Doucet et al.
q( xk | x0:k 1, Dk )  p( xk | x0:k 1, Dk )
•
Although common choice is the prior distribution:
q( xk | x0:k 1, Dk )  p( xk | xk 1)
We obtain then
wki  wki 1
p( zk | xki ) p( xki | xki 1 )
i
i

w
p
(
z
|
x
k 1
k
k)
i
i
q( xk | x0:k 1 , Dk )
Sequential Importance Sampling (III)
Time 1
w
• Illustration of SIS:
w
Time 10
w
Time 19
• Degeneracy problems:
– variance of importance ratios
p( x0:k | Dk ) / q( x0:k | Dk )
increases stochastically over time (Kong et al. 1994; Doucet
et al. 1999).
– In most cases then after a few iterations, all but one
particle will have negligible weight
Sequential Importance Sampling (IV)
Time 1
w
• Illustration of degeneracy:
w
Time 10
w
Time 19
SIS - why variance increase
• Suppose we want to sample from the posterior
– choose a proposal density to be very close to the posterior
density
• Then
• and
 p( x | D ) 
Eq  0:k k   1
 q( x0:k | Dk ) 
2

 p( x | D ) 
 p( x | D ) 
varq  0:k k   Eq   0:k k  1   0
  q( x0:k | Dk )  
 q( x0:k | Dk ) 


• So we expect the variance to be close to 0 to obtain
reasonable estimates
– thus a variance increase has a harmful effect on accuracy
Sampling
Importance
Resampling
Sampling-Importance Resampling
• SIS suffers from degeneracy problems so we
don’t want to do that!
• Introduce a selection (resampling) step to
~ ( xi )}
{x0i :k , w
k 0:k
eliminate samples with low importance ratios
and multiply samples with high importance
ratios.
{x0j:k N -1}
• Resampling maps the weighted random
i
{
x
i
~
0:k ; i  1,, N }
measure {wk ; i  1on
,,to
N }the equally weighted
random measure
– by sampling uniformly with replacement from Ni
with probabilities
~i
E ( N i )  Nw
k
• Scheme generates
children such that
and satisfies: var(N )  Nw~i (1  w~i )
i
k
k
N
 Ni  N
i 1
Basic SIR Particle Filter - Schematic
•Initialisation
k 0
•measurement
k  k 1
yk
{x0i :k , N 1}
•Resampling
•step
•Importance
•sampling step
~ ( xi )}
{x0i :k , w
k 0:k
•Extract estimate,xˆ 0:k
•
Basic SIR Particle Filter
algorithm
(I)
Initialisation
k 0
–
i  1,, N
– For
k 1
– and set
x0i ~ p( x0 )
sample
 Importance Sampling step
– Fori  1,, N
sample
~x i ~ q( x | x i )
k
k
k 1
i
i
i
~
x

(
x
,
x
•and set 0:k
0:k 1 k )
– Fori  1,, N
compute the importance weights
wik
N
~ i  wi / w j
w
k
k  k
– Normalise the importance weights,
j 1
Basic SIR Particle Filter algorithm
(II)
• Resampling step
–
N
i
(
x
 1,replacement
, N)
0:k ; iwith
Resample
particles:
( ~x0i:k ; i  1,, N )
– from the set:
~i
w
k
– according
k  to
k the
1 normalised importance weights,
• Set
– proceed to the Importance Sampling step, as the
next measurement arrives.
Resampling
M
M
 ~ ( m) 1 
 x k 1 , 
M m 1

 ( m) 1 
 xk  2 , 
M m1

x
(m)
k 1
, wk( m1)

M
m 1
M
M
 ~ ( m) 1 
xk , 
M m 1

 ( m) 1 
 xk 1 , 
M m1

x
(m)
k
, wk( m )

M
m 1
M
 ( m) 1 
 xk 1 , 
M m1

•x
•Generic SIR Particle Filter
algorithm
•M. S. Arulampalam, S.
Maskell,
N. Gordon, and T. Clapp,
“A tutorial on particle filters
…,”
IEEE Trans. on Signal
Processing,
50( 2), 2002.
Improvements to SIR (I)
var(Ni )
• Variety of resampling schemes with varying
performance in terms of the variance of the particles
:
– Residual sampling (Liu & Chen, 1998).
– Systematic sampling (Carpenter et al., 1999).
– Mixture of SIS and SIR, only resample when necessary (Liu &
Chen, 1995; Doucet et al., 1999).
• Degeneracy may still be a problem:
– During resampling a sample with high importance weight may
be duplicated many times.
– Samples may eventually collapse to a single point.
Improvements to SIR (II)
• To alleviate numerical degeneracy problems,
sample smoothing methods may be adopted.
– Roughening (Gordon et al., 1993).
• Adds an independent jitter to the resampled particles
– Prior boosting (Gordon et al., 1993).
• Increase the number of samples from the proposal
distribution to M>N,
• but in the resampling stage only draw N particles.
Improvements to SIR (III)
• Local Monte Carlo methods for alleviating
degeneracy:
– Local linearisation - using an EKF (Doucet, 1999; Pitt &
Shephard, 1999) or UKF (Doucet et al, 2000) to estimate the
importance distribution.
– Rejection methods (Müller, 1991; Doucet, 1999; Pitt & Shephard, 1999).
– Auxiliary particle filters (Pitt & Shephard, 1999)
– Kernel smoothing (Gordon, 1994; Hürzeler & Künsch, 1998; Liu & West,
2000; Musso et al., 2000).
– MCMC methods (Müller, 1992; Gordon & Whitby, 1995; Berzuini et al.,
1997; Gilks & Berzuini, 1998; Andrieu et al., 1999).
Improvements to SIR (IV)
• Illustration of SIR with sample smoothing:
w
Time 1
w
Time 10
w
Time 19
Ingredients for SMC
• Importance sampling function
– Gordon et al
– Optimal
– UKF



p( xk | xki 1 )
p ( xk | x0i :k 1, Dk )
pdf from UKF at xki 1
• Redistribution scheme
–
–
–
–
Gordon et al

Liu & Chen

Carpenter et al

Liu & Chen, Doucet et al
necessary
SIR
Residual
Systematic

Resample when
• Careful initialisation procedure (for efficiency)
Sources
•
•
•
•
•
•
•
•
•
•
•
•
•
•
Longin Jan Latecki
Keith Copsey
Paul E. Rybski
Cyrill Stachniss
Sebastian Thrun
Alex Teichman
Michael Pfeiffer
J. Hightower
L. Liao
D. Schulz
G. Borriello
Honggang Zhang
Wolfram Burgard
Dieter Fox
•
•
•
•
•
•
•
•
•
•
Giorgio Grisetti
Maren Bennewitz
Christian Plagemann
Dirk Haehnel
Mike Montemerlo
Nick Roy
Kai Arras
Patrick Pfaff
Miodrag Bolic
Haris Baltzakis
•51