Page 158 - Autonomous Mobile Robots

Data Fusion via Kalman Filter 141

on the IMU data because the F matrix includes a n , a e , and ψ. Due to the
dependence of F on the IMU data, the matrices and Q k must be computed
during operation as discussed in Section 3.2.1.1.
At the GPS measurement epoch, the GPS pseudorange measurements are
used in an EKF to estimate the INS error state. When the INS error state
is available from the EKF, it is used to correct the INS state according to
Equation (3.43). As time progressed the IMU errors are calibrated and the
rate of growth of the INS errors decreases.
The top graph in Figure 3.6a shows both the estimated and the actual vehicle
trajectories. The lateral maneuver occurs at approximately t = 15 sec. The
bottom graph shows the position estimation error components as a function
√
of time. In addition to the estimation errors, the graph shows ± P 11 + P 22
which represents the EKF prediction of the standard deviation of the position
estimation error. The variance of the position error decreases steadily over
the period of the simulation due to the decay of the initial position error, the
estimation of velocity, and the balancing of the acceleration biases with the yaw
estimation error.
The top graph of Figure 3.6b shows velocity estimation error components
and the EKF prediction of the standard deviation of the velocity estimation error
as functions of time. After the initial transients, the velocity estimation error
decreases steadily due to the decay of the initial velocity error and the balancing
of the acceleration biases with the yaw estimation error. The middle graph shows
the bias estimation error components as functions of time. The bottom graph
shows the yaw estimation error and the EKF prediction of the standard deviation
of the yaw estimation error as functions of time. Analysis of Equation (3.76)
shows that the yaw angle and gyro bias errors are observable only when the
acceleration vector [a n (t), a e (t)] is nonzero. Therefore, the yaw error is not
adjusted by the EKF except for a brief interval following the maneuver. Close
inspection of Figure 3.6b shows that the yaw error standard deviation is slowly
increasing due to the accumulation of gyro measurement noise during the atti-
tude integration process. Note that the yaw estimation error does not approach
zero; however, its net effect on the velocity and position does approach zero
(in the absence of maneuvering). From Equation (3.71) we see that (neglecting
noise)

δ˙v n =˙v n − ˆvˆn = a n − (˜a u cos ˆ ψ −˜a v sin ˆ ψ)
˙
δ˙v n = a n − ((a u − δˆa u ) cos(ψ − δψ) − (a v − δˆa v ) sin(ψ − δψ))
Even when the acceleration vector is zero, we have

δ˙v n = (cos(ψ) cos(δψ) + sin(ψ) sin(δψ))δˆa u
− (sin(ψ) cos(δψ) − cos(ψ) sin(δψ))δˆa v (3.78)

© 2006 by Taylor & Francis Group, LLC

FRANKL: “dk6033_c003” — 2006/3/31 — 16:42 — page 141 — #43