1

Lecture 5:Markov Localization

CS 395T: Intelligent RoboticsBenjamin Kuipers

Thanks to Dieter Fox for his slides.

Localization: “Where am I?”

• The map-building method we studiedassumes that the robot knows its location.– Precise (x,y,θ) coordinates in the same frame

of reference as the occupancy grid map.• This assumes that odometry is accurate,

which is often false.• We will need to relocalize at each step.

Odometry-Only Tracking:6 times around a 2m x 3m area

Merging Laser Range DataBased on Odometry-Only Tracking

SLAM: SimultaneousLocalization and Mapping

Alternate at each motion step:1. Localization:

• Assume accurate map.• Match sensor readings against the map to

update location after motion.2. Mapping:

• Assume known location in the map.• Update map from sensor readings.

Mapping Without Localization

2

Mapping With LocalizationModeling Action and Sensing

• Action model: P(xt | xt-1, ut-1)• Sensor model: P(zt | xt)• What we want to know is Belief:

the posterior probability distribution of xt, giventhe past history of actions and sensor inputs.

Xt-1 Xt Xt+1

Zt-1 Zt Zt+1

ut-1 ut

),,,|()( 121 ttttzuzuxPxBel != K

The Markov Assumption

• Given the state xt, the observation zt isindependent of the past.

• Given the present, the future is independentof the past.

Xt-1 Xt Xt+1

Zt-1 Zt Zt+1

ut-1 ut

!

P(zt| x

t) = P(z

t| x

t,u1,z2 K,u

t"1)

Dynamic Bayesian Network• The well-known DBN for local SLAM.

Law of Total Probability(marginalizing)

!=y

yxPxP ),()(

!=y

yPyxPxP )()|()(

!

P(y) =1y

"

Discrete

!

p(y) dy =1"

Continuous case

!= dyypyxpxp )()|()(

!= dyyxpxp ),()(

Bayes Law• We can treat the denominator in Bayes Law

as a normalizing constant:

• We will apply it in the following form:

)()|(

1)(

)()|()(

)()|()(

1

xPxyPyP

xPxyPyP

xPxyPyxP

x

!==

==

"#

#

),,,|(),,,,|( 121121 !!=tttttuzuxPuzuxzP KK"

),,,|()( 121 ttttzuzuxPxBel != K

3

Bayes Filter

),,,|(),,,,|( 121121 !!=tttttuzuxPuzuxzP KK"Bayes

),,,|()( 121 ttttzuzuxPxBel != K

Markov ),,,|()|( 121 !=ttttuzuxPxzP K"

1111 )(),|()|( !!!!"=ttttttt

dxxBelxuxPxzP#

Markov1121111 ),,,|(),|()|( !!!!!"=

ttttttttdxuzuxPxuxPxzP K#

11211

1121

),,,|(

),,,,|()|(

!!!

!!"=

ttt

ttttt

dxuzuxP

xuzuxPxzP

K

K#Total prob.

Markov Localization

• Bel(xt-1) and Bel(xt) are prior and posteriorprobabilities of location x.

• P(xt | ut-1, xt-1) is the action model, giving theprobability distribution over result of ut-1 at xt-1.

• P(zt | xt) is the sensor model, giving theprobability distribution over sense images zt at xt.

• η is a normalization constant, ensuring that totalprobability mass over xt is 1.

!

Bel(xt) =" P(z

t| x

t) P(x

t| u

t#1,xt#1)$ Bel(xt#1) dxt#1

Markov Localization• Evaluate Bel(xt) for every possible state xt.

• Prediction phase:

– Integrate over every possible state xt-1 to apply theprobability that action ut-1 could reach xt from there.

• Correction phase:

– Weight each state xt with likelihood of observation zt.

!

Bel"(x

t) = P(x

t| u

t"1,xt"1)# Bel(xt"1) dxt"1

!

Bel(xt) =" P(z

t| x

t) Bel

#(x

t)

Uniform prior probability Bel- (x0)

Sensor information P(z0 | x0)

!

Bel(x0) =" P(z0 | x0) Bel#(x0)

Apply the action model P(x1 | u0, x0)

!

Bel"(x1) = P(x1 | u0,x0)# Bel(x0) dx0

4

Combine with observation P(z1 | x1)

!

Bel(x1) =" P(z1 | x1) Bel#(x1)

Action model again: P(x2 | u1, x1)

!

Bel"(x2) = P(x2 | u1,x1)# Bel(x1) dx1

Local and Global Localization

• Most localization is local:– Incrementally correct belief in position after

each action.• Global localization is more dramatic.

– Where in the entire environment am I?• The “kidnapped robot problem”

– Detecting that I am lost.

Initial belief Bel(x0)

Intermediate Belief Bel(xt) Final Belief Bel(xt)

5

Global Localization Movie Markov Localization

• The integral is evaluated over all xt-1.– It computes the probability of reaching xt from

any location xt-1, using the action ut-1.• The equation is evaluated for every xt.

– It computes the posterior probabilitydistribution of xt.

• Computational efficiency is a problem.– o(k2) if there are k poses xt.– k reflects resolution of position and orientation.

!

Bel(xt) =" P(z

t| x

t) P(x

t| u

t#1,xt#1)$ Bel(xt#1) dxt#1

Action and Sensor Models

• Action model: P(xt | ut-1, xt-1)• Sensor model: P(zt | xt)

• Distributions over possible values of xt or ztgiven specific values of other variables.

• We discussed these last time (or next time).

Monte Carlo Simulation• Given a probability distribution over

inputs, computing the distribution overoutcomes can be hard.– Simulating a concrete instance is easy.

• Sample concrete instances (“particles”)from the input distribution.

• Collect the outcomes.– The distribution of sample outcomes

approximates the desired distribution.• This has been called “particle filtering.”

Actions Disperse the Distribution

• N particlesapproximate aprobability distribution.

• The distributiondisperses under actions

Monte Carlo Localization

• A concrete instance is a particular pose.– A pose is position plus orientation.

• A probability distribution is represented bya collection of N poses.– Each pose has an importance factor.– The importance factors sum to 1.

• Initialize with– N uniformly distributed poses.– Equal importance factors of N-1.

6

Representing a Distribution

• The distribution Bel(xt) is represented by aset St of N weighted samples:

where

• A particle filter is a Bayes filter that usesthis sample representation.

!

St= x

t

(i),w

t

( i)| i =1,LN{ }

!

wt

(i)

i=1

N

" =1

Importance Sampling• Sample from a proposal distribution.

– Correct to approximate a target distribution.

The Basic Particle Filter Algorithm• Input: ut-1, zt,

– St := ∅, i := 1, α := 0• while i ≤ N do

– sample j from the discrete distribution given bythe weights in St-1

– sample xt(i) from p(xt | ut-1, xt-1) given xt-1

(j) and ut-1.– wt

(i) := p(zt | xt(i))

– α := α + wt(i); i := i + 1

– St := St ∪ {〈xt(i), wt

(i)〉}• for i := 1 to N do wt

(i) := wt(i)/ α

• return St

!

St"1 = x

t"1

(i),w

t"1

( i)| i =1,LN{ }

Sampling from aWeighted Set of Particles

• Given

• Draw α from a uniform distributionover [0,1].

• Find the minimum k such that

• Return

!

St= x

t

(i),w

t

( i)| i =1,LN{ }

!

wt

(i)

i=1

k

" >#

!

xt

(k )

0

1

w(4)

w(1)

w(2)

w(3)

w(10)w(11)

KLD Sampling

• The number N of samples needed can beadapted dynamically, based on the discreteχ2 (chi-squared) statistic.

• See TBF, Probabilistic Robotics, sect.8.3.7

MCL Algorithm• Repeat to collect N samples.

– Draw a sample xt-1 from the distribution Bel(xt-1),with likelihood given by its importance factor.

– Given an action ut-1 and the action modeldistribution P(xt | ut-1, xt-1), sample state xt.

– Assign the importance factor P(zt | xt) to xt.• Normalize the importance factors.• Repeat for each time-step.

!

Bel(xt) =" P(z

t| x

t) P(x

t| u

t#1,xt#1)$ Bel(xt#1) dxt#1

7

MCL works quite well• N=1000 seems to work OK.• Straight MCL works best for sensors that

are not highly accurate.– For very accurate sensors, P(zt | xt) is very

narrow and highly peaked.– Poses xt that are nearly (but not exactly)

correct can get low importance values.– They may be under-represented in the next

generation.

Localization Movie (known map)

Action and Sensor Models

• The Markov localization equation dependson two types of knowledge about the robot.

• The action model: P(xt | ut-1, xt-1)– Given a state xt and odometry ut, the

distribution over possible next states xt+1

• The sensor model: P(zt | xt)– Given a state xt, the distribution over possible

sensor images zt.

!

Bel(xt) =" P(z

t| x

t) P(x

t| u

t#1,xt#1)$ Bel(xt#1) dxt#1

Interpolate Observation Times

• Odometry ut and laserscans zt actuallyarrive at slightlydifferent times.

• Interpolate to giveestimated odometry ut′at the same time asthe laser scan zt.

z1

z2

z3u1

u2

u3

z1

z2

z3u1’

u2’u3’

Action Model P(xt | ut-1, xt-1)

• Probability density function over poses, aftertraveling 40m or 80m.

The Action Error ModelSuppose odometry gives:

– (x1, y1, ϕ1)– (x2, y2, ϕ2)

in a slowly driftingframe of reference.

8

The Action Error ModelFrom odometry get:

– (x1, y1, ϕ1)– (x2, y2, ϕ2)

in a slowly driftingframe of reference.

Then:– (Δx, Δy, Δϕ)– Δx2 + Δy2 = Δs2

• Δs and Δϕ arerelatively reliable, andindependent of theframe of reference.

Δx

ΔyΔs

The Action Error ModelFrom odometry get:

– (x1, y1, ϕ1)– (x2, y2, ϕ2)

in a slowly drifting frameof reference.

Then:– (Δx, Δy, Δϕ)– Δx2 + Δy2 = Δs2

and:– α = atan2(Δy,Δx)−ϕ1

– (Measure angle counter-clockwise from x-axis)

Δx

ΔyΔs

Δϕ

α

The Action Error ModelFrom odometry get:

– (x1, y1, ϕ1)– (x2, y2, ϕ2)– (Δx, Δy, Δϕ)– Δx2 + Δy2 = Δs2

– α = atan2(Δy,Δx)−ϕ1

• Model the action as:– Turn(α)– Travel(Δs)– Turn(Δϕ−α)

Δx

ΔyΔs

Δϕ

α

Δϕ−α

The Action Error Model• Model the action as:

– Turn(α + ε1)– Travel(Δs + ε2)– Turn(Δϕ− α + ε3)

where- ε1 ~ N(0, k1α)- ε2 ~ N(0, k2 Δs)- ε3 ~ N(0, k1 (Δϕ− α))

• This combines threeGaussian errors.– Std dev proportional

to action magnitude

Δs

α

Δϕ−α

Tune the Action Error Model• Model the action as:

– Turn(α + ε1)– Travel(Δs + ε2)– Turn(Δϕ− α + ε3)

where- ε1 ~ N(0, k1α)- ε2 ~ N(0, k2 Δs)- ε3 ~ N(0, k1 (Δϕ− α))

• Tune the model byfinding k1 and k2.

• Compute errorbetween:– odometry observed– odometry corrected by

localization.• Divide by turn or

travel magnitude.• Compute standard

deviations– k1 = 1.0– k2 = 0.4

The Action Model P(xt | ut-1, xt-1)• Given a small motion ut-1 = (α, Δs, Δϕ−α)

where- ε1 ~ N(0, k1α)- ε2 ~ N(0, k2 Δs)- ε3 ~ N(0, k1 (Δϕ− α))

!

xt

yt

"t

#

$

% % %

&

'

( ( (

=

xt)1

yt)1

"t)1

#

$

% % %

&

'

( ( (

+

(*s+ +2)cos("t)1 +, + +1)

(*s+ +2)sin("t)1 +, + +1)

*- + +1 + +3

#

$

% % %

&

'

( ( (

9

Sensor Model P(zt | xt)• For a given range measurement at a given

location xt in the map:

Laser Rangefinder Scan zt

Density P(zt | xt) by Location Computing P(zt | xt)• Image probabilities are too small to be

meaningful:

• log probabilities can be accumulated

• but it’s still slow.

!

P(zt | xt ) = P(ray(i) = di | xt )i=1

180

"

!

logP(zt | xt ) = logP(ray(i) = di | xt )i=1

180

"

Q: Didn’t we use log odds before?• Yes, but that was in the occupancy grid.

– It is useful as a way to represent P(occ(i,j))

• Here, we’re doing Markov localization.

• We need to compute P(zt | xt).– Log helps us avoid numerical underflow.– Odds would just be a distraction.!

Bel(xt) =" P(z

t| x

t) P(x

t| u

t#1,xt#1)$ Bel(xt#1) dxt#1

P(ray(i) = di | xt)

10

Computing log P(ray(i) = di | xt)• If ray(i) terminates at the first obstacle:

log P(ray(i)=di | xt) = −4• If ray(i) terminates before an obstacle:

log P(ray(i)=di | xt) = −8• If ray(i) terminates after an obstacle:

log P(ray(i)=di | xt) = −12• Add them up for i=1 to 180.

– Total log P(zt | xt) is between −720 and −2160.– Pre-normalize by adding largest log P to all values.– Exponentiate and normalize.

Avoiding Numerical Underflow• We’re summing many negative log values

• Exponentiating will just give us zero!• But we’re just going to normalize, next.

– So multiply by a constant, and normalize it out.– I.e., add a constant to each log P(zt | xt)

• Shift the largest log P(zt | xt) value to zero.– I.e., add max log P(zt | xt) to each value.– Then we have values of P(zt | xt) we can normalize.

!

logP(zt | xt ) = logP(ray(i) = di | xt )i=1

180

"

References

• Thrun, Burgard & Fox. Probabilistic Robotics.MIT Press, 2005.

• Beeson, Murarka & Kuipers. Adapting proposaldistributions for accurate, efficient mobile robotlocalization. ICRA, 2006.

Future Attractions

• Particle filtering– elegant, simple algorithm– Monte Carlo simulation