Aerospace Science and Technology 9 (2005) 504516

www.elsevier.com/locate/aescte
Low cost navigation system for UAVs

Sistema de navegacin para aeronaves no tripuladas de fcil realizacin

Sergio de La Parra

, Javier Angel
Flight Mechanics Laboratory, Flight Test Department, INTA, Torrejon-Ajalvir s/n, Madrid 28850, Spain
Received 12 May 2004; received in revised form 14 June 2005; accepted 23 June 2005
Available online 27 July 2005
Abstract
This paper describes the development of a navigation system for SIVA family of UAVs that incorporates measurements from low cost
solid state IMU and magneto-resistive magnetometer, single receiver inexpensive GPS, and absolute and differential pressure transducers.
The navigator is composed of three modules: an attitude estimator, a position and velocity estimator and a mission management module. All
the UAV navigation, guidance, control and communication algorithms and processes run on a PC-104 Pentium I processor.
The attitude estimator algorithm is a computationally-inexpensive nonlinear observer developed using Lyapunov theory that computes
angular velocity corrections from two vector measurements: Earths magnetic eld and gravity, guaranteeing global Input to State Stability.
The position and velocity estimator consists of a set of three static linear Kalman lters that correct the integration of local level components
of acceleration with GPS, pressure derived altitude and airspeed measurements.
The autonomous navigation is performed by means of pre-programmed mission data, composed by a set of mission elements which dene
the trajectory to guide the UAV through a safe path. Mission data, mission phases and ground control modes are managed by the mission
management module.
2005 Elsevier SAS. All rights reserved.
Resumen
Este artculo describe el desarrollo de un sistema de navegacin para la familia de aeronaves no tripuladas SIVA que fusiona medidas de
sensores baratos: IMU, magnetmetro, GPS, presin diferencial y de impacto. El navegador consta de tres mdulos: un estimador de actitud,
un estimador de posicin y velocidad y un mdulo de gestin de misin. Todos los algoritmos del sistema de navegacin guiado y control de
la aeronave se ejecutan en un procesador Pentium I PC-104.
El algoritmo de estimacin de actitud es un observador no lineal que calcula correcciones de velocidad angular a partir de medidas de
campo magntico y gravedad, requiere muy poco proceso y garantiza estabilidad Input to Satate.
El estimador de posicin y velocidad consiste en tres ltros Kalman estticos que corrigen la integracin de la aceleracin con GPS, altitud
baromtrica y velocidad anemomtrica.
La navegacin autnoma se desarrolla en base a datos de misin preprogramados constituidos por un conjunto de elementos de misin que
denen la trayectoria que ha de seguir la aeronave para volar de forma segura. Los datos, fases de misin y modos de control son procesados
en el mdulo de gestin de misin.
2005 Elsevier SAS. All rights reserved.
Keywords: Navigation; Attitude determination; Sensor fusion
Palabras clave: Navegacin; Determinacin de Actitud; Fusin de sensores

This article was presented at ODAS 2004.

*
Corresponding author. Tel.: +34 915201424; fax: +34 915201703.
E-mail addresses: parracs@inta.es (S. de La Parra), angelmf@inta.es (J. Angel).
1270-9638/$ see front matter 2005 Elsevier SAS. All rights reserved.
doi:10.1016/j.ast.2005.06.005
S. de La Parra, J. Angel / Aerospace Science and Technology 9 (2005) 504516 505
Nomenclature
WGS-84 World Geodetic System 1984
INS Inertial Navigation System
IMU Inertial Measurement Unit
GCS Ground Control Station
DCM Direction cosine matrix
P Position vector relative to the origin of the
t-frame (range vector)
R Position vector relative to the center of mass of
the Earth
R The set of all real numbers
R
n
An n-dimensional real vector space
V Earth relative velocity vector
v Aerodynamics velocity
f Specic force vector. A specic force is the force
per unit mass of any non-gravitational force
g Apparent gravity vector
Geodetic latitude
Geodetic longitude
h Geodetic altitude
Notation based on subscripts and superscripts
X
j
Denotes vector X expressed in the j-frame
R
k
j
Position vector of the origin of the j-frame rela-
tive to the center of mass of the Earth expressed
in k-frame
P
k
j
Position vector of the origin of the j-frame rel-
ative to the origin of the t-frame expressed in
k-frame
V
k
j
Velocity vector of the origin of the j-frame rela-
tive to the t-frame expressed in the k-frame
C
j
k
DCM from k-frame to j-frame

j
lk
The rate of angular rotation of frame k respect to
frame l resolved in frame j

j
lk
Skew symmetric matrix equivalent to the angular
velocity vectors cross product matrix: [
j
lk
]
g
k
b
Gravity at the origin of the b-frame resolved in
k-frame
g
k
t
Gravity at the origin of the t-frame resolved in
k-frame
Notation based on diacritical marks
x Vector actual value
x Vector estimated or computed value
x Vector error dened as the actual value minus the
estimated or computed value. x =x x

x Vector measured value

x Vector error dened as the actual value minus the
measured value. x =x

x
x Derivative with respect to time
x Unit vector
1. Introduction
The Navigation System is one of the major subsystems
for a UAV. It provides the rest of subsystems with the posi-
tion, velocity and attitude information they require in order
to control the aircraft, manage the mission or inform the pi-
lot.
Early on, the majority of these systems were high-quality
and very expensive Inertial Navigation Systems. Advances
in Micro-machine technology have produced smaller and
low cost inertial sensors. These inertial sensors are low pre-
cision and not directly usable as sole navigation systems.
Nevertheless, by integration with GPS and other sensors,
they can achieve the navigation grade accuracy required.
Numerous approaches are possible for INS/GPS integration.
One of the most basic design questions is whether to use a
tightly coupled approach or a loosely coupled one [1]. Al-
though the rst method has better performance, the later one
is simpler to implement.
On the other hand, space-based augmentation systems
such as Wide-Area Augmentation System (WAAS) or Eu-
ropean Geostationary Navigation Overlay System (EGNOS)
enhance GPS accuracy, integrity and availability to the level
required for navigation, but still require some grade of in-
tegration with inertial sensors, in order to increase the low
GPS bandwidth up to the one needed by control subsystems
and to achieve attitude estimation.
The accelerations and angular rate provided by the Iner-
tial Measurement Unit (IMU) can be integrated by an Inertial
Navigation System (INS) algorithm to obtain the aircraft po-
sition, velocity and attitude, but because this integration is
neutrally stable, IMU inaccuracy and integration errors will
cause the solution to diverge quickly. Thus, loop closure is
required to stabilize the integration and this is accomplished
by incorporating other measurements to compute correc-
tions to the acceleration and angular rate. The most common
algorithms used to augment the INS are the Complemen-
tary Kalman Filter (CKF) and the Extended Kalman Filter
(EKF).
In the CKF implementation an INS algorithm is used to
obtain position and velocity estimates that are compared to
the position and velocity obtained from the GPS and the rest
of sensors to generate an error signal. The error signal drives
a linear Kalman Filter that estimates the INS errors that are
used to correct the position, velocity and attitude output and
the IMU input to the INS algorithm. This method has the
disadvantage that the assumption of linearity for the error
estimates is not always valid. In order to decrease the com-
putational load, the Kalman lter is often updated at a low
rate while the INS (which is a computationally-inexpensive
application) runs at higher rate.
506 S. de La Parra, J. Angel / Aerospace Science and Technology 9 (2005) 504516
Fig. 1. Block diagram for attitude estimation.
The EKF, which is more accurate than the CKF, performs
well in most practical applications, but it is an approximate
lter and global stability cant be guaranteed. It accepts non-
linear models but since the covariance equations are based
on the linearized system, the EKF has a higher risk of di-
vergence than the standard Kalman lter. The algorithm is
similar to the CKF in terms of processor time requirements.
However, since it runs at a higher rate, it does have higher
computational load.
This paper presents a different approach as shown in
Fig. 1. As in the CKF an INS algorithm is used to integrate
the angular velocity but, instead of using a linear Kalman
for estimating INS errors, the kinematic equations are sta-
bilized by means of a nonlinear observer. In this observer,
a nonlinear control law generates corrections to the angular
velocity from comparisons of body-axis observations of the
Earths magnetic eld and gravity against earth-axis compo-
nents of these two vectors provided by a reference model.
The body-axis gravity is computed using the translational
equations and measurements of acceleration, airspeed, an-
gular velocity and angles of attack and sideslip. The lter
provides Global Uniform Asymptotic Stability for perfect
measurements and global Input to State Stability (ISS) for
noisy measurements.
Once attitude is obtained, the body axis components of
acceleration are rotated with the Director Cosine Matrix
(DCM) to obtain the t-frame components of acceleration and
used as the inputs to three stationary linear Kalman lters for
the North, East and Down channels.
2. Notation and denitions
2.1. Denitions
Body Frame (b-frame): frame xed to the vehicle. The
origin is attached to the center of mass of the vehicle, the
x-axis is dened in the forward direction, the z-axis is de-
ned pointing to the bottom of the vehicle and the y-axis
completes the right- handed orthogonal coordinate system.
Inertial Frame (i-frame): is the Earth Centered Inertial
(ECI). The origin of the ECI system is the center of mass of
the Earth. The fundamental plane is the Earths equator with
the x-axis pointing towards the vernal equinox, the y-axis is
90 degrees to the east in the equatorial plane and the z-axis
points to the Conventional Terrestrial Pole (CTP).
Earth Frame (e-frame): is an Earth Centered Earth Fixed
(ECEF). Like the ECI frame, the origin of the ECEF system
is at the Earths center of mass and its fundamental plane
is the Earths equator. The difference is that it rotates with
the Earth. The x-axis is always aligned with the Greenwich
meridian, the z-axis points to the CTP and coincides with the
rotation axis of the WGS 84 ellipsoid.
Geodetic Frame (g-frame): Latitude (), longitude (),
and altitude (h) are the three parameters of this curvilinear
coordinate frame. Longitude is the angle between the Green-
wich meridian line and the position vector projected onto the
equatorial plane. The angle of latitude is between the plane
of equator and the gravity vector. The geodetic vertical is
everywhere normal to the WGS 84 reference ellipsoid and
S. de La Parra, J. Angel / Aerospace Science and Technology 9 (2005) 504516 507
altitude is the height of the measured location with respect
to the ellipsoid.
Local Geodetic Frame (n-frame): The origin is attached
to the center of mass of the vehicle. The x-axis points to the
direction of geodetic north; the y-axis is perpendicular to
the meridian plane containing the vehicle, directed towards
the east; and the z-axis points to the interior of the Earth
perpendicular to the reference ellipsoid and passing through
the vehicle.
Ground Station Frame (t-frame): local geodetic frame de-
termined by the tting of a tangent plane to the geodetic
ellipsoid at a xed point at the surface of the Earth near the
location of the Control Ground Station. The x-axis points to
true north, the y-axis points to east, and the z-axis points to
the interior of the Earth perpendicular to the reference ellip-
soid. The t-frame remains xed while the vehicle moves.
Class K, K
inf
, KL functions [6]: A function : R
0

R
0
is said to be of class K if it is continuous strictly
increasing and satises (0) =0; it is of class K
inf
if in ad-
dition (s) as s . A function : R
0
R
0

R
0
is said to be of class KL if for each xed t the mapping
(s, t ) is of class K and for each xed s it is decreasing to
zero on t as t .
3. Navigation around the GCS
The following subsection develops differential equations
relating to the time rate of velocity change to specic force,
velocity, attitude and position for vehicles that do not move
away but a few hundreds of km from the GCS. This is not
unusual for UAVs that have to maintain line-of-sight with
the GCS due to radio-link requirements.
3.1. Earth relative velocity
The UAV position relative to the center of the Earth is
R
e
b
=R
e
t
+P
e
b
. (3.1)
The derivative of Eq. (3.1) , taking into account that

R
e
t
=0
and that by denition V
e
b
=

P
e
b
, yields

R
e
b
=

P
e
b
=V
e
b
. (3.2)
Taking the time derivative of R
i
b
=C
i
e
R
e
b
using the Theorem
of Coriolis and substituting Eq. (3.2) results in

R
i
b
=
i
ie
R
i
b
+C
i
e

R
e
b
=
i
ie
R
i
b
+V
i
b
. (3.3)
The second time derivative of Eq. (3.3) is

R
i
b
=

i
ie
R
i
b
+
i
ie

R
i
b
+

V
i
b
=
i
ie
_

i
ie
R
i
b
+V
i
b
_
+

V
i
b
=
i
ie

i
ie
R
i
b
+
i
ie
V
i
b
+

V
i
b
,

V
i
b
=

R
i
b

i
ie

i
ie
R
i
b

i
ie
V
i
b
, (3.4)
where the Earths rotation rate is assumed to be constant, or

ie
=0. (3.5)
In an inertial frame of reference, Newtons law for transla-
tional motions and constant vehicle mass take the form

R
i
b
=f
i
+G
i
, (3.6)
where G
i
represent the position-dependent gravitational ac-
celeration and f
i
is the specic force, both expressed in the
i-frame. The local apparent gravity vector is dened as:
g
i
=G
i

i
ie

i
ie
R
i
b
, (3.7)
where the second term is the local centrifugal acceleration.
Substituting G
i
in Eq. (3.6) yields

R
i
b

i
ie

i
ie
R
i
b
=f
i
+g
i
. (3.8)
Substitution of specic force and local apparent gravity in
Eq. (3.4) results in

V
i
b
=f
i
+g
i

i
ie
V
i
b
. (3.9)
The Earth-relative velocity V
e
b
can be expressed in the t-
frame
V
t
b
=C
t
e
V
e
b
. (3.10)
The derivative of Eq. (3.10) taken into account that
et
=0,
is

V
t
b
=C
t
e

V
e
b
. (3.11)
Taken the time derivative of V
e
b
=C
e
i
V
i
b
, using the Theorem
of Coriolis and substituting Eq. (3.4) yields

V
e
b
=
e
ie
V
e
b
+C
e
i
_
f
i
+g
i

i
ie
V
i
b
_
=f
e
+g
e
2
e
ie
V
e
b
. (3.12)
Substituting in Eq. (3.11)

V
t
b
=f
t
+g
t
2
t
ie
V
t
b
. (3.13)
Eq. (3.13) yields the differential equation for the Earths rel-
ative velocity as expressed in the t-frame. The third term of
the second row represents Coriolis acceleration and is of the
order of 10
2
m/s
2
for subsonic aircrafts. The second term
is the specic force expressed in t-frame and can be calcu-
lated from
f
t
=C
t
b
f
b
, (3.14)
where f
b
is the specic force in b-frame obtained from
accelerometer measurements, once transformed from the
nearly orthogonal IMU input axis to b-frame and compen-
sated for position error of the origin of this sensor frame
relative to the b-frame.
The computation of the DCM matrix C
t
b
is a keystone for
this article and will be treated in the next section.
The second term of the second row in Eq. (3.13) is the
apparent gravity expressed in the t-frame and will be further
developed in a posterior subsection.
508 S. de La Parra, J. Angel / Aerospace Science and Technology 9 (2005) 504516
3.2. Transformation from geodetic latitude, longitude and
altitude to t-frame
GPS receivers provide position as geodetic (, , h) that
can be transformed to ECEF position coordinates [1,4]
R
e
b
=
_
_
x
y
z
_
_
e
;
x
e
=(R

+h) cos cos ,

y
e
=(R

+h) cos sin,

z
e
=
_
R

(1 e
2
) +h
_
sin,
(3.15)
where e, f are the eccentricity and atness of the WGS-84
ellipsoid dened as
f =
r
e
r
p
r
e
=0.0034, e =
_
f (2 f ) =0.0818,
semimajor axis length : r
e
=6378137.0 m,
semiminor axis length : r
p
=6356752.3142 m.
The radius of curvature normal to the ellipsoid surface along
lines of constant latitude is
R

=
r
e
_
1 e
2
sin
2

. (3.16)
The ECEF position for the origin of the t-frame
R
e
t
=
_
_
x
0
y
0
z
0
_
_
e
;
x
e
0
=(R
0
+h
0
) cos
0
cos
0
,
y
e
0
=(R
0
+h
0
) cos
0
sin
0
,
z
e
0
=
_
R
0
(1 e
2
) +h
0
_
sin
0
,
(3.17)
where
0
,
0
, h
0
are latitude, longitude and altitude of the
origin of the t-frame.
The position relative to the t-frame resolved in t-frame is
P
t
b
=C
t
e
_
R
e
b
R
e
t
_
, (3.18)
where [4]
C
t
e
=
_
_
sin
0
cos
0
sin
0
sin
0
cos
0
sin
0
cos
0
0
cos
0
cos
0
cos
0
sin
0
sin
0
_
_
.
(3.19)
3.3. Gravity model
In the local geodetic frame (n-frame) the local gravity
vector is dened to be
g
n
b
=
_
0
0
(, h)
_
+
g
, (3.20)
where
g
represents gravity anomaly terms. The formulas
for (, h) when the WGS-84 parameters are used are [1]
() =9.780327
_
1 +0.0053024sin
2
()
0.0000058sin
2
(2)
_
m/s
2
,
(, h) = () (3.21)

_
3.0877 10
6
0.0044 10
6
sin
2
()
_
h
+0.072 10
12
h
2
m/s
2
,
where is geodetic latitude and h is altitude.
The unit vector in the direction of the gravity expressed
in e-frame, neglecting gravity anomaly terms, is
g
e
b
=
_
cos cos
cos sin
sin
_
. (3.22)
Finally, gravity expressed in t-frame is compute as follows
g
t
b
=

(, h)

C
t
e
g
e
b
. (3.23)
4. Attitude estimation
As it has been said previously, for attitude estimation it
is necessary to stabilize the integration of kinematics equa-
tions. In order to achieve it, we will pose the problem like
one of control in order to nd corrections to the angular
velocity to cancel out errors in the estimation of body-axis
measurements of two non-collinear unitary vectors.
Recall that it takes three independent parameters to deter-
mine the attitude, and that a unit vector is actually only two
parameters because of the unit vector constraint. Therefore
we require at least two non-collinear unitary vector measure-
ments for attitude estimation. These vectors will be related
to the earths magnetic eld and the gravity.
4.1. Development of the algorithm
4.1.1. Problem posing
If x
j
, y
j
are two unitary constant vectors expressed in j-
frame, and x
k
, y
k
are the same vectors expressed in k-frame,
being j an k any two frames; C
j
k
is the rotation matrix from
k-frame to j-frame and
j
kj
is the angular velocity of j-frame
relative to k-frame expressed in j-frame, we have:
x
j
=C
j
k
x
k
, x
j
=
j
kj
x
j
,
y
j
=C
j
k
y
k
, y
j
=
j
kj
y
j
. (4.1)
In a similar way, if C
m
k
is the rotation matrix from k-frame to
m-frame and
m
km
is the angular velocity of m-frame relative
to k-frame expressed in m-frame, we have
x
m
=C
m
k
x
k
, x
m
=
m
km
x
m
,
y
m
=C
m
k
y
k
, y
m
=
m
km
y
m
. (4.2)
If x, y are two vectors (in general x = x and y = y) which
components in j-frame x
j
, y
j
have the same values as the
components of x
m
, y
m
in the m-frame, and is another
vector which components in the j-frame
j
have the same
values as the components of
m
km
in the m-frame, substitut-
ing in Eq. (4.2) results
x
j
=x
m
y
j
=y
m

j
=
m
km

x
j
=C
m
k
x
k
,

x
j
=
j
x
j
,
y
j
=C
m
k
y
k
,

y
j
=
j
y
j
.
(4.3)
S. de La Parra, J. Angel / Aerospace Science and Technology 9 (2005) 504516 509
Calling the errors between these vectors as
x
j
=x
j
x
j
,
y
j
=y
j
y
j
,
u =
j

j
kj
. (4.4)
The derivative of the rst and second row of Eq. (4.4) yields

x
j
= x
j

x
j
=
j
kj
x
j
+
j
x
j
,

y
j
= y
j

y
j
=
j
kj
y
j
+
j
y
j
. (4.5)
Substitution of the third row of Eq. (4.4) results in

x
j
=
j
kj
x
j
+
_

j
kj
+u
_
x
j
=
j
kj
x
j
+u x
j
,

y
j
=
j
kj
y
j
+
_

j
kj
+u
_
y
j
=
j
kj
y
j
+u y
j
. (4.6)
The goal now is, given initial errors x
j
(0), y
j
(0), to deter-
mine u(t ) in Eq. (4.6) in order to cancel out x
j
(t ), y
j
(t ) as
t . This is a problem of nonlinear control, that will be
further related to the stabilization of the angular kinematics
equations, and that will get worked out utilizing an exten-
sion of the Lyapunov function concept, the control Lyapunov
function (CLF) [8], in the following steps:
1. We will form an error system:
z =f (z, u, t ), z =
_
x
j
y
j
_
, f (0, 0, 0) =0.
(4.7)
Then our problem is to design a control law (z, t ) for
the control variable u such that the equilibrium z =0 of
the closed-loop system
z =f
_
z, (z, t ), t
_
(4.8)
is globally asymptotically stable.
2. We will nd a continuous differentiable function V(z)
satisfying the following property

1
_
|z|
_
V(z)
2
_
|z|
_
(4.9)
z R
3
, where
1
,
2
are class K
inf
functions [2, Ap-
pendix A], as a Lyapunov candidate.
3. We will require that the derivative of V(z) along the so-
lutions of (4.7) satisfy

V =
V
z
(z)f
_
z, (z, t ), t
_
W(z) 0, (4.10)
where W(z) is a continuous function.
Then, the LaSalleYoshizawa theorem [8] will guarantee
that the solutions of the system (4.8) are globally uniformly
bounded and satisfy
lim
t
W
_
z(t )
_
=0.
Additionally, if W(z) is positive denite, then the equi-
librium z = 0 is globally uniformly asymptotically stable
(GUAS).
4.1.2. Finding a solution
System (4.7) coincides with Eq. (4.6)

x
j
=
j
kj
x
j
+u x
j
,
y
j
=
j
kj
y
j
+u y
j
(4.11)
and we will try the following CLF:
V(z) =
1
2
|z|
2
=
1
2
z z =
1
2
[ x
j
x
j
+ y
j
y
j
] (4.12)
which derivative along the solution of (4.11) is:
V
z
(z)f (z, u, t ) =u (x
j
x
j
+y
j
y
j
). (4.13)
If we choose now as control law:
u =, (4.14)
where is an arbitrary diagonal positive denite gain matrix
and is dened as
=x
j
x
j
+y
j
y
j
. (4.15)
Substituting in Eq. (4.13) yields
V
z
(z)f (z, t ) =
T

inf
| |
2
, (4.16)
where
inf
> 0, which is a scalar, is the inmum of the ele-
ments of .
Noting that
=x
j
x
j
+y
j
y
j
= x
j
x
j
+ y
j
y
j
, (4.17)
and using the fact that
_
c
2
+b
2
|b| +|c|, (4.18)
results in
| | | x
j
x
j
| +| y
j
y
j
|. (4.19)
Taking into account that x, y are unit vectors, it is clear that
|x x| =| x x| | x|,
|y y| =| y y| | y| (4.20)
and therefore
| | | x| +| y|,
| |
2
| x|
2
+| y|
2
+2| x|| y|. (4.21)
Using Youngs Inequality
2| x|| y| =| x|
_
2| y|
_
| x|
2
+
1
4
_
2| y|
_
2
=| x|
2
+| y|
2
. (4.22)
Substituting in Eq. (4.21), taking into account denition of
|z|
2
in Eq. (4.12), yields the desired relation
| |
2
2
_
| x|
2
+| y|
2
_
=2|z|
2
. (4.23)
510 S. de La Parra, J. Angel / Aerospace Science and Technology 9 (2005) 504516
Substitution of Eq. (4.23) in Eq. (4.16) yields
V
z
(z)f (z, t )
inf
| |
2
2
inf
|z|
2
. (4.24)
Since 2
inf
|z|
2
is positive denite, (4.10) is satised and the
control (4.14), applied to the system (4.11), renders z = 0
the GUAS equilibrium of (4.11) and
lim
t
|z|
2
=0.
4.1.3. Robustness
The previous control law complies with the proposed ob-
jectives provided that we have perfect measures of vectors
x
j
, y
j
. In this subsection it will be demonstrated that the
control law continues to be valid with erroneous measures
when they are bounded.
Assuming that computed values from measurements

x
j
,

y
j
contain only additive errors x
j
, y
j
, we have the fol-
lowing relationship between error, measurement and true
vector:
x
j
=

x
j
+ x
j
,
y
j
=

y
j
+ y
j
(4.25)
and substituting Eq. (4.25) in Eq. (4.15), the denition of
results in
=(

x
j
+ x
j
) x
j
+(

y
j
+ y
j
) y
j
=(

x
j
x
j
+

y
j
y
j
) +( x
j
x
j
+ y
j
y
j
). (4.26)
Calling

=
_
(

x
j
x
j
) +(

e
b
y
j
)
_
,

=
_
( x
j
x
j
) +( y
j
y
j
)
_
. (4.27)
Substitution in Eq. (4.26) yields
=

+

, (4.28)
where

is the part of computed from measurements
whereas

is the unknown part of .
Using Eq. (4.18) and Eq. (4.27) we obtain that
|

| | x
j
x
j
| +| y
j
y
j
|. (4.29)
Taking into account that x, y are unit vectors, we get a rela-
tionship between |

| and measurements errors

|

| | x
j
| +| y
j
|. (4.30)
As we can only use the known part of in the control law,
we will use u =

= (

) and substitute in (4.16)
using (4.23)
V
z
(z)f (z, t ) =
T
+

inf
| |
2
+

2
inf
|z|
2
+

T
, (4.31)
where

T
is not positive denite, (4.10) is not satised
and cannot be guaranteed that z =0 is a GUAS equilibrium
for (4.11).
This result does not mean that the control law is useless,
but that GUAS requirement is too strong for systems with
uncertain nonlinearities. We will instead use another stability
concept, which is appropriate for this type of systems: Input-
to-State Stability (ISS) [2,6,7].
Substituting u =

in (4.11)

x
j
=
j
kj
x
j
+
_
[ x
j
x
j
+y
j
y
j
]

_
x
j
,
y
j
=
j
kj
y
j
+
_
[ x
j
x
j
+y
j
y
j
]

_
y
j
.
(4.32)
The system (4.32) is said to be ISS if there exist a class KL
function and a class K function , such that, for any z(0),
where z
T
=[ x
j
y
j
], and for any input

() continuous and
bounded on [0, ) the solution exists for all t 0 and satis-
es
|z(t )|
_
|z(0)|, t t
0
_
+
_
sup
0t
|

()|
_
(4.33)
for all t
0
and t such that 0 t
0
t [2, Appendix C].
In order to demonstrate that the system (4.32), that we
will write in compact form as
z =f (z,

, t ) (4.34)
satises the ISS property, we will use a theorem formulated
by Eduardo Sontag [6] and extended to time-varying system
in [2, Theorem C.2], stating that if there exists a V(z) such
that for all z R
3
and

R
3
,

1
_
|z|
_
V(z)
2
_
|z|
_
,
|z|
_
|

|
_

V
z
f (z,

, t )
3
_
|z|
_
, (4.35)
where
1
,
2
are class K
inf
functions and
3
is a class K
function. Then the system (4.34) is ISS with =
1
1

2

.
Noting that the Lyapunov function V(z), dened in
Eq. (4.12), is a class K
inf
function, we choose

1
_
|z|
_

=
1
2
|z|
2
,

2
_
|z|
_

=
1
2
|z|
2
. (4.36)
Using Youngs Inequality for the elements of and

we
have

2
i
+
1
4

2
i
, i =1, 2, 3 (4.37)
and therefore

T

sup
_
|

|
2
+
| |
2
4
_
, (4.38)
where
sup
> 0, which is a scalar, is the supremum of the
elements of . Substituting in (4.31)
S. de La Parra, J. Angel / Aerospace Science and Technology 9 (2005) 504516 511
V
z
(z)f (t, z,

)
=
T
+

inf

sup
4
_
| |
2
+
sup
|

|
2
2
_

inf

sup
4
_
|z|
2
+
sup
|

|
2
. (4.39)
Choosing
sup
<4
inf
, we see that, whenever
|z|
2
>

sup
2(
inf

sup
4
)
|

|
2
, (4.40)

V(z) is negative.
Calling:
=4
inf

sup
,

=
2
sup

. (4.41)
Eq. (4.35) can be rewritten as

1
_
|z|
_
=
2
_
|z|
_
=V(z) =
1
2
|z|
2
,
|z|

V
z
(z)f (z,

, t )

2
|z|
2
+
sup
|

|
2
<0,
(4.42)
where, using Eq. (4.30)

= sup
0t <
|

| sup
0t <
_
| x
j
| +| y
j
|
_
. (4.43)
Eq. (4.42) means that if

z(0)

, (4.44)
then
V
_
z(t )
_

1
2
_

_
, (4.45)
which implies that
|z(t )|

. (4.46)
Else if
|z(0)| >

, (4.47)
then
V(z(t )) V
_
z(0)
_
, (4.48)
which implies that

z(t )

z(0)

. (4.49)
Therefore, the control law u =

when applied to (4.11)

renders the closed loop system ISS with respect to the dis-
turbance

. Eq. (4.42) proves the convergence of z(t ) to the
residual set
=
_
z: |z|

sup
0t <
_
| x
j
| +| y
j
|
__
.
(4.50)
Eqs. (4.44) and (4.49) combined prove the global uniform
boundedness of z(t )
z

max
_

z(0)

_
. (4.51)
Using similar arguments as in [2], Eq. (4.51) can be written
in an explicit form. Let us rewrite Eq. (4.39) as
d
dt
_
1
2
|z|
2
_

2
|z|
2
+
sup
|

|
2
. (4.52)
Deriving |z|
2
e
t
and substituting (4.52) leads to
d
dt
_
|z|
2
e
t
_
=
d
dt
_
|z|
2
_
e
t
+ |z|
2
e
t
|z|
2
e
t
+2
sup
|

|
2
e
t
+ |z|
2
e
t
=2
sup
|

|
2
e
t
. (4.53)
Integrating both sides
|z|
2
e
t

z(0)

2
+2
sup
|

|
2

t
_
0
e
s
ds
=

z(0)

2
+2

sup

|

|
2

(e
t
1). (4.54)
Using Eq. (4.18)

z(t )

z(0)

e
t
+|

_
2

sup

_
1 e
t

z(0)

e
t
+

. (4.55)
Therefore, using Eq. (4.30)

z(t )

z(0)

e
t
+

z(0)

e
t
+

sup
0t <
_
| x
j
| +| y
j
|
_
(4.56)
which again shows that z(t) converges to the set dened
in (4.50).
4.1.4. Summary of the algorithm
In conclusion, if our objective is to compute attitude C
j
k
of any j-frame relative to k-frame; we have measurements
of two unit constant vectors

x
j
,

y
j
expressed in the j-frame
containing additive errors x
j
, y
j
, and we know the k-frame
components of the same vectors x
k
, y
k
; we also have erro-
neous measurements of the angular velocity between j-frame
and k-frame

j
kj
expressed in the j-frame; we compute an es-
timated attitude

C
j
k
by integrating angular velocity

j
kj
+u
and, using

C
j
k
, also compute
x
j
=

C
j
k
x
k
,
y
j
=

C
j
k
y
k
. (4.57)
Then, if the correction u is calculated as in (4.15): =x
j

x
j
+y
j
y
j
, the attitude matrix

C
j
k
converges to the vicinity
of attitude matrix C
j
k
such as is described in Eq. (4.56).
512 S. de La Parra, J. Angel / Aerospace Science and Technology 9 (2005) 504516
Note that we have no imposed model or size constraint
whatsoever to the error in the angular velocity measure-
ments, and the sole constraint imposed to the errors of the
other measurements is to be additive.
4.2. Implementation
In this section the previous algorithm is applied to solve
the problem of attitude determination. Our objective is to
compute the rotation matrix C
b
t
that transforms a vector ex-
pressed in the t-frame to the same vector expressed in the
b-frame. As it has been previously said, the following mea-
surements are required:
1.

b
t b
: angular velocity of the b-frame relative to the t-
frame expressed in the b-frame. This vector is not directly
measured, but it can be approximately computed from gyro
measurements

b
ib
corresponding to the angular velocity of
the b-frame relative to the i-frame expressed in b-frame. Us-
ing the angular velocity addition theorem

b
t b
=

b
ib

b
it
=

b
ib
C
b
t
C
t
e

e
it
. (4.58)
The vector
e
it
is the Earth rotation rate in ECEF coordinates.
Using WGS-84 constants

e
it
=
_
0
0
7.292115 10
5
_
rad/s. (4.59)
The matrix C
t
e
is dened in Eq. (3.19) and is a function of
geodetic latitude and longitude of the GCS, which are sup-
posed to be known.
The matrix C
b
t
is unknown but can be approximated by
the estimated

C
b
t
since C
b
t
C
t
e

e
it
is a small vector. Therefore
C
b
t
C
t
e

e
it


C
b
t
C
t
e

e
it
, (4.60)
and

b
ib
can be calculated using Eq. (4.58).
2. g
t
b
: a unit vector in the direction of the apparent gravity
at the origin of the b-frame, expressed in t-frame. This vector
is computed using Eq. (3.23) as a geodetic latitude function,
longitude and altitude provided by the GPS. The attitude
lter requires a constant vector, while g
t
b
is not constant.
Nevertheless, changes of g
t
b
due to variations of , , h dur-
ing the characteristic time of the attitude lter (seconds) are
very small and, therefore, it will be used in the attitude lter
as if it was a constant vector.
The vector g
b
b
is computed as
g
b
b
=

C
b
t
g
t
b
. (4.61)
3.

g
b
b
: measured direction of the apparent gravity at the
origin of the b-frame, expressed in b-frame. This vector is
not measured directly but it can be approximately computed
from measurements of air relative velocity, specic force
and angular velocity. Calling V
w
the wind velocity, the Earth
relative velocity can be expressed as a function of the aero-
dynamic velocity and the wind velocity
V
b
b
=v
b
+V
b
w
,

V
b
b
= v
b
+

V
b
w
. (4.62)
Taking the time derivative of V
b
b
=C
b
t
V
t
b
using the Theorem
of Coriolis and substituting Eq. (3.13) results in

V
b
b
=
b
t b
V
b
b
+f
b
+g
b
b
2C
b
t

t
ie
V
t
b
. (4.63)
Substitution of Eq. (4.62) leads to
v
b
+

V
b
w
=
b
t b

_
v
b
+V
b
w
_
+f
b
+g
b
b
2C
b
t

t
ie
V
t
b
.
(4.64)
Derivation of V
b
w
=C
b
t
V
t
w
yields

V
b
w
=
b
t b
V
b
w
+C
b
t

V
t
w
. (4.65)
Substitution of (4.65) in Eq. (4.64) results in
g
b
b
= v
b
+
b
t b
v
b
f
b
+2C
b
t

t
ie
V
t
b
+C
b
t

V
t
w
. (4.66)
The terms v
b
, v
b
can be computed from air data measure-
ments as it will be further explained. The accelerometers
provide specic force

f
b
. The Coriolis acceleration is very
small and is approximated as follows
2C
b
t

t
ie
V
t
b
2

C
b
t

t
ie

V
t
b
, (4.67)
where

V
t
b
is provided by the GPS. The last term C
b
t

V
t
w
is
zero for constant wind velocity or a calm atmosphere, and
will be neglected. Nevertheless, under turbulent atmospheric
conditions, this term does not vanish, but its nal effect on
estimated attitude is very small as will be shown by simula-
tion in a posterior subsection.
Calling

a
b
=

f
b
2

C
b
t
_

t
ie


V
t
b
_
(4.68)
the specic force measured by the accelerometers corrected
with the Coriolis acceleration, and substitution in Eq. (4.66)
yields
g
b
b

v
b
+
b
t b

v
b

a
b
,

g
b
b
=
g
b
b
|g
t
b
|
. (4.69)
4. e
t
b
: direction of the projection of the earth magnetic
eld over a plane perpendicular to the apparent gravity,
expressed in t-frame. This vector is computed using the In-
ternational Geomagnetic Reference Field (IGRF) model [3],
as a function of the geodetic latitude, longitude and altitude
provided by the GPS. Although the attitude lter requires a
constant vector and e
t
b
is not constant, changes of e
t
b
due to
variations of , , h during the characteristic time of the at-
titude lter (seconds) are very small and, therefore, it will be
used in the attitude lter as if it was a constant vector.
The vector e
b
b
is computed as
e
b
b
=

C
b
t
e
t
b
. (4.70)
5.

e
b
b
: measured direction of the projection of the Earth
magnetic eld over a plane perpendicular to the apparent
S. de La Parra, J. Angel / Aerospace Science and Technology 9 (2005) 504516 513
gravity, expressed in b-frame. This vector is obtained from
magnetometer measurements

m
b
as follows

e
b
b
=
1
|e
t
b
|
_

m
b

m
b

g
b
b
|

g
b
b
|
_
g
b
b
|

g
b
b
|
_
. (4.71)
Once all needed vectors are at hand, the control law u =

can be computed as described in the next subsection.

4.2.1. Implementation of the attitude lter
A block diagram of the attitude estimation module im-
plemented in SIVA UAV is shown in Fig. 1. The angular
velocity

b
t b
(t ) is delayed a time (the reason of this delay
will be explained later on) to get

b
t b
(t ) and corrected
with the control law output u(t ) to provide the estimated
angular velocity

b
t b
(t ) =

b
t b
(t ) +u(t ) (4.72)
which is used in the discrete-time recursion algorithm for
updating quaternion for time t [2]:
q(t ) =(
t
t b
(t )) q(t t ), (4.73)
where t is the sampling period and q =[ q
0
q
1
q
2
q
3
]
T
is the quaternion.
Now we can compute the DCM for time t as a
function of quaternion

C
b
t
( q(t )). The angular veloc-
ity

b
t b
(t ) is used for integrating the quaternion differential
equation [8] from time t to t and obtaining quaternion
for time t :

q(t ) =
1
2

q
_

b
t b
(t )
_
q(t )

q(t ) = q(t ) +
t
_
t

q() d.
(4.74)
The control law is computed as follows
u =
_
(

g
b
b
g
b
b
) +(

e
b
b
e
b
b
)
_
, (4.75)
where vectors

g
b
b
, g
b
b
,

e
b
b
, e
b
b
are computed as indicated in
Eqs. (4.69), (4.61), (4.71), (4.70), respectively.
The body-axis aerodynamics velocity

v
b
, required by
Eq. (4.69) is obtained rotating wind-axis airspeed to body-
axis:

v =
_
_

cos

cos

sin

sin

cos

_
_
, (4.76)
where is true airspeed obtained from measurements of sta-
tic pressure, impact pressure and initial (prior to ight) outer
temperature,

is measured angle of attack,

is measured
angle of sideslip. The derivative of the body-axis aerody-
namics velocity

v
b
is obtained numerically, using a four
order SavitzkyGolay [5] lter with a window of 11 sam-
ples, where the derivative is computed for the sample with
index ve. That is, the derivative is delayed a time cor-
responding to ve sampling periods. This justies the need
of using a delayed angular velocity and correction in (4.72)
and to partitioning the integration of the kinematics equa-
tions (4.73), (4.74).
4.2.2. Examples from simulation and ight test
As an example, Fig. 2 compares the time history of roll as
estimated in the attitude estimation algorithm and measured
by an IMU 600 from Crossbow, and Fig. 3 shows the dif-
ferences. Data has been obtained from ight test of SIVA II.
Note that accuracy of IMU 600 is 2

rms and therefore dif-

ferences can be greater than estimation error.
Figs. 4, 5 and 6 correspond to a simulation performed
twice: one with vertical wind gust and another one with-
out atmospheric disturbances. Turbulence is modeled ac-
cording to Mil. Spec. 1797 with a intensity of 5 m/s cor-
responding to a storm. The gyros were corrupted with a
random error of 2.0 deg/s and a rate random walk of
0.5

/s/

hr, the accelerometers with 0.4 m/s

2
and a rate
random walk of 0.05 m/s/

hr, magnetometer with a ran-

dom error of 2.10
3
gauss and pressure with random error
of 20 pascal. The gures do not show appreciable differ-
ences for the case of turbulence.
Fig. 2. Estimated roll and measured roll (Flight Test SIVA II).
514 S. de La Parra, J. Angel / Aerospace Science and Technology 9 (2005) 504516
Fig. 3. Difference between estimated and measured roll (Flight Test SIVA II).
Fig. 4. True roll time history (simulation).
Fig. 5. Measured angle of attack with and without turbulence (simulation).
5. GCS relative position and velocity estimation
Once attitude matrix

C
b
t
is obtained, acceleration in t-
frame can be computed

a
t
=

C
t
b

f
b
2
_

t
ie


V
t
b
_
+g
t
b
, (5.1)
where 2(
t
ie

V
t
b
), which is an approximation to the Corio-
lis acceleration, has been already computed in Eq. (4.68) and
g
t
b
is obtained in Eq. (3.23). We now form three stationary
Kalman lters for the North, East and Down channels us-
ing

a
t
as input. Assuming that

a
t
contains colored noise, we
will augment the dynamics with a rst-order exponentially
correlated process noise:
a
t
=

a
t
+r,
r =c
r
r +
r
, (5.2)
where c
r
is a diagonal matrix which elements are 1/
i
, i =
1, 2, 3, being
i
the time constant, and
r
is white, zero mean,
noise. The dynamics equations for channel North and East
result in:

P
t
bi
=V
t
bi
,

V
t
bi
=

a
t
i
+r
i
, i =x, y,
r
i
=c
ri
r +
ri
,

V
t
wi
=
wi
, (5.3)
where P
t
b
, V
t
b
, V
t
w
are position, velocity and wind velocity of
the origin of the b-frame expressed in t-frame. A white noise
acceleration model was assumed for wind. The measurement
equation for these channels are:
S. de La Parra, J. Angel / Aerospace Science and Technology 9 (2005) 504516 515
Fig. 6. Roll, pitch and yaw estimation error with and without turbulence (simulation).

v
t
i
=V
t
bi
V
t
wi
+
vi
,

V
t
bi
=V
t
bi
+
Vi
, i =x, y,

P
t
bi
=P
t
bi
+
pi
, (5.4)
where

P
t
bi
,

V
t
bi
are GPS position and velocity, and

v
t
is ob-
tained rotating body-axis aerodynamic velocity to t-frame:

v
t
=

C
t
b

v
b
.
For the Down channel we will augment the dynamics with a
GustShaping lter [8]:
_
s
0
s
1
_
=
_
0 1
c
s0
c
s1
__
s
0
s
1
_
+
_
0
1
_

s
,
w
g
=[ c
g0
c
g1
]
_
s
0
s
1
_
, (5.5)
where
s
is white, zero mean, noise and the constants c
s
, c
g
depend of the turbulence intensity, the turbulence scale
length and the true airspeed, and can be determined from
Mil. Spec. 1987.
The dynamic equation for the Down channel results in:

P
t
bz
=V
t
bz
,

V
t
bz
=

a
t
z
+r
z
,
r
z
=c
rz
r
z
+
rz
,
s
0
=s
1
,
s
1
=c
s0
s
0
+c
s1
s
1
+
s
,

V
t
wz
=0. (5.6)
The measurement equations for this channel are:

v
t
z
=V
t
bz
V
t
wz
c
g0
s
0
c
g1
s
1
+
vz
,

V
t
bz
=V
t
bz
+
Vz
,

P
t
bz
=P
t
bz
+
pz
,
h h
0
=P
t
bz
+
h
, (5.7)
where h is barometric altitude. From Eqs. (5.3), (5.6), (5.4),
(5.7), and once the variances of the noises are known, using
standard software (e.g. Matlab) for solving the Riccati equa-
tion and obtaining the gains K
i
, i = x, y, z, we form three
static Kalman lters that provide estimations of GCS relative
position, velocity and wind velocity.
6. Conclusions
In this paper, a nonlinear observer for attitude estima-
tion has been developed and proven to be input-to-state
stable (ISS) with respect to additive sensor errors. Global
boundedness and convergence to a compact residual set is
516 S. de La Parra, J. Angel / Aerospace Science and Technology 9 (2005) 504516
guaranteed in the presence of bounded sensor errors. An ex-
plicit bound for the estimation errors has been obtained and
the convergence residual set dened as a function of sensor
errors.
Once attitude is obtained, the body axis components of
acceleration are rotated to t-frame and used to drive three
stationary linear Kalman lters for improving GPS position
and velocity bandwidth.
The main advantage of this approach is its simplicity,
low computational requirements and by guaranteeing con-
vergence. The main disadvantage is to have lower accuracy
than other methods. This can be compensated by using GPS
enhanced with a space-based augmentation system for posi-
tion and velocity, whereas accuracy of the attitude estimation
could be sufcient for many applications.
References
[1] J.A. Farrell, M. Barth, The Global Positioning System and Inertial Nav-
igation, McGraw-Hill, 1998.
[2] M. Krstic, P. Kokotovic, I. Kanellakopoulos, Nonlinear and Adaptive
Control Design, Willey, 1995.
[3] M. Mandea, et al., International Geomagnetic Reference FieldEpoch,
2000, Revision of the IGRF for 20002005, 26 May 2000, http://www.
ngdc.noaa.gov/IAGA/wg8/igrf.html.
[4] R.M. Rogers, Applied mathematics in Integrated Navigation Systems,
AIAA, 2003.
[5] H.W. Schssler, P. Stefen, Some Advanced Topics in Filter Design, in:
Advanced Topics in Signal Processing, Prentice Hill, 1988.
[6] E.D. Sontag, Smooth stabilization implies co-prime factorization, IEEE
Trans. Automat. Control AC-34 (1989).
[7] E.D. Sontag, Y. Wang, On characterizations of the input-to-state stabil-
ity property, Systems Control Lett. 24 (1995).
[8] B.L. Stevens, F.L. Lewis, Aircraft Control and Simulation, John Willey,
1992.