Page 370 - Autonomous Mobile Robots

360 Autonomous Mobile Robots

to form the joint state vector x B we only need to transform the features of
F+E
the second map to reference B using the fact that F i = E j :

ˆ x
 B 
F
B  B E j 
ˆ x  ˆ x F i ⊕ ˆ x E 0 
ˆ x B = F =  .  (9.23)
F+E B  . 
ˆ x .
E  
ˆ x B ⊕ ˆ x E j
F i E m
The covariance P B of the joined map is obtained from the linearization of
F+E
Equation (9.23), and is given by:
B
T
E j T
P B = J F P J + J E P J
F+E F F E E

B B T
P P J 0 0
= F B F 1 T + E j T (9.24)
B
J 1 P J 1 P J 0 J 2 P J
F F 1 E 2
where
∂x B I
F+E
J F = B B E j =
∂x (ˆ x ,ˆ x ) J 1
F F E
∂x B 0
J E = F+E E j =
B
(ˆ x ,ˆ x ) J 2
∂x F E
E j
E
 B E j 
0 ... J 1⊕ ˆ x , ˆ x ... 0
F i E 0
 .
 
J 1 =  . . . . . . 
.
. 
 B E j 
0 ··· J 1⊕ ˆ x , ˆ x ... 0
F i E m
 B E j 
J 2⊕ ˆ x , ˆ x ··· 0
F i E 0
. . .
 
. . .
 
. . . 
J 2 = 
 E j 
B
0 ··· J 2⊕ ˆ x , ˆ x
F i E m
Obtaining vector ˆ x B with Equation (9.23) is an O(m) operation. Given
F+E
that the number of nonzero elements in J 1 and J 2 is O(m), obtaining matrix
2
P B with Equation (9.24) is an O(nm + m ) operation. Thus when n m,
F+E
map joining is linear with n.
© 2006 by Taylor & Francis Group, LLC
FRANKL: “dk6033_c009” — 2006/3/31 — 16:43 — page 360 — #30