Page 258 - Introduction to Autonomous Mobile Robots
P. 258
Mobile Robot Localization
r 243
model space
image space
No match!!
Wall was not ob-
served.
match j,i
r i
r j
-π α α 0 π
i j
Figure 5.33
Matching: The observations (thick) and measurement prediction (thin) are matched and the innova-
tion and its uncertainties are calculated.
(
T
⋅
v k +( 1) Σ – IN ij ( k + 1) v k + 1) ≤ g 2 (5.66)
⋅
1
,
ij
ij
with
(
,
(
v k +( 1) = [ z k + 1) – h z p k + 1 k))]
(
ˆ
t
ij
i
j
ˆ
α W α – W θ k + 1 k)
(
,
(
v k – 1) = j – ti
ij
r j W r – ( W x ˆ k + 1 k) cos ( W α ) + W y ˆ k + 1 k) sin ( W α ))
(
(
,
,
,
ti ti ti
(5.67)
T
(
⋅
⋅
Σ IN ij ( k + 1) = ∇h Σ k + 1 k) ∇h + Σ Ri ( k + 1) (5.68)
,
,
i
i
p
