Advanced Mobile Robotics
Probabilistic Robotics:
Review/SLAM
Dr. Jizhong Xiao
Department of Electrical Engineering
CUNY City College
jxiao@ccny.cuny.edu
City College of New York
1
Bayes Filter Revisit
Bel ( xt )   P( zt | xt )  P( xt | ut , xt 1 ) Bel ( xt 1 ) dxt 1
• Prediction (Action)
bel ( xt )   p ( xt | ut , xt 1 ) bel ( xt 1 ) dxt 1
• Correction (Measurement)
bel( xt )   p( zt | xt ) bel( xt )
City College of New York
Probabilistic Robotics
Probabilistic Sensor Models
Beam-based Model
Likelihood Fields Model
Feature-based Model
City College of New York
Beam-based Proximity Model
Measurement noise
0
zexp
Phit ( z | x, m)  
Unexpected obstacles
zmax
1
e
2b
1 ( z  zexp )

2
b
0
2
zexp
  e  z
Punexp ( z | x, m)  
 0
Gaussian Distribution
zmax
z  zexp 

otherwise
Exponential Distribution
City College of New York
4
Beam-based Proximity Model
Random measurement
0
zexp
Prand ( z | x, m)  
zmax
1
zmax
Max range
0
zexp
zmax
 1 if z  z max
Pmax ( z | x, m)  
0 otherwise
Uniform distribution
Point-mass distribution
City College of New York
5
Resulting Mixture Density
  hit 


 unexp 
P ( z | x, m)  
 max 


 
 rand 
T
 Phit ( z | x, m) 


 Punexp ( z | x, m) 

Pmax ( z | x, m) 


 P ( z | x, m) 
 rand

Weighted average, and  hit   un exp   max   rand  1
How can we determine the model parameters?
  { hit ,  un exp ,  max ,  rand ,  , }
System identification method: maximum likelihood estimator (ML estimator)
City College of New York
6
Likelihood Fields Model
• Project the end points of a sensor scan Zt into the
global coordinate space of the map
 x ztk

 y zk
 t
  x   cos
  
  y   sin 

cos(   ksens ) 
 sin   x ksens 
k
  z t 


cos  y ksens 
 sin(   ksens ) 
• Probability is a mixture of …
– a Gaussian distribution with mean at distance to closest
obstacle,
– a uniform distribution for random measurements, and
– a small uniform distribution for max range measurements.
p( ztk xt , m)  hit  phit   max  pmax   rand  prand
• Again, independence between different components is
7
assumed.
City College of New York
Likelihood Fields Model
 x ztk

 y zk
 t
  x   cos
  
  y   sin 

cos(   ksens ) 
 sin   x ksens 
k
  z t 


cos  y ksens 
 sin(   ksens ) 
Distance to the nearest obstacles. Max range reading ignored
City College of New York
8
Example
Example environment
P(z|x,m)
Likelihood field
The darker a location, the less likely
it is to perceive an obstacle
Sensor
probability
O1 O2
O3
Zmax
City College of New York
Oi : Nearest point to
obstacles
9
Feature-Based Measurement Model
•
•
•
 rt1   rt 2 
 1  2
1
2
f ( zt )  { f t
f t }  { t  ,  t ,}
 s1   s 2 
 t  t 
Sensors that measure range, bearing, & a signature (a numerical value,
e.g., an average color)
Conditional independence between features
Feature vector is abstracted from the measurement:
p( f ( zt ) xt , m)   p(rti , ti , sti xt , m)
i
• Feature-Based map: m  {m1 , m2 , } with m j  {m j , x , m j , y , s j }
i.e., a location coordinate in global coordinates & a signature
• Robot pose:
i
 (m  x) 2  (m  y ) 2
 2 


r
t
j,x
j, y

 r 
T
 i
xt  {x y  }
 t    a tan 2(m j , y  y, m j , x  x)   )     2 


• Measurement model:
 s i  


s

j
 t 
  s2 
T

2
r
Zero-mean Gaussian error variables with standard deviations
City College of New York
r
10
Probabilistic Robotics
Probabilistic Motion Models
City College of New York
Odometry Model
• Robot moves from
x , y ,
to
x ' , y ' , '
.
u   rot 1,  rot
. 2 ,  trans
• Odometry information
 trans  ( x ' x ) 2  ( y ' y ) 2
 rot 1  atan2( y' y, x ' x )  
 rot 2
 rot 2   '   rot 1
x , y ,
 rot 1
 trans
x ' , y ' , '
Relative motion information, “rotation”  “translation”  “rotation”
City College of New York
Noise Model for Odometry
• The measured motion is given by the true
motion corrupted with independent noise.
ˆ    
rot 1
rot 1
1 | ro t1| 2 | tra n s|
ˆtrans   trans  
3
| tra n s| 4 | ro t1  ro t2 |
ˆrot 2   rot 2  
1 | ro t2 | 2
  ( x) 
2

1
2
2
e
1 x2
2 2
| tra n s|
0 if | x | 6 2

  2 ( x)   6 2  | x |

6 2

City College of New York
How to
calculate :
p( xt ut , xt 1 )
Calculating the Posterior
An initial pose X
Given xt, xt-1, and u
A hypothesized final pose X
t-1
1.
2.
3.
4.
t
A pair of poses u obtained from odometry
Algorithm motion_model_odometry (xt, xt-1, u)
u  xt 1
 trans  ( x ' x )  ( y ' y )
 rot 1  atan2( y' y, x ' x )  
 rot 2   '   rot 1
2
2
xt 
T
odometry values (u)
xt 1  ( x y  )T
xt  ( x  y   )T
10.
ˆtrans  ( x' x) 2  ( y ' y ) 2
values of interest (xt-1, xt)
ˆrot 1  atan2( y' y, x'x) 
ˆrot 2   '  ˆrot 1
p1  prob( rot1  ˆrot1,1 | ˆrot1 | 2ˆtrans )
p2  prob( trans  ˆtrans ,3ˆtrans  4 (| ˆrot1 |  | ˆrot2 |))
p  prob(  ˆ , | ˆ |  ˆ )
11.
return p1 · p2 · p3
5.
6.
7.
8.
9.
3
rot 2
rot 2
1
rot 2
p( xt ut , xt 1 )
2 trans
prob(a, b)
Implements an error distribution over a
with zero mean and standard deviation b
City College of New York
14
Application
• Repeated application of the sensor model for
short movements.
• Typical banana-shaped distributions obtained for
2d-projection of 3d posterior.
p(xt| u, xt-1)
x’
u
x’
u
Posterior distributions of the robot’s pose upon executing the motion command
illustrated by the solid line. The darker a location, the more likely it is.
City College of New York
Velocity-Based Model
v 
control u   
 
City College of New York
Rotation r  v
radius

16
Equation for the Velocity Model
Instantaneous center of curvature (ICC) at (xc , yc)
T
Initial pose xt 1  x y  
x  xc  r sin 
y  yc  r cos
Keeping constant speed, after ∆t time interval,
ideal robot will be at x  x y  T
t
 x    xc  r sin(  t ) 
 x    r sin   r sin(  t ) 
  

  


 y    y c  r cos(  t )    y    r cos  r cos(  t ) 
   

  

  t
t
  

  

City College of New York
Corrected,
-90
17
Velocity-based Motion Model
With xt 1
vˆt
 vˆt

  sin   sin(  ˆ t t ) 
ˆ t

 x '   x   ˆ t
 '     vˆt
vˆt

y

y


cos


cos(  ˆ t t ) 
    
ˆ t
 '     ˆ t

    
ˆ t t





T
 x y   and xt  x ' y '  ' Tare the state vectors at time t-1 and t respectively
The true motion is described by a translation velocity vˆ t and a rotational velocity
T
Motion Control ut  (vt t ) with additive Gaussian noise
 ( v    ) 2
M t  

 vˆt   vt    (1 vt  2 t ) 2   vt 
     
    (0, M t )



 ˆ t   t    3 vt  4 t 2   t 
Circular motion assumption leads to degeneracy ,
2 noise variables v and w  3D pose
Assume robot rotates ˆ when arrives at its final pose
City College of New York
1
t
2
0
t
ˆ t

0

( 3 vt   4  t ) 2 

     ˆt  t
ˆ   5 v 6 
18
Velocity-based Motion Model
Motion Model:
vˆt
 vˆt

ˆ

sin


sin(




t
)


t
'
ˆ
ˆ
t

 x   x   t
 '     vˆ
vˆt

t
ˆ
y

y


cos


cos(




t
)
    
t

ˆ
ˆ


 '    
t
t



 
ˆ t t  ˆt






 vˆt   vt    (1 vt  2 t ) 2 
     
 ˆ t   t     3 vt  4 t 2 
ˆ   
5
v 6 
1 to 4 are robot-specific error parameters
determining the velocity control noise
5 and 6 are robot-specific error parameters determining the
standard deviation of the additional rotational noise
City College of New York
19
Probabilistic Motion Model
How to compute p( xt ut , xt 1 ) ?
Center of circle:
Move with a fixed velocity during ∆t
resulting in a circular trajectory from
T
T
xt 1  x y   to xt  x y
with
Radius of the circle:
r *  ( x  x * ) 2  ( y  y * ) 2  ( x  x * ) 2  ( y   y * ) 2
Change of heading direction:
  a tan2( y  y * , x  x* )  a tan2( y  y * , x  x* )
dist r *  
vˆ 

t
t
  

ˆ


 ˆ

t
t
City College of New York

(angle of the final rotation)
20
Posterior Probability for Velocity Model
Center of circle
Radius of the circle
Change of heading direction
Motion error: verr ,werr and ˆ
City College of New York
21
Examples (velocity based)
City College of New York
Map-Consistent Motion Model
p( x | u, x' )

p( x | u, x' , m)
Obstacle grown
by robot radius
Map free estimate
of motion model
p( x | u , x' )
“consistency” of
pose in the map
p( x | m)
“=0” when placed
in an occupied cell
Approximation: p( x | u, x' , m)   p( x | m) p( x | u, x' )
City College of New York
Summary
• We discussed motion models for odometry-based and
velocity-based systems
• We discussed ways to calculate the posterior probability
p(x| x’, u).
• Typically the calculations are done in fixed time intervals
t.
• In practice, the parameters of the models have to be
learned.
• We also discussed an extended motion model that takes
the map into account.
City College of New York
24
Probabilistic Robotics
Localization
“Using sensory information to locate the robot in its environment is
the most fundamental problem to providing a mobile robot with
autonomous capabilities.”
[Cox ’91]
City College of New York
Localization, Where am I?
?
• Given
– Map of the environment.
– Sequence of measurements/motions.
• Wanted
– Estimate of the robot’s position.
• Problem classes
– Position tracking (initial robot pose is known)
– Global localization (initial robot pose is unknown)
– Kidnapped robot problem (recovery)
City College of New York
26
Markov Localization
p( x1:t | z1:t , u1:t , m)
City College of New York
27
Bayes Filter Revisit
Bel ( xt )   P( zt | xt )  P( xt | ut , xt 1 ) Bel ( xt 1 ) dxt 1
• Prediction (Action)
bel ( xt )   p ( xt | ut , xt 1 ) bel ( xt 1 ) dxt 1
• Correction (Measurement)
bel( xt )   p( zt | xt ) bel( xt )
City College of New York
EKF Linearization
First Order Taylor Expansion
• Prediction:
g (ut , t 1 )
g (ut , xt 1 )  g (ut , t 1 ) 
( xt 1  t 1 )
xt 1
g (ut , xt 1 )  g (ut , t 1 )  Gt ( xt 1  t 1 )
• Correction:
h( t )
h( xt )  h( t ) 
( xt  t )
xt
h( xt )  h( t )  H t ( xt  t )
City College of New York
29
EKF Algorithm
1.
Extended_Kalman_filter( t-1, St-1, ut, zt):
2.
3.
4.
Prediction:
t  g (ut , t 1 )
5.
6.
7.
8.
Correction:
Kt  St HtT (Ht St HtT  Qt )1
t  t  Kt ( zt  h(t ))
9.
Return t, St
 t  At t 1  Bt ut
St  Gt St 1GtT  Rt
St  At St 1 AtT  Rt
St  ( I  Kt Ht )St
h( t )
Ht 
xt
City College of New York
Kt  St CtT (Ct St CtT  Qt )1
t   t  Kt ( zt  Ct  t )
St  (I  Kt Ct )St
g (ut , t 1 )
Gt 
xt 1
30
1.
EKF_localization ( t-1, St-1, ut, zt, m):
Prediction:
3. Gt 

g (ut , t 1 )
xt 1
 x'

 t 1, x
 y '
 
 t 1, x
  '

 t 1, x
g (ut , t 1 )
ut
 x'

 vt
 y '
 
 vt
  '
 v
 t
5.
Vt
6.
 1 | vt |  2 | t |2
M t  
0

x'
t 1, y
y '
t 1, y
 '
t 1, y
x' 
t 1, 
y ' 
 Jacobian of g w.r.t location
t 1, 
 ' 

t 1, 
x' 

t 
y ' 

t 
 ' 
t 


2
 3 | vt |  4 | t | 
t  g (ut , t 1 )
8. St  Gt St 1GtT  Vt MtVtT
0
7.
City College of New York
Jacobian of g w.r.t control
Motion noise covariance
Matrix from the control
Predicted mean
Predicted covariance
31
Velocity-based Motion Model
With xt 1  x y
vˆt
 vˆt

  sin   sin(  ˆ t t ) 
ˆ t

 x '   x   ˆ t
 '     vˆt
vˆt

 y    y     cos  cos(  ˆ t t ) 
ˆ t
 '     ˆ t



 
ˆ t t






 T and xt  x ' y '  ' Tare the state vectors at time t-1 and t respectively
The true motion is described by a translation velocity
vˆ t and a rotational velocity ˆ t
T
Motion Control ut  (vt t ) with additive Gaussian noise
 vˆt   vt    (1 vt  2 t ) 2   vt 
     
    (0, M t )



 ˆ t   t    3 vt  4 t 2   t 
 ( 1 vt   2  t ) 2
M t  
0

City College of New York

0

( 3 vt   4  t ) 2 
32
Velocity-based Motion Model
Motion Model:
vt
 vt


sin


sin(




t
)


t
'
t

 x   x   t
 '     vt
vt

 y    y     cos  cos(   t t )   N (0, Rt )
t
 '      t

    
 t t





xt  g (ut , xt 1 )  N (0, Rt )
g (ut ,  t 1 )
g (u t , xt 1 )  g (u t ,  t 1 ) 
( xt 1   t 1 )
xt 1
g (u t , xt 1 )  g (u t ,  t 1 )  Gt ( xt 1   t 1 )
City College of New York
33
Velocity-based Motion Model
v
 vt

  sin   t sin(   t t ) 
t

 x '   x   t
 '     vt
v

 y    y     cos  t cos(   t t )   N (0, Rt )
t
 '      t



 


t
t






 x '

  t 1, x
g (u t ,  t 1 )  y '
Gt ( xt 1 ,  t 1 ) 

xt 1
  t 1, x
  

  t 1, x
x '
 t 1, y
y 
 t 1, y
 
 t 1, y
x ' 

 t 1, 
y  

 t 1, 
  

 t 1, 
City College of New York
x 
Derivative of g
along x’ dimension,
w.r.t. x at  t 1
 t 1, x
Jacobian of g w.r.t location
34
Velocity-based Motion Model
v
Mapping between the motion noise in control
 vt

  sin   t sin(   t t ) 
t

space to the motion noise in state space
 x '   x   t
 '     vt
v

 y    y     cos  t cos(   t t )   N (0, Rt )
t
 '      t



 


t
t






 x' x' 


Jacobian of g w.r.t control
 vt t 
 y ' y ' 
g (ut , t 1 )
Vt 
 

Derivative of g w.r.t. the motion parameters,
ut

v


t 
 t
evaluated at u t and  t 1
  '  ' 
 v  
t 
 t
vt (sin   sin(   t t )) vt cos(   t t )t 
 sin   sin(   t t )



2





t
t
t
 cos  cos(   t t )
v (cos  cos(   t t )) vt sin(   t t )t 

 t


2



t
t
t





t
0



St  Gt St 1GtT  Vt MtVtT
City College of New York
35
1.
EKF_localization ( t-1, St-1, ut, zt, m):
Correction:
3.
zˆt
2
2


mx  t , x   my  t , y  


 atan2m   , m      
y
t,y
x
t,x
t , 


h( t , m)
xt
5.
Ht
6.
  r2 0 

Qt  
2
 0 r 
 rt


  t,x
 t

 t , x
rt
t , y
t
t , y
Predicted measurement mean
rt
t ,
t
t ,






Jacobian of h w.r.t location
7.
St  Ht St HtT  Qt
Pred. measurement covariance
8.
Kt  St HtT St1
Kalman gain
9.
t  t  Kt ( zt  zˆt )
Updated mean
10. St  I  Kt Ht St
Updated covariance
City College of New York
36
Feature-Based Measurement Model
•
Jacobian of h w.r.t location
 2 
 rti   (m j , x  x) 2  (m j , y  y ) 2
 r 
 i
 t    a tan 2(m j , y  y, m j , x  x)   )     2 


 s i  


s

j
 t 
  s2 
zti  h( xt , j, m)  N (0, Qt )
h( t )
h( xt )  h( t ) 
( xt   t )
xt
Ht

h( t , m)
xt
 rt


  t,x
 t

 t , x
rt
t , y
t
t , y
rt
t ,
t
t ,






j  Cti Is the landmark that corresponds to the measurement of
City College of New York
zti
37
EKF Localization
with known
correspondences
City College of New York
38
EKF Localization
with unknown
correspondences
Maximum likelihood estimator
City College of New York
39
EKF Prediction Step
Initial estimate is represented by
the ellipse centered at  t 1
Small noise in translational & rotation
High translational noise
Moving on a circular arc of 90cm & turning
45 degrees to the left, the predicted position
is centered at  t
High rotational noise
High noise in both translation & rotation
City College of New York
40
EKF Measurement Prediction Step
Measurement Prediction
True robot (white circle) & the
observation (bold line circle)
Innovations (white arrows) : differences
between observed & predicted
measurements
City College of New York
41
EKF Correction Step
Resulting correction
Measurement Prediction
Update mean estimate & reduce
position uncertainty ellipses
City College of New York
42
Estimation Sequence (1)
EKF localization with an accurate landmark detection sensor
Dashed line: estimated robot trajectory
Solid line: true robot motion
Dashed line: corrected robot trajectory
Uncertainty ellipses: before (light gray)
& after (dark gray) incorporating
43
landmark detection
City College of New York
Estimation Sequence (2)
EKF localization with a less accurate landmark detection sensor
Uncertainty ellipses: before (light gray)
& after (dark gray) incorporating
landmark detection
City College of New York
44
Comparison to Ground Truth
City College of New York
45
UKF Localization
?
• Given
– Map of the environment.
– Sequence of measurements/motions.
• Wanted
– Estimate of the robot’s position.
• UKF localization
City College of New York
46
Unscented Transform
Sigma points
Weights
 
w 
0
i   
0
m

( n   )S

i

n
wmi  wci 
w 
0
c
1
2(n   )

n
 (1   2   )
for i  1,...,2n
Pass sigma points through nonlinear function
 i  g( i )
For n-dimensional Gaussian
λ is scaling parameter that determine how far
the sigma points are spread from the mean
If the distribution is an exact Gaussian, β=2 is
the optimal choice.
Recover mean and covariance
2n
 '   wmi  i
i 0
2n
S'   wci ( i   )( i   ) T
i 0
City College of New York
47
UKF_localization ( t-1, St-1, ut, zt, m):
Prediction:
 1 | vt |  2 | t |2
M t  
0



2
 3 | vt |  4 | t | 
0
Motion noise
  r2 0 

Qt  
2
0

r 

Measurement noise
ta1  tT1
Augmented state mean

 S t 1

S ta1   0
 0


0 0T 0 0T 
0
Mt
0
0

0
Qt 
Augmented covariance
 ta1  ta1 ta1   Sta1
tx  g ut  tu , tx1 
ta1   Sta1

Sigma points
Prediction of sigma points
2L
t   wmi  ix,t
i 0
2L

Predicted mean

St   w   t   t
i 0
i
c
x
i ,t
x
i ,t

T
Predicted covariance
City College of New York
48
UKF_localization ( t-1, St-1, ut, zt, m):
Correction:
t  h tx    tz
The predicted robot locations  tx are used
to generate the measurement sigma points
Measurement sigma points
2L
zˆt   wmi i ,t
Predicted measurement mean
i 0
2L


St   w i ,t  zˆt i ,t  zˆt
i 0
S
i
c
2L
x, z
t



T
  w   t i ,t  zˆt
i 0
i
c
Kt  Stx, z St
x
i ,t
1
Pred. measurement covariance

T
Cross-covariance
Kalman gain
t  t  Kt ( zt  zˆt )
Updated mean
St  St  Kt St K
Updated covariance
T
t
City College of New York
49
UKF Prediction Step
High rotational noise
Moving on a circular arc of 90cm & turning
45 degrees to the left, the predicted position
is centered at 
t
High translational noise
High noise in both translation & rotation
City College of New York
50
UKF Measurement Prediction Step
Measurement Prediction
Predicted Sigma points
City College of New York
51
UKF Correction Step
Resulting correction
Measurement Prediction
City College of New York
52
Estimation Sequence
EKF
UKF
Robot path estimated with different techniques, with UKF being slightly closer
City College of New York
53
Prediction Quality
EKF
UKF
Robot moves on a circle, estimates based
on EKF prediction, & UKF prediction
City College of New York
54
Kalman Filter-based System
• [Arras et al. 98]:
• Laser range-finder and vision
• High precision (<1cm accuracy)
[Courtesy of Kai Arras]
City College of New York
55
Multihypothesis
Tracking
City College of New York
56
Localization With MHT
MHT: Multi-Hypothesis Tracking filter
• Belief is represented by multiple hypotheses
• Each hypothesis is tracked by a Kalman filter
• Additional problems:
• Data association: Which observation corresponds to which
hypothesis?
• Hypothesis management: When to add / delete hypotheses?
• Huge body of literature on target tracking, motion
correspondence etc.
City College of New York
57
MHT: Implemented System (1)
• Hypotheses are extracted from Laser Range Finder (LRF) scans
• Each hypothesis has probability of being the correct one:
Hi  { xˆ i, Si , P (Hi )}
• Hypothesis probability is computed using Bayes’ rule
P( s | H i ) P( H i )
P( H i | s) 
P( s)
• Hypotheses with low probability are deleted.
• New candidates are extracted from LRF scans.
C j  {z j , Rj }
City College of New York
[Jensfelt et al. ’00]
58
MHT: Implemented System (2)
Courtesy of P. Jensfelt and S. Kristensen
City College of New York
59
MHT: Implemented System (3)
Example run
# hypotheses
P(Hbest)
Map and trajectory
Courtesy of P. Jensfelt and S. Kristensen
#hypotheses vs. time
City College of New York
60
Probabilistic Robotics
SLAM
City College of New York
SLAM Problem : Chicken or Egg
Fundamental problems for localization and mapping
 The task of SLAM is to build a map while estimating the
pose of the robot relative to this map.
 Without a map, robot cannot localize itself
 Without knowing its location, robot cannot build a map
 Which needed to be done first? Localization or mapping?
City College of New York
62
The SLAM Problem
A robot is exploring an unknown, static environment.
Given:
– The robot’s controls (U1:t)
– Observations of nearby features (Z1:t)
Estimate:
– Map of features (m)
– Pose / Path of the robot (xt)
City College of New York
63
Why is SLAM a hard problem?
Uncertanties
• Error
in pose
• Error in observation
• Error in mapping
• Error accumulated
City College of New York
64
Why is SLAM a hard problem?
SLAM: robot path and map are both unknown
Robot path error correlates errors in the map
City College of New York
65
Why is SLAM a hard problem?
Robot pose
uncertainty
• In the real world, the mapping between
observations and landmarks is unknown
• Picking wrong data associations can have
catastrophic consequences
City College of New York
66
Data Association Problem
• A data association is an assignment of observations
to landmarks
• In general there are more than
(n observations, m landmarks) possible associations
• Also called “assignment problem”
City College of New York
67
Nature of the SLAM Problem
 Continuous
component
Location of objects in
the map
Robot’s own pose
 Discrete
component
Correspondence
reasoning
Object is the same or not
City College of New York
68
SLAM:
Simultaneous Localization and Mapping
• Full SLAM:
Estimates entire path and map!
p( x1:t , m | z1:t , u1:t )
Estimates
Given
Entire pose (x1:t) and map (m)
Previous knowledge (Z1:t-1, U1:t-1)
Current measurement (Zt, Ut)
City College of New York
69
Graphical Model of Full SLAM:
p( x1:t , m | z1:t , u1:t )
Compute a joint posterior over the whole
path of robot and the map
City College of New York
70
SLAM:
Simultaneous Localization and Mapping
• Online SLAM:
p( xt , m | z1:t , u1:t )
Estimates
Given
Most recent pose (xt) and map (m)
Previous knowledge (Z1:t-1, U1:t-1)
Current measurement (Zt, Ut)
Estimates most recent pose and map!
City College of New York
71
Graphical Model of Online SLAM:
Estimate a posterior over the current
robot pose, and the map
p( xt , m | z1:t , u1:t )      p( x1:t , m | z1:t , u1:t ) dx1 dx2 ...dxt 1
Integrations typically done one at a time
City College of New York
72
SLAM with Extended Kalman Filter
• Pre-requisites
– Maps are feature-based (landmarks)
small number (< 1000)
– Assumption - Gaussian Noise
– Process only positive sightings
No landmark = negative
Landmark = positive
City College of New York
73
EKF-SLAM with known correspondences
Correspondence
Data association problem
Landmarks can’t be
uniquely identified
Correspondence variable
True identity of
observed feature
(Cit)
between feature (fit) and real landmark

f t  rt
i
i

i
t
S

i T
t
Make correspondence variables explicit
p( xt , m, ct | z1:t , u1:t )
p( x1:t , m, c1:t | z1:t , u1:t )
City College of New York
74
EKF-SLAM with known correspondences
Signature
Numerical value (average color)
Characterize type of landmark
(integer)
Multidimensional vector
(height and color)
City College of New York
75
EKF-SLAM with known correspondences
Similar development to EKF localization
Diff  robot pose + coordinates of all landmarks
Combined state vector
 xt 
yt     x
 m
y 
m1, x
p( yt | z1:t , u1:t )
m1, y
S1 ... mN , x
(3N + 3)
mN , y
Online posterior
City College of New York
SN 
T
76
Mean
Motion update
Covariance
Test for new landmarks
Initialization
of elements
Expected measurement
Iteration through
measurements
Filter is updated
City College of New York
77
EKF-SLAM with known correspondences
Observing a landmark
improves robot pose estimate
eliminates some uncertainty
of other landmarks
Improves position estimates of
the landmark + other landmarks
We don’t need to model past poses
explicitly
City College of New York
78
Example
City College of New York
79
EKF-SLAM with known correspondences
Example:
• Uncertainty of landmarks are mainly due to
robot’s pose uncertainty (persist over time)
Estimated location of landmarks are correlated
City College of New York
80
EKF-SLAM with unknown correspondences
• No correspondences for landmarks
• Uses an incremental maximum likelihood (ML) estimator
Determines most likely value of the correspondence variable
Takes this value for granted later on
City College of New York
81
EKF-SLAM with unknown correspondences
Mean
Motion update
Covariance
Hypotheses of
new landmark
City College of New York
82
City College of New York
83
General Problem
Gaussian noise assumption
Unrealistic
Spurious measurements
Fake landmarks
Outliers
Affect robot’s localization
City College of New York
84
Solutions to General Problem
Provisional landmark list
New landmarks do not augment the map
Not considered to adjust robot’s pose
Consistent observation
City College of New York
regular map
85
Solutions to General Problem
Landmark Existence Probability
Landmark is observed
Observable variable (o) increased by fixed value
Landmark is NOT observed when it should
Observable variable decreased
Removed from map when (o) drops below
threshold
City College of New York
86
Problem with Maximum Likelihood (ML)
Once ML estimator determines likelihood of correspondence, it takes
value for granted
always correct
Makes EKF susceptible to landmark confusion
Wrong results
City College of New York
87
Solutions to ML Problem
Spatial arrangement
Greater distance between landmarks
Less likely confusion will exist
Trade off:
few landmarks
harder to localize
Little is known about optimal density of
landmarks
Signatures
Give landmarks a very perceptual distinctiveness
(e,g, color, shape, …)
City College of New York
88
EKF-SLAM Limitations
• Selection of appropriate landmarks
• Reduces sensor reading utilization to presence or absence of those
landmarks
Lots of sensor data is discarded
• Quadratic update time
Limits algorithm to scarce maps (< 1000
features)
• Low dimensionality of maps
harder data association
problem
City College of New York
89
EKF-SLAM Limitations
• Fundamental Dilemma of EKF-SLAM
It might work well with dense maps (millions of features)
It is brittle with scarce maps
BUT
It needs scarce maps because of complexity of the
algorithm (update process)
City College of New York
90
SLAM Techniques
• EKF SLAM  (chapter 10)
• Graph-SLAM  (chapter 11)
• SEIF (sparse extended information
filter)  (chapter 12)
• Fast-SLAM  (chapter 13)
City College of New York
91
Graph-SLAM
• Solves full SLAM problem
• Represents info as a graph of soft constraints
• Accumulates information into its graph without resolving it
(lazy SLAM)
• Computationally cheap
• At the other end of EKF-SLAM
Process information right
away (proactive SLAM)
Computationally expensive
City College of New York
92
Graph-SLAM
• Calculates posteriors over robot path (not incremental)
• Has access to the full data
• Uses inference to create map using stored data
Offline algorithm
City College of New York
93
Sparse Extended Information Filter (SEIF)
• Implements a solution to online SLAM problem
• Calculates current pose and map (as EKF)
• Stores information representation of all knowledge (as GraphSLAM)
Runs Online and is computationally efficient
• Applicable to multi-robot SLAM problem
City College of New York
94
FastSLAM Algorithm
• Particle filter approach to the SLAM problem
• Maintain a set of particles
• Particles contain a sampled robot path and a map
• The features of the map are represented by own local Gaussian
• Map is created as a set of separate Gaussians
Map features are conditionally independent
given the path
Factoring out the path (1 per particle)
Map feature become independent
Eliminates the need to maintain
correlation among them
City College of New York
95
FastSLAM Algorithm
• Updating in FastSLAM
Sample new pose
update the observed features
• Update can be performed online
• Solves both online and offline SLAM problem
• Instances
Feature-based maps
Grid-based algorithm
City College of New York
96
Approximations for SLAM Problem
• Local submaps
[Leonard et al.99, Bosse et al. 02, Newman et al. 03]
• Sparse links (correlations)
[Lu & Milios 97, Guivant & Nebot 01]
• Sparse extended information filters
[Frese et al. 01, Thrun et al. 02]
• Thin junction tree filters
[Paskin 03]
• Rao-Blackwellisation (FastSLAM)
[Murphy 99, Montemerlo et al. 02, Eliazar et al. 03, Haehnel et al. 03]
City College of New York
97
Thank You
98
City College of New York