from bayesian filtering to particle filters dieter fox university of washington joint work with w....
TRANSCRIPT
![Page 1: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/1.jpg)
From Bayesian Filtering to Particle Filters
Dieter FoxUniversity of Washington
Joint work withW. Burgard, F. Dellaert, C. Kwok, S.
Thrun
![Page 2: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/2.jpg)
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:
),,,|()( 121 tttt zuzuxPxBel
},,,{ 121 ttt zuzud
![Page 3: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/3.jpg)
Graphical Model Representation
Underlying Assumptions• Static world• Independent noise• Perfect model, no approximation errors
),|(),,|( 1:11:11:1 ttttttt uxxpuzxxp )|(),,|( :11:1:0 tttttt xzpuzxzp
![Page 4: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/4.jpg)
Bayes Filters
),,,|(),,,,|( 121121 ttttt uzuxPuzuxzP Bayes
z = observationu = actionx = state
),,,|()( 121 tttt zuzuxPxBel
Markov ),,,|()|( 121 tttt uzuxPxzP
1111 )(),|()|( ttttttt dxxBelxuxPxzP
Markov1121111 ),,,|(),|()|( tttttttt dxuzuxPxuxPxzP
11211
1121
),,,|(
),,,,|()|(
ttt
ttttt
dxuzuxP
xuzuxPxzP
Total prob.
![Page 5: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/5.jpg)
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)
111 )(),|()|()( tttttttt dxxBelxuxPxzPxBel
![Page 6: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/6.jpg)
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]
![Page 7: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/7.jpg)
Bayes Filters for Robot Localization
![Page 8: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/8.jpg)
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
![Page 9: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/9.jpg)
Particle Filters
![Page 10: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/10.jpg)
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
![Page 11: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/11.jpg)
Sample-based Representation
![Page 12: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/12.jpg)
Weight samples: w = f / g
Importance Sampling
![Page 13: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/13.jpg)
Importance Sampling with Resampling:Landmark Detection Example
![Page 14: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/14.jpg)
Distributions
![Page 15: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/15.jpg)
Distributions
Wanted: samples distributed according to p(x| z1, z2, z3)
![Page 16: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/16.jpg)
This is Easy!We can draw samples from p(x|zl) by adding noise to the detection parameters.
![Page 17: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/17.jpg)
Importance Sampling with Resampling
),...,,(
)()|(),...,,|( :fon distributiTarget
2121
n
kk
n zzzp
xpxzpzzzxp
)(
)()|()|( :gon distributi Sampling
l
ll zp
xpxzpzxp
),...,,(
)|()(
)|(
),...,,|( : w weightsImportance
21
21
n
lkkl
l
n
zzzp
xzpzp
zxp
zzzxp
g
f
![Page 18: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/18.jpg)
Importance Sampling with Resampling
Weighted samples After resampling
![Page 19: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/19.jpg)
Particle Filters
![Page 20: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/20.jpg)
)|()(
)()|()()|()(
xzpxBel
xBelxzpw
xBelxzpxBel
Sensor Information: Importance Sampling
![Page 21: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/21.jpg)
'd)'()'|()( , xxBelxuxpxBel
Robot Motion
![Page 22: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/22.jpg)
)|()(
)()|()()|()(
xzpxBel
xBelxzpw
xBelxzpxBel
Sensor Information: Importance Sampling
![Page 23: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/23.jpg)
Robot Motion
'd)'()'|()( , xxBelxuxpxBel
![Page 24: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/24.jpg)
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
xzpxBeluxxp
xBeluxxpxzp
w
1111 )(),|()|()( tttttttt dxxBeluxxpxzpxBel
Particle Filter Algorithm
![Page 25: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/25.jpg)
Resampling
• Given: Set S of weighted samples.
• Wanted : Random sample, where the probability of drawing xi is given by wi.
• Typically done n times with replacement to generate new sample set S’.
![Page 26: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/26.jpg)
w2
w3
w1wn
Wn-1
Resampling
w2
w3
w1wn
Wn-1
• Roulette wheel
• Binary search, n log n
• Stochastic universal sampling
• Systematic resampling
• Linear time complexity
• Easy to implement, low variance
![Page 27: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/27.jpg)
Start
Motion Model Example
![Page 28: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/28.jpg)
Proximity Sensor Model Example
Laser sensor Sonar sensor
![Page 29: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/29.jpg)
Sample-based Localization (sonar)
![Page 30: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/30.jpg)
Localization for AIBO robots
![Page 31: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/31.jpg)
How Many Samples?
![Page 32: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/32.jpg)
• 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- ), the KL-distance between the true posterior and the sample-based approximation is less than .
• Observation: • For fixed and , number of samples only depends on
number k of bins with support:
KLD-sampling
3
12
)1(9
2
)1(9
21
2
1)1,1(
2
1
zkk
kkn
[Fox NIPS-01, Fox IJRR-03]
![Page 33: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/33.jpg)
Evaluation
![Page 34: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/34.jpg)
KLD-Sampling: Sonar
[Fox NIPS-01, Fox IJRR-03]
![Page 35: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/35.jpg)
KLD-Sampling: Laser
![Page 36: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/36.jpg)
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!
![Page 37: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/37.jpg)
Rao-Blackwellised Particle Filter
Example
![Page 38: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/38.jpg)
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.
![Page 39: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/39.jpg)
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
![Page 40: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/40.jpg)
Dynamic Bayesian Network for Ball Tracking
k-2b k-1b kb
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 kz k-2
kmk-1mk-2m
Ball observation
Ball location and velocity
Ball motion mode
Map and robot location
Robot control
Landmark detection
Bal
l tra
ckin
gR
obot
loca
liza
tion l l l
b b b
![Page 41: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/41.jpg)
Robot Location
k-2b k-1b kb
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 kz k-2
kmk-1mk-2m
Ball observation
Ball location and velocity
Ball motion mode
Map and robot location
Robot control
Landmark detection
Bal
l tra
ckin
gR
obot
loca
liza
tion l l l
b b b
![Page 42: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/42.jpg)
Robot and Ball Location (and velocity)
k-2b k-1b kb
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 kz k-2
kmk-1mk-2m
Ball observation
Ball location and velocity
Ball motion mode
Map and robot location
Robot control
Landmark detection
Bal
l tra
ckin
gR
obot
loca
liza
tion l l l
b b b
![Page 43: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/43.jpg)
Ball-Environment Interactions
None Grabbed
BouncedKicked
(0.8)
Robot loses grab (0.2)
(1.0)
Ro
bo
t kicks b
all (0.9)
(1.0
)
Within grab range and robot grabs
(prob. from model )
(0.1
)
Kick fails (0.1)
Deflected
(residual prob .) (1
.0)
Col
lisio
n w
ith
obje
cts
on m
ap (1
.0)
![Page 44: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/44.jpg)
Integrating Discrete Ball Motion Mode
k-2b k-1b kb
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 kz k-2
kmk-1mk-2m
Ball observation
Ball location and velocity
Ball motion mode
Map and robot location
Robot control
Landmark detection
Bal
l tra
ckin
gR
obot
loca
liza
tion l l l
b b b
![Page 45: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/45.jpg)
Grab Example (1)
k-2b k-1b kb
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 kz k-2
kmk-1mk-2m
Ball observation
Ball location and velocity
Ball motion mode
Map and robot location
Robot control
Landmark detection
Bal
l tra
ckin
gR
obot
loca
liza
tion l l l
b b b
![Page 46: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/46.jpg)
Grab Example (2)
k-2b k-1b kb
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 kz k-2
kmk-1mk-2m
Ball observation
Ball location and velocity
Ball motion mode
Map and robot location
Robot control
Landmark detection
Bal
l tra
ckin
gR
obot
loca
liza
tion l l l
b b b
![Page 47: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/47.jpg)
Inference: Posterior Estimation
k-2b k-1b kb
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 kz k-2
kmk-1mk-2m
Ball observation
Ball location and velocity
Ball motion mode
Map and robot location
Robot control
Landmark detection
Bal
l tra
ckin
gR
obot
loca
liza
tion l l l
b b b
),,|,,( 1:1:1:1 klk
bkkkk uzzrmbp
![Page 48: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/48.jpg)
Particle Filter for Robot Localization
![Page 49: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/49.jpg)
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
iiiiiiiiii myxbmrbmrskkk
,,,,,,,,,:1:1
),|(),,|(),,,|(
),|,,(
1:1:1:11:1:1:1:11:1:1:1:1
1:1:1:1:1
kkkkkkkkkkkk
kkkkk
uzrpuzrmpuzrmbp
uzrmbp
![Page 50: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/50.jpg)
Rao-Blackwellised Particle Filter for Inference
k-1b kb
r k-1 r k
kmk-1m
Ball location and velocity
Ball motion mode
Map and robot location
Bal
l tra
ckin
gR
obot
loca
liza
tion
)(1
)(1
)(1 ,, i
ki
ki
k bmr
Draw a sample from the previous sample set:
![Page 51: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/51.jpg)
Generate Robot Location
r k-1 r k
u k-1
z k
Map and robot location
Robot control
Landmark detection
Rob
ot lo
cali
zati
on l
k-1b
k-1m
Ball location and velocity
Ball motion mode
Bal
l tra
ckin
g
kb
km
__,,),,,,|(~ )(1
)(1
)(1
)(1
)( ikkk
ik
ik
ikk
ik ruzbmrrpr
![Page 52: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/52.jpg)
Generate Ball Motion Model
k-1b
r k-1 r k
u k-1
z k
kmk-1m
Ball location and velocity
Ball motion mode
Map and robot location
Robot control
Landmark detection
Bal
l tra
ckin
gR
obot
loca
liza
tion l
kb
_,,),,,,|(~ )()(1
)(1
)(1
)()( ik
ikkk
ik
ik
ikk
ik mruzbmrmpm
![Page 53: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/53.jpg)
Update Ball Location and Velocity
k-1b
r k-1 r k
u k-1
z k
kmk-1m
Ball location and velocity
Ball motion mode
Map and robot location
Robot control
Landmark detection
Bal
l tra
ckin
gR
obot
loca
liza
tion l
kb
z k b
)()()()(1
)()()( ,,),,,|(~ ik
ik
ikk
ik
ik
ikk
ik bmrzbmrbpb
![Page 54: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/54.jpg)
Importance Resampling
Weight sample by
if observation is landmark detection and by
if observation is ball detection.
Resample
)|( )()( ik
lk
ik rzpw
ki
ki
ki
ki
ki
ki
ki
kbk
ik
ik
ik
bk
ik
bbrmbpbrmzp
brmzpw
d),,|(),,|(
),,|()(1
)()()()()()(
)(1
)()()(
![Page 55: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/55.jpg)
Ball-Environment Interaction
![Page 56: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/56.jpg)
Ball-Environment Interaction
![Page 57: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/57.jpg)
Tracking and Finding the Ball
Cluster ball samples by discretizing pan / tilt angles
Uses negative information
![Page 58: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/58.jpg)
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
Pe
rce
nta
ge
of
ba
ll lo
st
Number of ball samples
With MapWithout Map
Robot kicks ball 100 times, tries to find it afterwards
Finds ball in 1.5 seconds on average
![Page 59: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/59.jpg)
Error vs. Prediction Time
0
10
20
30
40
50
60
70
80
0 0.5 1 1.5 2
RM
S E
rror
[cm
]
Prediction time [sec]
RBPFKF'KF*
![Page 60: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/60.jpg)
Reference* Observations
Reference* Observations
Simulation Runs
![Page 61: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/61.jpg)
Comparison to KF* (optimized for straight motion)
RBPFKF*
Reference* Observations
RBPFKF*
Reference* Observations
![Page 62: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/62.jpg)
RBPFKF’
Reference* Observations
RBPFKF’
Reference* Observations
Comparison to KF’ (inflated prediction noise)
![Page 63: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/63.jpg)
Orientation Errors
0
20
40
60
80
100
120
140
160
180
2 3 4 5 6 7 8 9 10 11
Orie
nta
tion
Err
or [
deg
rees
]
Time [sec]
RBPFKF*KF'
![Page 64: From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun](https://reader036.vdocuments.mx/reader036/viewer/2022062422/56649e7d5503460f94b8070f/html5/thumbnails/64.jpg)
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)