![Page 1: Particle Filtering - University of Washington · 2019-04-15 · Particle Filtering TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Sanjiban Choudhury (Slides adapted from Drew Bagnell,](https://reader036.vdocuments.net/reader036/viewer/2022070914/5fb55f0f9f68f058c138a4a2/html5/thumbnails/1.jpg)
1
Particle Filtering
TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle
Sanjiban Choudhury
(Slides adapted from Drew Bagnell, Dieter Fox)
![Page 2: Particle Filtering - University of Washington · 2019-04-15 · Particle Filtering TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Sanjiban Choudhury (Slides adapted from Drew Bagnell,](https://reader036.vdocuments.net/reader036/viewer/2022070914/5fb55f0f9f68f058c138a4a2/html5/thumbnails/2.jpg)
Assembling Bayes filter
2
TasksLocalization
P(pose | data)
Mapping P(map | data)
SLAM P(pose, map | data)
Belief Representations
Probabilistic Models
Bayes Filter
![Page 3: Particle Filtering - University of Washington · 2019-04-15 · Particle Filtering TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Sanjiban Choudhury (Slides adapted from Drew Bagnell,](https://reader036.vdocuments.net/reader036/viewer/2022070914/5fb55f0f9f68f058c138a4a2/html5/thumbnails/3.jpg)
Tasks that we will cover
3
Tasks Belief Representation Probabilistic Models
Localization P(pose | data) Gaussian / Particles Motion model
Measurement model
Mapping P(map | data) Discrete (binary) Inverse measurement model
SLAM P(pose, map |
data)
Particles+Gaussian (pose, landmarks)
Motion model, measurement model, correspondence model
(Week 3)
(Week 4)
(Week 4)
![Page 4: Particle Filtering - University of Washington · 2019-04-15 · Particle Filtering TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Sanjiban Choudhury (Slides adapted from Drew Bagnell,](https://reader036.vdocuments.net/reader036/viewer/2022070914/5fb55f0f9f68f058c138a4a2/html5/thumbnails/4.jpg)
Example: Indoor localization
4
![Page 5: Particle Filtering - University of Washington · 2019-04-15 · Particle Filtering TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Sanjiban Choudhury (Slides adapted from Drew Bagnell,](https://reader036.vdocuments.net/reader036/viewer/2022070914/5fb55f0f9f68f058c138a4a2/html5/thumbnails/5.jpg)
Today’s objective
5
1. Understand the need for non-parametric filtering whenfaced with complex pdf in continuous space.
2. Importance sampling as an effective tool for dealing with complex pdf
![Page 6: Particle Filtering - University of Washington · 2019-04-15 · Particle Filtering TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Sanjiban Choudhury (Slides adapted from Drew Bagnell,](https://reader036.vdocuments.net/reader036/viewer/2022070914/5fb55f0f9f68f058c138a4a2/html5/thumbnails/6.jpg)
Why can’t we just use parametric filters?
6
bel(xt) P (xt+1|zt+1)bel(xt+1)
(Gaussian)(Gaussian)(Gaussian)
bel(xt+1)
bel(xt) = ⌘P (zt|xt)
ZP (xt|xt�1, ut) bel(xt�1)dxt�1
(Gaussian)
Everything is a Gaussian - prior, motion, observation, posterior!
![Page 7: Particle Filtering - University of Washington · 2019-04-15 · Particle Filtering TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Sanjiban Choudhury (Slides adapted from Drew Bagnell,](https://reader036.vdocuments.net/reader036/viewer/2022070914/5fb55f0f9f68f058c138a4a2/html5/thumbnails/7.jpg)
Good things about parametric filters
7
1. They are exact (when correct model)
E.g. Kalman Filter
2. They are efficient to compute
E.g. Sparse matrix inversion
We have so far been thinking about parametric filter (Kalman)
![Page 8: Particle Filtering - University of Washington · 2019-04-15 · Particle Filtering TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Sanjiban Choudhury (Slides adapted from Drew Bagnell,](https://reader036.vdocuments.net/reader036/viewer/2022070914/5fb55f0f9f68f058c138a4a2/html5/thumbnails/8.jpg)
Problems with parametric filters
8
1. Posterior has to have a fixed functional form (e.g. Gaussian)
- even if our prior was a Gaussian, if control/measurementmodel is non-linear, posterior is NOT a Gaussian
2. We can always approximate with parametric belief (e.g. EKF)
- what if true posterior was multi-modal? danger of losinga mode completely
How can we realize Bayes filters in a non-parametric fashion?
![Page 9: Particle Filtering - University of Washington · 2019-04-15 · Particle Filtering TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Sanjiban Choudhury (Slides adapted from Drew Bagnell,](https://reader036.vdocuments.net/reader036/viewer/2022070914/5fb55f0f9f68f058c138a4a2/html5/thumbnails/9.jpg)
Tracking a landing pad with laser only
9(Arora et al.)
![Page 10: Particle Filtering - University of Washington · 2019-04-15 · Particle Filtering TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Sanjiban Choudhury (Slides adapted from Drew Bagnell,](https://reader036.vdocuments.net/reader036/viewer/2022070914/5fb55f0f9f68f058c138a4a2/html5/thumbnails/10.jpg)
10
Question: What are our options for non-parametric
belief representations?
1. Histogram filter
2. Normalized importance sampling
3. Particle filter
![Page 11: Particle Filtering - University of Washington · 2019-04-15 · Particle Filtering TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Sanjiban Choudhury (Slides adapted from Drew Bagnell,](https://reader036.vdocuments.net/reader036/viewer/2022070914/5fb55f0f9f68f058c138a4a2/html5/thumbnails/11.jpg)
Approach 1: Histogram filter
11
Simplest approach - discretize the space!
Prior Posteriorbel(xt) bel(xt+1)
![Page 12: Particle Filtering - University of Washington · 2019-04-15 · Particle Filtering TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Sanjiban Choudhury (Slides adapted from Drew Bagnell,](https://reader036.vdocuments.net/reader036/viewer/2022070914/5fb55f0f9f68f058c138a4a2/html5/thumbnails/12.jpg)
Example: Grid-based localization
X
![Page 13: Particle Filtering - University of Washington · 2019-04-15 · Particle Filtering TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Sanjiban Choudhury (Slides adapted from Drew Bagnell,](https://reader036.vdocuments.net/reader036/viewer/2022070914/5fb55f0f9f68f058c138a4a2/html5/thumbnails/13.jpg)
Issues with grid-based localization
X
1. Curse of dimensionality
2. Wasted computational effort
3. Wasted memory resources
Remedy: Adaptive discretization
Remedy: Pre-cache measurements from cell centers
Remedy: Update a select number of cells only
![Page 14: Particle Filtering - University of Washington · 2019-04-15 · Particle Filtering TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Sanjiban Choudhury (Slides adapted from Drew Bagnell,](https://reader036.vdocuments.net/reader036/viewer/2022070914/5fb55f0f9f68f058c138a4a2/html5/thumbnails/14.jpg)
12
If discretization is expensive, can we sample?
![Page 15: Particle Filtering - University of Washington · 2019-04-15 · Particle Filtering TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Sanjiban Choudhury (Slides adapted from Drew Bagnell,](https://reader036.vdocuments.net/reader036/viewer/2022070914/5fb55f0f9f68f058c138a4a2/html5/thumbnails/15.jpg)
Monte-Carlo method
13
Q: What do we intend to do with the belief ? bel(xt+1)
Ans: Often times we will be evaluating the expected value
E[f ] =Z
x
f(x)bel(x)dx
Mean position:
Probability of collision:
Mean value / cost-to-go:
f(x) ⌘ x
f(x) ⌘ I(x 2 O)
f(x) ⌘ V (x)
![Page 16: Particle Filtering - University of Washington · 2019-04-15 · Particle Filtering TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Sanjiban Choudhury (Slides adapted from Drew Bagnell,](https://reader036.vdocuments.net/reader036/viewer/2022070914/5fb55f0f9f68f058c138a4a2/html5/thumbnails/16.jpg)
Monte-Carlo method
14
E[f ] =Z
x
f(x)bel(x)dx
Problem: Can’t evaluate the integral below since we don’t know bel
E[f ] ⇡ 1
N
NX
i
f(xi)
Solution: Sample from the distribution
(originated in Los Alamos)
+ Incremental, any-time. + Converges to the true expectation under a mild set of assumptions
Lots of general applications!
x1, . . . , xN ⇠ bel(x)
![Page 17: Particle Filtering - University of Washington · 2019-04-15 · Particle Filtering TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Sanjiban Choudhury (Slides adapted from Drew Bagnell,](https://reader036.vdocuments.net/reader036/viewer/2022070914/5fb55f0f9f68f058c138a4a2/html5/thumbnails/17.jpg)
Can we always sample?
15
bel(xt) = ⌘P (zt|xt)
Zp(xt|ut, xt�1)bel(xt�1)dxt�1
How can we sample from the product of two distributions?
![Page 18: Particle Filtering - University of Washington · 2019-04-15 · Particle Filtering TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Sanjiban Choudhury (Slides adapted from Drew Bagnell,](https://reader036.vdocuments.net/reader036/viewer/2022070914/5fb55f0f9f68f058c138a4a2/html5/thumbnails/18.jpg)
16
Question: How can we sample from a complex distribution p(x)?
![Page 19: Particle Filtering - University of Washington · 2019-04-15 · Particle Filtering TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Sanjiban Choudhury (Slides adapted from Drew Bagnell,](https://reader036.vdocuments.net/reader036/viewer/2022070914/5fb55f0f9f68f058c138a4a2/html5/thumbnails/19.jpg)
Solution: Importance sampling
17
Trick: 1. Sample from a proposal distribution (easy), 2. Reweigh samples to fix it!
![Page 20: Particle Filtering - University of Washington · 2019-04-15 · Particle Filtering TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Sanjiban Choudhury (Slides adapted from Drew Bagnell,](https://reader036.vdocuments.net/reader036/viewer/2022070914/5fb55f0f9f68f058c138a4a2/html5/thumbnails/20.jpg)
Solution: Importance sampling
18
Don’t know how to generate these samples!!
p(x)
![Page 21: Particle Filtering - University of Washington · 2019-04-15 · Particle Filtering TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Sanjiban Choudhury (Slides adapted from Drew Bagnell,](https://reader036.vdocuments.net/reader036/viewer/2022070914/5fb55f0f9f68f058c138a4a2/html5/thumbnails/21.jpg)
Solution: Importance sampling
19
Trick: 1. Sample from a proposal distribution (easy),
Proposal distribution
(easy to sample)
p(x) q(x)
![Page 22: Particle Filtering - University of Washington · 2019-04-15 · Particle Filtering TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Sanjiban Choudhury (Slides adapted from Drew Bagnell,](https://reader036.vdocuments.net/reader036/viewer/2022070914/5fb55f0f9f68f058c138a4a2/html5/thumbnails/22.jpg)
Solution: Importance sampling
20
Trick: 1. Sample from a proposal distribution (easy), 2. Reweigh samples to fix it!
p(x) q(x)
![Page 23: Particle Filtering - University of Washington · 2019-04-15 · Particle Filtering TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Sanjiban Choudhury (Slides adapted from Drew Bagnell,](https://reader036.vdocuments.net/reader036/viewer/2022070914/5fb55f0f9f68f058c138a4a2/html5/thumbnails/23.jpg)
Solution: Importance sampling
21
Trick: Sample from a proposal distribution (easy), reweigh samples to fix it!
E
p(x)[f(x)] =
Xp(x)f(x) (1a)
=X
p(x)f(x)q(x)
q(x)(1b)
=X
q(x)p(x)
q(x)f(x) (1c)
= E
q(x)[p(x)
q(x)f(x)] (1d)
⇡ 1
N
NX
i=1
p(xi
)
q(xi
)f(x
i
). (1e)
p(x)q(x) is called the importance weight. When we are undersampling an area, it weights it stronger;likewise, oversampled areas are weighted more weakly.
Consider the following (somewhat contrived) pair of functions:
Figure 2: Comparison of distribution we wish to sample from, p, and distribution we can samplefrom, q.
Notice that there are places where p is nonzero and q is zero, which means we have regions tosample that we aren’t sampling at all.
Our estimate of the expectation of p becomes:
1
N
NX
i=1
p(xi
)
q(xi
)f(x
i
) !N!1 E
p
[f(x)] (2)
2.1 Example
Set q as the motion model. Note that in reality we never use this – we use a better approximationof p(x
t
|xt�1) because the motion model takes too many samples to be reliable.
2
For convergence, make sure support of target is a subset of proposal
![Page 24: Particle Filtering - University of Washington · 2019-04-15 · Particle Filtering TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Sanjiban Choudhury (Slides adapted from Drew Bagnell,](https://reader036.vdocuments.net/reader036/viewer/2022070914/5fb55f0f9f68f058c138a4a2/html5/thumbnails/24.jpg)
Question: What makes a good proposal distribution?
22
p(x) q(x)
![Page 25: Particle Filtering - University of Washington · 2019-04-15 · Particle Filtering TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Sanjiban Choudhury (Slides adapted from Drew Bagnell,](https://reader036.vdocuments.net/reader036/viewer/2022070914/5fb55f0f9f68f058c138a4a2/html5/thumbnails/25.jpg)
23
bel(xt) = ⌘P (zt|xt)
Zp(xt|ut, xt�1)bel(xt�1)dxt�1
Target distribution : Posterior
Importance Ratio:
r =bel(xt)
bel(xt)= ⌘P (zt|xt)
Proposal distribution : After applying motion model
bel(xt) =
Zp(xt|ut, xt�1)bel(xt�1)dxt�1
Why is this easy to sample from?
Applying importance sampling to Bayes filtering
![Page 26: Particle Filtering - University of Washington · 2019-04-15 · Particle Filtering TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Sanjiban Choudhury (Slides adapted from Drew Bagnell,](https://reader036.vdocuments.net/reader036/viewer/2022070914/5fb55f0f9f68f058c138a4a2/html5/thumbnails/26.jpg)
24
Question: What are our options for non-parametric
belief representations?
1. Histogram filter
2. Normalized importance sampling
3. Particle filter
![Page 27: Particle Filtering - University of Washington · 2019-04-15 · Particle Filtering TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Sanjiban Choudhury (Slides adapted from Drew Bagnell,](https://reader036.vdocuments.net/reader036/viewer/2022070914/5fb55f0f9f68f058c138a4a2/html5/thumbnails/27.jpg)
Approach 2: Normalized Importance Sampling
25
bel(xt�1) =
⇢x
1t�1
w
1t�1
,
x
2t�1
w
2t�1
, · · · , xMt�1
w
Mt�1
�0.5
0.250.25
for i = 1 to M
sample x
it ⇠ P (xt|ut, x
it)
0.5
0.25
0.25
0.5 * 0.02
0.25 * 0.1
0.25 * 0.05
for i = 1 to M
w
it = P (zt|x̄i
t)wit�1
=0.01
=0.025
=0.0125
for i = 1 to M
wit =
witP
i wit
0.21
0.53
0.26bel(xt) =
⇢x̄
1t
w
1t, · · · , x̄
Mt
w
Mt
�