Proceedings

The of
of the
International
Proceedings 20th
20th World
Federation
the Worldof Congress
Automatic Control
Congress
Proceedings
The of the
International
Toulouse, France, 20th9-14,
World
Federation
July of Congress
Automatic Control
2017
The International Federation of Automatic Control
Available online at www.sciencedirect.com
The International Federation
Toulouse, of Automatic Control
Toulouse, France,
France, July
July 9-14,
9-14, 2017
2017
Toulouse, France, July 9-14, 2017

ScienceDirect
StateIFACEstimation
PapersOnLine 50-1 (2017) 11683–11688
for a Multirotor using
State Estimation for a Multirotor using
StateTight-Coupling
Estimation for aofMultirotor GNSS andusing
Tight-Coupling of GNSS and
Tight-Coupling
Inertial Navigation of GNSS and
Inertial Navigation 
Inertial
∗
Navigation ∗ ∗
Thomas Konrad ∗ Michael Breuer ∗ Thomas Engelhardt ∗
Thomas
Thomas Konrad ∗∗ Michael
Michael
DirkBreuer
Abel ∗∗ ∗∗ Thomas
Thomas Engelhardt
Engelhardt ∗∗
Thomas Konrad
Konrad Michael Dirk
Breuer
Breuer
Abel ∗ Thomas Engelhardt
Dirk
Dirk Abel
Abel ∗
∗
∗
Institute of Automatic Control, RWTH Aachen University,
∗ Institute of Automatic Control, RWTH Aachen University,
Institute
∗ Aachen, of Automatic
Germany
Institute Germany Control,
(e-mail:
of Automatic RWTH
RWTH Aachen
Aachen University,
t.konrad@irt.rwth-aachen.de).
Control, University,
Aachen,
Aachen, Germany (e-mail:
(e-mail: t.konrad@irt.rwth-aachen.de).
t.konrad@irt.rwth-aachen.de).
Aachen, Germany (e-mail: t.konrad@irt.rwth-aachen.de).
Abstract: Multicopters are becoming a major tool for diverse industrial outdoor usecases,
Abstract:
Abstract: Multicopters
Multicopters are
are becoming
relativeaaalocalization
becoming major
major tool andfor diverse
control. industrial outdoor usecases,
requiring accurate
Abstract:
requiring Multicopters
accurate
absolute
absolute
and
areand becoming
relative major tool
localizationtool andfor
for diverse
diverse
control.
industrial
Robust and
industrial
Robust and
outdoor
outdoor usecases,
high-rate state
usecases,
high-rate state
requiring
estimation
requiring accurate
is therefore
accurate absolute
crucial,
absolute and
and relative
even in
relative the localization
event of
localization poorand
and control.
reception
control. Robust
of global
Robust and high-rate
navigation
and high-rate state
satellite
state
estimation
estimation is
is therefore crucial, even
thisin the eventthis
of poor reception of global navigation satellite
systems (GNSS).
estimation
systems is therefore
(GNSS). therefore
To
crucial,
crucial, even
To overcome
overcome even
this
in the
the event
problem,
inproblem, of
of poor
eventthis paper
poor
paper
reception
addresses
reception
addresses
of
of global
global
the
navigation
navigation satellite
the implementation
implementation
and
satellite
and
systems
validation
systems (GNSS).
of
(GNSS).a To overcome
tightly-coupled
To overcomefusion this
fusion
this ofofproblem,
GNSS
problem, andthis paper
inertial
thisinertial addresses
sensors
paper sensors
addressesfor the
micro implementation
aerial
the implementation vehicles. and
The
and
validation
validation of
of a tightly-coupled GNSS an and for micro aerial vehicles. The
validation
targeted of a
targeted experimentala tightly-coupled
quadcopter
tightly-coupled
experimental quadcopter
fusion
fusion of
of GNSS
provides
provides GNSS an
and
and inertial
inertial sensors
industrial-class
industrial-class sensors for
for micro
inertial
inertial micro aerial
aerial vehicles.
measurement
measurement
unit and
vehicles.
unit
The
Theaa
and
targeted experimental
multi-frequency
targeted experimentalGNSS quadcopter
receiver. The
quadcopter provides an
an industrial-class
sensorfusion
provides inertial
inertial measurement
including initialization
industrial-class unit
unit and
and parameterization
measurement and aa
multi-frequency
multi-frequency
is explained in GNSS
GNSS
detail, receiver.
receiver.
and The
The
practical sensorfusion
sensorfusion
implementations including
including
are initialization
initialization
given. The work and
and parameterization
parameterization
focuses on outdoor
multi-frequency
is explained in GNSS receiver.
detail, The implementations
sensorfusion including initialization and parameterization
is
is explained
validation
explained in
tests
in using aand
detail,
detail, and
and
practical
practical
highly accurate
practical implementations
optical tracking
implementations
are
are
are
given.
given.
system
given.
The
The
as
The
work
work
reference.
work
focuses
focuses
Results
focuses
on
on outdoor
outdoor
show
on show
outdoor an
validation
validation
absolute tests
tests
horizontal using
using a
a
positionhighly
highly
error accurate
accurate
of below optical
optical
1 m and tracking
tracking
low system
system
standard as
as reference.
reference.
deviations of 5 Results
Results
cm for show
horizontal an
an
validationhorizontal
absolute tests using a highly
position error accurate
of below optical tracking systemdeviations
as reference. Results show an
absolute
position, horizontal
absolute 0.15 m/s for
horizontal position error
velocity,
position of
and
errorand 1◦◦ for111 m
of below
below m and
and
and low
low standard
low standard
orientation deviations
deviations aaof
errors. Moreover, ofcomparison
55 cm
5 cm
for horizontal
cm for
for horizontal
with a
position, 0.15 m/s for velocity, 1 ◦ for m orientation standard
errors. Moreover, ofcomparison horizontal
with a
position,
loosely-coupled
position, 0.15
0.15 m/sm/s
filterfor velocity,
using
forusing and 1 ◦ for orientation
Real-Time-Kinematic
velocity, and 1 for orientation (RTK) errors. Moreover,
measurements
errors. is a comparison
given,
Moreover,is agiven, with
with aa
demonstrating
comparison
loosely-coupled
loosely-coupled
potential of filterfilter using Real-Time-Kinematic
Real-Time-Kinematic (RTK)
(RTK) measurements
measurements
of bothis given, demonstrating
the
loosely-coupled
the potential of
further
filter
further
research
using
research
when exploiting the
Real-Time-Kinematic
when exploiting (RTK)
the
advantages
measurements
advantages of both given, demonstrating
isapproaches.
approaches. demonstrating
the
the potential
potential of further research
of further research when
when exploiting the
exploitingControl) advantages
the advantages of both approaches.
© 2017, IFAC (International Federation of Automatic Hosting byofElsevier
both approaches.
Ltd. All rights reserved.
Keywords: navigation, multirotor, GNSS, INS, tight-coupling, parameterization, validation
Keywords: navigation,
Keywords: navigation, multirotor, GNSS, GNSS, INS, tight-coupling,
tight-coupling, parameterization, validation validation
Keywords: navigation, multirotor, multirotor, GNSS, INS, INS, tight-coupling, parameterization,
parameterization, validation
1. INTRODUCTION
1. INTRODUCTION
1. INTRODUCTION
1. INTRODUCTION
Micro aerial vehicles (MAVs) in form of multicopters are
Micro aerial
Micro
becoming aerial vehicles
vehicles
a major (MAVs) for
(MAVs)
instrument in diverse
in form of
form of multicopters
multicopters are
are
Micro
becoming aerial
a vehicles
major (MAVs) for
instrument form ofoutdoor
in diverse multicopters
outdoor
usecases,
usecases, are
becoming
e.g., takingaa aerial
becoming major
major instrument
footage
instrument for
for filming
for diverse outdoor
purposes,
diverse outdoor and usecases,
inspec-
usecases,
e.g., taking
e.g.,
tion taking aerial footage
aerial
of industrial footage
structures. for filming
for filming purposes,
Due topurposes,
their high and
and inspec-
inspec-
mobility,
e.g., taking
tion prices aerial footage
of industrial
industrial structures. for filming
Due purposes,
toprovide
their high and inspec-
mobility,
tion
low
tion of and
of industrial ease structures.
of use,
structures. they Due
Due can to their
toprovide high
their hightime mobility,
and
mobility,cost
low
low prices
prices
advantages and
and ease
ease
compared of
of use,
use,
to they
they can
can
conventional provide
methods time
time ofand
and cost
cost
filming
low prices and
advantages ease of use,
compared to they can provide
conventional methods timeofand cost
filming
advantages
and inspection.
advantages compared
Nowadays,
compared to
to conventional
most of the
conventional methods
commercial
methods of
of filming
mul-
filming
and inspection.
and inspection.
ticopters Nowadays,
are stillNowadays,
manually most most
piloted, of the
of the commercial
commercial
providing only basicmul-
mul-
and inspection.
ticopters stillNowadays,
are still manually most
piloted, of the commercial
providing only mul-
basic
ticopters
levels
ticoptersof are
automation
are still manually
based
manually piloted,
on standard
piloted, providing
Global
providing only
only basic
Position
basic
levels of
levels
System of (GPS)
automation
automation based on
based
measurements. on standard
standard Global Position
Global Position
levels
System of (GPS)
automation based on For
measurements. For
full automatic
standard
full Global Position
automatic
flight,
flight,
System
robust
Systemand (GPS)
and measurements.
accurate
(GPS) positioning
measurements. For
is
For full
required automatic
for
full automatic flying flight,
near Fig. 1. Unmanned experimental quadcopter
robust
robust
or even and accurate
accurate
under solid positioning
positioning
structures. isPrecise
is required
required and for flyingflight,
forrobust
flying esti- Fig.
near
near Fig. 1.
1. Unmanned
Unmanned experimental
experimental quadcopter
quadcopter
robust
or even and accurate
under solid positioning
structures. isPrecise
required and forrobust
flying nearesti- Fig.
The 1. Unmanned
basis of these experimental
inertial quadcopter
navigation systems (INS) has
or even
mation under
of the solid
full structures.
state can lead Precise
to and
promising robust
results esti-
in The basis of these inertial navigation systems
or even
mation
mation
controller
under
of
of the
the
solid
full
full
performance
structures.
state
state can
can
using lead
lead
Precise
to
to
and
promising
promising
flatness-based
robust
results
results
(Engelhardt
esti-
in
in The
been
The basis
basis of
analyzed these
in
of these inertial
various navigation
literature
inertialliterature
navigation (cf. Farrell (INS)
systems
systems (2008)has
(INS)
(INS) has
or
has
mation
controller of the full
performance state can lead to
using flatness-based promising
flatness-based results
(Engelhardt in been
been
Wendel analyzed
analyzed
(2011)), in
in various
various
showing literature
its potential(cf.
(cf. Farrell
Farrell
by using (2008)
(2008) or
or
military
controller
et al.,
controller2016)performance
or model
performance using
predictive controllers.
using flatness-based (Engelhardt
Looking
(Engelhardt at been
Wendel analyzed
(2011)), in showing
various literature
its potential
potential(cf. by
Farrell
using (2008) or
military
et
et al.,
al.,
current 2016)
2016) or
or
professionalmodel
model predictive
predictive
multicopters, controllers.
controllers.
however, algorithm Looking
Looking at
at
ro- Wendel
grade
Wendel (2011)),
IMUs and
(2011)), showing
GNSS
showing its
receivers with
its potential by
large using
antennas
by using military
and
military
et al., 2016) or model predictive controllers. Looking at grade
grade IMUs
IMUs and
and GNSS
GNSS receivers
receivers with
with large antennas and
current professional
current
bustness
current
professional
professional
multicopters,
in the case multicopters,
of poor satellite
multicopters,
however,
however,
reception
however,
algorithm
algorithm
is still ro-
algorithm an thereby
ro-
ro- grade
thereby
reaching
IMUs and high
reaching GNSS
high
positional
positional with large
receiversaccuracy
accuracy large antennas
during
antennas
during long
and
long time
and
time
bustness
bustness
issue. As in
in the
the case
case
higher-grade of
of poor
poor
inertial satellite
satellite
measurements reception
reception units is
is still
still
(IMUs) an
an thereby
ranges.
thereby reaching
Godha
reaching high
and
high positional
Cannon
positional accuracy
(2007) show
accuracy during
the
during long
performance
long time
time
bustness
issue.global in the case ofinertial
As higher-grade
higher-grade poor satellite
measurements reception units is (IMUs)
still an ranges. ranges.
of Godha
such Godha
a GPS/INSand Cannon
and Cannon (2007)
in the (2007) show of
show
application theground-based
the performance
performance
issue.
and As
issue.global navigationinertial
As higher-grade satellitemeasurements
inertial system (GNSS)
measurements units (IMUs)
receivers
(IMUs) of
unitsreceivers ranges.
such Godha and Cannon
aa GPS/INS in the (2007) show of
application theground-based
performance
and
and
get global
cheaper navigation
navigation
and more compact, satellite
satellite system
system
MAVs are (GNSS)
(GNSS)
enabled receivers
to use of of
urbansuch GPS/INS
vehicles,
suchvehicles,
a GPS/INS and in
in the
achieve application
good
the application of
results ground-based
using model-
of ground-based
and
get global
cheaper navigation
and more satellite
compact, system
MAVs (GNSS)
are enabled receivers
to use urban
urban
based vehicles,
reduction and
and
of the achieve
achieve
possible good
good
degreesresults
results
of using
using
freedom. model-
model-
For the
get
more
get cheaper
advanced
cheaper and
and more
approaches
more compact,
compact, MAVs
in filter
MAVs are
design.
are enabled
IMU measure-
enabled to
to use
use urban
based vehicles, of
reduction and the achieve
possible good
degreesresults
of using For
freedom. model-
the
more
more
ments advanced
advanced
can be used approaches
approaches in
in
in a strapdown filter
filter design.
design.
algorithm IMU
IMU measure-
tomeasure-
provide based based
practical reduction
reduction of
application the
of theon possible
on a
possible degrees
multicopter, of freedom.
however,
degrees ofhowever,
freedom.the For
the the
high
Forhigh
the
more advanced
ments can canstate
be usedapproaches
used in
in aa strapdown filter
strapdown design.
algorithm IMU measure-
to provide
provide practical
practical
dynamics application
application
of the flighton a
a
havemulticopter,
multicopter,
to be however,
estimated the high
accurately,
ments
high-rate
ments be
canstate
be used in
propagation,
in a strapdownupdated algorithm
with
algorithm to
absolute sensor
to provide practical
dynamics application
of the flighton a
havemulticopter,
to be however,
estimated the high
accurately,
high-rate
high-rate state propagation,
propagation, updated
updated with sensors.
with absolute
absolute sensor and dynamics of
of the
restrictions theto flight have
weight,
havesize to be
be estimated
to and cost of theaccurately,
employed
measurements
high-rate state taken with GNSS
propagation, updated or visual absolute sensor
with sensors. sensor dynamicsand
and restrictions
restrictions to
to
flight
weight,
weight, size
size and
and
estimated
cost
cost of
of theaccurately,
employed
measurements
measurements
measurements
taken
taken
taken
with
with
with
GNSS
GNSS
GNSS
or
or
or
visual
visual
visual sensors.
sensors.
equipment
and pose
restrictions
equipment pose
additional
to weight, size
additional
challenges.
and cost
challenges. of the employed
the
Abeywardena
Abeywardena
and
employed
and
 The authors gratefully acknowledge the contribution of the joint equipment
Dissanayake
equipment pose
pose additional
(2015) use
additional a challenges.
monocoular Abeywardena
camera
challenges.camera for
Abeywardena and
Simulta-
and
 The authors
 The authors gratefully acknowledge the contribution of the
the joint
Dissanayake
Dissanayake
neous (2015)
(2015)
Localization use
use
and a
a monocoular
monocoular
Mapping (SLAM) camera
on anfor
for Simulta-
Simulta-
MAV, and
research project Galileo
gratefully Online:
acknowledge GO! supported
the by
contribution
 The authors gratefully acknowledge the contribution of the joint the
of German
joint Dissanayake
neous (2015)and
Localization use Mapping
a monocoular (SLAM) camera
on anforMAV,
Simulta-
and
research
Aerospace project Galileo
Centre Galileo
research project (DLR) withOnline:
Online: GO!
aid GO! supported
of thesupported by
German Federal the German
by the Ministry
German neous
neous Localization
tightly-couple the
Localization and Mapping
measurements
and Mapping (SLAM) (SLAM)
with an on
on an
INS MAV,
for
an MAV, and
indoor
and
research
Aerospace project
Centre Galileo
(DLR) Online:
and with aid GO!
of supported by the German tightly-couple
tightly-couple
state estimation.the
the measurements
measurements
However, since with
with
many an
an INS
INS for
for
industrial indoor
indoor
appli-
of Economic
Aerospace Affairs
Centre (DLR) Energy
with of the
the German
aid (BMWi), German Federal
Federal Ministry
in compliance with a
Ministry tightly-couple the measurements with an INS for indoor
Aerospace
of
of Economic
Centre (DLR)
Affairs and with
Energy aid of the German
Energy (BMWi),
(BMWi), in
Federal Ministry
in compliance
compliance with with a state
state estimation.
estimation. However,
However, since
since many
many industrial appli-
resolution
of
Economic of the
Economic the
resolution
German
Affairs and Bundestag.
Affairs and Bundestag.
Energy (BMWi), in compliance with a
a cations
state
cations
require outdoor
estimation.
require However,
outdoor
flight
flight since
in many industrial
in unknown
unknown industrial appli-
environments,
appli-
environments,
resolution ofof the German
German Bundestag. cations require outdoor flight in unknown environments,
resolution of the German Bundestag. cations require outdoor flight in unknown environments,
Copyright © 2017 IFAC 12180
2405-8963 ©
Copyright © 2017,
2017 IFAC (International Federation of Automatic Control)
IFAC 12180Hosting by Elsevier Ltd. All rights reserved.
Copyright
Peer review©under
2017 responsibility
IFAC of International Federation of 12180
Automatic Control.
Copyright © 2017 IFAC 12180
10.1016/j.ifacol.2017.08.1684
Proceedings of the 20th IFAC World Congress
11684
Toulouse, France, July 9-14, 2017 Thomas Konrad et al. / IFAC PapersOnLine 50-1 (2017) 11683–11688
the number of trackable features in a SLAM approach is
questionable and fallback to GNSS measurements is prob-
ably necessary. George and Sukkarieh (2005) present an
approach of tightly-coupled fusion with inertial sensor bias
estimation on-board an unmanned aerial vehicle (UAV),
although, in-flight validation with a suitable reference
sensor is not provided and accuracy is still limited. For
high precision positioning, Pilz et al. (2011) employ Real
Time Kinematic (RTK) for centimetre-precision, but do
not consider loss of the RTK solution, and focus on static
measurements without dynamics.
The present paper aims to fill these gaps by presenting
an approach of a tightly-coupled GNSS-aided INS for un-
manned multicopters, and validating its accuracy during
outdoor flight with a quadrotor experimental vehicle (see
figure 1). Instead of updating the propagated state using
a pre-computed position solution based on the combined
visible GNSS satellites (loosely-coupling), individual pseu-
doranges and Doppler ranges to each satellite are tightly-
coupled directly in an error-state extended Kalman filter
(EKF). Time delays in these GNSS measurements are
considered. Parameterization and initialization of the filter
is presented and characteristics of the system, e.g. short
flight times and sensor restrictions, are considered in this
work. For outdoor experimental validation, an infrared
camera based tracking system is used as a reference, al-
lowing to discuss the accuracy of position, velocity and
orientation estimation of the quadcopter during flight.
The paper is organized as follows: section 2 presents the
tightly-coupled filter approach focusing on the implemen-
tation on a multicopter. In section 3, the state initialization
algorithm is explained, followed by an overview of the
choice of filter parameters in section 4. Further on, results
from an experimental outdoor validation comparing to the
infrared tracking system are provided in section 5. Finally,
a short comparison with an RTK-based loosely-coupled
filter is shown in section 6.
2. NAVIGATION FILTER DESIGN
2.1 System Overview
In this research, a nonlinear model in state-space form
xk+1 = F k (xk )xk + Gk (xk )wk wk ∼ N (0, Qk )
(1)
z k = H k (xk )xk + v k v k ∼ N (0, Rk )
is considered, where xk is the state of the system at time
step k, wk is the process noise, z k is the measurement
and v k is the measurement noise. F k , Gk and H k are
the nonlinear state transition, noise and measurement
matrices, respectively. Several coordinate frames are used
during the work, denoted by a superscript: e-frame and t-
frame reference Earth-fixed Earth-centered frame (ECEF)
and tangent frame (longitude λ, latitude φ, height h),
respectively. The n-frame is a local Cartesian frame with
origin at the current position, x-axis pointing north, y-axis
east, and z-axis downwards. For defining orientation and
heading, the Cartesian b-frame is fixed to the IMU and
has its axis aligned with front, right and down orientation
relative to the quadcopter.
The chosen state vector of the filter
x = pt v neb q nb bba bbω cr
xk-1
IMU Strapdown x-k x+k
propagation
zk,pred ba,k, bg,k δxk
GNSS GNSS zk ek EKF
pre-processing update
Fig. 2. Filter structure, adapted from Breuer et al. (2016)
consists of position pt in the t-frame (3×1), velocity v neb in
the n-frame (3 × 1), orientation quaternion q nb , describing
the rotational transformation in order to align the b-frame
with the n-frame (4 × 1), accelerometer and gyroscope
biases bba , bbω (3 × 1), and GNSS-receiver clock errors cr
(2 × 1).
Accelerometer and gyroscope biases are estimated to allow
a more precise propagation of the quadcopter’s state based
on IMU measurements. As proposed in literature, e.g.
by George and Sukkarieh (2005), both accelerometer and
gyroscope biases are modeled as a random walk process
according to
ba,k+1 = ba,k + wa
(3)
bω,k+1 = bω,k + wω ,
where wa , wω are zero-mean white noise processes.
Since the GNSS receiver clock in general is floating, e.g.
it is not synchronized to GPS time, the receiver clock
error has to be estimated as well. Modeling the clock
error allows for updating the GNSS-aided position in the
event of less than four available satellites and is therefore
necessary for the proposed tightly-coupled filter design.
The implemented model consists of an offset and a drift
component (Wendel, 2011):
1 T
cr,k+1 = cr,k + wc , (4)
1 0
where cr contains the two scalar values for receiver clock
offset and drift, wc is a vector of two uncorrelated zero-
mean white noise components and T is the sample time.
As sketched in figure 2, the filter design is conse-
quently split into three parts: the GNSS measurement
pre-processing, the IMU strapdown mechanization, and
an extended Kalman filter for error estimation using the
GNSS measurements. The concept is based on a previous
publication (Breuer et al., 2016) and is presented for the
specific implementation on a quadcopter in the following
subsections.
2.2 GNSS Measurement
As GNSS measurements, GPS L1 and L2c pseudorange
and Doppler observables are used in combination with
the broadcasted ionospheric coefficients and the navigation
message.
A Pseudorange observable ρr is generated using the GPS
L1 coarse/acquisition (C/A)-code (index 1) and consists of
(2) the real range ρ̂ between satellite and receiver antennas at
12181
 Proceedings of the 20th IFAC World Congress
Toulouse, France, July 9-14, 2017 Thomas Konrad et al. / IFAC PapersOnLine 50-1 (2017) 11683–11688 11685
emission and reception time and numerous additive error
terms:
f2
ρr = ρ̂ + cb − cr,o + Ir + Tr + Mρ + νp , (5)
f1
where cb is the satellite clock error, cr,o the receiver clock
offset, ff21 Ir the frequency dependent ionospheric delay, Tr
the tropospheric delay, Mρ delays caused by multipath and
νp additive noise.
In this work, the ionospheric delay is partly compensated
by a Klobuchar parametric correction model, as proposed
by Klobuchar (1987). Since this model only provides a
compensation of about 50 % of the total ionospheric delay,
GPS L2c measurements (index 2) are used if available to
eliminate this error. A per-satellite correction factor Iaρ is
then formed to calculate an ionospheric-free pseudorange
ρ̄ for the L1 signal (Farrell, 2008):
f 1 f2
I aρ = 2 (ρ2 − ρ1 ) (6)
f1 − f22
f2
ρ̄ = ρ1 − Iaρ (7)
f1
where f1/2 denotes the GPS frequencies of L1 and L2c
signals, respectively.
For correction of the tropospheric delay within pseudor-
ange observables, a model considering day of the year and
satellite elevation angle is implemented, as proposed by
Collins (1999). Satellite clock errors are corrected using
a second order model, as broadcasted by GPS in the
navigation message, reducing these clock errors and rel-
ativistic effects to under 1 m. In this work, errors induced
by multipath are not specifically treated, since the GNSS
receiver already provides basic multipath mitigation based
on signal strength analysis.
For measuring velocity, Doppler observables are used that
give the relative velocity between satellite and receiver
velocity based on the Doppler effect. Since these obserables
are not prone to significant atmospheric delays, only
satellite clock error drift is compensated by using a second
order polynomial with GPS-broadcasted coefficients.
In the event of receiving more than five satellites, elevation
angle based masking is implemented. Satellites at an
elevation angle lower than 20 ◦ are eliminated due to their
large atmospheric errors (Wieser et al., 2005).
Satellite position and velocity are calculated based on the
broadcasted ephemerides (Farrell, 2008), complementing
the measurement inputs to the Kalman filter.
2.3 Strapdown Propagation
For dynamic propagation of position, velocity and orienta-
tion between GNSS measurements, measured accelerations
and angular rates are integrated at a rate of 100 Hz. The
equations follow the standard of inertial navigation, as
described by e.g. Farrell (2008) and Wendel (2011):
ω bnb = ω bib,est − C bn (ω nen + ω nie ) (8)
v̇ neb,k = C Tbn abib,est − (ω nen + 2ω nie ) × v neb,k + g n
ṗnk = v neb,k
where ω bnb is the body angular rate, ω nie and ω nen are earth
and transport angular rates, C bn is the direction cosine
matrix from n to b-frame, and g n the local gravitational
acceleration. abib,est and ω bib,est are estimated using the
current IMU measurements abib,meas and ω bib,meas and the
estimated bias values ba and bω according to:
abib,est = abib,meas − ba (11)
ω bib,est = ω bib,meas − bω . (12)
Equations (8) to (10) are integrated using a forth-order
Runge-Kutta integration to yield the updated state at
time step k + 1. Thereby, states for accelerometer and
gyroscope biases remain constant and receiver clock errors
are updated according to (4).
Earth’s local gravitational acceleration vector g n is mod-
eled using the Welmec-equation, which is an approxima-
tion based on the current user latitude φ  and height h:
gdn = g0 1 + g1 sin2 (φ) − g2 sin2 (2φ) − g3 h , (13)
where gdn is the vertical component of g n and g1 to g3 are
constant parameters.
As part of the propagation step, the covariance matrix
P is updated by solving the continuous-time form of the
well-known Kalman filter relation
Ṗ = F k P + P F Tk + Gk QGTk , (14)
where F k and Gk are the linearized state transition and
noise matrices at time step k (Wendel, 2011), and Q
the process noise covariance matrix, containing IMU and
receiver clock noise variances (see section 4).
2.4 Kalman Filter Error Model
For fusing the GNSS measurements with the estimated
a-priori state x−k , an error-state extended Kalman filter
update step is used to find the optimal state estimate,
running at a rate of 10 Hz. Since an error-state formulation
is used, the state mean equals zero at the beginning of each
update step, e.g. δxk−1 = 0.
GNSS observables are prone to a measurement delay in the
region of 20-60 ms. This delay can be accurately measured
using the receiver timestamp in a measurement and a
digital pin of the receiver, that is high at every full second
of receiver time. The Kalman filter update step is then
performed back in time using the corresponding stored
past state vector and the measurement equation
ek = H k δxk + ν k , (15)
where H k is the linearized measurement matrix and ek
contains the difference between predicted and measured
pseudorange and Doppler delta-range for each satellite i:
 
ρpred,i − ρmeas,i
ek,i = (16)
dpred,i − dmeas,i
The predicted observables are calculated using the esti-
mated position, velocity and receiver clock states, and
satellite position and velocity at time of signal transmis-
sion, considering the lever arm between IMU and GNSS
antenna. The entries of the matrix H k can be found in
Wendel (2011). Using the measurement error, δxk results
by evaluating the standard Kalman filter equations. The
posteriori state estimate x+ k is then formed by
(9) −
x+k = x k − δxk , (17)
(10) followed by several strapdown propagation steps (8) to
(10) using stored IMU measurements until the present
time step is reached again.
12182
Proceedings of the 20th IFAC World Congress
11686
Toulouse, France, July 9-14, 2017 Thomas Konrad et al. / IFAC PapersOnLine 50-1 (2017) 11683–11688
3. INITIALIZATION
Before the navigation filter is run, the state vector x0
needs to be initialized using proper values to ensure con-
vergence of the extended Kalman filter. Since a quadcopter
is started at the ground, stillstand is assumed during ini-
tialization. Position and receiver clock error and drift are
estimated by an iterative nonlinear least-square algorithm
using the pseudorange and Doppler observables, as pro-
posed e.g. by Wendel (2011). Velocity and accelerometer
bias values are set to zero, and gyroscope bias to the mean
of the sensor readings during a five second interval. Initial
roll φ and pitch θ angles are then estimated by using the
accelerometer a readings:
φ = atan2 (−ay , −az )
   (18)
θ = atan2 ax , a2y + a2z , experimental test data, minimizing the root mean squared
where atan2 is the quadrant-corrected arctangent func-
tion. For calculating the yaw angle ψ, estimated roll and
pitch angles are used to get corrected readings mn in the
horizontal plane from the raw magnetometer readings mb
in the b-frame:
mn = C n  b m b (19)
ψ = atan2 (−mn,y , mn,x ) , (20)
where C n b is the direction cosine matrix rotating the b-
frame in roll and pitch to align with the local horizontal
plane.
A validation versus the external reference system shows
accuracies of below 5 m for position and below 10 ◦ for
orientation states of the presented initialization approach,
which is sufficient for convergence of the filter algorithm.
4. PARAMETERIZATION
Filter parameters, e.g. entries in the covariance matrices
Qk and Rk , have been determined experimentally, and
fine-tuned for the targeted quadrotor platform.
Accelerometers and gyroscopes are parameterized based
on an Allan Deviation test, (cf. Breuer et al. (2016)),
yielding the noise parameters in table 1 for σa , σg and
σb,a , σb,g of the employed IMU. These values are in a
typical range for a modern industrial-class IMU and are
sufficient for the use in this inertial navigation filter.
Table 1. Standard deviations of the
continuous-time IMU sensor model for
measurement noise (σ) and bias noise (σb )
Sensor σ σb
√ √
Accelerometer 0.8 · 10−3 m/s s 0.4 · 10−4 m/s2 s
√ √
Gyroscope 0.52 · 10−3 rad/ s 0.87 · 10−5 rad/s s
Pseudorange and Doppler ranges are weighed using the
carrier-to-noise ratio C/N0 determined by the receiver.
Thus, a sigma-epsilon model is implemented to calculate
pseudorange and Doppler range variances σ 2 , as proposed
by Wieser et al. (2005):
C/N0
σ 2 = CGN SS 10− 10 , (21)
where C/N0 is the carrier-to-noise ratio in dB Hz and
CGN SS is a constant factor, separate for pseudorange
12183
50 0.2
deltarange σ [m/s]
pseuorange σ [m]
40 0.15
30 0.1
20 0.05
260 time [s] 280 260 time [s] 280
Fig. 3. Typical standard deviations for pseudorange (left)
and Doppler range (right) measurements for three
tracked satellites during the outdoor experiment.
and Doppler observables. CGN SS has been determined
using nonlinear unconstrained optimization on a set of
(RMS) position error versus the reference system. Re-
sulting standard deviations for the GNSS measurements
during a typical outdoor measurement on the targeted
quadcopter are depicted in figure 3, leading to 20 − 50 m
for pseudoranges, and 0.08 − 0.2 m/s for Doppler ranges.
5. EXPERIMENTAL VALIDATION
Experimental validation tests are carried out with a Pel-
ican quadrotor from Ascending Technologies (cf. fig. 1).
GNSS measurements are taken by a Septentrio AsteRx3
receiver in combination with an Antcom G5Ant1AS4
L1/L2 multi-frequency patch antenna, providing uncor-
rected pseudorange and Doppler observables at a rate of
10 Hz, and a digital pulse for time delay calculation. As
IMU, an industrial-grade MicroStrain R
3DM-GX3 R
-25-
OEM is employed. Its axes coincide with the b-frame.
In order to reduce coning and skulling effects caused by
high-frequency vibrations, delta-velocity and delta-angle
measurements are used at 100 Hz, from which mean accel-
erations and angular-rates during the last time step are
calculated.
The high-accuracy reference measurements are provided
by an ART TrackPack motion capture system, consisting
of two infrared cameras and allowing a tracking space of
approximately 10 m3 . Thereby, position and orientation
measurements are generated at a rate of 60 Hz. Due to the
limited tracking space, only parts of the complete flight are
observed. The coordinate system of the tracking system
is leveled and calibrated using static RTK measurements
with a standard deviation of below 1 cm, both in frame
origin and orientation versus north. This calibration pro-
cedure allows direct comparison of the estimated state xk
with measurements of the tracking system.
An overview of the conducted test flight is given in
figure 4, comparing estimated position, least-mean squares
position (LMS) based on raw GPS measurements and
RTK measurements. For best filter convergence, circles
are flown prior entering the area tracked by the reference
system. Thereby, IMU bias and receiver clock error states
all converge to reasonable values, however their actual
value is not known and therefore cannot be validated in
this experiment. As expected, the raw GPS measurements
are smoothed by the filter, however, a static offset to the
RTK solution is observed due to the remaining errors
in pseudoranges. In the north-eastern circle, a loss of
 Proceedings of the 20th IFAC World Congress
Toulouse, France, July 9-14, 2017 Thomas Konrad et al. / IFAC PapersOnLine 50-1 (2017) 11683–11688 11687

15 2

north [m/s]
0
10
north [m]

-2
5 2

east [m/s]
reference 0
available
0
-2
-10 -5 0 5
east [m] 2

down [m/s]
Fig. 4. 2D-overview of flight: estimated state (red), 0
LMS solution (cyan), RTK measurements (blue), and
tracking space of the reference system.
-2
370 380 390 400 410
3 time [s]
north [m]

2
Fig. 6. Estimated velocity (red) compared to reference
1 system (black) and RTK measurements (blue).
0 roll [deg]
20
3
east [m]

2 0
1
0 -20
20
pitch [deg]

3
down [m]

2 0
1
0 -20
370 380 390 400 410
yaw [deg]

time [s]
Fig. 5. Estimated position (red, static errors of 0.2 m in
north, −0.7 m in east, and −2.5 m in down removed)
compared to reference system (black) and RTK mea-
surements (blue).
RTK-fix is visible, causing the receiver position to jump
and thus showing a problem when using unfiltered RTK
measurements for position estimation.
An extract of this flight with typical results for position,
velocity and orientation compared to the reference track-
ing system and RTK measurements is depicted in figures
5 to 7. Note, that the tracking system can only track parts
of the flight, therefore measurement gaps are visible.
For position, static absolute errors of below 1 m in north
and east, and 2.5 m in down are reached by the filter, as
expected due to the remaining errors in pseudorange mea-
surements. Figure 5 shows the estimated position with-
out these static errors, therefore demonstrating the high
relative accuracy during the flight: standard deviations
of below 5 cm horizontally and 15 cm vertically could be
reached during the 500 s flight. At 381 s, a loss of the RTK
ambiguity fix is visible which causes jumps in the receiver’s
position solution, but does not affect the tightly-coupled
estimation.
50
0
-50
370 380 390 400 410
time [s]
Fig. 7. Estimated orientation (red) compared to reference
system (black).
Both velocity and orientation show very high accuracy
(cf. figures 6 and 7). During dynamic movements, no
significant error compared to the tracking system can
be identified. The velocity estimates have a zero-mean
error, and standard deviations below 0.15 m/s in all three
directions. Equally, the errors in orientation have zero
mean and reach standard deviations below 1◦ .
6. COMPARISON WITH RTK-BASED
LOOSELY-COUPLED FILTER
To show the potential of using tightly-coupled filter-
ing with standard GNSS measurements, an RTK-based
loosely-coupled filter is briefly presented to show the dif-
ferences in performance. RTK position pmeas and velocity
v meas measurements as calculated by the GNSS receiver
are used to update the strapdown state propagation in the

12184
Proceedings of the 20th IFAC World Congress
11688
Toulouse, France, July 9-14, 2017 Thomas Konrad et al. / IFAC PapersOnLine 50-1 (2017) 11683–11688

3 7. CONCLUSION AND FUTURE WORK

north [m]

2
In this paper, a tightly-coupled GNSS-based inertial nav-
1 igation filter for multirotor MAVs is presented and exper-
loss of RTK imentally validated. Velocity and orientation states pro-
0
1 vide high dynamic accuracy, even compared to an RTK-
based filter. In future work, atmospheric errors in individ-
east [m]

0.5 ual pseudoranges are minimized using network correction

data, and estimation of the floating ambiguities in carrier
phase measurements are carried out to increase the posi-
0
1 tional accuracy. Moreover, the comparison with the RTK-
based filter motivates a combined approach by switching
down [m]

between both filter designs, such that both the high-

0.5 accuracy of an RTK position and the robustness against
poor satellite coverage of tight-coupling can be exploited.
0
380 381 382 383 384 385
time [s] REFERENCES
Abeywardena, D. and Dissanayake, G. (2015). Tightly-
Fig. 8. Tightly-coupled estimated position (red, static Coupled Model Aided Visual-Inertial Fusion for
errors of 0.2 m in north, −0.7 m in east, and −2.5 m Quadrotor Micro Air Vehicles. In Results of the 9th
in down removed) compared to RTK-based loosely- International Conference, Field and Service Robotics,
coupled filter (green) and reference system (black). volume 105, 153–166. Springer-Verlag Berlin, Berlin.
Breuer, M., Konrad, T., and Abel, D. (2016). High Pre-
1 cision Localisation in Customised GNSS Receiver for
down [m/s]

Railway Applications. In Proceedings of the 29th In-

0 ternational Technical Meeting of The Satellite Division
loss of RTK
-1
380 381 382 383 384
time [s]
Fig. 9. Tightly-coupled estimated velocity (red) compared
to RTK-based loosely-coupled filter (green) and ref-
erence system (black).
EKF. Thereby, local GNSS differential corrections are fed
to the receiver to eliminate ionospheric and tropospheric
delays, and correct for satellite clock and ephemerides
errors. The propagation step stays identical to the one
presented in section 2, (8) to (14). During the Kalman
filter update step, the measurement equation (15) differs
by the matrix H k , cf. Wendel (2011). The error (16) is
then defined as
p − pmeas
ek = . (22)
v − v meas
For parameterizing the filter, IMU variances remain identi-
cal, and Rk is changed to using estimated covariances pro-
vided by the GNSS receiver, reaching standard deviations
of below 2 cm for position and below 0.3 m/s for velocity.
For analysis, results during a short extract of 5 s of the
above flight are depicted in figures 8 and 9. As expected,
the loosely-coupled RTK-based filter shows very low static
position errors (below 2 cm in all directions), but performs
only slightly better concerning the error standard devia-
tions. Furthermore, the loss of RTK ambiguity fix between
381 s and 384 s leads to a drift in the loosely-coupled
position towards the degraded receiver measurement, and
a jump at the point when RTK-fix is regained (cf. circle
in figure 8). Velocity and orientation of both the tightly-
coupled and the loosely-coupled filter perform very similar,
leading to only small differences in the order of reference
measurement uncertainty.
of the Institute of Navigation (ION GNSS 2016).
Collins, J.P. (1999). Assessment and development of a
385 tropospheric delay model for aircraft users of the global
positioning system. Ph.D. thesis, University of New
Brunswick.
Engelhardt, T., Konrad, T., Schäfer, B., and Abel, D.
(2016). Flatness-based Control for a Quadrotor Camera
Helicopter Using Model Predictive Control Trajectory
Generation. In Proceedings of the 24th Mediterranean
Conference on Control and Automation (MED).
Farrell, J. (2008). Aided navigation: GPS with high rate
sensors. McGraw-Hill, Inc.
George, M. and Sukkarieh, S. (2005). Tightly coupled
INS/GPS with bias estimation for UAV applications.
In Proceedings of Australiasian Conference on Robotics
and Automation (ACRA).
Godha, S. and Cannon, M.E. (2007). GPS/MEMS INS
integrated system for navigation in urban areas. GPS
Solutions, 11(3), 193–203.
Klobuchar, J.A. (1987). Ionospheric Time-Delay Algo-
rithm for Single-Frequency GPS Users. IEEE Transac-
tions on Aerospace and Electronic Systems, AES-23(3),
325–331.
Pilz, U., Gropengießer, W., Walder, F., Witt, J., and
Werner, H. (2011). Quadrocopter Localization Using
RTK-GPS and Vision-Based Trajectory Tracking. In
Lecture Notes in Computer Science, PART 1, 12–21.
Springer Berlin Heidelberg.
Wendel, J. (2011). Integrierte Navigationssysteme: Sen-
sordatenfusion, GPS und Inertiale Navigation. Walter
de Gruyter.
Wieser, A., Gaggl, M., and Hartinger, H. (2005). Improved
Positioning Accuracy with High-Sensitivity GNSS Re-
ceivers and SNR Aided Integrity Monitoring of Pseudo-
Range Observations. In Proceedings of the 18th Interna-
tional Technical Meeting of the Satellite Division of The
Institute of Navigation (ION GNSS 2005), 1545–1554.

12185