Navigation Functionalities for an Autonomous UAV Helicopter

5.1. FILTER ARCHITECTURE 61
filter provides more accurate attitude angles information than the one given
by vision only as will be shown by the experimental results.
5.1 Filter architecture
The filter developed is composed of two main functions: the INS mechanization
function and the Kalman filter function. The INS mechanization
function performs the time integration of the inertial sensors while the
Kalman filter function estimates the errors of the INS mechanization. The
errors estimated by the Kalman filter are then used to correct the final
solution and are fed back into the mechanization process. The feedback
architecture is shown in Paper III.
5.1.1 Filter initialization
The filter turns on automatically as soon as the first valid data from the
vision system is available, then the first step is the calculation of the heading
of the helicopter relative to the pattern. The heading is given directly by
the vision system but a median filter over two seconds period is applied
on the raw vision heading. The results from the median filter are taken as
initial heading for the filter initialization. The assumption is that during
the two seconds after the filter has turned on, the helicopter does not make
any large yaw maneuver. Otherwise the median value might differ too
much from the initial heading. The raw vision heading is not used directly
in order to avoid the risk of initializing the filter with an outlier. The
initialization of the heading is done carefully because the convergence of
the filter to the correct heading is very slow when the helicopter is near a
hovering condition, which is true in the case of a landing maneuver.
Once the heading is available, the filter can be initialized. The raw
relative position coming from the vision system is taken as initial position.
The vertical and horizontal velocities are initialized to zero since the landing
approach starts from a hovering condition. Even if the initial position and
velocity have a relatively small error, the filter converges very fast to the
right value since the covariance of the vision measurement is quite low. The
initial pitch and roll angles are taken directly from the Yamaha Attitude