
A Robust and Modular Multi-Sensor Fusion Approach
Applied to MAV Navigation
Simon Lynen
1
, Markus W. Achtelik
1
, Stephan Weiss
2
, Margarita Chli
1
and Roland Siegwart
1
Abstract— It has been long known that fusing information
from multiple sensors for robot navigation results in increased
robustness and accuracy. However, accurate calibration of the
sensor ensemble prior to deployment in the field as well as
coping with sensor outages, different measurement rates and
delays, render multi-sensor fusion a challenge. As a result,
most often, systems do not exploit all the sensor information
available in exchange for simplicity. For example, on a mission
requiring transition of the robot from indoors to outdoors, it is
the norm to ignore the Global Positioning System (GPS) signals
which become freely available once outdoors and instead,
rely only on sensor feeds (e.g., vision and laser) continuously
available throughout the mission. Naturally, this comes at
the expense of robustness and accuracy in real deployment.
This paper presents a generic framework, dubbed Multi-
Sensor-Fusion Extended Kalman Filter (MSF-EKF), able to
process delayed, relative and absolute measurements from a
theoretically unlimited number of different sensors and sensor
types, while allowing self-calibration of the sensor-suite online.
The modularity of MSF-EKF allows seamless handling of
additional/lost sensor signals during operation while employing
a state buffering scheme augmented with Iterated EKF (IEKF)
updates to allow for efficient re-linearization of the prediction
to get near optimal linearization points for both absolute and
relative state updates. We demonstrate our approach in outdoor
navigation experiments using a Micro Aerial Vehicle (MAV)
equipped with a GPS receiver as well as visual, inertial, and
pressure sensors.
I. INTRODUCTION
Precise and consistent localization is a core problem in
many areas of mobile robotics, in both research and industrial
applications. Driven by the need for effective solutions, the
literature is currently host to an abundance of approaches
to state estimation. Addressing different choices of on-board
sensor suites the employed frameworks however are tailored
tightly to the task at hand. The use of GPS feeds, for exam-
ple, is a common and convenient approach to localization
for platforms operating in open (GPS-accessible) spaces.
Conversely, in GPS-denied environments, vision or laser
based approaches are often employed instead. The transi-
tion, however, across domains with different sensor-signal
availability and suitability, remains a challenging problem.
In this paper, we present an effective approach to tackle
the problem of seamless sensor-feed integration within state
estimation. We put the focus on rotor-based Micro Aerial
Vehicles (MAVs), as they are most capable of acting in and
traversing across different domains, while imposing delicate
* This work has been supported by the European Commission’s Seventh
Framework Programme (FP7) under grant agreements n. 285417 (ICARUS)
and n.266470 (myCopter).
1
Autonomous Systems Laboratory, ETH Zurich, Switzerland
2
Computer Vision Group, Nasa JPL, California, USA
5
10
15
20
25
-5
0
5
10
15
0
50
100
150
200
250
300
pos. y [m]
travelled distance [m]
estimate
5
10
15
20
25
-5
0
5
10
15
0
50
100
150
200
250
300
pos. y [m]
travelled distance [m]
Fig. 1: The scale error of a visual SLAM system combined with our sensor-
fusion framework commonly is in the area of 3-5% depending on the
structure observed and the movements carried out. The left plot shows the
deviations of the trajectory with the scale error not accounted for. The right
plot shows potential benefits which additional sensors can provide when
fusing e.g., a height sensor with visual and inertial cues.
challenges due to their high agility and limitations on both
payload and computational power. Building on our earlier
work [16], [17], we propose a highly generic, open source
c++ state estimation framework which comprises:
• Modular support for an unlimited number of sensors
providing relative and absolute measurements.
• Estimation of calibration states between sensors and
dynamic compensation of measurement delays.
• Re-linearization of constraints from both proprioceptive
and exteroceptive information sources in filter form.
• Efficient tracking of cross covariance terms for relative
updates allowing estimation rates of several kHz.
Following an analysis of the limitations of our earlier
work, we also present a derivation to include relative poses
from key-frame based Simultaneous Localization And Map-
ping (SLAM) system, which is essential when employing
visual/laser odometric sensors. Finally, we demonstrate the
MSF-EKF framework in real experiments, flying trajectories
of more than 800 m with speeds of up to 4 m/s.
1
A. Sensor Fusion for State Estimation
Autonomous MAV navigation and control has seen great
success over the last couple of years, demonstrating impres-
sive results with the aid of external motion capture systems.
However, the complex preparation of the operation space
required with such systems is clearly not an option in large
scale missions in unknown environments. Tackling this chal-
lenge is core in enabling operation for common tasks such as
industrial inspection, search-and-rescue and surveillance. As
a result, a series of approaches have been proposed, using
1
A video of the experiments is available at http://youtu.be/neG8iEf8XiQ