Page 353 - Autonomous Mobile Robots

Map Building and SLAM Algorithms 343

with

H k, j = ∂h k, j (9.7)
B
∂x
k|k−1 (ˆ x B )
k|k−1
The discrepancy between the observation i and the predicted observation of
map feature j is measured by the innovation term ν k,ij , whose value and
covariance are:

ν k,ij = z k,i − h k, j (ˆ x B )
k|k−1
(9.8)
B
S k,ij = H k, j P H T + R k,i
k k, j
The measurement can be considered corresponding to the feature if the
Mahalanobis distance D 2 [28] satisfies:
k,ij
−1
T
D 2 = ν k,ij k,ij k,ij <χ 2 (9.9)
ν
S
k,ij d,1−α
where d = dim(h k, j ) and 1 − α is the desired confidence level, usually 95%.
This test, denominated individual compatibility (IC), applied to the predicted
state, can be used to determine the subset of map features that are compat-
ible with a measurement, and is the basis for some of the most popular data
association algorithms discussed later in this chapter.
An often overlooked fact, that will be discussed in more detail in Section 9.3,
is that all measurements should be jointly compatible with their corres-
ponding features. In order to establish the consistency of a hypothesis H k ,
:
measurements can be jointly predicted using function h H k

 (x B ) 
hhhh h j 1 k|k−1
(x B  . .  (9.10)
k|k−1 ) =  . 
h H k
(x B )
h j m
k|k−1
which can also be linearized around the current estimate to yield:
 
H j 1
B
(x B (ˆ x B (x − ˆ x B );  . 
.
h H k k|k−1 ) h H k k|k−1 ) + H H k k k|k−1 H H k =  . 
H j m
(9.11)

© 2006 by Taylor & Francis Group, LLC

FRANKL: “dk6033_c009” — 2006/3/31 — 16:43 — page 343 — #13