Proceedings of the International Conference on

Electrical Engineering and Informatics

Institut Teknologi Bandung, Indonesia June 17-19, 2007

D-03

Development of AHRS (Attitude and Heading Reference System) for

Autonomous UAV (Unmanned Aerial Vehicle)
Widyawardana Adiprawita1*, Adang Suwandi Ahmad1, Jaka Sembiring1
1

School of Electric Engineering and Informatics, Bandung Institute of Technology, Jalan Ganesha 10, Bandung 40132,
Indonesia
This paper describes an attitude and heading system development based on inertial and magnetic sensors. This system will use
triad of orthogonal accelerometers, gyroscopes and magnetometers to measure gravity vector, angular rate vector and earth
magnetic field vector. The mechanization involved integration of angular rate measurement from gyroscope that enables high
update rate but have unbounded error due to integration error and drift. To bound the error, this integrative measurement will be
combined with absolute measurement from magnetometer and accelerometer using Wahba Problem formulation and TRIAD
algorithm. The fusion algorithm used is Kalman Filter.

1. Background
Previous work had been completed on implementing an
autopilot system for autonomous aerial vehicle. One of the
drawbacks of this autopilot system is the attitude and heading
reference. The attitude reference system of the existing
system is based on thermal horizon (temperature difference
between ground and sky) sensing. This attitude measurement
based on thermal horizon sensing relies on clear visibility to
horizon, so when the airframe enters a homogeneous
environment (such as high dense fog or cloud) this attitude
reference will not work. Beside that, the thermopile sensors
used for thermal horizon sensing have limited field of view
(typically 100 degree) so the maximum roll and pitch angle
that can be measured is limited to plus-minus of 50 degree. A
new attitude and heading reference system (AHRS) needs to
be developed to solve this problem.

2. Attitude Representations
In order to control an aerial platform correctly, one of the
input needed by the autopilot control system is attitude.
Attitude is usually represented by 3 rotations of the aerial
platform. These rotations are roll, pitch and yaw. There are
several different rotations representations used. Among them
are the euler angle, Cbn matrix and the quaternion angle
representation. The euler angle (roll , pitch and yaw ) is
very intuitive and widely used in aerospace field. But this
representation suffers singularity near 90 degree of pitch
angle. Cbn matrix or direction cosine matrix is a 3x3 matrix
that represent sequential rotation of roll, pitch and yaw. This
representation doesn't suffer from singularity, but it's not
intuitive and uses 9 values to represent attitude. Here is the
Cbn representation
cos cos
Cbn ( , , ) = cos sin
sin

cos sin + sin sin cos

cos cos + cos sin sin
sin cos

sin sin + cos sin cos

sin cos + cos sin sin

cos cos

(1)
The quaternion representation uses four variables for
rotation instead of three. Here is the quaternion representation

Fig. 1. roll, pitch and yaw

E = [e0 e1 e 2 e3]

(2)

e0 = cos(f/2)
e1 = Ax sin(f/2)
e2 = Ay sin(f/2)
e3 = Az sin(f/2)
where
A = unit vector along axis of rotation
f = total rotation angle
To measure the attitude there 2 approach that can be used.
The first is inertial mechanization which uses discrete time
integration over rotation rate measurement. In this approach
the attitude determination depends not only to the most
current measurement but also depends on the previous
attitude value. The second is absolute attitude measurement
which can compute the attitude based only on the most
current measurement.

2.1 Body Rotation Rate / Inertial Mechanization

In this approach the attitude is updated using body
rotation rate. The body rotation rate is usually represented
using a [p q r]T vector. Each of the value in the body rotation
rate vector represents rotation rate in the x, y and z axis in the
local coordinate frame of the aerial platform (body frame).
The [p q r]T vector is measured using 3 gyroscopes on each of
the x, y and z axis in the body frame. Each Attitude
representation has its own formulation for update.
Euler angle change rate formulation :

* Responsible author. E-mail: wadiprawita@yahoo.com

ISBN 978-979-16338-0-2

714

Proceedings of the International Conference on

Electrical Engineering and Informatics
Institut Teknologi Bandung, Indonesia June 17-19, 2007

1 sin tan
= 0
cos

0 sin sec

D-03

cos tan p
sin q
cos sec r

(3)

C bn = C bn

r
0

0
r

q
p
0

(4)

e1 e2 e3
e0

p
e1 1 e0 e3 e2
q
=

2 e3
e0 e1
e2

r
e1
e0
e3
e2

(5)

After obtaining the attitude change rate, the result can be

integrated in discrete time to obtain the final attitude value.

2.2 Absolute Attitude Determination

Integration of the Body Rotation Rate (Inertial Rate
Mechanization) suffers from the risk of continuous discrete
time integration error. So beside the attitude determination
that relies on integration and angular rate [p q r]T
measurement, it will be very advantageous if we have attitude
determination mechanism that only relies on single
measurement in time. Several approaches will be presented,
and the sensors involved here are triad of accelerometers (to
measure gravity vector) and magnetometers (to measures
earth magnetic vector).

2.2.1 Roll and Pitch Absolute Determination

Roll and pitch measurement from gravity vector measured
by accelerometer:

= arcsin(

gx
)
g
gy

g cos

(6)
)

And here is the formulation to get the gravity vector :

ameasured = adynamic + v g body

Yaw or heading can be calculated from 3 axis earth

magnetic field [Mx My Mz]T and roll - pitch angle using the
following equation
Xh = Mx*cos() + My*sin()*sin() - Mz*cos()*sin()
Yh = Mx*cos() + My*sin()
= arctan(Yh/Xh)
(8)

Quaternion change rate formulation :

= arcsin(

such as GPS receiver and 3 axis gyroscopes.

2.2.2 Yaw Measurement from Earth Magnetic

Vector

Cbn change rate formulation :

adynamic and v can be obtained from other sensors

(7)

a measured : 3 axis accelerations measured by accelerometers

a dynamic : 3 axis dynamic accelerations that cause the

airframe to move
v : centrifugal acceleration, it's the cross product of 3
axis angular rate [p q r]T and the 3 axis velocity
g body : the gravity vector in body frame

It should be noted that this measurement depends on

accurate tilt (roll and pitch) measurement, so yaw
determination also depends on external reference to get the
accurate roll and pitch measurement that free from centrifugal
and dynamic acceleration effect.

2.2.3 TRIAD algorithm

The previous two algorithms treat gravity and magnetic
vector field as separate quantities. In TRIAD algorithm we
can use both gravity and magnetic vector simultaneously to
find the attitude represented by rotation matrix. This problem
of determining a rotation matrix needed to rotate one
reference vector (a vector reading in inertial / reference
frame) to another vector (a vector reading in body frame) is
first formulated by Wahba. Several solution has been
introduced to solve the Wahba Problem, most of them
recursive (i.e. QUEST algorithm). This recursive algorithm to
solve Wahba Problem offers the most accurate solution, but
from implementation consideration this recursive algorithm
will not guarantee the real time aspect needed for the UAV
autopilot control system. Fortunately there is one non
recursive solution for the Wahba Problem known as TRIAD
algorithm. Here is the formulation of the TRIAD algorithm
The gravity vector and magnetic field vector can define a
Cartesian coordinate system, with unit vectors along three
axes i, j, and k. Two Cartesian systems can be determined by
two pairs of (ab, mb ) and (aR, mR)
a
iB = B
aB

iR =

aR
aR

jB =

iB mB
iB mB

jR =

iR mR
iR mR

(9)

k B = iB j B
k R = iR j R
Cbn = i B iR + j B j R + k B k R
T

3 Implementation

ISBN 978-979-16338-0-2

715

Proceedings of the International Conference on

Electrical Engineering and Informatics
Institut Teknologi Bandung, Indonesia June 17-19, 2007

3.1 Hardware Implementation

The Attitude and Heading Reference System will be
implemented as an embedded system with the following main
components
Microchip's dsPIC30F4013 as the main processor,
3 Analog Device's Gyroscope ADXRS150 arranged
orthogonal each other to form a gyro triad,
1 Freescale's 3 Axis Accelerometer MMA7260Q,
1 PNI's Micromag 3 axis magnetometer,
1 uBlox TIM LA GPS receiver.

D-03

using gyro reading [p q r]T. This quaternion angle

representation is chosen because it doesn't suffer from
singularity problem in 90 degrees pitch. This algorithm is
executed in 0.02 second or 50Hz, this frequency is chosen
because it's considerably matched to actuator servo
frequency.
The absolute attitude determination is implemented using
TRIAD algorithm, because this algorithm is considered
computationally efficient (no recursive computation) and
combines the gravity acceleration and magnetic filled
elegantly. To be able to correctly get the gravity acceleration
external information of speed and dynamic acceleration from
GPS receiver is needed, so this algorithm is executed every
0.25 second or 4 Hz.
So in overall, we have attitude update every 0.02 second
(50Hz) and the attitude integration error is zeroed every 0.25
second (4Hz). By combining those 2 algorithms we can have
a stable AHRS for considerably long term operation time,
including in high G turn (centrifugal or coordinated turn). The
fusion algorithm is Kalman Filter.

3.3 Kalman Filter Formulation

Fig. 2. Diagram block of the AHRS Hardware

Time varying Kalman filter is used to combine the high

update rate rotation rate integration (state update) and low
update rate absolute attitude determination (measurement
update). Here is the AHRS state space formulation
xk+1 = Akxk + Bkuk
zk = Hxk

(10)

x = [e0 e1 e2 e3 bp bq br]T
u = [p q r]T
z = [e0 e1 e2 e3] T from TRIAD algorithm solution,
with this conversion

Fig. 3. finished AHRS Hardware

3.2 Algorithm and Firmware Implementation

The firmware is implemented in C language and compiled
with Microchip's C30 compiler. The general algorithm is the
combination of two algorithm
rotation rate integration to get high update rate, and
absolute attitude determination to bound the discrete
integration error in rotation rate integration.
The analog sensors (accelerometers and gyros) are
sampled in 250Hz and filtered using IIR butterworth 2 order
low pass filter. The cut of frequency chosen is 5Hz. This low
pass filtering is needed to avoid the vibration caused by
UAV's engine. The 5Hz cut of frequency is chosen with the
assumption that the UAV's manoeuvre frequency is below
5Hz. The magnetometer is read through Synchronous Serial
Interface / Serial Peripheral Interface (SPI) in 50 Hz update
rate. The GPS receiver can give velocity and position updates
every 0.25 second or 4Hz.
The Rotation Rate Integration is implemented using
quaternion angle representation. The quaternion is updated

ISBN 978-979-16338-0-2

c11 c12 c13

Cbn = c 21 c 22 c 23
c31 c32 c33

1
1 + c11 + c 22 + c33
2
1
e1 =
(c32 c 23)
4e 0
1
e2 =
(c13 c31)
4e0
1
e3 =
(c 21 c12)
4 e0
e0 =

(10b)

where
[e0 e1 e2 e3]T : quaternion attitude representation
[bp bq br]T : bias of rotation rate measurements
[p q r]T : rotation rate measurements

716

Proceedings of the International Conference on

Electrical Engineering and Informatics
Institut Teknologi Bandung, Indonesia June 17-19, 2007

0
Ak =

0
0

0 0 0

dt.e1k
2
dt.e0 k

2
dt.e3 k

2
dt.e2 k
2
1

dt.e2 k
2
dt.e1k
2
dt.e0 k

2
dt.e1k

2
0

0 0 0
0 0 0

0
0

1
0

0 0 0
1 0 0
0 1 0
0 0 1

dt .e1k
2
dt.e0
k

2
dt.e3k
2
Bk =
dt.e2
k

0
0

1
0
H =
0

0
1
0
0

0
0
1
0

0
0
0
0

0
0
0
0

dt.e3 k
2
dt.e2 k

2
dt.e1k
2
dt.e0 k

2
0
0

dt.e3k
2
dt.e2 k

2
dt.e1k

2
dt.e0 k

2
0
0

dt.e2 k
2
dt.e3k

2
dt.e0 k
2
dt.e1k
2
0
0

0
0
0
1

D-03

Fig. 4. AHRS Monitoring Software

4 Testing
Several testing has been done. These includes

Static roll and pitch test

Vibration test on ground

Vibration test using UAV test bed

Ground based magnetic heading test

Qualitatively the AHRS performs very well on those tests.
Quantitative data still needs to be analyzed. The final test is
still needed to test the performance of the AHRS in
centrifugal turn.

0
0
0

Here is the Kalman filter formulation

Time Update (high update rate, 50Hz) :
Project state ahead :

xk = Axk 1 + Buk 1

(11)

Project error covariance ahead :

Pk = APk 1 AT + Q

(12)

Measurement Update (low update rate, 4Hz) :

Compute Kalman Gain :

K k = Pk H T ( HPk H T + R) 1

(13)

Fig. 5. Radio controlled airplane for AHRS aerial testing

Update estimate with measurement zk

xk = x + K k ( zk Hx )

(14)

Update the error covariance

Pk = ( I K k H ) Pk

References
1.

(15)

2.

3.4 AHRS Monitoring Software

AHRS monitoring software is developed to asses the
performance of the developed AHRS. The software is
developed with Borland C++ Builder. The visualization for
attitude is using 3D graphics. This 3D graphics is developed
with OpenGL library.

3.

4.

5.

ISBN 978-979-16338-0-2

Sven Ronnback, Development of a INS/GPS navigation loop for an

UAV, Lulea Tekniska Universiteit, 2000
Yong Li, Andrew Dempster, Binghao Li, Jinling Wang, Chris Rizos, A
low-cost attitude heading reference system by combination of GPS and
magnetometers and MEMS inertial sensors for mobile applications,
University of New South Wales, Sydney, NSW 2052, Australia
Stelian Persa, Pieter Jonker, Multi-sensor Robot Navigation System,
Pattern Recognition Group, Technical University Delft, Lorentzweg 1,
Delft, 2628 CJ,The Netherlands
J.F. Vasconcelos, J. Calvario, P. Oliveira, C. Silvestre, GPS AIDED
IMU FOR UNMANNED AIR VEHICLES, Instituto Superior Tecnico,
Institute for Systems and Robotics, Lisboa, Portugal, 2004
Michael J. Caruso, Applications of Magnetic Sensors for Low Cost
Compass Systems, Honeywell, SSEC

717