Page 247 - Introduction to Autonomous Mobile Robots
P. 247
Chapter 5
232
The new, fused estimate of robot position provided by the Kalman filter is again subject
to a Gaussian probability density curve. Its mean and variance are simply functions of the
inputs’ means and variances. Thus the Kalman filter provides both a compact, simplified
representation of uncertainty and an extremely efficient technique for combining heteroge-
neous estimates to yield a new estimate for our robot’s position.
Dynamic estimation. Next, consider a robot that moves between successive sensor mea-
surements. Suppose that the motion of the robot between times k and k + 1 is described
by the velocity u and the noise w which represents the uncertainty of the actual velocity:
dx
------ = u + w (5.41)
dt
If we now start at time , knowing the variance σ 2 k of the robot position at this time and
k
knowing the variance σ 2 w of the motion, we obtain for the time k′ just when the measure-
ment is taken,
(
x ˆ k′ = x ˆ + ut k + 1 – t ) (5.42)
k
k
2
2
[
σ 2 k′ = σ + σ t k + 1 – t ] (5.43)
k
k
w
where
t = t ;
k ' k + 1
t and are the time in seconds at k + 1 and respectively.
k
t
k + 1 k
Thus x ˆ k′ is the optimal prediction of the robot’s position just as the measurement is
taken at time k + 1 . It describes the growth of position error until a new measurement is
taken (figure 5.27).
We can now rewrite equations (5.38) and (5.39) using equations (5.42) and (5.43).
x ˆ = x ˆ + K z ( – x ˆ )
k + 1 k′ k + 1 k + 1 k′
(
(
x ˆ k + 1 = [ x ˆ + ut k + 1 – t )] + K k + 1 z [ k + 1 – x ˆ – ut k + 1 – t )] (5.44)
k
k
k
k
2
[
σ 2 σ + σ t – t ]
2
k′
k +
k
1
k
w
K = ------------------- = -------------------------------------------------------- (5.45)
k + 1 2 2 2 2 2
[
σ + σ z σ + σ t k + 1 – t ] + σ z
k′
k
w
k