Landmark Map
• Locations and uncertainties of n landmarks,
with respect to a specific frame of reference.
L11: Landmark Mapping
– World frame: fixed origin point
– Robot frame: origin at the robot
CS 344R/393R: Robotics
Benjamin Kuipers
• Problem: how to combine new information
with old to update the map.
A Spatial Relationship is a Vector
Rotation Matrix
• A spatial relationship holds between two
poses: the position and orientation of one, in
the frame of reference of the other.
B
x AB
#x AB &
% (
= %y AB (
%$" AB ('
x AB = [ x AB
A
$cos " #sin "'$ xG ' $ x B '
&
)& ) = & )
%sin " cos " (% yG ( % y B (
Inverse:
!
y AB
" AB ]
$ cos " sin " '
&
)
%#sin " cos "(
G
T
"
B
!
!
!
!
Uncertain Spatial Relationships
Relation to Affine Transformations
• The spatial
relationship
• is equivalent to
the affine
!
transformation
x AB = [ x AB
y AB
$cos " AB
&
&sin " AB
&% 0
#sin " AB
cos " AB
0
" AB ]
xˆ = E[x]
T
C(x) = E[(x " xˆ )(x " xˆ ) T ]
x AB '
)
y AB )
1 )(
• An uncertain spatial relationship is described
by a probability distribution of vectors, with a
! mean and a covariance matrix.
#xˆ &
% (
xˆ = %yˆ (
%$"ˆ('
• Rotation plus
translation
!
!
$ " x2
&
C(x) = &" xy
&%" x#
" xy
" y2
" y#
" x# '
)
" y# )
" #2 )(
!
1
A Map with n Landmarks
• Concatenate n vectors into one big state vector
" x1 %
$ '
x
x = $ 2'
$M'
$ '
#x n &
" xˆ 1 %
$ˆ '
x2
xˆ = $ '
$M'
$ˆ '
#x n &
Example
•
•
•
•
The robot senses object #1.
The robot moves.
The robot senses a different object #2.
Now the robot senses object #1 again.
• And one big 3n×3n covariance matrix.
!
" C(x1 )
C(x1,x 2 )
!
$
C(x
,x
)
C(x 2 )
2
1
C(x) = $
$
M
M
$
#C(x n ,x1 ) C(x n ,x 2 )
L C(x1,x n )%
'
L C(x 2 ,x n )'
'
O
M
'
L C(x n ) &
• After each step, what does the robot know
(in its landmark map) about each object,
including itself?
!
Robot Senses Object #1 and Moves
Robot Senses Object #2
Robot Senses Object #1 Again
Updated Estimates After Constraint
2
Compounding
Compounding
x AC = [ x AC
x BC = [ x BC
x AB = [ x AB
y AB
" AB ]
y BC
" BC ]
T
x BC = [ x BC
x AB = [ x AB
!
y AB
" AB ]
y BC
" BC ]
T
T
!
!
!
Compounding
Computing Covariance
• Let xAB be the pose of object B in the frame
of reference of A.
(Sometimes written B A.)
• Given xAB and xBC, calculate xAC.
• Consider the linear mapping y = Mx+b
y = Mx + b
C(y) = C(Mx + b)
= E[(Mx + b " (Mxˆ + b)) (Mx + b " (Mxˆ + b)) T ]
= E[M(x " xˆ ) (M(x " xˆ )) T ]
= E[M (x " xˆ )(x " xˆ ) T M T ]
= M E[(x " xˆ )(x " xˆ ) T ]M T
= MC(x) M T
% x BC cos # AB $ y BC sin # AB + x AB (
'
*
x AC = x AB " x BC = 'x BC sin # AB + y BC cos # AB + y AB *
'&
*)
# AB + # BC
!
" AC ]
T
!
T
y AC
• Compute C(xAC) from C(xAB), C(xBC), and
C(xAB,xBC).
!
• Apply this to nonlinear functions by using
Taylor Series.
The Inverse Relationship
Inverse Relationship
• Let xAB be the pose of object B in the frame
of reference of A.
• Given xAB, calculate xBA.
x BA = (")x AB
x AB = [ x AB
y AB
" AB ]
T
x BA = [ x BA
y BA
" BA ]
T
$"x AB cos # AB " y AB sin # AB '
&
)
= & x AB sin # AB " y AB cos # AB )
&%
)(
"# AB
• Compute C(xBA) from C(xAB)
!
!
!
3
Composite Relationships
• Compounding combines relationships headto-tail: xAC = xAB ⊕ xBC
• Tail-to-tail combinations come from
observing two things from the same point:
xBC = (xAB) ⊕ xAC
• Head-to-head combinations come from two
observations of the same thing:
xAC = xAB ⊕ (xCB)
• They provide new relationships between
their endpoints.
EKF Update Equations
• Predictor step:
xˆ "k = f (xˆ k "1 ,uk )
Pk" = APk "1A T + Q
Merging Information
• An uncertain observation of a pose is
combined with previous knowledge using
the extended Kalman filter.
"
"
– Previous knowledge: x k , Pk
– New observation: zk, R
• Update: x =!x(new) ⊗ x(old)
• Can integrate dynamics as well.
Reversing and Compounding
• OBJ1R = (ROBOTW) ⊕ OBJ1W
•
= WORLDR ⊕ OBJ1W
• Kalman gain: K k = Pk"H T (HPk"H T + R)"1
• Corrector step:
xˆ k = xˆ "k + K k (z k " h(xˆ "k ))
Pk = (I" K kH)Pk"
Sensing Object #2
• OBJ2W = ROBOTW ⊕ OBJ2R
Observing Object #1 Again
• OBJ1W = ROBOTW ⊕ OBJ1R
4
Combining Observations (1)
• OJB1W = OJB1W(new) ⊗ OBJ1W(old)
• OJB1R = OJB1R(new) ⊗ OBJ1R(old)
Combining Observations (2)
• ROBOTW(new) = OJB1W ⊕ (OBJ1R)
• ROBOTW = ROBOTW(new) ⊗ ROBOTW(old)
Useful for Feature-Based Maps
• We’ll see this again when we study
FastSLAM.
5