Page 129 - Autonomous Mobile Robots

112 Autonomous Mobile Robots

∗
over the time interval t ∈[t k , t k+1 ] with initial condition x (t k ) =
ˆ x k|k . At the completion of the integration, let ˆ x k+1|k = x (t k+1 ).
∗
Along the solution x (t), compute
∗

∂f (x) ∗
F(t) = and G(t) = g(x , t), for t ∈[t k , t k+1 ]
∂x x=x ∗
2. Compute the state transition matrix k and compute the process
noise covariance matrix Q k . Predict the error state vector and error
covariance matrix:

δˆ x k+1|k = k δˆ x k|k = k 0 = 0 (3.39)
T
P k+1|k = k P k|k + Q k (3.40)
k
The reason that δˆ x k|k is set to 0 in (3.39) is clarified in the discussion
following (3.43).
3. Increment k = k + 1.
4. Linearize the measurement matrix at x (t k ) and calculate the EKF
∗
gain matrix:

∂h
H k = H(t k ) =
∂x
x=ˆ x k|k−1 (3.41)
T T −1
K k = P k|k−1 H [H k P k|k−1 H + R k ]
k k
5. Compute the error states using the residual measurements:
δˆ x k|k = δˆ x k|k−1 + K k [δy k − H k δˆ x k|k−1 ]
(3.42)
= K k δy k
where δˆ x k|k−1 is the error state vector estimated prior to the new
measurements, which by Equation (3.39) is zero.
6. Update the estimated states ˆ x k|k :

ˆ x k|k = ˆ x k|k−1 + δˆ x k|k (3.43)

Since the error state has been included in the state estimate, the error
has been corrected; therefore, the new best estimate of the error state
is zero. Therefore, δˆ x k|k must be set to zero: δˆ x k|k = 0.
7. Update the posterior error covariance matrix:

P k|k =[I − K k H k ]P k|k−1

© 2006 by Taylor & Francis Group, LLC

FRANKL: “dk6033_c003” — 2006/3/31 — 16:42 — page 112 — #14