Download Probabilistic Robotics

Probabilistic Robotics
Bayes Filter Implementations
Particle filters
Sample-based Localization (sonar)
Particle Filters


Represent belief by random samples

Monte Carlo filter, Survival of the fittest,
Condensation, Bootstrap filter, Particle filter



Filtering: [Rubin, 88], [Gordon et al., 93], [Kitagawa 96]
Estimation of non-Gaussian, nonlinear processes
Computer vision: [Isard and Blake 96, 98]
Dynamic Bayesian Networks: [Kanazawa et al., 95]d
Importance Sampling with
Resampling
Target distributi on f : p( x | z1 , z2 ,..., zn ) 
Sampling distributi on g : p( x | zl ) 
Importance weights w :
 p( z
k
| x)
p ( x)
k
p( z1 , z2 ,..., zn )
p ( zl | x ) p ( x )
p ( zl )
p( x | z1 , z2 ,..., zn )
f


g
p ( x | zl )
p ( zl )  p ( z k | x )
k l
p( z1 , z2 ,..., zn )
Importance Sampling with
Resampling
Weighted samples
After resampling
Particle Filters
Sensor Information: Importance Sampling
Bel ( x)   p ( z | x) Bel  ( x)
 p( z | x) Bel  ( x)
w

Bel  ( x)
  p( z | x)
Robot Motion
Bel  ( x) 
 p( x | u x' ) Bel ( x' )
,
d x'
Sensor Information: Importance Sampling
Bel ( x)   p ( z | x) Bel  ( x)
 p( z | x) Bel  ( x)
w

Bel  ( x)
  p( z | x)
Robot Motion
Bel  ( x) 
 p( x | u x' ) Bel ( x' )
,
d x'
Particle Filter Algorithm
1. Algorithm particle_filter( St-1, ut-1 zt):
2. St  ,
 0
3. For 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
10.
wti  wti / 
Normalize weights
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
w 
proposal distributi on
 p( zt | xt ) p( xt | xt 1 , ut 1 ) Bel ( xt 1 )

p ( xt | xt 1 , ut 1 ) Bel ( xt 1 )
 p ( zt | xt )
i
t
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’.
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
Motion Model Reminder
Start
Proximity Sensor Model Reminder
Laser sensor
Sonar sensor
17
18
19
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
Sample-based Localization (sonar)
36
Initial Distribution
37
After Incorporating Ten
Ultrasound Scans
38
After Incorporating 65
Ultrasound Scans
39
Estimated Path
40
Using Ceiling Maps for Localization
Vision-based Localization
P(z|x)
z
h(x)
Under a Light
Measurement z:
P(z|x):
Next to a Light
Measurement z:
P(z|x):
Elsewhere
Measurement z:
P(z|x):
Global Localization Using Vision
Robots in Action: Albert
47
Application: Rhino and Albert
Synchronized in Munich and
Bonn
48
[Robotics And Automation Magazine, to appear]
Localization for AIBO
robots
Limitations
• The approach described so far is able
to
• track the pose of a mobile robot and to
• globally localize the robot.
• How can we deal with localization
errors (i.e., the kidnapped robot
problem)?
50
Approaches
• Randomly insert samples (the robot
can be teleported at any point in
time).
• Insert random samples proportional
to the average likelihood of the
particles (the robot has been
teleported with higher probability
when the likelihood of its observations
drops).
51
Random Samples
Vision-Based Localization
936 Images, 4MB, .6secs/image
Trajectory of the robot:
52
Odometry Information
53
Image Sequence
54
Resulting Trajectories
Position tracking:
55
Resulting Trajectories
Global localization:
56
Global Localization
57
Kidnapping the Robot
58
Summary
• Particle filters are an implementation of
•
•
•
•
recursive Bayesian filtering
They represent the posterior by a set of
weighted samples.
In the context of localization, the particles
are propagated according to the motion
model.
They are then weighted according to the
likelihood of the observations.
In a re-sampling step, new particles are
drawn with a probability proportional to
the likelihood of the observation.
60