Bayesian Approaches to Localization,

Mapping, and SLAM

Robotics Institute 16-735

[Link]

Howie Choset
[Link]

RI 16-735, Howie Choset

Quick Probability Review: Bayes Rule

p(b | a) p(a)
p(a | b) =
p(b)

p(b | a, c) p(a | c)
p(a | b, c) =
p(b | c)

RI 16-735, Howie Choset

 QPR: Law of Total Probability

p (a ) = ∑ p (a ∧ b )
i
i

Discrete = ∑ p ( a | b ) p (b )
i
i i

Continuous p ( a ) = ∫ p ( a | b) p (b) db

it follows that:

p ( a | b) = ∫ p ( a | b, c ) p (c | b) dc

RI 16-735, Howie Choset

 QPR: Markov Assumption

Future is Independent of Past Given

Current State

“Assume Static World”

RI 16-735, Howie Choset

 The Problem
• What is the world around me (mapping)
– sense from various positions
– integrate measurements to produce map
– assumes perfect knowledge of position

• Where am I in the world (localization)

– sense
– relate sensor readings to a world model
– compute location relative to model
– assumes a perfect world model

• Together, these are SLAM (Simultaneous Localization and

Mapping)

RI 16-735, Howie Choset

 Localization

Tracking: Known initial position Challenges

– Sensor processing
Global Localization: Unknown initial position – Position estimation
Re-Localization: Incorrect known position – Control Scheme
– Exploration Scheme
(kidnapped robot problem)
– Cycle Closure
– Autonomy
SLAM –
–
Tractability
Scalability
Mapping while tracking locally and globally

RI 16-735, Howie Choset

 Representations for Robot Localization

Kalman filters (late-80s?)

Discrete approaches (’95) • Gaussians
• Topological representation (’95) • approximately linear models
• uncertainty handling (POMDPs) • position tracking
• occas. global localization, recovery
• Grid-based, metric representation (’96)
• global localization, recovery
Robotics
AI
Particle filters (’99) Multi-hypothesis (’00)
• sample-based representation • multiple Kalman filters
• global localization, recovery • global localization, recovery

RI 16-735, Howie Choset

 The Basic Idea
Robot can be anywhere

Robot senses a door

Robot moves on
(note, not unimodal)

Robot senses another door

(note, high likelihood, but multimodal)

[Simmons/Koenig 95]
[Kaelbling et al 96]
RI 16-735, Howie Choset
[Burgard et al 96]
 Notes
• Perfect Sensing
– No false positives/neg.
– No error

• Data association

RI 16-735, Howie Choset

 Notation for Localization

The posterior

• At every step k

• Probability over all configurations

• Given
– Sensor readings y from 1 to k
– Control inputs u from 0 to k-1
– Interleaved:

Map m Velocities, force, odometry, something more complicated

(should be in condition statements too)

RI 16-735, Howie Choset

 Predict and Update, combined

posterior

prior

Motion model: commanded motion moved from robot x(k-1) to x(k)

Sensor model: robot perceives y(k) given a map and that it is at x(k)

Features Issues

Generalizes beyond Gaussians Realization of sensor and motion models

Recursive Nature Representations of distributions

RI 16-735, Howie Choset

 Prediction Step
• Occurs when an odometry measurement (like a control) or when
a control is invoked…. Something with u(k-1)

• Suppose u(0: k-2) and y(1: k-1) known and

Current belief is

• Obtain
– Integrate/sum over all possible x(k-1)
– Multiply each
by Motion model
Not k

RI 16-735, Howie Choset

 Update Step
• Whenever a sensory experience occurs… something with y(k)

• Suppose is known
and we just had sensor y(k)

• For each state x(k)

Sensor model
Multiply by &η

Normalization constant: make it all sum to one

RI 16-735, Howie Choset

 That pesky normalization factor
• Bayes rule gives us

• This is hard to compute:

– What is the dependency of y(k) on previous controls and sensor
readings without knowing your position or map of the world?

• Total probability saves the day

• We know these terms

RI 16-735, Howie Choset

 Summary

RI 16-735, Howie Choset

 Issues to be resolved
• Initial distribution P(0)
– Gaussian if you have a good idea
– Uniform if you have no idea
– Whatever you want if you have some idea

• How to represent distributions: prior & posterior, sensor & motion

models

• How to compute conditional probabilities

• Where does this all come from? (we will do that first)

RI 16-735, Howie Choset

 The derivation:
• Consider odometry and sensor information separately

• Lets start with new sensor reading comes in – a new y(k)

– Assume y(1:k-1) and u(0:k-1) as known
– Apply Bayes rule

=η

Once state is known, then all previous controls and measurements are independent
of current reading

Denominator is a normalizer which is the same for all of x(k)

RI 16-735, Howie Choset

 Incorporate motions
• We have

• Use law of total probability on right-most term

assume that x(k) is independent of sensor readings y(1:k-1) and controls u(1:k-2)
that got the robot to state x(k-1) given we know the robot is at state x(k-1)

assume controls at k-1 take robot from x(k-1) to x(k), which we don’t know x(k) x(k-
1) is independent of u(k-1)

RI 16-735, Howie Choset

 Incorporate motions
• We have

RI 16-735, Howie Choset

 Representations of Distributions
• Kalman Filters

• Discrete Approximations

• Particle Filters

RI 16-735, Howie Choset

 Extended Kalman Filters

as a Gaussian

The Good
Computationally efficient
Easy to implement

The Bad
Linear updates
Unimodal

RI 16-735, Howie Choset

 Discretizations
• Topological structures

• Grids

Spatial 10-30cm
Angular 2-10 degrees

RI 16-735, Howie Choset

 Algorithm to Update Posterior P(x)
Start with u(0: k-1) and y(1:k)

k loops

Integrate u(i-1) and y(i-1) in each loop

Incorporate motion u(i-1) with motion

model

Sensor model
Normalization Constant

Bypass with convolution details we will skip

RI 16-735, Howie Choset
 Convolution Mumbo Jumbo
• To efficiently update the belief upon robot motions, one typically
assumes a bounded Gaussian model for the motion uncertainty.
• This reduces the update cost from O(n2) to O(n), where n is the number
of states.
• The update can also be realized by shifting the data in the grid according
to the measured motion.
• In a second step, the grid is then convolved using a separable Gaussian
Kernel.
• Two-dimensional example:

1/16 1/8 1/16 1/4

1/8 1/4 1/8 ≅ 1/2 + 1/4 1/2 1/4

1/16 1/8 1/16 1/4

• Fewer arithmetic operations

• Easier to implement
RI 16-735, Howie Choset
 Probabilistic Action model
Continuous probability density Bel(st) after moving

p(x(k)|u(k-1),x(k-1))

xk-1 xk-1
uk-1
uk-1

40m 80m.

Darker area has higher probability.

Thrun et. al.

RI 16-735, Howie Choset

 Probabilistic Sensor Model

P(y|x)

Probabilistic sensor model for laser range finders

RI 16-735, Howie Choset

One of Wolfram et al’s Experiments

A, after 5 scans;
Known map
B, after 18 scans,
C, after 24 scans

5 scans 18 scans 24 scans

RI 16-735, Howie Choset

 What do you do with this info?
• Mean, continuous but may not be meaningful

• Mode, max operator, not continuous but corresponds to a robot

position

• Medians of x and y, may not correspond to a robot position too

but robust to outliers

RI 16-735, Howie Choset

 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

RI 16-735, Howie Choset

 Basic Idea
• Maintain a set of N samples of states, x, and weights, w, in a set
called M.

• When a new measurement, y(k) comes in, the weight of particle

(x,w) is computed as p(y(k)|x) – observation given a state

• Resample N samples (with replacement) from M according to

weights w

RI 16-735, Howie Choset

 Particle Filter Algorithm
and Recursive Localization

Bel(xt ) = η p(yt | 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 distribution
wti =
proposal distribution
η p( yt | xt ) p( xt | xt −1 , ut −1 ) Bel ( xt −1 )
=
p ( xt | xt −1 , ut −1 ) Bel ( xt −1 )
∝ p ( yt | xt )

RI 16-735, Howie Choset

Particle Filters

RI 16-735, Howie Choset

 Sensor Information: Importance Sampling
Bel ( x) ← α p ( y | x) Bel − ( x)
α p ( y | x) Bel − ( x)
w ← −
= α p ( y | x)
Bel ( x)

RI 16-735, Howie Choset

 Robot Motion
Bel − ( x) ← ∫ p( x | u x' ) Bel ( x' )
, d x'

RI 16-735, Howie Choset

 Sensor Information: Importance Sampling

Bel ( x) ← α p ( y | x) Bel − ( x)
α p ( y | x) Bel − ( x)
w ← −
= α p ( y | x)
Bel ( x)

RI 16-735, Howie Choset

 Robot Motion

Bel − ( x) ← ∫ p( x | u x' ) Bel ( x' )

, d x'

RI 16-735, Howie Choset

Motion Model Reminder

Start

Or what if robot keeps moving and there are no observations

RI 16-735, Howie Choset

 Proximity Sensor Model Reminder

Laser sensor Sonar sensor

RI 16-735, Howie Choset

 Particle Filter Algorithm

1. Algorithm particle_filter( Mt-1, ut-1 yt):

2. M t = ∅, η =0
3. For i = 1K n Generate new samples
4. Sample index j(i) from the discrete distribution given by Mt-1
5. Sample xti from p( xt | xt −1 , ut −1 ) using xtj−(1i ) and ut −1
6. wti = p ( yt | xti ) Compute importance weight
7. η = η + wti Update normalization factor
8. M t = M t ∪ {< xti , wti >} Insert
9. For i = 1K n
10. wti = wti / η Normalize weights
11. RESAMPLE!!!
RI 16-735, Howie Choset
 Resampling
• Given: Set M 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 M’.

RI 16-735, Howie Choset

 Resampling Algorithm
1. Algorithm systematic_resampling(M,n):

2. M ' = ∅, c1 = w
1

3. For i = 2K n Generate cdf

4. c i = ci −1 + w i

−1
u
5. 1 ~ U ]0 , n ], i = 1 Initialize threshold

6. For j = 1K n Draw samples …

7. While ( u j > ci ) Skip until next threshold reached
8. i = i +1
9. M ' = M '∪ {< x i
, n −1
>} Insert
−1
10. u j +1 = u j + n Increment threshold

11. Return M’
RI 16-735, Howie Choset
 Resampling, an analogy Wolfram likes

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
RI 16-735, Howie Choset
Initial Distribution

RI 16-735, Howie Choset

After Incorporating Ten Ultrasound Scans

RI 16-735, Howie Choset

After Incorporating 65 Ultrasound Scans

RI 16-735, Howie Choset

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

RI 16-735, Howie Choset

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

RI 16-735, Howie Choset

 Summary

• Recursive Bayes Filters are a robust tool for estimating the pose of a
mobile robot.
• Different implementations have been used such as discrete filters
(histograms), particle filters, or Kalman filters.
• Particle filters represent the posterior by a set of weighted samples.

RI 16-735, Howie Choset

Change gears to

RI 16-735, Howie Choset

Occupancy Grids [Elfes]

• In the mid 80’s Elfes starting implementing cheap

ultrasonic transducers on an autonomous robot

• Because of intrinsic limitations in any sonar, it is

important to compose a coherent world-model
using information gained from multiple reading

RI 16-735, Howie Choset

 Occupancy Grids Defined

• The grid stores the probability that Ci =

cell(x,y) is occupied O(Ci) = P[s(Ci) =
OCC](Ci)

y • Phases of Creating a Grid:

– Collect reading generating O(Ci)
– Update Occ. Grid creating a map
– Match and Combine maps from multiple
locations
x

Ci

Binary variable
Original notation

Cell is occupied Given sensor observations Given robot locations

RI 16-735, Howie Choset

 Bayes Rule Rules!
• Seek to find m to maximize

Local map

Assume that current readings is independent of all previous states and readings
given we know the map

Bayes rule on

RI 16-735, Howie Choset

 A cell is occupied or not
• The m

• Or not the m

RI 16-735, Howie Choset

The Odds

RECURSION

RI 16-735, Howie Choset

 Recover Probability

Given a sequence of measurements y(1:k), known positions x(1:k), and an initial distribution P0(m)

Determine
THE PRIOR

RI 16-735, Howie Choset

 Actual Computation of
• Big Assumption: All Cells are Independent
Local map

• Now, we can update just a cell

The prior

Depends on current cell, distance to

cell and angle to central axis

RI 16-735, Howie Choset

 More details on s

Else if’s

Deviation from occupancy probability from the prior given a reading and angle

RI 16-735, Howie Choset

 Break it down
• d1, d2, d3 specify the intervals

• Between the arc and current location, lower probability

• Cells close to the arc, ie. Whose distances are close to readings

Some linear function

• Immediately behind the cell (obstacles have thickness)

<d<

• No news is no news is prior beyond

RI 16-735, Howie Choset

Example

y(k) = 2m, angle = 0, s(2m,0) = .16

RI 16-735, Howie Choset

Example

y(k) = 2m y(k) = 2.5m

RI 16-735, Howie Choset

 A Wolfram Mapping Experiment
with a B21r with 24 sonars

18 scans, note each scan looks a bit uncertain but

result starts to look like parallel walls
RI 16-735, Howie Choset
 Are we independent?
• Is this a bad assumption?

RI 16-735, Howie Choset

 SLAM!
• A recursive process.

Motion model Sensor model

Posterior, hard to calculate

RI 16-735, Howie Choset

 “Scan Matching”

At time the robot is given

1. An estimate of state

2. A map estimate

The robot then moves and takes measurement y(k)

And robot chooses state estimate which maximizes

And then the map is updated with the new sensor reading

RI 16-735, Howie Choset

 Another Wolfram Experiment

28m x 28m, .19m/s, 491m

after

RI 16-735, Howie Choset

 Another Wolfram Experiment

before after

28m x 28m, .19m/s, 491m

RI 16-735, Howie Choset

 Tech Museum, San Jose

CAD map occupancy grid map

RI 16-735, Howie Choset

 Issues
• Greedy maximization step (unimodal)

• Computational burden (post-processing)

• Inconsistency (closing the loop, global map?)

Solutions [still maintain one map, but update at loop closing]

• Grid-based technique (Konolodige et. al)
• Particle Filtering (Thrun et. al., Murphy et. al.)
• Topological/Hybrid approaches (Kuipers et. al, Leonard et al,
Choset et a.)

RI 16-735, Howie Choset

 Probabilistic SLAM
Rao-Blackwell Particle Filtering

If we know the map, then it is a localization problem

If we know the landmarks, then it is a mapping problem

Some intuition: if we know x(1:k) (not x(0)), then we know the “relative map” but
Not its global coordinates

The promise: once path (x(1:k)) is known, then map can be determined analytically

Find the path, then find the map

RI 16-735, Howie Choset

 Mapping with Rao-Blackwellized Particle
Filters
• Observation:
Given the true trajectory of the robot, all measurements are independent.

• Idea:
– Use a particle filter to represent potential trajectories of the robot
(multiple hypotheses). Each particle is a path (maintain posterior of
paths)

– For each particle we can compute the map of the environment (mapping
with known poses).

– Each particle survives with a probability that is proportional to the

likelihood of the observation given that particle and its map.

RI 16-735, Howie Choset [Murphy et al., 99]

 RBPF with Grid Maps

3 particles

map of particle 1 map of particle 3

map of particle 2 RI 16-735, Howie Choset

 Some derivation

P(A,B) = P(A|B)P(B)

given

We can compute Use particle filtering

Computing prob map (local map) given trajectory for each particle
RI 16-735, Howie Choset
 Methodology
• M be a set of particles where each particle starts at [0,0,0]T

• Let h(j)(1:k) be the jth path or particle

• Once the path is known, we can compute most likely map

Hands start waving….. Just a threshold here

• Once a new u(i-1) is received (we move), do same thing as in
localization, i.e., sample from
Not an issue, but in book
– Note, really sampling from
– Ignore the map for efficiency purposes, so drop the m

• Get our y(k)’s to determine weights, and away we go

(use same sensor model as in localization)
RI 16-735, Howie Choset
Rao-Blackwell Particle Filtering

RI 16-735, Howie Choset

Wolfram Experiment

RI 16-735, Howie Choset

Most Recent Implementations

ƒ 15 particles
ƒ four times faster
than real-time
P4, 2.8GHz
ƒ 5cm resolution
during scan
matching
ƒ 1cm resolution in
final map

RI 16-735, Courtesy by Giorgio Grisetti & Cyrill Stachniss

Howie Choset
 Maps, space vs. time
Maintain a map for each particle

OR

Compute the map each time from scratch

Subject of research
Montermerlou and Thrun look for tree-like structures that capture
commonality among particles.

Hahnel, Burgard, and Thrun use recent map and subsample

sensory experiences

RI 16-735, Howie Choset

 How many particles?
• What does one mean?

• What does an infinite number mean?

RI 16-735, Howie Choset