IEEE TRANSACTIONS ON INDUSTRIAL ELECTRONICS, VOL. 61, NO.

1, JANUARY 2014

495

A Real-Time Adaptive High-Gain EKF, Applied to a

Quadcopter Inertial Navigation System
Kenneth D. Sebesta and Nicolas Boizot

AbstractThe authors demonstrate the practical application

of the adaptive high-gain extended Kalman filter (EKF) (AEKF)
onboard a quadcopter unmanned aerial vehicle (UAV). The AEKF
presents several advantages in state estimation, as it combines
good filtering properties with an increased sensitivity to large
perturbations. It does this by varying the high-gain parameter
according to a metric called innovation. Unlike many adaptive observers, the AEKF is mathematically proven to globally converge,
a significant advantage over the traditional EKF when considering
robust controls. The AEKF is implemented on the UAVs inertial navigation system (INS). Full INSs can have problems when
sensors are noisy and limited, particularly in the case of highly
dynamically unstable systems such as a quadcopter. Simulation
and experimental data show that the AEKF is suitable for this INS.
Index TermsAdaptive, high gain, inertial navigation system
(INS), Kalman filter, unmanned aerial vehicle (UAV).

I. I NTRODUCTION

XTENDED Kalman filters (EKFs) hold a special place in

the world of controls as popular and practical estimation
tools [1], [2]. However, for nonlinear observable systems, the
EKFs convergence is only locally theoretically guaranteed [3],
[4]. If the estimated state is not close enough to the state of the
system, the EKF can permanently diverge.
High-gain observers were thus proposed as a solution to this
problem. They are based on two components:
1) a special representation of the system that characterizes
its observability property;
2) the use of a single scalar parameter (G 1) to modify the
structure of the state and measurement noise covariance
matrices (see Appendix A for details). When G = 1, a
high-gain observer reduces to a classical observer.
In this framework, convergence has been proven to be exponential and global, for any initial error taken in an arbitrarily
large compact set, both for Luenberger- [5][7] and Kalmanstyle observers [8], [9].
The drawback of such observers is their tendency to amplify
measurement noise. Therefore, the use of high-gain observers
seemed to be limited to processes having low noise levels. On

Manuscript received December 27, 2010; revised December 30, 2011,

April 30, 2012, August 2, 2012, November 19, 2012, and January 28, 2013;
accepted January 31, 2013. Date of publication March 15, 2013; date of current
version July 18, 2013. This work was supported in part by the research fund of
the Institut Universitaire de Technologie Toulon-Var.
K. D. Sebesta is with the Department of Mechanical Engineering, Boston
University, Boston, MA 02215 USA (e-mail: kenn@bu.edu).
N. Boizot is with the Laboratoire des Sciences de lInformation et des
Systmes, University of the South Toulon-Var, BP 132-83957 La Garde, France
(e-mail: boizot@univ-tln.fr).
Color versions of one or more of the figures in this paper are available online
at http://ieeexplore.ieee.org.
Digital Object Identifier 10.1109/TIE.2013.2253063

the other hand, experience has shown the EKF to be robust to

noise [10]. The motivation to combine the EKFs noise rejection
with the high-gain observers perturbation sensitivity led to the
adaptive high-gain EKF (AEKF).
The AEKF is based on the concept of innovation, a metric
for the deviation between the historical predicted state and the
historical observed state. For observable systems, innovation
upper bounds the estimation error with a small delay [3].
Therefore, when the innovation becomes arbitrarily high, the
gain is increased, and the AEKF switches from unity-gain mode
to high-gain mode. This scheme provides the observer with a
global convergence property.
Unlike many nonlinear adaptive filters, the convergence of
the AEKF is mathematically proven within the framework of
[8]. Continuouscontinuous time systems are dealt with in [3],
and continuous-discrete time systems are dealt with in [11] and
the references herein.
Quadcopters are a popular topic in unmanned aerial vehicle
(UAV) research. Although the basic dynamic model is relatively
simple [12], the practical application had to wait many years
before hardware was sufficiently quick [13]. Quadcopters are
particularly interesting because their mechanics are simple and
robust, reducing the time spent on fine-scale modeling.
There are several nonlinear control approaches available, and
many of these, such as those using feedback linearization [14],
attitude tracking [15], [16], or geometric tracking [17], depend
on reliable high-quality state estimations.
One challenge with quadcopters is rapid data sensing and
filtering. Even fundamentals such as speed estimation can be
problematic [18]. Furthermore, vibrations inherent in a microquadcopter design can overwhelm inertial sensors, such as accelerometers and microelectromechanical systems gyroscopes
[19][22].
Microquadcopters represent a direct tradeoff between performance and processing power: Higher computing needs directly
impact the flight time and payload. In addition, in an outdoor
environment, absolute positioning systems such as GPS are
difficult to use, because their error and noise are of the same
magnitude as the micro-UAVs radius of action. As such, it
is challenging to balance the filter performance with hardware
requirements. The authors propose that the AEKF, implemented
in a microcontroller, is a suitable solution.
In Section II, the authors present a newer version of the
AEKF and the notion of observability normal form. Section III
details the classic quadcopter model, and in Section IV, the
authors discuss the normal form transformation for the quadcopter, an essential requirement for implementing high-gain observers. Section V gives practical considerations for tuning the

0278-0046/$31.00 2013 IEEE

496

IEEE TRANSACTIONS ON INDUSTRIAL ELECTRONICS, VOL. 61, NO. 1, JANUARY 2014

filter. In Section VI, experimental results are shown, followed

by a conclusion in Section VII. Finally, two short Appendixes
provide in-depth information on how to construct covariance
matrices and calculate innovation.
II. AEKF
High-gain observers have been proven to achieve a global
exponential convergence. This property requires the model to
be in a specific form, called observability normal form, which
characterizes the observability of the system. If there exists a
one-to-one transformation that brings the model into the normal
form of observability, then the model is de facto observable
over this transformations domain [8]. It is important to note
that there is no unique multiple-output normal form. In-depth
discussions can be found in [3], [8], [11], and references herein.
A. Observability Normal Form

is a matrix.
is the estimated state, and P
In the following, z
The algorithm is made of two steps:
1) the continuous prediction step, for t [tk1 , tk ]
d
z
= A(u)
z + b(
z, u)
dt

dP
+P
(A(u) + b (
= (A(u) + b (
z, u)) P
z, u))T + QG
dt
(3)
where
k1
k1 ) = P
P(t

k)
= P(t
P
k

(tk1 ) = z
k1
z

(tk )
z
k =z

2) the discrete correction step, at time tk

For a system with nx states and ny measurements taken at

time instants tk , a multiple-output normal form is

z = A (u(t)) z(t) + b(z(t), u(t)) + v(t)
(1)
y(tk ) = C(tk )z(tk ) + w(tk ).
The following list shows the definitions of the variables in (1).
1) b(z, u) is a Lipschitz, compactly supported vector field.
2) v(t) and w(tk ) are the continuous process and discrete
measurement noises, respectively. They are assumed
white and Gaussian with covariance matrices denoted Qc
and Rd , respectively.
3) The i,j (u) values are bounded nonvanishing functions
except on a set of measure null (otherwise, observability
is lost).
i , of dimenThe vector z is split into p subvectors,
 denoted z
sion nx,i . For i {1, . . . , p}, with pi=1 nx,i = nx
i = Ai (u)
z
zi + bi (z, u).

B. ContinuousDiscrete AEKF

(2)

The structure of the normal form is

0 i,2 (u)
0
...
0

..
.

i,3 (u) . .
.

.
..
..
Ai (u) = .

.
.
0

i,nx,i (u)
0
...
0

bi,1 (
zi,1 , u)
zi,1 , zi,2 , u)
bi,2 (

..
.
bi (z, u) =
.

zi,1 , . . . , zi,nx,i 1 , u)
bi,nx,i 1 (
bi,nx,i (z, u)
While the other bi,j () cannot, the subvector field components
of the form bi,nx,i ()can depend on the full state; see, e.g., [8].
Ck is a (ny nx ) matrix for which the ith row is composed of zeroes except for the element at index (nx,1 + nx,2 +
+ nx,i1 + 1), which equals i,1 (u)i.e., a measurement
is the first element of a subvector up to multiplication by a
u-dependent function.

CT (Ck P
CT + RG )1
Kk = P
k k
k k

k = z

k + Kk y(tk ) Ck z
z
k)

k = (Id Kk Ck )P
P
k
Id,k =

(ti ))2Rny
(ti ti1 )  (y(ti ) y

i=m+1

Gk = F(Gk1 , Id,k ).

(4)

The following list shows the definitions of the functions and

variables in (4).
1) z0 = z(0) and P0 = P(0) are the initial state and covariance. P0 is a symmetric positive-definite matrix.
2) Gk 1 is the high-gain parameter, and G0 = 1.
3) QG and RG are the high-gain modified covariance matrices of v(t) and w(tk ) (cf., Appendix A).
4) Id,k is the innovation, and d is the innovation calculation
horizon. The parameter m refers to the first sample time
at which Id,k is calculated; see details in Appendix B.
(ti ) can also be found there.
The definition of y
5) is a diagonal matrix of normalizing constants, as explained in [23] and given by (21).
6) F(., .) is the adaptation, defined as follows:
[Id,k threshold] = [Gk = min(Gk + 1, Gmax )]
[Id,k < threshold] = [Gk = max(Gk 1, 1)] .
Here, threshold > 0 discriminates nonnull innovation
values due to the measurement noise from nonnull innovation values due to actual perturbations [3], [11].
7) b (
z, u) is the Jacobian matrix (b(z, u)/z)|z,u .
8) Id denotes the identity matrix.
Note that, just like the EKF, the AEKF does not require a
constant time step nor synchronous measurements [11], [23].
In the case of the experimental vehicle, the gyroscope and
accelerometer measurements were sent simultaneously. However, this is an unnecessary simplification, as the measurements
are converted at varying times by an analog-to-digital converter.

SEBESTA AND BOIZOT: REAL-TIME AEKF, APPLIED TO A QUADCOPTER INERTIAL NAVIGATION SYSTEM

497

TABLE I
QUADCOPTER PHYSICAL CONSTANTS

Fig. 1.

Quadcopter diagram.

Discussions on the observability of general asynchronous systems and derivation of an adequate AEKF will be treated in
future works.

,
)
T and body angular velocity = (1 , 2 , 3 )T is ob(,
tained by inverting the relation [28]

1
0
sin()

= 0 cos() sin() cos() .

(6)
0 sin() cos() cos()

III. M ODELING
While more complex models exist for components such as
rotor blades [24] or the effects of wind [25], the authors have
opted to use a model that takes into account only the kinematic
inertia and idealizes the propulsion units as perfect propellers
[26]. This decision is justified by the desire to first and foremost
examine the AEKF approach and capabilities.
Fig. 1 shows a diagram of a quadcopter. Contrary to much
of the literature, the authors have set the positive rotation sense
to be the same for all motors and for the quadcopter heading.
This facilitates future work where individual rotors can change
direction and/or pitch sufficiently to push or pull [27]. For space
reasons, the model is presented in brief. Readers interested in its
derivation may refer to [2], [12], and [14].
As is common in the aerospace field [28], the world frame
is attached to some arbitrary reference point in 3-D space,
with its axes oriented in the north, east, and down (NED)
directions. The body frame is attached to the quadcopter center
of mass. The xyz-directions are along branches 1 and 2 of the
quadcopter and downward (cf., Fig. 1). The SO(3) rotation from
the body to world frame is parameterized by the use of three
successive plane rotations: roll (), pitch (), and yaw (), also
known as the TaitBryan rotation [29]. The associated matrix
representation, where c denotes cos() and s denotes sin(), is

c c s s c c s c s c + s s
R = c s s s s + c c c s s s c . (5)
s
c s
c c
The quadcopter state derives from the full SE(3) representation, modified in order to permit a clean one-to-one invertible
transformation between the original (i.e., physical) coordinates
and the normal form coordinates (cf., Sections II-A and IV).
Thus, the and states are represented by their sines and
cosines.
With the aforementioned discussion in mind, the state
vector is defined as x = (x, y, z, x,
y,
z,
cos , sin , , cos ,
sin , 1 , 2 , 3 )T .
The dynamic model for the 3-D translations (x, y, z)T is
obvious, and the relationship between Euler angle speeds

Readers interested in alternative formulations for the attitude

dynamics and a corresponding observer formulation may refer
to [22], where the quaternion formulation is used.
The dynamic model for the translational velocities (x,
y,
z)
T
and the angular velocities about the body frame axis is
given by force and moment analysis. To this end, denote the
motor speeds 1 , 2 , 3 , and 4 as front, right, rear, and left,
respectively. The control vector is u = (u1 , u2 , u3 , u4 )T , and
the relations between motor speeds and the resulting forces and
torques are


(7a)
u1 = b 21 + 22 + 23 + 24


2
2
(7b)
u2 = b 2 4


(7c)
u3 = b 23 21
4
u4 = i=1 (motor,i )
(7d)
(8)
motor,i = sign(i )2i d.
Finally, b, d, L, Ir , M , Ixx , Iyy , and Izz are physical model
constants. They are detailed in Table I, and values for the
experimental quadcopter platform are provided.
Note 1: Henceforth, x refers to the elements of the x
state vector, instead of the xyz 3-D coordinates (i.e., x8 =
sin ).
The dynamic model is thus

x4
x5

x6

(x7 sin(x9 )x10 + x8 x11 ) u1 /M

(x7 sin(x9 )x11 x8 x10 ) u1 /M

g + (x7 cos(x9 )) u1 /M

(x12 + x8 tan(x9 )x13 + x7 tan(x9 )x14 ) x8

x =
(9)
(x12 + x8 tan(x9 )x13 + x7 tan(x9 )x14 ) x7

x7 x13 x8 x14

(x8 x13 + x7 x14 )x11 / cos(x9 )

(x8 x13 + x7 x14 )x10 / cos(x9 )

(x13 x14 (Iyy Izz ) Ir x13 + Lu2 )/Ixx

(x12 x14 (Izz Ixx ) + Ir x12 + Lu3 ) /Iyy

(x12 x13 (Ixx Iyy ) + u4 ) /Izz

498

IEEE TRANSACTIONS ON INDUSTRIAL ELECTRONICS, VOL. 61, NO. 1, JANUARY 2014


where = 4i=1 |i |. Gravity g is assumed to be constant
and equal to 9.803 m/s2 .
The experimental quadcopter is equipped with a GPS, a
three-axis accelerometer, a three-axis gyroscope, a three-axis
magnetometer, and a barometric pressure sensor.
The GPS and barometric sensor provide measurements for
x1 , x2 , and x3 . The accelerometer measurements, denoted a1 ,
a2 , and a3 in accordance with Fig. 1, are derived into model
outputs by using the following steady-state equation, which
assumes that the quadcopter is not undergoing an external force
aside from gravity

0
a1
a2 = (R )T 0 ( r)
(10)
g
a3
where r is the vector from the quadcopters center of gravity to
the accelerometer. The gyroscope gives , and the accelerometer is assumed to be at the center of gravity, so r = 0.
Finally, with (mN , mE , mD )T being the locally constant
earth magnetic field vector in the NED frame, this yields

mN
mag1
mag2 = (R )T mE .
(11)
mag3
mD
It is instructive to discuss the following situation regarding
the observability normal form. Remark that six equations are
needed to account for the T(3) position and the derivative
in NED frame. The corresponding measurements come from
the GPS and the barometer, leaving nine measurements still
available. Recall from Section II-A that, in this representation,
one output variable corresponds to at least one component of
the state. Hence, the use of all the measurements implies a
15-dimension state at a minimum for a normal form that uses
all the outputs.
A first option is to treat x9 = in the same way as and .
This solution removes all sine-related nonlinearities from the
observers inner algorithm. Another option is to estimate the
extra parameters.
A third option, the one chosen for this paper, is to use only
two magnetometer measurements, denoted maga and magb , and
a 14-dimension state which lessens the observers computational demand. The measurement model is therefore

x1
x2

x3

g sin(x9 )

gx8 cos(x9 )

y = gx7 cos(x9 ) .

maga

magb

x12

x13
x14

(12)

The pair of magnetometer measurements actually used is made

clear from the observability analysis in the next section.

IV. O BSERVABILITY N ORMAL F ORM T RANSFORMATION

The quadcopter and inertial measurement unit observability
have already been demonstrated; see, for example, [2] and
[30]. The normal form for the system made by (9) and (12)
is relatively simple to find, and the only work that remains
is to determine the conditions for the transformation to be
one to one. In other words, we are searching for observability
conditions.
In the domain defined in Section III, the transformation

z1 = x 1
z8 = x8 cos(x9 )

=
x
z9 = x7 cos(x9 )
z

2
4

z10 = maga
z3 = x 2
z11 = magb
(13)
= z4 = x 5

z
=
x
z
=
x

5
3
12
12

z13 = x13

z6 = x 6
z7 = sin(x9 ) z14 = x14
brings the model into a normal form. The analysis shows that,
in order to avoid nonobservability when the UAV is level, mag1
from (11) must be present in the output. Consequently, set
maga = mag1 and magb = mag2 . Conversely, the transformation given in (14) brings the model back into the physical
coordinates

x 1 = z1
x 8 = z8 2

1z7

x 2 = z3
x9 = arcsin(z7 )

x10 = cf., (15)

x 3 = z5
1
x11 = cf., (15)
(14)
= x 4 = z2

=
z
x
=
z
x

5
4
12
12

x 6 = z6
x13 = z13

x7 = z9 2 x14 = z14
1z7

where the complete expressions for x10 and x11 are given in
(15), shown at the bottom of the next page.
As is clear from the expressions of x7 , x8 , x10 , and x11 ,
this transformation is valid for (/2, /2) and as long
as = /2. Also, remark that taking into account only two
magnetometer outputs leads to the m2E + m2N = 0 constraint,
which is valid in areas without magnetic disturbances.
Although it is not reproduced here for space reasons, the
normal form clearly appears through (13). Implementationwise,
it is easy to obtain the equations needed with the help of any
symbolic computation software.
It is also interesting to note the two following facts.
1) First, had there been a direct measurement for heading ,
then the transformation to normal form would have been
trivial and would not have required the addition of the
extra two states in order to disambiguate the estimation of
roll and heading . Indeed, with additional information,
it becomes possible to fully observe an extended state,
such as estimating the wind speed or the quadcopters
center of gravity.
2) Second, in the absence of magnetometer measurements,
heading can only be recovered from the xy accelerations expressed in the world frame. This is possible since
information about the heading () is available in the
fourth and fifth equations of system (9). Since x7 , x8 , and
x9 are recovered from the acceleration measurements,

SEBESTA AND BOIZOT: REAL-TIME AEKF, APPLIED TO A QUADCOPTER INERTIAL NAVIGATION SYSTEM

Fig. 2.

499

Simulation results, innovation, and high-gain adaptation. (a) Innovation. (b) High-gain adaptation (Gmax = 5).

x10 and x11 are the solutions of a system of two

equations. However, if roll and pitch ( and , respectively) are null (i.e., no acceleration in the xy plane),
there are no solutions, and observability is lost.

V. P RACTICAL C ONSIDERATIONS
The AEKF is more computationally demanding than the
EKF, principally due to the calculation of innovation. Fortunately, an extensive horizon is not reasonable, as, due to model
imperfections if the innovation calculation begins too far back
in time, there will always be a large divergence between the
expectation and estimated states.
From experience, the innovation horizon should not be larger
than the expected rise time of a perturbation. For instance, in the
case of a quadcopter, the most common perturbation that one
could (or at least reasonably should) expect is a wind gust. For
micro-UAVs, a wind perturbation has a rise time of less than
1 s, so, by applying the aforementioned rule, the AEKF horizon
should be no more than 1 s.
Note 2: The discussion on innovation is extensive and,
as such, has been completed in Appendix B. There, the
main ideas are developed, and implementation hints are
given. An important topic treated in the Appendix is a
method to compute the normalizing matrix .
Significant speed gains can be made by taking advantage of
the models sparse layout to simplify and propagate ones and
zeroes through the intermediate steps. For instance, the calcula + P(A

+ b )T + QG is simplified greatly in
tion (A + b )P
this case, as most elements in A, b , and QG are null.
Finally, fixed-point algorithms can be used as an alternative
to floating-point calculations, for additional speed. However,
the authors prefer to avoid this approach, as it is less general
and is best suited to a production run where the engineering
time to properly implement and test fixed point is amortized

x10
x11

over a large number of parts, or the additional costs of using

floating point are prohibitive, e.g., in spacecraft.
A. Microcontroller Results
The AEKF and benchmark EKF were implemented in an
ARM Cortex-M4 microcontroller, running at 168 MHz with a
floating-point unit, at a sampling rate of 220 Hz. In addition, the
innovation algorithm can be parallelized, so it is possible to use
additional hardware in case the horizon is so large as to prevent
real-time operation on a single core.
B. Coping With Insufficient Computational Power
1) Real-Time Telemetry: If the embedded resources are insufficient for onboard filtering, the innovation can be calculated
remotely. This allows real-time operation without any concerns
about unknown bus, radio, or interrupt latencies. The innovation
algorithm can be run on a remote platform, which simply sends
the high-gain parameter to the embedded platform.
Using this strategy, the embedded platforms filter time is
completely deterministic, and thus, robustness is increased.
If either the remote computer crashes or communications are
interrupted, the platform is still able to function normally,
with only the loss of the high-gain parameters adaptation.
Furthermore, the required bandwidth is dramatically reduced,
as only the high-gain parameter needs to be sent back to the
model, instead of the entire dynamic state.
As an additional consideration, the high-gain parameter
could be designed to drift toward a preset value in the case
where the high-gain parameter is not refreshed. This strategy
further improves robustness, as it protects against the parameter
staying at a high gain in case communications are lost at an
inopportune time.
2) Innovation: This calculation can be made less demanding if done at a less frequent interval than the observer estimation. However, the result stating that innovation upper bounds


mE z11 (1 z72 ) + z8 (z10 z7 mD ) + mN z9 (z10 + mD z7 )

=
(m2E + m2N ) z9 1 z72


mN z11 (z72 1) + z8 (z10 z7 + mD ) + mE z9 (z10 + mD z7 )

=
(m2E + m2N ) z9 1 z72

(15)

500

IEEE TRANSACTIONS ON INDUSTRIAL ELECTRONICS, VOL. 61, NO. 1, JANUARY 2014

Fig. 3. Simulation yaw results, AEKF versus EKF.

estimation error [3] must be preserved. Promising results were

obtained in practice; however, a formal study has yet to be
completed.
Fig. 4.

VI. S IMULATION R ESULTS

A. AEKF Performance
Simulated estimation results for heading , in Figs. 2 and 3,
provide insight into the AEKFs expected behavior versus that
of the fixed-gain EKF.
In the simulation, the UAV flies in circles; the initial estimated states of both observers are set to the same erroneous
value, and the system is perturbed at time t = 5 with a 1-m/s
velocity step along the N -axis.
Recall that AEKF is tuned to have strong noise-rejection
characteristics with G = 1. However, the AEKF has the benefits
of an adaptation mechanism which increases the value of G
according to the innovation and thus improves its convergence
capabilities.
Fig. 2(a) shows that, when the estimation is poor (i.e., t = 0
and t = 5), the high-pass-filtered innovation increases shortly
thereafter.1 This, in turn, triggers the adaptation of the highgain parameter, shown in Fig. 2(b).
In Fig. 3, the corresponding estimations of are given. The
influence of the high-gain parameter adaptation is visible both
after initial estimation and in the interval 6.1 < t < 6.9.
B. AEKF Tuning
The Rd matrix of the observer was initially tuned using the
sensors datasheets, although sensors such as the GPS require
tuning in order to avoid complicated error models.
Finding an appropriate Qc matrix for high-gain observers
remains an open problem because of the transformation from
physical coordinates to the normal form coordinates.
A first approach is to transform the Qc matrix obtained from
the physical system using the Jacobian of the change of variables. Originally proposed in [23], experimental results show
that it is less efficient when the quadcopter is in a neighborhood
of nonobservability. Moreover, the theoretical convergence of
this method has yet to be examined, since it leads to matrices
depending on the estimated state.
A promising solution is investigated in [31] for systems having continuous measurements: Solve the observers equations in
the physical form coordinates. These equations only necessitate
the knowledge of the transformation and its double-tangent
1 The high-pass filter (HPF) eliminates static model biases. It is normal that
the filtered innovation value be negative from time to time, due to the effect of
the HPF. Innovation itself is always positive, as it is a sum of squares.

Tau Labs Freedom, with optional Gumstix.

TABLE II
QUADCOPTER NORMALIZING VALUES

mapping. However, this solution has not yet been applied to

the continuousdiscrete case, where it is still unclear if the
formulation found for the fully continuous case has either a
continuous-discrete or a fully discrete equivalent.
A third solution is a hybrid-form implementation of the
observer: Use the normal form structure in the correction step
and the physical form structure in the prediction.
As a fourth option, the usage of state and measurement
covariance estimation techniques, such as maximum likelihood
or expectation maximization, can be considered (see, e.g., [32]
and references herein). However, it should be noted that those
methods must be interrupted when innovation increases in order
to avoid conflicts with the effects of an increase in G.
In the present work, experience showed that the normal form
representation was close enough to the physical representation
of the system to be able to tune Q using a standard approach.
VII. E XPERIMENTAL R ESULTS
A. Experimental Platform
Stability tests were performed on a custom-built quadcopter,
running Tau Labs (Fig. 4). The quadcopters parameters are
given in Table I. Table II gives the values used for maximizing
, as explained in (21).
B. Real-Time Performance
A C version of the AEKF algorithm was implemented on
both an Intel Core2Duo and an STM32F4 ARM Cortex-M4.
None of the suggestions presented in Section V-B were necessary for real-time operation.
Table III presents speed results with and without the innovation calculation. As expected, calculating innovation is very
costly, in comparison to the rest of the control loop. It alone can
count for more than 50% of the processing time.

SEBESTA AND BOIZOT: REAL-TIME AEKF, APPLIED TO A QUADCOPTER INERTIAL NAVIGATION SYSTEM

TABLE III
0.5-S HORIZON AEKF SPEED RESULTS PER LOOP

501

estimations, increasing the gain (cf., Fig. 6). This high gain
drives the estimate to the new perturbed state within 30 ms.
The yaw results (Fig. 7) demonstrate how the filter reacts
quickly to gyroscope measurements but slowly to magnetometer measurements. This is desired, as the magnetic field in
theory is constant but, in reality, varies rapidly in the presence
of current-carrying wires or ferromagnetic materials. Owing to
the normalizing matrix , the error between expected yaw angle
and measured yaw angle can be kept from triggering the highgain adaptation, keeping noise rejection high.
VIII. C ONCLUSION

Fig. 5.

Pitch, AEKF versus EKF.

The authors think that the AEKF is a practical observer,

both in performance and in computational requirements. The
authors have demonstrated real-time INS performance on a
quadcopter UAV.
A further note is that, as stated in Section IV, had there been a
pure heading measurement, then the model would have already
been in normal form, opening up the possibility of using the
AEKF to estimate other unknown parameters or state variables.
Two interesting possibilities are the following: 1) estimating
the quadcopters center of gravity and 2) estimating the wind
perturbation [33]. Obviously, both of these play a heavy role in
the control of any UAV, and, particularly in the case of the wind,
are not practically measurable in the field.
A PPENDIX A
C ALCULATING QG AND RG

Fig. 6.

Innovation and high-gain versus time (Gmax = 8).

QG and RG are derived from Qc and Rd , the covariance

matrices of the continuous process and discrete measurement
noises of system (1), respectively. The noise is assumed white
and Gaussian
QG = GG Qc G

(16)

RG = G1 G Rd G

(17)

where G and G are as explained hereinafter.

As mentioned in Section II, the normal form given by the
change of coordinates (13) is made of 11 blocks of the form (2).
Let nx denote the dimension of the largest block. The matrix
G is block diagonal and made of 11 diagonal square blocks
G,i for i {1, . . . , 11}, such that
for i = 1, . . . , 11
Fig. 7.

Yaw, AEKF versus EKF.

C. Filter Performance
Figs. 57 show the effects of a perturbation on the hovering
quadcopter inertial navigation system (INS). At t = 102.55, the
vehicle is externally perturbed (i.e., thumped with a broomstick). As per Section VI-B, the EKF filter was tuned to reject
noise, with the result that it responds very slowly, peaking
almost 100 ms later.
On the other hand, the AEKF innovation metric captures the
difference between the perturbed measurements and the state

for j = 1, . . . , nx,i

G,i (j, j) = Gnx nx,i +j1 .

The matrix G Rny ny is diagonal, with elements determined by taking the first element of each subdiagonal of G
for i = 1, . . . , 11

G (i, i) = G,i (1, 1).

The dimensions of the blocks that compose the normal form

proposed for the quadcopter, together with the corresponding
diagonal elements of both G and G , are given in Table IV.
The interested reader can find the complete justifications for
this structure in [3], [8], and references herein.

502

IEEE TRANSACTIONS ON INDUSTRIAL ELECTRONICS, VOL. 61, NO. 1, JANUARY 2014

TABLE IV
C ALCULATING G AND G FOR THE Q UADCOPTER

Fig. 8. Innovation timescale.

A PPENDIX B
C ALCULATING I NNOVATION
The innovation at time t is calculated in a moving window
from (t d) to t, and as such, d is the characterizing parameter.
Due to the variable time-step nature, t d need not necessarily
be a time when a sensor measurement was taken (see Fig. 8).
Innovation for a variable-step asynchronous continuous
discrete system is written as
Id,k (t)

i )2Rny
(ti ti1 ) (yi y

ACKNOWLEDGMENT
(18)

i=m+1

where

m = index of

anticipated northerly velocity as the normalizing value. See

also Table II.
This is not the only possible solution to normalizing the
innovation. Indeed, as a noisy measurement can attain any arbitrarily large value in infinite time, it is impossible to normalize
in a strict sense. The reader is advised that different approaches
might be more appropriate for other systems.
To put it in other words, innovation is the sum of the lengths
of the time interval, (ti ti1 ), times the weighted difference
between the measured state and the predicted state, y
2Rny , of each time interval since t d seconds. See Fig. 8.
y
Innovation can be thought of as an experiment to see what
happens if the observer correction is turned off at time t d
and the system is allowed to continue uncorrected. In some
sense, the amount of innovation is proportional to the square
of the norm of the error between the measured trajectory and
the uncorrected trajectory, at time t d. See [3], [11], and
references herein for a thorough treatment.
Finally, a simple HPF, for example, of the type Id,k =
(Id,k1 + (Id,k Id,k1 )), is used in order to ensure that
innovation is not affected by static model biases. is any
constant between zero and one that gives acceptable filtering
behavior. The authors suggest = 0.7 as a starting value.

The authors would like to thank the Syn2Cat [34] for the
generous use of its quadcopter and R. J. Cotton and Tau Labs
[35] for providing much technical assistance.


min (tm t d)

tm [t0 ,tk ]

(19)

i is
and is a normalizing matrix (defined hereinafter) and y
found by integrating the following initial value problem, with
(tm ):
z(tm ) = z

z = A(u)z + b(z, u)
(20)
i = Ci z(ti ).
y
The normalizing matrix is meant to ease the use of innovation. Since innovation is computed from variables that do not
have uniform amplitudes, these same must be weighted such
that comparable variations have a comparable impact on the
result. As such, can be constructed as a (ny ny ) diagonal
matrix, each diagonal element corresponding to an output variable yi , for i {1, . . . , ny }. The maximal expected variation
for a given output is sought by using the time derivative of
its analytic expression in the physical coordinates: d/dt(yi ).
Then, we consider the set defined by the maximum anticipated
state values, xi [xi,max , xi,max ], and take the inverse of the
maximum for d/dt(yi ) over this set


1
d(yi )
i,i = max
.
(21)

x,x
dt
For example, if there were a direct measurement of North, then
the normalizing corresponding to this would be found by first
deriving North to obtain a velocity and then using the highest

R EFERENCES
[1] A. Gelb, Ed., Applied Optimal Estimation. Cambridge, MA, USA: MIT
Press, 1974.
[2] S. Lesecq, S. Gentil, and N. Daraoui, Quadrotor attitude estimation with
data losses, in Proc. ECC, Budapest, Hungary, 2009, CD-ROM.
[3] N. Boizot, E. Busvelle, and J.-P. Gauthier, An adaptive high-gain observer for nonlinear systems, Automatica, vol. 46, no. 9, pp. 14831488,
Sep. 2010.
[4] A. B. J. S. Baras and M. R. James, Dynamic observers as asymptotic
limits of recursive filters: Special cases, SIAM J. Appl. Math., vol. 48,
no. 5, pp. 11471158, Oct. 1988.
[5] J. P. Gauthier, H. Hammouri, and S. Othman, A simple observer for nonlinear system. Application to bioreactors, IEEE Trans. Autom. Control,
vol. 37, no. 6, pp. 875880, Jun. 1992.
[6] F. Esfandiari and H. K. Khalil, Output feedback stabilization of fully
linearizable systems, Int. J. Control, vol. 56, no. 5, pp. 10071037, 1992.
[7] A. Tornamb, High-gain observers for non-linear systems, Int. J. Syst.
Sci., vol. 23, no. 9, pp. 14751489, 1992.
[8] J. Gauthier and I. Kupka, Deterministic Observation Theory and
Applications. Cambridge, U.K.: Cambridge Univ. Press, 2001.
[9] F. Deza, E. Busvelle, and J. Gauthier, Exponentially converging observers for distillation columns and internal stability of the dynamic
output feedback, Chem. Eng. Sci., vol. 47, no. 15/16, pp. 39353941,
Oct./Nov. 1992.
[10] J. Picard, Efficency of the EKF for nonlinear systems with small noise,
SIAM J. Appl. Math., vol. 51, no. 3, pp. 843885, Jun. 1991.
[11] N. Boizot, E. Busvelle, and J.-P. Gauthier, Adaptive-gain extended
Kalman filter: Extension to the continuous-discrete case, in Proc. 10th
ECC, Budapest, Hungary, 2009, CD-ROM.
[12] P. Castillo, R. Lozano, and A. Dzul, Stabilization of a mini rotorcraft
with four rotors, IEEE Control Syst. Mag., vol. 25, no. 6, pp. 4555,
Dec. 2005.
[13] S. B. Anderson, Historical overview of V/STOL aircraft technology,
NASA, Washington, DC, USA, Tech. Rep., 1981.
[14] H. Voos, Nonlinear control of a quadrotor micro-UAV using feedbacklinearization, in Proc. IEEE Int. Conf. Mechatron., 2009, pp. 16.

SEBESTA AND BOIZOT: REAL-TIME AEKF, APPLIED TO A QUADCOPTER INERTIAL NAVIGATION SYSTEM

[15] B. Zheng and Y. Zhong, Robust attitude regulation of a 3-DOF helicopter

benchmark: Theory and experiments, IEEE Trans. Ind. Electron., vol. 58,
no. 2, pp. 660670, Feb. 2011.
[16] Y. Xia, Z. Zhu, M. Fu, and S. Wang, Attitude tracking of rigid spacecraft
with bounded disturbances, IEEE Trans. Ind. Electron., vol. 58, no. 2,
pp. 647659, Feb. 2011.
[17] T. Lee, M. Leoky, and N. McClamroch, Geometric tracking control of a
quadrotor UAV on SE (3), in Proc. CDC, 2010, pp. 54205425.
[18] K. Benzemrane, G. Damm, and G. L. Santosuosso, Adaptive observer
and Kalman filtering, in IFAC World Congr. Proc., 2008, pp. 38653870.
[19] The Influence of Body Structure Vibration on Sensor Performance, 2010. [Online]. Available: http://www.xaircraft.org/2010/10/
influence-of-body-structure-vibration.html
[20] J. Lim and D. Hong, Cost reference particle filtering approach to highbandwidth tilt estimation, IEEE Trans. Ind. Electron., vol. 57, no. 11,
pp. 38303839, Nov. 2010.
[21] Y. S. Suh, Attitude estimation by multiple-mode Kalman filters, IEEE
Trans. Ind. Electron., vol. 53, no. 4, pp. 13861389, Jun. 2006.
[22] H. G. de Marina, F. J. Pereda, J. M. Giron-Sierra, and F. Espinosa, UAV
attitude estimation using unscented Kalman filter and TRIAD, IEEE
Trans. Ind. Electron., vol. 59, no. 11, pp. 44654474, Nov. 2012.
[23] K. Sebesta, N. Boizot, E. Busvelle, and J. Sachau, Using an adaptive
high-gain extended Kalman filter with a car efficiency model, presented
at the ASME Dynamic Systems Control Conf., Cambridge, MA, USA,
2010, Paper WeAT5.5.
[24] P.-J. Bristeau, P. Martin, E. Salan, and N. Petit, The role of propeller
aerodynamics in the model of a quadrotor UAV, in Proc. Eur. Control
Conf., 2009, pp. 683688.
[25] P. Pounds, R. Mahony, and P. Corke, Modelling and control of a large
quadrotor robot, Control Eng. Pract., vol. 18, no. 7, pp. 691699, 2010.
[26] H. Voos and B. Haitham, Nonlinear tracking and landing controller for
quadrotor aerial robots, in Proc. IEEE Int. CCA, 2010, pp. 21362141.
[27] M. Cutler, N. K. Ure, B. Michini, and J. P. How, Comparison of fixed and
variable pitch actuators for agile quadrotors, in Proc. AIAA Guid., Navig.
Control Conf., 2011, pp. 117.
[28] T. Kane, P. Likins, and P. Levinson, Spacecraft Dynamics. New York,
NY, USA: McGraw-Hill, 1983.
[29] J. Diebel, Representing attitude: Euler angles, unit quaternions, and rotation vectors, Tech. Rep., 2006.
[30] A. Freddi, S. Longhi, and A. Monteriu, A model-based fault diagnosis system for a mini-quadrotor, in Proc. Adv. Control Diagn., 2009,
pp. 1920.
[31] F. Lafont, E. Busvelle, and J.-P. Gauthier, An adaptive high-gain observer
for wastewater treatment systems, J. Process Control, vol. 21, no. 6,
pp. 893900, Jul. 2011.

503

[32] V. A. Bavdekar, A. P. Deshpande, and S. C. Patwardhan, Identification

of process and measurement noise covariance for state and parameter
estimation using extended Kalman filter, J. Process Control, vol. 21,
no. 4, pp. 585601, Apr. 2011.
[33] A. Mokhtari and A. Benallegue, Dynamic feedback controller of Euler
angles and wind parameters estimation for a quadrotor, in Proc. IEEE
Robot. Autom. Conf., 2004, pp. 23592366.
[34] Syn2Cat, 2013. [Online]. Available: http://www.hackerspace.lu
[35] Tau Labs, 2013. [Online]. Available: http://www.taulabs.org

Kenneth D. Sebesta received the M.S. degree in

mathematics and process control from the University
of Burgundy, Dijon, France, in 2005 and the Ph.D.
degree jointly from the University of Luxembourg
and the University of Burgundy in 2010.
He is currently with Boston University, Boston,
MA, USA. His research interests are autonomous
flight and collision avoidance.

Nicolas Boizot was born in 1977. He received the

M.S. degree in mathematics and process control from
the University of Burgundy, Dijon, France, in 2005
and the Ph.D. degree jointly from the University
of Luxembourg and the University of Burgundy
in 2010.
He is currently an Associate Professor with the
Laboratoire des Sciences de lInformation et des
Systmes (Unit mixte de recherche 7296), University of the South Toulon-Var, La Garde, France.
His research interests include motion planning,
sub-Riemannian geometry, nonlinear observer design, and their practical
implementation.