Page 148 - Autonomous Mobile Robots
P. 148
Data Fusion via Kalman Filter 131
3.4.2 Error Model of INS System
The INS dynamics as represented in Equation (3.66) are nonlinear functions of
the INS state variables. As discussed in Section 3.2.3, the EKF will utilize a
linearized error-state model. The dynamic model for the error state of the INS
is derived in several references, for example, in References 27 and 41–43. After
minor simplification, it can be expressed as
δ ˙ p 0 F pv 0 0 0 δp ω p
δ˙ v F vp F vv F vρ F vx a 0 δv ω v + v a
ρ
δ ˙ = 0 0 F ρρ 0 δρ + ω ρ + v g (3.68)
F ρx g
0
˙ x a 0 0 F x a x a 0 x a ω a
˙ x g 0 0 0 0 F x g x g x g ω g
which is the required time-varying linear model in the form of Equation (3.1).
The 15 state errors are defined as: tangent plane position error δp =
T
T
[δn, δe, δd] , tangent plane velocity error δv =[δv N , δv E , δv D ] , attitude error
T
δρ =[ N , E , D ] , platform frame accelerometer bias error x a , and plat-
form frame gyro bias error x g . Additional IMU error calibration states (e.g.,
gyro scale factors) could be considered for state augmentation. The various F
matrices in Equation (3.68) are derived and defined, for example, in chapter 6
of Reference 27. The F matrix is time-varying, since it is a function of velocity,
attitude, angular rate, and specific force. The F matrix contains unstable and
neutrally stable components; therefore, initial condition errors and measure-
ment errors can cause the INS error state to diverge. Fusion of the INS state
with external sensors, such as GPS, using the EKF can estimate and compensate
for these errors.
3.4.3 EKF Latency Compensation
During each INS integration step, the INS will first compensate the IMU meas-
urements for the calibration factors (e.g., biases) estimated by the EKF. Next,
the INS will integrate Equation (3.64) and Equation (3.66) (or similar equations
depending on the choice of navigation frame and attitude representation). The
equations are integrated for the duration of the application regardless of the
availability of aiding measurements. With regard to aiding, two time instants
should be distinguished. The time-of-applicability of an aiding measurement
is the time at which the measurement is accurate. The time-of-availability of a
measurement is the time at which the aiding measurement is available for use by
the computer performing the data fusion operation. While the INS integration
process is ongoing, the INS state must be saved at the time-of-applicability of
the aiding measurements.
© 2006 by Taylor & Francis Group, LLC
FRANKL: “dk6033_c003” — 2006/3/31 — 16:42 — page 131 — #33
