Page 122 - Autonomous Mobile Robots

Data Fusion via Kalman Filter 105

where by subdividing (3.13) into subintegrals and using (τ i+1 , τ 0 ) =
(τ i+1 , τ i ) (τ i , τ 0 ) we obtain
T T
Q(τ i , τ 0 ) = (τ i , τ i−1 )[GQ G τ i + Q(τ i−1 , τ 0 )] (τ i , τ i−1 ) (3.17)
w
for i = 1, ... , N with Q(τ 0 , τ 0 ) = 0.

Example 3.1 Since a common GPS measurement epoch uses t k = k, this
example considers computation of k and Q k over the unit interval t ∈[k, k+1)
where k is an integer. First, the unit interval is subdivided into N subintervals
of length dτ = 1/N sec. Each subinterval is [τ i , τ i+1 ) where τ i = k + idτ for
i = 0, ... , N. For small dτ,

(τ i+1 , τ i ) = (I + F(τ i )dτ)

and

(τ i+1 , τ 0 ) = (τ i+1 , τ i ) (τ i , τ 0 ) (3.18)

therefore,

(τ i+1 , τ 0 ) = (τ i , τ 0 ) + F(τ i ) (τ i , τ 0 ) dτ

Similarly, over each 1 sec interval, Q k = Q k (τ N , τ 0 ) can be integrated as
follows:
T
T
Q k (τ 1 , k) = (τ 1 , τ 0 )GQ G (τ 1 , τ 0 ) dτ
w
T
T
Q k (τ 2 , k) = (τ 2 , τ 1 )[GQ G dτ + Q k (τ 1 , k)] (τ 2 , τ 1 )
w
. (3.19)
.
.
T
T
Q k (τ N , k) = (τ N , τ N−1 )[GQ G dτ + Q k (τ N−1 , k)] (τ N , τ N−1 )
w
3.2.2 Basic KF
Since there are numerous books devoted to the derivation of the KF, such as
References 19, 20, and 24, the derivation is not included herein. Instead, the
KF algorithm and its properties are reviewed.
The KF estimates the state of a stochastic system. To determine optimal
gains for the filter at time t k, the KF compares the covariance of the state estimate
at t k with the covariance of the measurement at t k . To enable this comparison,
the KF algorithm will propagate the covariance of the state estimate as well
as the state estimate. Prior to discussing the KF algorithm, it will be useful to
summarize the new notation that will be used. The KF gain valid at time t k

© 2006 by Taylor & Francis Group, LLC

FRANKL: “dk6033_c003” — 2006/3/31 — 16:42 — page 105 — #7