Particle Filters Importance Sampling Unfortunately it is often not possible to sample directly from the posterior distribution, but we can use importance.

Particle Particle FiltersFilters

Importance Importance SamplingSampling

Importance SamplingImportance 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

)(

)(i

ii

xq

xpw

)()(1

in

i

i xxwxp

•where

Bayesian Importance SamplingBayesian Importance Sampling

• By drawing samples from a known easy to sample proposal distribution we obtain:

N

i

ikk

ikkk xxwDxp

1:0:0:0 )()|(

)|( :0 kk Dxq

ikx :0

)|(

)|(

:0

:0

ki

k

ki

kik Dxq

Dxpw

•where

•are normalized weights.

Sensor Information: Importance Sampling

Sequential Importance Sampling (I)Sequential Importance Sampling (I)• Factorizing the proposal distribution:

• and remembering that the state evolution is modeled as a Markov process

• we obtain a recursive estimate of the importance weights:

• Factorizing is obtained by recursively applying

k

jjjjkk DxxqxqDxq

11:00:0 ),|()()|(

),|(

)|()|(

1:0

11

kkk

kkkkkk Dxxq

xxpxypww

)|(),|()|( 11:01:0:0 kkkkkkk DxqDxxqDxq

•Sequential Importance Sampling (SIS) Particle Filter

•SIS Particle Filter Algorithm

],},[{]},[{ 1111 kNi

ik

ik

Ni

ik

ik zwxSISwx

•for i=1:N

•Draw a particle

•Assign a weight

•end

),|(~ 1 kik

ik

ik zxxqx

),|(

)|()|(

1:0

11

ki

kik

ik

ik

ikki

kik Dxxq

xxpxzpww

•(k is index over time and i is the particle index)

Rejection Rejection SamplingSampling

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 keep the sampleotherwise reject the sample

Rejection SamplingRejection Sampling

•c

•x

•f(x)

•c’

•x’

•f(x’)

•OK

Importance Sampling with Importance Sampling with Resampling:Resampling:

Landmark Detection ExampleLandmark 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 After ResamplingResampling

Particle Particle Filter Filter

AlgorithmAlgorithm

weight =

target distribution / proposal distribution

•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

xzp

xBeluxxp

xBeluxxpxzp

w

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

Particle Filter Algorithm

1. Algorithm particle_filter( St-1, ut-1 zt):

2.

3. For Generate new samples

4. Sample index j(i) from the discrete distribution given by wt-

1

5. Sample from using and

6. Compute importance weight

7. Update normalization factor

8. Insert

9. For

10. Normalize weights

Particle Filter Algorithm

0, tS

ni 1

},{ it

ittt wxSS

itw

itx ),|( 11 ttt uxxp )(

1ij

tx 1tu

)|( itt

it xzpw

ni 1/i

tit ww

Particle Filter Algorithm

Particle Filter for LocalizationParticle Filter for Localization

Particle Filter in Particle Filter in Matlab Matlab

•Matlab code: truex is a vector of 100 positions to be tracked.Matlab code: truex is a vector of 100 positions to be tracked.

Application: Particle Filter for Localization Application: Particle Filter for Localization (Known Map)(Known Map)

ResamplingResampling

ResamplingResampling

ResamplingResampling

1. Algorithm systematic_resampling(S,n):

2.

3. For Generate cdf4. 5. Initialize threshold

6. For Draw samples …7. While ( ) Skip until next threshold reached8. 9. Insert10. Increment threshold

11. Return S’

Resampling Algorithm

11,' wcS

ni 2i

ii wcc 1

1],,0]~ 11 inUu

nj 1

11

nuu jj

ij cu

1,'' nxSS i

1ii

•Also called stochastic universal sampling

Low Variance Resampling

SIS SIS weightsweights

Derivation of SIS weights (I)Derivation of SIS weights (I)• The main idea is Factorizing :

k

jjjk xxpxpxp

110:0 )|()()( •and

k

jjjkk xypxDp

1:0 )|()|(

)|(

)|(

:0

:0

ki

k

ki

kik Dxq

Dxpw

)|(),|()|( 11:01:0:0 kkkkkkk DxqDxxqDxq

•Our goal is to expand p and q in time t

Derivation of SIS weights (II)

)()|(

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

)(

)()|()|(

11

1:01:0:01:01:0:0:0

kkk

kkkkkkkk

k

kkkkk DpDzp

xpxxpxDpxDzp

Dp

xpxDpDxp

)|(

)|()|(),|(

1

11:01:0:01

kk

kkkkkkk

Dzp

DxpxxpxDzp

)()|(

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

11

1:01:011:0:01

kkk

kkkkkkkk

DpDzp

xpxDpxxpxDzp

)|()|(),|( 11:01:0:01 kkkkkkk DxpxxpxDzp

)|(

)|(

),|(

)|(),|(

)|(

)|(

11:0

11:0

1:0

1:0:01

:0

:0

k

ik

ki

k

ki

kik

ik

ik

ikkk

ki

k

ki

kik Dxq

Dxp

Dxxq

xxpxDzp

Dxq

Dxpw

•Derivation of SIS weights (II)

),|(

)|(),|(

1:0

1:0:011

ki

kik

ik

ik

ikkki

k Dxxq

xxpxDzpw

),|(

)|()|(

1

11

kik

ik

ik

ik

ikki

kik zxxq

xxpxzpww

•and under Markov assumptions

SIS Particle Filter Foundation• At each time step k• Random samples are drawn from the proposal distribution for i=1, …, N

• They represent posterior distribution using a set of samples or particles

• Since the weights are given by

• and q factorizes as

N

i

ikk

ikkk xxwDxp

1

)()|(

ikx

),|( 1:0 kkk Dxxq

)|(

)|(

:0

:0

ki

k

ki

kik Dxq

Dxpw

k

jjjjkk DxxqxqDxq

11:00:0 ),|()()|(

Sequential Importance Sampling (II)

• Choice of the proposal distribution:

• Choose proposal function to minimize variance of (Doucet et al. 1999):

• Although common choice is the prior distribution:

We obtain then

),|( 1:0 kkk Dxxq

kw

),|(),|( 1:01:0 kkkkkk DxxpDxxq

)|(),|( 11:0 kkkkk xxpDxxq

)|(),|(

)|()|(1

1:0

11

ikk

ik

ki

kik

ik

ik

ikki

kik xzpw

Dxxq

xxpxzpww

• Illustration of SIS:

• Degeneracy problems:– variance of importance ratios

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

w

Time 19

w

Time 10

w

Time 1

)|(/)|( :0:0 kkkk DxqDxp

Sequential Importance Sampling (IV)

• Illustration of degeneracy:

w

Time 19

w

Time 10

w

Time 1

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

• So we expect the variance to be close to 0 to obtain reasonable estimates– thus a variance increase has a harmful effect on accuracy

1)|(

)|(

:0

:0

kk

kkq Dxq

DxpE

01)|(

)|(

)|(

)|(var

2

:0

:0

:0

:0

kk

kkq

kk

kkq Dxq

DxpE

Dxq

Dxp

SamplingSampling--

Importance Importance ResamplingResampling

Sampling-Importance ResamplingSampling-Importance Resampling• SIS suffers from degeneracy problems so we don’t

want to do that!• Introduce a selection (resampling) step to eliminate

samples with low importance ratios and multiply samples with high importance ratios.

• Resampling maps the weighted random measure on to the equally weighted random measure – by sampling uniformly with replacement from

with probabilities

• Scheme generates children such that and satisfies:

)}(~,{ :0:0i

kki

k xwx

} { 1-:0 Nx jk

},,1;{ :0 Nixik },,1;~{ Niwi

k

NNN

ii

1

iN

iki wNNE ~)(

)~1(~)var( ik

iki wwNN

Basic SIR Particle Filter - Schematic•Initialisation

•Importance•sampling step

•Resampling•step

0k

1 kk

)}(~,{ :0:0i

kki

k xwx

},{ 1:0

Nxik

•measurement

ky

•Extract estimate, kx :0ˆ

Basic SIR Particle Filter algorithm (I)• Initialisation

– – For sample – and set

0k

Ni ,,1 )(~ 00 xpxi

1k

Importance Sampling step

– For sample

– For compute the importance

weights wik

– Normalise the importance weights,

N

j

jk

ik

ik www

1

/~

Ni ,,1 )|(~~1

ikk

ik xxqx

),(~1:0:0

ik

ik

ik xxx •and set

Ni ,,1

Basic SIR Particle Filter algorithm (II)

• Resampling step– Resample with replacement particles:

– from the set:

– according to the normalised importance weights,

• Set– proceed to the Importance Sampling step, as the next

measurement arrives.

N

),,1;( :0 Nixik

),,1;~( :0 Nix ik

ikw~

1 kk

Resampling

M

m

mk M

x1

)(1

1,

•x

M

mm

km

k wx 1)()( ,

M

m

m

kM

x1

)(~ 1,

M

m

mk M

x1

)(1

1,

M

mm

km

k wx 1)(1

)(1 ,

M

m

m

kM

x1

)(

1

~ 1,

M

m

mk M

x1

)(2

1,

•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)Improvements to SIR (I)• 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.

)var( iN

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 19

w

Time 10

w

Time 1

Ingredients for SMCIngredients for SMC• Importance sampling function

– Gordon et al – Optimal – UKF pdf from UKF at

• Redistribution scheme– Gordon et al SIR– Liu & Chen Residual– Carpenter et al Systematic– Liu & Chen, Doucet et al Resample when necessary

• Careful initialisation procedure (for efficiency)

)|( 1ikk xxp

),|( 1:0 ki

kk Dxxp ikx 1

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

•51

• Giorgio Grisetti• Maren Bennewitz• Christian Plagemann • Dirk Haehnel• Mike Montemerlo• Nick Roy• Kai Arras• Patrick Pfaff• Miodrag Bolic• Haris Baltzakis