IEEE Robotics & Automation Magazine - September 2014 - 32

completeness, the extrinsic
calibration parameters
describing the relative rotation q si and position p si
between the IMU and the
camera frames were also
added. Note that the calibration parameters could be
omitted from the state vector
and be set to a premeasured
constant to increase robustness and faster state convergence. Having p si as a constant and not as a filter state
would increase the convergence performance of the
visual scale. We did not
notice significant performance improvement when
removing q si from the state
vector. We assume that this is
because the attitude is directly
measured by the visual pipeline whereas the scale ambiguity occurs in both p si and
the MAV position p iw . In this
article, we show that even
with continuously estimating
both parameters p si and q si ,
we can achieve good and
robust results. This yields a
24-element state vector X:

HLP
Attitude
Commands

Position Controller at 1 kHz

LLP
Position, Velocity, Attitude
IMU Data

EKF Prediction at 1 kHz
...

FCU

Correction

Local Pose Estimation
with Monocular Vision 5-DoF Pose
at 30 Hz

State

EKF Propagation at 100 Hz
EKF Update(s)

Onboard Computer
MAV (Local Navigation)

Images

Global Pose

MAV

Global Mapping, Global Localization at 1 Hz
Ground Station (Global Navigation)

Figure 8. An overview of the different processing tasks and how these are distributed and interact with
each other in our navigation framework. Waypoint commands are sent from the ground station to the
onboard computer, which forward them to the position controller. The ground station and onboard
computer communicate over a Wi-Fi connection. Note that all critical parts necessary to keep the
helicopter airborne run entirely on board and do not rely on the Wi-Fi link. The parts on the HLP refer to
the "user-defined programs" section in Figure 3.

new algorithms. The current implementation uses only
60% of one core of the Core 2 Duo processor at 30 Hz,
leaving enough resources for future higher-level tasks. As a
reference, the same implementation on an Atom 1.6-GHz
single-core computer runs at 20 Hz using 100% of the processing power.
The 5-DoF pose of the MAV camera output by the visualodometry algorithm was fused with the inertial measurements
of an IMU using an EKF. More details are given in [42]. An
EKF framework consists
of a prediction and an
update step. The compuFor safe operation, the
tational load required by
these two steps is distribmost important aspect is
uted among the different
redundancy against at least units of the MAV, as
described in [43]. The
state of the filter is coma single-rotor failure.
posed of the position p iw,
the attitude quaternion
q iw, and the velocity v iw of the IMU in the world frame. The
gyroscope and accelerometer biases b ~ and b a as well as the
missing scale factor m are also included in the state vector. For
32

IEEE ROBOTICS & AUTOMATION MAGAZINE

september 2014

X = {p iw v iw q iw b ~T b Ta

p si q si }.

(1)

Figure 9 shows the setup with the IMU and camera coordinate frames and the state variables introduced above.
The equations of the EKF prediction step for the considered IMU-camera fusion are given in [42]. The equations of
the update step are derived by computing the transformation
from the world reference frame to the camera frame as follows. For the position z p, we can write:
T

z p = p sw = (p iw + C (q iw) p si ) m + n p,

(2)

where C (q iw) ! SO (3) is the rotation matrix associated with
the IMU attitude quaternion q iw in the world frame, z p
denotes the observed position (the output of the visual odometry), m is the scale factor, and n p the measurement noise. For
the rotation measurement z q, we apply the notion of error
quaternion. Since the visual-odometry algorithm yields the
rotation q sw from the world frame to the camera frame, we
can write:
z q = q ws = q si 7 q iw .

(3)

Table of Contents for the Digital Edition of IEEE Robotics & Automation Magazine - September 2014