Transcript X - Willow Garage

Probabilistic Models of Sensing and Movement
Move to probability models of sensing and movement



Project 2 is about complex behavior using sensing
Sensor interpretation is difficult – simple interpretation in this section
Artifacts [goal-directed motion] and reactive behaviors
Lectures



Probabilistic sensor models
Probabilistic representation of uncertain movement
Particle filter implementation
Project



PF for motion model
Markov localization with PF
Stretch – feature-based localization
Slides thanks to Steffen Gutmann
4/13/2015
CS225B Kurt Konolige
Markov Localization
Bayes filter
Bel ( xt )   P( zt | xt )  P( xt | ut , xt 1 ) Bel ( xt 1 ) dxt 1
Motion:
Bel ( x)   P ( x | u , x' ) Bel ( x' ) dx '
Sense:
Bel( x)  P( z | x) Bel( x)
Implementations





Discrete filter
Particle filter
Kalman filter
Multi-Hypotheses tracking (MHT)
…
4/13/2015
CS225B Kurt Konolige
Discrete Filter
• Piecewise constant
• Histogram
• Probability grid
4/13/2015
CS225B Kurt Konolige
Discrete Bayes Filter Algorithm
2.
Algorithm Discrete_Bayes_filter( Bel(x),d ):
0
3.
If d is a perceptual data item z then
1.
4.
5.
6.
7.
8.
9.
For all x do
Bel' ( x)  P( z | x) Bel( x)
    Bel' ( x)
For all x do
Bel' ( x)   1Bel' ( x)
Else if d is an action data item u then
10.
11.
For all x do
Bel' ( x)   P( x | u, x' ) Bel( x' )
x'
12.
Return Bel’(x)
4/13/2015
CS225B Kurt Konolige
Piecewise Constant Representation
Bel( xt  x, y, )
4/13/2015
CS225B Kurt Konolige
Grid-based Localization
4/13/2015
CS225B Kurt Konolige
Xavier: Localization in a Topological Map
[Courtesy of Reid Simmons]
4/13/2015
CS225B Kurt Konolige
Sample-based Localization (Sonar)
4/13/2015
CS225B Kurt Konolige
Particle Filters

Represent belief by random samples + weight
x, w
state + weight

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]
4/13/2015
CS225B Kurt Konolige
Robot Motion
Sample:
4/13/2015
Bel  ( x) 
 p( x | u x' ) Bel ( x' )
xt[m]  p( xt | ut , xt[m1] )
CS225B Kurt Konolige
,
d x'
Motion Model in Particle Filter
Start
4/13/2015
CS225B Kurt Konolige
Sensor Information: Importance Sampling
Bel( x)   p( z | x) Bel ( x)
 p( z | x) Bel ( x)
w

  p ( z | x)

Bel ( x)
4/13/2015
CS225B Kurt Konolige
Importance Sampling
Weight samples: w = f / g
4/13/2015
CS225B Kurt Konolige
Importance Sampling with Resampling
Draw sample i with probability  wi
Weighted samples
4/13/2015
After resampling
CS225B Kurt Konolige
Particle Filter Algorithm
Bel ( xt )   p( zt | xt )  p( xt | xt 1 , ut ) Bel ( xt 1 ) dxt 1
draw xit1 from Bel(xt1)
draw xit from p(xt | xit1,ut)
Importance factor for xit:
target distribution
proposaldistribution
 p( zt | xt ) p( xt | xt 1 , ut ) Bel ( xt 1 )

p( xt | xt 1 , ut ) Bel ( xt 1 )
wti 
 p( zt | xt )
4/13/2015
CS225B Kurt Konolige
Particle Filter Algorithm
1. Algorithm particle_filter(Xt-1, ut, zt):
2.
3.
4.
5.
6.
Xt  Xt  
for m  1 M
Generate new samples
xt[m]  sample_motion_model(ut , xt[m1] )
wt[m]  measurement_model(zt , xt[m] )
X t  X t  xt[ m ] , wt[ m ]
Insert into temporary set
7. endfor
8. for m  1 M
9.
10.
Resample
draw i with probability  wt[i ]
X t  X t  xt[i ]
Insert into final set
11. endfor
12. return Xt
4/13/2015
CS225B Kurt Konolige
Resampling
Given: Set X 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 X’.
4/13/2015
CS225B Kurt Konolige
Resampling
wn
Wn-1
wn
w1
w2
Wn-1
4/13/2015
w2
w3
w3
• Roulette wheel
• Binary search, n log n
w1
• Stochastic universal sampling
• Systematic resampling
• Linear time complexity
• Easy to implement, low variance
CS225B Kurt Konolige
Resampling Algorithm
1.
2.
3.
4.
Algorithm systematic_resampling( X t):
Xt  
r  rand(0; M 1 )
c  wt[1]
5. i  1
6. for m  1 M
7.
u  r  (m 1)  M 1
8.
while u  c
9.
i  i 1
c  c  wt[i ]
10.
11.
endwhile
X t  X t  xt[i ]
12.
13. endfor
14. return Xt
4/13/2015
Initialize threshold
Increment threshold
Skip until next threshold
Insert
CS225B Kurt Konolige
4/13/2015
CS225B Kurt Konolige
4/13/2015
CS225B Kurt Konolige
4/13/2015
CS225B Kurt Konolige
4/13/2015
CS225B Kurt Konolige
4/13/2015
CS225B Kurt Konolige
4/13/2015
CS225B Kurt Konolige
4/13/2015
CS225B Kurt Konolige
4/13/2015
CS225B Kurt Konolige
4/13/2015
CS225B Kurt Konolige
4/13/2015
CS225B Kurt Konolige
4/13/2015
CS225B Kurt Konolige
4/13/2015
CS225B Kurt Konolige
4/13/2015
CS225B Kurt Konolige
4/13/2015
CS225B Kurt Konolige
4/13/2015
CS225B Kurt Konolige
4/13/2015
CS225B Kurt Konolige
4/13/2015
CS225B Kurt Konolige
4/13/2015
CS225B Kurt Konolige
Berkeley Street Localization [Freuh]
4/13/2015
CS225B Kurt Konolige
Limitations
The approach described so far is able


to globally localize the robot, and
to track the pose of a mobile robot
How can we deal with localization errors such as


the kidnapped robot problem, or
recovery from localization failures?
4/13/2015
CS225B Kurt Konolige
Approaches
Random samples:


During re-sampling, insert a few random samples
(the robot can be teleported at any point in time)
Insert random samples inverse proportional to the average likelihood of the particles
(the robot has been teleported with higher probability when the likelihood of its
observations drops).
Samples drawn from observations: p(x|z)



Inverse proportional to average likelihood of particles (Sensor Resetting Localization)
[Lenser, Veloso, 2000]
Constant number but weight each sample by motion model (Mixture MCL) [Thrun et
al., 2000]
Proportional to noise in observations estimated over time (Adaptive MCL) [Fox 2002]
4/13/2015
CS225B Kurt Konolige
Particle Filter with Random Samples
1. Algorithm particle_filter_random(Xt-1, ut, zt):
2. X  X  
t
t
3. for m  1 M
Generate new samples
4.
5.
6.
7.
8.
9.
10.
11.
12.
13.
14.
15.
16.
xt[m]  sample_motion_model(ut , xt[m1] )
wt[m]  measurement_model(zt , xt[m] )
X t  X t  xt[ m ] , wt[ m ]
endfor
form  1 M
with probability prand do
Resample
X t  X t  random_pose()
else
draw i with probability  w[i ]
t
X t  X t  xt[i ]
endwith
endfor
return Xt
4/13/2015
Insert into temporary set
CS225B Kurt Konolige
Insert random pose
Insert sample
Recovery from Failure
failure
4/13/2015
CS225B Kurt Konolige
Random Samples
Vision-Based Localization
936 Images, invariant features for
matching images
Trajectory of the robot:
4/13/2015
CS225B Kurt Konolige
Kidnapping the Robot
4/13/2015
CS225B Kurt Konolige
Adaptive Monte Carlo Localization
1. Algorithm Augmented_MCL(Xt-1, ut, zt):
2. static wslow, wfast
3.
4.
5.
6.
7.
8.
9.
10.
11.
12.
13.
14.
15.
16.
17.
18.
19.
20.
Xt  Xt  
for m  1 M
xt[m]  sample_motion_model(ut , xt[m1] )
wt[m]  measurement_model(zt , xt[m] )
X t  X t  xt[ m ] , wt[ m ]
wavg  wavg  M 1wt[ m]
endfor
wslow  wslow  slow (wavg  wslow )
wfast  wfast  fast (wavg  wfast )
for m  1 M
with probability max{0, 1– wfast / wslow} do
X t  X t  sample_pose( zt )
else
draw i with probability  wt[i ]
X t  X t  xt[i ]
endwith
endfor
4/13/2015
return
Xt
CS225B Kurt Konolige
Generate new samples
Insert into temporary set
Resample
Insert random pose
Insert sample