Page 252 - Introduction to Autonomous Mobile Robots

237
Mobile Robot Localization
the best matching candidate is selected or multiple hypotheses are tracked. Observations
that do not fall in the validation gate are simply ignored for localization. Such observations
could have resulted from objects not in the map, such as new objects (e.g., someone places
a large box in the hallway) or transient objects (e.g., humans standing next to the robot may
form a line feature). One approach is to take advantage of such unmatched observations to
populate the robot’s map.

5. Estimation: applying the Kalman filter. Next we compute the best estimate
(
ˆ p k + 1 k + 1) of the robot’s position based on the position prediction and all the observa-
tions at time k + 1 . To do this position update, we first stack the validated observations
(
z k + 1) into a single vector to form z k +( 1) and designate the composite innovation
j
(
∇
vk + 1) . Then we stack the measurement Jacobians h i for each validated measurement
together to form the composite Jacobian h∇ and the measurement error (noise) vector
Σ k +( 1) = diag Σ Ri ( k + 1)] . We can then compute the composite innovation covari-
[
,
R
ance Σ ( k + 1) according to equation (5.51) and by utilizing the well-known result
IN
[3] that the Kalman gain can be written as
T
1
–
(
⋅
Kk + 1) = Σ k +( 1 k) ∇ h ⋅ Σ ( k + 1) (5.53)
p
IN
we can update the robot’s position estimate
(
(
(
⋅
p k +( 1 k + 1) = p ˆ k + 1 k) + Kk + 1) vk + 1) (5.54)
ˆ
with the associated variance
T
(
(
⋅
⋅
(
(
–
Σ k + 1 k + 1) = Σ k + 1 k) K k + 1) Σ ( k + 1) K k + 1) (5.55)
IN
p
p
For the 1D case and with h z p k +(,( t ˆ 1 k)) = z t we can show that this formula corre-
i
sponds to the 1D case derived earlier
Equation (5.53) is simplified to
σ k +( 1 k) σ k +( 1 k)
2
2
p
p
(
Kk + 1) = ---------------------------- = ---------------------------------------------------------- (5.56)
(
2
2
σ ( k + 1) σ k +( 1 k) + σ k + 1)
2
R
p
IN
corresponding to equation (5.45), and equation (5.54) simplifies to