Probabilistic Robotics

Bayes Filter Implementations

Particle filters
Sample-based Localization (sonar)
 Particle Filters

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]d
Importance Sampling

Weight samples: w = f / g
Importance Sampling with Resampling:
Landmark Detection Example
Distributions
Distributions

Wanted: samples distributed according to

p(x| z1, z2, z3)

7
This is Easy!
We can draw samples from p(x|zl) by adding
noise to the detection parameters.
Importance Sampling with
Resampling
p( z k | x) p ( x)
Target distribution f : p( x | z1 , z2 ,..., zn ) k
p( z1 , z2 ,..., zn )

p ( zl | x ) p ( x )
Sampling distribution g : p( x | zl )
p ( zl )

p ( zl ) p ( z k | x )
f p( x | z1 , z2 ,..., zn )
Importance weights w : k l
g p ( x | zl ) 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
p ( z | x)
Bel ( 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
p ( z | x)
Bel ( 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 1n 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 1n
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 xit1 from Bel(xt1)

draw xit from p(xt | xit1,ut1)

Importance factor for xit:

target distribution
w
i
t
proposaldistribution
p( zt | xt ) p( xt | xt 1 , ut 1 ) Bel ( xt 1 )

p( xt | xt 1 , ut 1 ) Bel ( xt 1 )
p( zt | xt )
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
w1 wn w1
wn
w2 w2
Wn-1 Wn-1

w3 w3

Stochastic universal sampling

Roulette wheel Systematic resampling
Binary search, n log n Linear time complexity
Easy to implement, low variance
Resampling Algorithm
1. Algorithm systematic_resampling(S,n):

2. S ' , c1 w1
3. For i 2n Generate cdf
4. ci ci 1 w i

5. u1 ~ U ]0, n 1 ], i 1 Initialize threshold

6. For j 1n Draw samples

7. While ( u j ci ) Skip until next threshold reached
8. i i 1
9.
S ' S ' xi , n 1 Insert
10. u j 1 u j n 1 Increment threshold

11. Return S

Also called stochastic universal sampling

Motion Model Reminder

Start
Proximity Sensor Model Reminder

Laser sensor Sonar sensor

23
24
25
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
Sample-based Localization (sonar)

42
Initial Distribution

43
After Incorporating Ten
Ultrasound Scans

44
After Incorporating 65
Ultrasound Scans

45
Estimated Path

46
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

53
Application: Rhino and Albert
Synchronized in Munich and
Bonn

54
[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)?

56
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).

57
Random Samples
Vision-Based Localization
936 Images, 4MB, .6secs/image
Trajectory of the robot:

58
Odometry Information

59
Image Sequence

60
Resulting Trajectories

Position tracking:

61
Resulting Trajectories

Global localization:

62
Global Localization

63
Kidnapping the Robot

64
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.

66