Navigation Functionalities for an Autonomous UAV Helicopter

5.1. FILTER ARCHITECTURE 63
north, east and down velocity error; roll, pitch and heading error), and
three accelerometer biases modeled as first order Markov processes. The
Kalman filter algorithm estimates the errors of the INS mechanization by
fusing the estimate of these errors provided by an internal linear model and
the measurements from the vision system. The theory behind the Kalman
filter can be found in [12].
The linear model used for the INS errors is represented by the system
of differential equations in 5.2:
δ ˙r = −ωen×δr+δv
δ ˙v = −(ωie+ ωin)×δv−ψ×f+δa (5.2)
˙ψ = −ωin × ψ
δ ˙a = −βδa
where δr is the position error, δv is the velocity error and δψ is the
attitude error. δa represents the three accelerometer biases. In addition
ωen is the rotation rate vector of the navigation reference system relative
to the Earth reference system, ωin is the rotation rate vector of the navigation
reference system relative to the inertial reference system and ωie
is the rotation rate vector of the Earth reference system relative to the
inertial reference system. The update of the filter is performed when a
new measurement from the image processing system is available. The raw
measurement delivered by the image processing is the 3D helicopter position
relative to the pattern. The image processing measurement covariance
used in the filter is scheduled with the distance to the pattern. The relation
between the uncertainty of the vision measurement and the distance to the
pattern is found in Paper III.
The Kalman filter is implemented in discretized form and the recursive
implementation of the discretized Kalman filter equations can be found
easily in the literature. A detailed implementation of a nine state Kalman
filter can be found in [23].