NAVIGATION ALGORITHMS AND AUTOPILOT APPLICATION

FOR AN UNMANNED AIRVEHICLE

A THESIS SUBMITTED TO
THE GRADUATE SCHOOL OF NATURAL AND APPLIED SCIENCES
OF
MIDDLE EAST TECHNICAL UNIVERSITY

BY

EREN KAHRAMAN

IN PARTIAL FULFILLMENT OF THE REQUIREMENTS

FOR
THE DEGREE OF MASTER OF SCIENCE
IN
AEROSPACE ENGINEERING

DECEMBER 2010

1
 Approval of the thesis:

NAVIGATION ALGORITHMS AND AUTOPILOT APPLICATION

FOR AN UNMANNED AIR VEHICLE

submitted by EREN KAHRAMAN in partial fulfillment of the requirements for the

degree of Master of Science in Aerospace Engineering Department, Middle East
Technical University by,

Prof. Dr. Canan Özgen ____________________

Dean, Graduate School of Natural and Applied Sciences
Prof. Dr. Ozan Tekinalp ____________________
Head of Department, Aerospace Engineering
Prof. Dr. Nafiz Alemdaroğlu ____________________
Supervisor, Aerospace Engineering Dept., METU
Dr. Volkan Nalbantoğlu ____________________
Co-Supervisor, Aselsan Inc.

Examining Committee Members:

Prof. Dr. Serkan Özgen ____________________
Aerospace Engineering Dept., METU
Prof. Dr Nafiz Alemdaroğlu ____________________
Supervisor, Aerospace Engineering Dept., METU
Prof. Dr Kemal Leblebicioğlu ____________________
Electrical and Electronics Engineering Dept., METU
Asst. Prof. Dr. Ali Türker Kutay ____________________
Aerospace Engineering Dept., METU
Dr. Volkan Nalbantoğlu ____________________
Aselsan Inc.

Date: 17.12.2010

ii
I hereby declare that all information in this document has been obtained and
presented in accordance with academic rules and ethical conduct. I also declare
that, as required by these rules and conduct, I have fully cited and referenced
all material and results that are not original to this work.

Name, Last name : EREN KAHRAMAN

Signature :

iii
 ABSTRACT

NAVIGATION ALGORITHMS AND AUTOPILOT APPLICATION

FOR AN UNMANNED AIRVEHICLE

Kahraman, Eren
M.Sc., Department of Aerospace Engineering
Supervisor : Prof. Dr. Nafiz Alemdaroğlu
Co-Supervisor : Dr. Volkan Nalbantoğlu

December 2010, 99 pages

This study describes the design and implementation of the altitude and heading
autopilot algorithms for a fixed wing unmanned air vehicle and navigation algorithm
for attitude and heading reference outputs. Algorithm development is based on the
nonlinear mathematical model of Middle East Technical University Tactical
Unmanned Air Vehicle (METU TUAV), which is linearized at a selected trim
condition. A comparison of nonlinear and linear mathematical models is also done.
Based on the linear mathematical model of the METU TUAV, the classical control
methods are applied during the design process of autopilot algorithms. For the
confirmation purposes of the autopilot and navigation algorithms, a nonlinear
simulation environment is developed in Matlab/Simulink including nonlinear model

iv
of the METU TUAV, altitude and heading autopilot loops, nonlinear actuator
models, sensor models and navigation model. In the first part of the thesis, feedback
signals for the controller are provided by IMU free measurements. In the second part,
the feedback signals are provided by an attitude and heading reference mode, which
incorporates the gyroscope solutions with the magnetic sensor and accelerometer
sensor measurements by using a Kalman filter algorithm. The performance
comparison of the controller is done for both cases where the effects of having
different modes of the measurement sources are investigated.

Key words: Autopilot, Classical Control, Kalman Filter, Magnetic Sensor

v
 ÖZ

İNSANSIZ BİR HAVA ARACI İÇİN SEYRÜSEFER ALGORİTMASI VE

OTOMATİK PİLOT UYGULAMASI

Kahraman, Eren
Yüksek Lisans, Havacılık ve Uzay Mühendisliği Bölümü
Tez Yöneticisi : Prof. Dr. Nafiz Alemdaroğlu
Ortak Tez Yöneticisi : Dr. Volkan Nalbantoğlu

Aralık 2010, 99 sayfa

Bu çalışmada sabit kanatlı insansız bir hava aracı için yükseklik ve kuzey açısı
otomatik pilot algoritmaları, yönelim ve kuzey açısı referans çıktıları için navigasyon
algoritması tasarımı ve uygulaması anlatılmaktadır. Algoritma tasarımı Orta Doğu
Teknik Üniversitesi Taktik İnsansız Hava Aracı'nın (ODTÜ TİHA) doğrusal
olmayan matematiksel modeli baz alınarak, istenilen bir denge durumunda
doğrusallaştırılarak yapılmıştır. Ayrıca doğrusal olmayan ve doğrusal matematiksel
model karşılaştırılmıştır. Otomatik pilot algoritması tasarımında klasik kontrol
metotları uygulanmıştır.

Otomatik pilot ve navigasyon algoritmasının doğrulanması amacıyla,

Matlab/Simulink' de ODTÜ TİHA'nın doğrusal olmayan modelinin, yükseklik ve
kuzey açısı otomatik pilot döngülerinin, doğrusal olmayan eyleyici, sensör ve
navigasyon modelinin simülasyon ortamı oluşturulmuştur. Simülasyon
uygulamasının ilk kısmında, kontrolcü için geri besleme sinyali ataletsel ölçüm
biriminden alınmıştır. İkinci kısımda, geri besleme sinyali olarak, yönelim ve kuzey

vi
açısı referans modunda, dönüölçer çözümlerinin manyetik sensör ve ivmeölçer ile
Kalman filtresi algoritmasından çıkan ölçümler kullanılmıştır. İki durum için
kontrolcü performansları karşılaştırılmıştır.

Anahtar Kelimeler: Otomatik pilot, Klasik Kontrol, Kalman Filtre, Manyetik Sensor

vii
To my family…

viii
 ACKNOWLEDGMENTS

I would like to thank several people who made this thesis work possible. First, I
would like to acknowledge the guidance of my supervising advisor Prof. Dr. Nafiz
Alemdaroğlu and co-advisor Dr. Volkan Nalbantoğlu during my studies.

I would also like to thank my committee members for their constructive comments
and for taking time to serve in my defense committee.

I owe my gratitude to Burak Seymen and Dr. Uğur Zengin for their encouragement,
guidance and endless support.

I am thankful to my colleagues for their friendship, moral support and suggestions

throughout this study. I am also thankful to Aselsan Inc. for the resources and
facilities that were made available to me during my studies.

I would like to thank my parents for their continuous support, and encouragement
during this thesis and my life, without which, it would have been very challenging
for me to maintain my focus on education and research.

Lastly, but certainly not least, I would like to thank my wife, Gülin Kahraman, for
her continuous support, patience and endless help. This thesis would not be done
without her.

ix
 TABLE OF CONTENTS

ABSTRACT ................................................................................................................ iv

ÖZ ............................................................................................................................... vi

ACKNOWLEDGMENTS .......................................................................................... ix

TABLE OF CONTENTS ............................................................................................. x

LIST OF TABLES .................................................................................................... xiii

LIST OF FIGURES .................................................................................................. xiv

CHAPTERS ................................................................................................................. 1

1. INTRODUCTION ............................................................................................. 1

2. 6 DEGREE OF FREEDOM MATHEMATICAL MODELING ........................ 5

2.1 Platform ................................................................................................. 5

2.2 Aerodynamic Model ............................................................................. 6

2.3 Propulsion Model .................................................................................. 7

2.4 Mass Inertia Model ............................................................................... 9

2.5 Atmosphere Model ................................................................................ 9

2.6 Actuator Model ................................................................................... 10

2.7 Gust Model .......................................................................................... 11

2.8 Coordinate Systems and Transformations .......................................... 14

2.9 SIX-DOF Equations of Motion ........................................................... 17

2.10 Flight Parameters Calculation ............................................................. 18

 Equation ............................................................................ 18
2.10.1 VT

2.10.2  Equation............................................................................. 19

x
 2.10.3  Equation ............................................................................. 20

2.11 Non-Linear Simulation ....................................................................... 21

3. CLASSICAL CONTROLLER DESIGN .......................................................... 23

3.1 Introduction ......................................................................................... 23

3.2 Trim ..................................................................................................... 24

3.3 Linearization ....................................................................................... 25

3.4 Comparison of Linear and Non-Linear Model.................................... 28

3.5 PID Controller Design ........................................................................ 32

3.5.1 Altitude Hold Autopilot Design ............................................... 35

3.5.1.1 Pitch Attitude Hold Autopilot Design ......................... 36

3.5.1.2 Altitude Hold Autopilot Design .................................. 39

3.5.2 Heading Hold Autopilot Design .............................................. 41

3.5.2.1 Bank Angle Hold Autopilot Design ............................ 42

3.5.2.2 Heading Hold Autopilot Design ................................. 44

3.6 Nonlinear Simulation of UAV ............................................................ 46

4. SENSOR MODELLING................................................................................... 49

4.1 Inertial Measurement Unit .................................................................. 49

4.2 Inertial Sensor Errors .......................................................................... 51

4.2.1 Gyroscope Error Model............................................................ 52

4.2.2 Accelerometer Error Model ..................................................... 53

4.3 Magnetometer ..................................................................................... 54

4.3.1 Magnetometer Error Modeling ................................................ 56

4.3.2 Magnetometer Calibration ....................................................... 58

5. ATTITUDE & HEADING REFERENCE SYSTEM ....................................... 62

5.1 AHRS Mechanization ......................................................................... 62

xi
 5.1.1 Attitude Integration .................................................................. 63

5.2 Kalman Filter ...................................................................................... 64

5.3 Discrete Kalman Filter ........................................................................ 66

5.4 Linearized Error Dynamics ................................................................. 68

5.4.1 Attitude Error Dynamics .......................................................... 68

5.4.2 Accelerometer Measurement Error Model............................... 70

5.4.3 Magnetometer Measurement Error Model ............................... 71

5.5 AHRS Kalman Filter Design and Simulation ..................................... 73

6. SYNTHESIS OF UAV AUTOPILOT AND NAVIGATION ALGORITHM . 79

7. CONCLUSIONS ............................................................................................... 84

REFERENCES........................................................................................................... 87

APPENDICES ........................................................................................................... 90

A. DISCRETE KALMAN FILTER........................................................................ 90

B. AERODYNAMIC COEFFICIENTS and DERIVATIVES ............................... 96

xii
 LIST OF TABLES

TABLES
Table 1 Specifications of the METU Tactical UAV .................................................... 6

Table 2 Specifications of the METU Tactical UAV .................................................... 9

Table 3 ISA Mean Sea Level Atmospheric Parameters [9] ......................................... 9

Table 4 Longitudinal Mode Characteristics ............................................................... 28

Table 5 Lateral Mode Characteristics ........................................................................ 28

Table 6 Pitch Angle Hold Controller Gains ............................................................... 37

Table 7 Altitude Hold Autopilot Controller Gains .................................................... 39

Table 8 Table Bank Angle Hold Autopilot Controller Gains .................................... 42

Table 9 Heading Hold Autopilot Controller Gains .................................................... 44

Table 10 Table IMU Parameters [29] ........................................................................ 75

Table 11 Aerodynamic Parameters ........................................................................... 99

xiii
 LIST OF FIGURES

FIGURES
Figure 1 METU Tactical UAV .................................................................................... 6

Figure 2 Limbach L275 E [6]....................................................................................... 7

Figure 3 Power, Torque and Specific Fuel Consumption (SFC) vs. RPM of Limbach
L275 E [6] .................................................................................................................... 8

Figure 4 Gust and Resulting Aircraft Velocity for L/V = 7.5 s and   1.5 m / s ..... 14

Figure 5 Earth Fixed and Body Fixed Coordinate Systems [2] ................................. 15

Figure 6 Aircraft Axes [7] .......................................................................................... 16

Figure 7 Simulink Block Diagram of the Non-Linear Simulation............................. 22

Figure 8 Elevator Input .............................................................................................. 28

Figure 9 Total Velocity .............................................................................................. 29

Figure 10 Angle of Attack ......................................................................................... 29

Figure 11 Pitch Rate ................................................................................................... 29

Figure 12 Pitch Angle ................................................................................................ 30

Figure 13 Altitude ...................................................................................................... 30

Figure 14 Aileron Input.............................................................................................. 30

Figure 15 Sideslip Angle............................................................................................ 31

Figure 16 Roll Rate .................................................................................................... 31

Figure 17 Yaw Rate ................................................................................................... 31

Figure 18 Roll Angle.................................................................................................. 32

Figure 19 Yaw Angle ................................................................................................. 32

Figure 20 Generic PID Control Scheme .................................................................... 34

Figure 21 Altitude-hold autopilot............................................................................... 35

xiv
Figure 22 Pitch Angle controller block diagram ........................................................ 36

Figure 23 Control architecture chosen for Pitch Attitude Control ............................. 36

Figure 24 Step Response of the Pitch Angle Control System.................................... 37

Figure 25 Elevator deflections due to unit step pitch angle command ...................... 38

Figure 26 Elevator deflections rate due to unit step pitch angle command ............... 38

Figure 27 Control architecture chosen for Altitude Control ...................................... 39

Figure 28 Step Response of the Altitude Hold Control System ................................ 40

Figure 29 Elevator deflections due to unit step altitude command ............................ 40

Figure 30 Elevator deflections rate due to unit step altitude command ..................... 41

Figure 31 Heading hold autopilot .............................................................................. 41

Figure 32 Step Response of the Bank Angle Control System ................................... 43

Figure 33 Aileron deflections due to unit step bank angle command ........................ 43

Figure 34 Aileron deflections rate due to unit step bank angle command ................. 44

Figure 35 Step Response of the Heading Angle Control System .............................. 45

Figure 36 Aileron deflections due to unit step heading angle command ................... 46

Figure 37 Aileron deflections rate due to unit step heading angle command ............ 46

Figure 38 Attitude Response of the aircraft ............................................................... 47

Figure 39 Altitude of the aircraft ............................................................................... 47

Figure 40 Deflection of control surfaces .................................................................... 48

Figure 41 Schematic of an inertial measurement unit [13] ........................................ 49

Figure 42 Common input/output error types [32] ...................................................... 52

Figure 43 Magnetic Sensor Technology Field ranges[19] ......................................... 55

Figure 44 Error Free Horizontal Magnetic Field Magnitude ..................................... 59

Figure 45 Error Free Horizontal Magnetic Field Magnitude X vs. Y........................ 59

Figure 46 Erroneous Horizontal Magnetic Field Magnitude ..................................... 60

xv
Figure 47 Erroneous Horizontal Magnetic Field Magnitude (X vs. Y) ..................... 61

Figure 48 Block Diagram for attitude and heading reference system ........................ 63

Figure 49 Basic complementary filter ........................................................................ 65

Figure 50 Equivalent form of complementary filter .................................................. 65

Figure 51 Kalman Filter Loop ................................................................................... 68

Figure 52 True Trajectory .......................................................................................... 76

Figure 53 Attitude Solution Error .............................................................................. 76

Figure 54 Kalman filter attitude solution ................................................................... 77

Figure 55 Accelerometer bias estimation error .......................................................... 78

Figure 56 Gyroscope Bias estimation error ............................................................... 78

Figure 57 Simulation Model of the UAV .................................................................. 79

Figure 58 Aircraft attitude with free IMU solution.................................................... 80

Figure 59 Aircraft altitude with free IMU solution.................................................... 80

Figure 60 Aircraft heading with free IMU solution ................................................... 81

Figure 61 Aircraft attitude with AHRS solution ........................................................ 82

Figure 62 Aircraft Altitude with AHRS solution ....................................................... 83

Figure 63 Aircraft heading with AHRS solution ....................................................... 83

Figure 64 C D vs.  ................................................................................................... 96

Figure 65 C L vs.  ................................................................................................... 96

Figure 66 C m vs.  different elevator positions ....................................................... 97

Figure 67 C Y vs.  different rudder positions .......................................................... 97

Figure 68 C l vs.  different rudder positions ........................................................... 97

Figure 69 C l vs.  different aileron positions .......................................................... 98

Figure 70 C n vs.  .................................................................................................... 98

xvi
Figure 71 C n vs.  different aileron positions ......................................................... 98

Figure 72 C n vs.  different rudder positions ........................................................... 99

xvii
 CHAPTER 1

INTRODUCTION

Purpose of the present work is to develop an attitude heading reference system

(AHRS) algorithm using micro electromechanical systems (MEMS) inertial sensors
and strapdown magnetometer for an unmanned air vehicle (UAV) control system. In
low-cost UAV applications, integration of the Global Positioning System (GPS) and
MEMS based inertial navigation system (INS) is vital for the navigation and the
control performance. However, if GPS signal is unavailable or suspicious GPS signal
is detected, the integrated system performance is severely affected due to large error
characteristic of the MEMS inertial sensors. In such a situation MEMS inertial
sensors provide very poor solutions for both navigation and control system. The main
purpose of AHRS algorithm is to provide attitude and heading outputs with
acceptable performance by the already available onboard inertial and magnetic-
heading and/or true air speed sensors. In this work, inertial sensors (gyroscopes and
accelerometers) and magnetometer sensors are fused by an indirect Kalman filter to
provide attitude and heading reference.

The term Unmanned Aerial Vehicle (UAV) can be defined as a type of aircraft that
does not carry human operator so eliminates human life risks, can fly autonomously
or be remotely controlled, can be disposable or recoverable and can carry various
types of payload whether is lethal or non-lethal [1]. In recent years UAVs have been
accepted in both civilian and military purposes, some applications of UAVs are listed
below;

1
 Civil applications;
-Security
-Search and Rescue
-Airborne Antenna
-Environmental Monitoring

Military Applications;
-Reconnaissance and Surveillance
-Target acquisition
-Surface attack
-Air to air combat
-Antisubmarine search
-Relay command/control/ communications (reference)

There are several reasons for the wide acceptance of UAVs. First of all, as it is
mentioned in UAVs definition before the projects conducted with UAVs do not carry
human life risks, by the means of UAVs the accidents resulted in human life loss
have been completely eliminated. For second advantage of UAV it can be asserted
that, UAVs have low costs both in the processes of manufacturing and operational.

A UAVs’ autonomy and their “control systems” are strictly correlated; the autonomy
of UAVs is strongly dependent on efficient and reliable control systems. For
gathering efficient and reliable control systems which is necessary for UAVs’
autonomy, it is important to model platform carefully. Model building is a
fundamental process in the development of an UAV. In the process of modeling
accurate and reliable platform, the factors such as aerodynamic, engine, actuator,
sensor, and atmosphere should be considered cautiously. If these factors are
examined in detail before the modeling process of platform, the designer can guess
the responses of the platform under various conditions.

For obtaining accurate and reliable control system the measurements of the UAV
states are important. So the measurement devices namely sensors have a critical role

2
on the control systems. In most of the autopilot application, the micro mechanical
gyros and accelerometers are used due to their small sizes, light weight, low cost,
low power consumption and availability. Besides, all these advantages, they also
have some disadvantages like their noises. These devices have large noises which
cause degradation of their performance. This large noise may cause divergences in a
regular inertial self alignment process in few seconds or kilometer level position
errors [30]. So, it is necessary to get rid of these errors. The usual way of this is,
using the sensors together in a clever way. The Kalman Filter technique which is one
of the integration techniques uses different sensors measurement to give more
accurate and reliable results.

In the literature, there are various examples of this application. In [30], a study which
was conducted in Montreal Canada, states that an attitude determination system
based on low cost Micro Electro Mechanical Systems (MEMS) inertial sensor, a triad
magnetometers and a commercial Global Positioning System (GPS) receiver. The
study focuses on the design of an approach applying low cost MEMS sensor
integrated by magnetometer and GPS in attitude determination system in dynamic
environment. The [31] study conducted by Huang and Fang, presents a practical
approach of applying an Inertial Navigation System (INS) using MEMS inertial
sensors, GPS receiver, magnetometer and barometer for the guidance, navigation and
control(GNC). The study presents a guidance, navigation, and control of a UAV
using MEMS sensor. A GPS, MEMS baro-altimeter and a magnetometer is used to
estimate and correct the MEMS inertial navigation system error via Kalman filter.
The benefits of this integrated system for accurate position, velocity and attitude
determination are shown.

For this thesis study, it is aimed to design controllers and a navigation mode for the
UAV. Proportional Integral Derivative (PID) which is a classical control design was
selected as controller and the navigation mode was used as the attitude heading
reference system (AHRS). The AHRS uses IMU and magnetometer measurement
together with Kalman filter technique in order to reduce the sensors error. In order to
achieve the goals of this study, the following chapters were prepared. In chapter 2,

3
dynamic modeling of the UAV based on Six Degrees of Freedom (6-DOF) equations
of motion, atmosphere and gust models and engine model will be explained. In
Chapter 3, basic principles of classical control theory and the design of PID
controllers will be examined. The sensor models are going to be reviewed in Chapter
4. In Chapter 5, the AHRS Kalman filters will be discussed and designed according
the sensors measurements. In chapter 6, the autopilot and the Kalman filter algorithm
synthesis is going to be done. In Chapter 7, conclusions and recommendations for
future work will be presented.

4
 CHAPTER 2

6 DEGREE OF FREEDOM MATHEMATICAL MODELING

This chapter comprised of the dynamic model of the UAV. The aerodynamic,
propulsion atmospheric and gust models are also presented. For the control system
design the linear model is also needed. So this chapter also includes the linearization
of the non-linear model. The lateral and longitudinal dynamics decoupling and their
state space representation is also presented in this chapter as well as open loop
responses.

2.1 Platform

Middle East Technical University Tactical UAV (METU TUAV) is a fixed wing
aircraft with take of weight 120 kg and span of 4.3 meter. It has a high wing
configuration with twin tail boom and a T tail. It is designed as the aerial
observation of an area and transmittal of data to the ground station in real time. As a
payload it will carry a forward looking infrared camera (FLIR). The UAV is powered
by 21 HP, 2 cylinder 2 stroke opposed engine. It is controlled by ailerons and flaps
on wing, rudders on vertical tail and elevators on horizontal tail. The throttle and the
nose landing gear can also be controlled. Technical specifications of the METU
TUAV are given in Table 1. The UAV is manufactured from fiber glass, carbon fiber
and kevlar clothes. A mock up model is shown in Figure 1.

5
 Figure 1 METU Tactical UAV

Table 1 Specifications of the METU Tactical UAV

Wing Span : 4.3 m

Length : 3.0 m
Fuselage Width : 0.3 m
Maximum Take of Weight : 120 kg
Payload Weight : 30 kg
Maximum Speed : 300 km/hr
Cruise Speed : 145 km/hr
Stall Speed : 65 km/hr
Maximum Range : 600 km
Maximum Endurance : 5 hr
Operation Altitude : 3000 m
Payload Weight : FLIR Camera
Propulsion : 21 HP Two Cylinder Gasoline Engine

2.2 Aerodynamic Model

Aerodynamic and thrust forces and moments on aircraft can be determined by

experimental methods, theoretical methods and empirical methods. By the
experimental methods the aircraft behavior over wide range of flight conditions can
be determined. However the method is both time and money consuming. The

6
theoretical methods are simple and suits well for the preliminary and parametric
design phase [2].

In this study, as an aerodynamic parameters the wind tunnel test results of the
SCAUT which has similar properties with METU TUAV is used. The values for the
aerodynamic coefficients and derivatives can be found in Appendix B.

2.3 Propulsion Model

For the propulsion of the UAV, the engine is modeled by the parameters supplied by
the manufacturer. The engine of the METU TUAV is Limbach L275 E (Figure 2). It
is two cylinder two stroke engine.

Figure 2 Limbach L275 E [6]

Characteristic of the engine with respect to Revolution per Minute (RPM) is given in
Figure 3 [6].

7
Figure 3 Power, Torque and Specific Fuel Consumption (SFC) vs. RPM of Limbach
L275 E [6]

The propeller is also modeled for the simulation purposes. The characteristics of the
propeller is given by

Thrust Coefficient :

C  (1)
n 2 D 4

Power Coefficient :

C  (2)
n 3 D 5

Advance Ratio :
V
J (3)
nD

where

 is the thrust.
n is the revolution per second.

8
D is the diameter of the propeller.

2.4 Mass Inertia Model

The tensor of inertia and mass of the aircraft constitute the mass inertia model. In this
model, the mass and inertia are assumed constant with respect to time. The mass
distribution is also assumed constant with time. This means that the effects of the
fuel decreasing and sloshing are neglected. The UAV’s weight portions and the
inertia properties are given in Table 2 [3].

Table 2 Specifications of the METU Tactical UAV

2 2 2
MMTOW (kg) mfuel (kg) Ixx (kgm2) Iyy (kgm ) Izz(kgm ) Ixz (kgm )
105 15 37.58 34.12 67.04 -6.91

2.5 Atmosphere Model

As an atmosphere model, the International Standard Atmosphere (ISA) model is

used. ISA model computes ambient temperature, pressure, density, and speed of
sound for the input geo-potential altitude between 0 km and 20 km using lapse rate
method. The ISA assumes the mean sea level conditions as given in Table 3[9].

Table 3 ISA Mean Sea Level Atmospheric Parameters [9]

2
Pressure Po = 101325 n/m
3
Density ρo = 1.225 kg/m
o
Temperature To = 288.15 K
Speed of Sound ao = 340.294 m/sec
2
Acceleration of Gravity go = 9.80665 m/sec

The equations for the ISA model calculation are given in the equations from 4 to 7 .

Ambient temperature [K] :

T  To  Lh (4)

9
where

L is the Lapse Rate (0.0065 K/m)

h is the geo-potential height [m]
Speed of sound [m/s] :
a  RT (5)

where

γ is the adiabatic index (1.4 for air)

R is the specific gas constant (287.0531 joule/Kkg for dry air)
Air pressure [Pa] :

  h troposphere  h R 
 g
 g
T  LR
P    Po  e T
(6)
 To  
 

htroposphere is the troposphere height (11000 meter)


  g
 h troposphere  h R 
 
e T
is the stratosphere model
Air density [kg/m3] :
 g


 T   LR
  h troposphere  h  g 
  To    R 
 o  e T
(7)
 T 
  T  
  o  

2.6 Actuator Model

METU TUAV is controlled by ailerons and flaps on wing; rudders on the vertical
tails and elevator on the horizontal tail. The throttle setting can also be controlled.
The control surfaces and the throttle setting are actuated by radio controlled servos.
These servos have a limit for the rate of change of position and for the deflections.
For these servos, it is assumed that they are second order systems with transfer
function

10
  2n
G act  2
s  2 n s   2n

For ailerons, rudders and elevator servos,   0.7063 and  n  20Hz are selected.
The actuators deflection is limited to ±30 degrees and rate is limited 60o/0.11 sec.
The engine rpm is limited to 0-7000 rpm.

2.7 Gust Model

Air movement on a small scale is named as atmospheric turbulence. It is caused by

the instabilities of pressure and temperature distribution in the atmosphere. The
changes are both time and space dependent. Since the behavior of the air molecules
is not predictable, statistical properties like standard deviation and power spectral
density functions are used to classify the turbulence [4].

In this thesis, for simulation purposes, it is assumed that the turbulence is

independent of time, location and direction. Since it is a small aircraft, we assume
that all parts of the aircraft are equally affected from turbulence.

In order to generate the gust velocity based on one dimensional Dryden model, first
the spatial penetration of turbulent air need to be converted into a temporal process.
Considering an aircraft is flying with velocity V and run into a turbulence field with
“sine type” vertical velocity distribution of length L. The spatial frequency of the
wave is

2

T
L
Thus, the aircraft passes the wave during the time T  , so the aircraft experiences
V
2
the wave with the temporal frequency of   . Turbulent air consists of not just
L
 
V

11
one wave and one frequency but of a whole spectrum. According to Dryden’s
function models, this spectral density can be formulated as

 2 L 1  3 2
 zz  (8)

2 V 1  3 2 2 
L
expressed in the non-dimensional frequency   T   and  is the standard
V
deviation of the turbulence in meters/second and it depends on altitude.

Equation 9 is the converted form of the power spectral density equation 8 into a filter
equation. When it is driving with white Gaussian noise, the time traces from the
Dryden spectrum is generated.

v gust (s) LV 1  3 (L V )s

w (s) 2 (L V ) 2 s 2  (L V )s  1 (9)

Equation 9 can also be converted into state space representation as follows

 0 1   0 
 x 1    x1  

V     V   w (t)
2 2
 x      V   2   x 2    
 2
 L L  L  
L V L x 
v gust ( t )   1 3  1 
2  V  x 2  (10)
where

x1 , x 2 are Gust filter states

V is the aircraft velocity
L is the correlation length
w (t) is the gaussian white noise
v gust ( t ) is the gust velocity

 is the gust velocity variance (m/s)

12
The velocity generated from the gust causes a vertical specific force. This results in a
vertical velocity v of the aircraft and a corresponding vertical acceleration a gust  sv .

v is derived from the v gust by the longitudinal stability derivatives of the aircraft.

The approximate relationship between v and v gust is given in reference [4]

Zw
v v gust
s  Zw (11)
Z w can be calculated as [5]

VS
Zw  C D  C L,  (12)
2m
where
 is the density of air
S is the aircraft wing reference area
m is the aircraft mass
CD is the drag coefficient
C L , is the slope of C L   plot

In Figure 4, a gust realization is shown for L = 300 m, V = 40 m/s at an altitude of

3000 meters and   1.5 m / s

13
 Realization of Gust
2
Gust Velocity
1.5 Resulting Aircraft Velocity

0.5
Velocity(m/s)

-0.5

-1

-1.5

-2
0 50 100 150 200 250 300
Time(second)

Figure 4 Gust and Resulting Aircraft Velocity for L/V = 7.5 s and   1.5 m / s

2.8 Coordinate Systems and Transformations

An inertial coordinate system is defined as space fixed (non-rotating) coordinate

system in which Newton’ second law is valid [10, 33]. Due to the fact that the
rotation of the earth is sufficiently slow, the rigidly attached coordinate system to the
earth, XEYEZE can be assumed to be stationary and inertial frame. In this frame, it is
assumed that ZE-axis points to the local direction of the gravity, XE-axis points to the
north and YE-axis points to the east. The earth is also assumed to be flat.

Body frame is fixed on the body of the aircraft (XBYBZB) and moves with it. XB-axis
points the nose of the aircraft and YB-axis points the right wing of the aircraft. ZB-
axis points the bottom of the aircraft. The equations of motion are written in this
frame. In Figure 5 the coordinate systems and positive direction are shown.

14
 Figure 5 Earth Fixed and Body Fixed Coordinate Systems [2]

Wind-Axes reference coordinate system (XWYWZW) is another coordinate system

that is used for describing the motion of the aircraft. In this system, the origin is at
the aircraft centre of mass and the XW-axis is directed along the total velocity vector.

Stability-Axis coordinate system (XSYSZS) is another system that differs from body
fixed coordinate system by an angle of attack, α. The origin is at the centre of mass,
the XS-axis is parallel to the projection of the total velocity of the aircraft centre of
mass on the XBZB plane (Figure 6).

15
 Figure 6 Aircraft Axes [7]

The transformation from the earth fixed frame to the body axes proceeds through
three consecutive rotations namely yaw, pitch and roll. The transformation matrix
from the body axis to the earth axis is found by rotating the system first around the z-
axis by  , then a rotation around the y-axis  and finally around the x-axis by an
angle  . The resultant matrix

 coscos - sincos + cossinsin sinsin + cossincos 

C EB   sincos coscos + sinsinsin - cossin  sinsincos
 - sin cossin coscos 

Since the aerodynamic forces are defined in wind axis frame, the body to wind
transformation matrix is also needed. The transformation matrix from the body axis
to the wind axis is found by rotating the system first around the y-axis by α, then a
rotation around the z-axis by an angle -β. The resultant matrix

 coscos sin sincos 

C W
B   - cossin cos - sinsin
 - sin 0 cos 

16
2.9 SIX-DOF Equations of Motion

Assuming the aircraft is rigid and XZ plane is the plane of symmetry (Ixy=Iyz=0), the
non-linear equations of motion for the fixed wing aircraft can be written as [2] ;

Rate of change of translational velocity

1
u  (mg x  FXA  FXT )  vr  wq
m

1
v  (mg y  FYA  FYT )  ur  wp
m

1
 
w (mg z  FZA  FZT )  uq  vp
m

where

u, v, w is the components of the body velocity vector

FXA , FYA , FZA is the aerodynamic force components along XYZ

FXT , FYT , FZT is the thrust force components along XYZ

Rate of change of angular velocity

p  (c1 r  c 2 p)q  c 3 (M XA  M XT )  c 4 (M ZA  M ZT )

q  c 5 pr  c 6 ( p 2  r 2 )  c 7 ( M YA  M YT )

r  (c 8 p  c 2 r )q  c 4 (M XA  M XT )  c 9 (M ZA  M ZT )
where
p, q, r is the components of the angular rate vector

M XA , M YA , M ZA is the aerodynamic moment components along XYZ

M XT , M YT , M ZT is the thrust moment components along XYZ

(I yy  I zz )(I zz )  (I xz ) 2 ( I xx  I yy  I zz ) I xz I zz
c1  2
c2  2
c3  2
I xx  I zz  I xz I xx I zz  I xz I xx I zz  I xz

17
 I xz I zz  I xx I xz
c4  2
c5  c6 
I xx I zz  I xz I yy I yy

1 (I xx )(I xx  I yy )  (I xz ) 2 I xx
c7  c8  2
c9  2
I yy I xx I zz  I xz I xx I zz  I xz
Here
I xx , I yy , I zz is the aircraft moments of inertia about XYZ

I xy , I yz , I xz is the products of inertia about XYZ

Kinematical relationship between Euler angles and body-fixed angular rates are
given as;

Rate of change of angular position

   cos  sin  sin  sin  cos   p 
 1 
    cos   0 cos  cos   cos  sin  q 
   0 sin  cos    r  G
 

Another important set of equation is given as

Rate of change of translational position

x E  u cos  cos   vcos  sin  sin   sin  cos   w cos  sin  cos   sin  sin 
y E  u sin  cos   vsin  sin  sin   cos  cos   w sin  sin  cos   cos  sin 
z E  u sin   v cos  sin   w cos  cos 

2.10 Flight Parameters Calculation

The aerodynamic force and moment components in 6-DOF equations of motion

depend on the aerodynamic angles and true airspeed; therefore, these must be
calculated [7]. By using linear velocity components in body fixed coordinate system,
the flight parameters can be calculated.

 Equation
2.10.1 VT

Using the velocity components in body fixed axis

VT  u 2  v 2  w 2 (13)

18
  d V  uu  vv  ww
V

(14)
T T
dt VT

Using transformation matrix from the wind axis to the body axis

u VT  cos  cos   cos  sin   sin   VT 

 v   C B  0    sin  cos  0   0  (15)
  W  
 w   0   sin  cos   sin  sin  cos    0 

 u  VT cos  cos 

 v    V sin   (16)
   T 
 w   VT sin  cos  

Substituting equation 16 in equation 14

  u cos  cos   v sin   w

V  sin  cos  (17)
T

Substituting the u , v , w  equation

 values into the VT

  cos cos F  sin F  sin cos F

V (18)
T x y z
m m m

where
Fx  mg x  FXA  FXT

Fy  mg y  FYA  FYT

Fz  mg z  FZA  FZT

2.10.2  Equation

Using body fixed velocity components, rate of change of angle of attack can be
found as

 w
  arctan   (19)
u

d  w  1
   arctan( )   2 (uw  uw) (20)
dt  u  u  w2

Using equation 16

19
 1
  2 2
(VT cos cos w
  u VT sin cos)
VT cos  cos   VT sin 2  cos 2 
2 2

1
  2
VT cos(cos w
  u sin)
VT cos (cos 2   sin 2 )
2

1

cos w
  u sin
  (21)
VT cos

 values into the  equation

Substituting the u , w
1 1
cos Fz  cos(uq  vp)  sin Fx  sin( vr  wq)
  m m (22)
VT cos

Fz cos  Fx sin uqcos  vpcos  vrsin  wqsin

   (23)
mVT cos VT cos

Using equation 14 into the equation 23

Fz cos  Fx sin
   q  tan(pcos  rsin) (24)
mVT cos

  F  q  tan(pcos  rsin) (25)

where F is the force term in the  derivative equations.

2.10.3  Equation

Using body fixed velocity components, rate of change of sideslip angle can be found
as

v
  arcsin( ) (26)
VT

d v  1 v VT  vV
   arcsin( )   2
T
(27)
dt  VT  v VT
1  ( )2
VT
 
s 2

20
 
1 1
cos VT
v  V T sin (28)

Substituting VT

 
1 1
cos VT

v  u cos cos sin  v sin 2   w
 sin cos sin  (29)

 
 
1  v (1  sin 2

   sin sin 
)  u cos sin  w (30)
VT   cos
 
 
 
 cos 

 values into the  equation

Substituting u , v , w

 mV  Fx cos sin  Fy cos  Fz sin sin  

 1 

   T 
 (31)
 1 (ur  wp)cos  ( vr  wq)cos sin  (uq  vp) sin sin
 VT 

Substituting u , v, w values into the  equation

 
1
 Fx cos sin  Fy cos  Fz sin sin  p sin  r cos  (32)
mVT

  F  p sin  r cos  (33)

where F is the force term in the  derivative equations.

2.11 Non-Linear Simulation

The non-linear simulation environment is developed in the MATLAB/SIMULINK.

The simulation program is mostly formed by m-files, which allow easy to make the
necessary changes. The block diagram is shown in Figure 7.

21
 Figure 7 Simulink Block Diagram of the Non-Linear Simulation

In the non-linear simulation model, the gravity is assumed to be constant. Also the
aircraft mass and inertia are assumed to be constant and the forces acting on the
aircraft are assumed to be acting at center of the mass of the aircraft.

22
 CHAPTER 3

CLASSICAL CONTROLLER DESIGN

3.1 Introduction

As stated in CHAPTER 2, aircraft motion can be described by a set of non-linear

equations as [7]

x  f ( x , u )

where

x is the state vector

x is the derivative of the state vector with respect to time
u is the time varying input vector
In a similar way, the output states can be represented as

y  h(x, u )

The 6 DOF rigid body motion is represented by 12 state variables and controlled by 4
input variables and there are 10 output variables of interest.


X  u y z
T
v w p q r    x

Y  VT h
T
  p q r   

  e  elevator deflection
   a   aileron deflection 
U  
  r   rudder deflection 
   
dT   thrust change 

23
3.2 Trim

Trim is the equilibrium condition of the aircraft which means that total forces and
moments acting on the center of mass of the aircraft are both zero. Thus, in a trim
condition there should not be any change in motion variables with respect to time. In
other words, all derivatives should be zero.

Matlab trim function is used for the trim analysis of the non-linear model of the
UAV. This function uses sequential quadratic programming (SQP) algorithm, which
is based on solving a series of sub problems designed to minimize a quadratic model
of the objective subject to a linearization of the constraints [8]. Different flight
conditions can be selected for the trim analysis of an aircraft.

For the level flight at operational altitude of 3000 meter and a cruise velocity of 40
m/s, the trim analysis gives the following results

 u   39.92  m / s  u   0  m / s 2
 v   2.08  m / s  v   0  2
        m/s
 w   2.50  m / s w    0  m / s2
        2
 p   0  rad / s  p   0  rad / s
 q   0  rad / s  q   0  rad / s 2
        2
 r   0  rad / s    r    0  rad / s

X  , X
 0  rad     0  rad / s
       
    0.063  rad     0  rad / s
    0  rad     0  rad / s
       
x  0  m  x  40 m / s
      
y  0  m  y  0  m / s
 z   3000 m  z   0  m / s

24
 VT   40  m / s
   0.062 rad
  
    0  rad
  
  e   0.025 rad  p   0  rad / s
    0  rad  q   0  rad / s
U a  Y    
  r   0  rad  r   0  rad / s
        0  rad
dT   98.43  rps   
    0  rad
    0  rad
  
 h   3000  meter

The first 9 states describe the motion of the aircraft and the last three states are used
for navigation purposes. Due to the assumption that the earth is flat, the position
equations, do not couple back into the equations of motion and need not to be used in
finding steady state flight condition. Nevertheless, the atmospheric density is
calculated as a function of altitude and the altitude state is utilized in the feedback
control laws. Thus, altitude state is included in steady state conditions.

3.3 Linearization

The standard linear state space representation is given in the form of

  AX  BU
X
Y  CX  DU

where

A is the n by n state matrix where n is the number of states

B is the n by k input matrix where k is the number of inputs
C is the n by m output matrix where m is the number of outputs
D is the k by m matrix

The nonlinear model of the selected aircraft is linearized for the calculated trim
conditions in the previous section. For this purpose, Matlab linmod function is
utilized.

25
The state space representation of the UAV for the total motion variables is

 - 0.069812 0.26248 - 2.4799 - 9.7907 6.32  10 -5 0 0 - 2.22  10 -11 0 0

 
 - 0.36877 - 1.9401 39.523 - 0.61432 - 0.0010083 0 0 0 0 0
 0.12573 - 2.0038 - 2.4662 0 - 1.63  10 -12 0 0 0 0 0
 
 0 0 1 0 0 0 0 6.22  10 -13 1.82  10 - 21 0
 - 0.062622 0.99804 0 - 40 0 0 0 0 0 0
A 
 - 5.55  10 - 3.48  10-14 3.82  10-13 - 1.01  10-15
-13
0 - 0.14248 2.4803 - 39.749 9.7907 0
 - 3.75  10 -12 - 3.51  10-13 1.29  10 - 21 0 - 7.36  10-15 - 0.44467 - 4.7409 0.59699 0 0
 
 1.33  10-12 - 5.42  10-13 4.84  10 - 22 0 2.52  10 -15 0.16661 - 0.63074 - 0.49941 0 0
 
 0 0 - 3.90  10 -14 - 1.82  10- 21 0 0 1 0.062745 - 1.76  10- 22 0
 0 0 - 6.23  10 -13 - 1.14  10- 22 0 0 0 1.002 - 2.81  10- 21 0

- 0.71898 0.034119 0 0 
 - 6.5722 0 0 0 

 - 40.795 0 0 0 
 
 0 0 0 0 
 0 0 0 0 
B 
 0 0 0 1.6762
 0 0 47.557 17.696
 
 0 0 3.2527 - 3.323
 0 0 0 0 
 
 0 0 0 0 

As it can be seen from the A matrix, there is no coupling between the lateral and
longitudinal dynamics. The upper right block and lower left block of the A matrix
consist of zeros. As a result, longitudinal and lateral dynamics can be separated. The
resulting motion dynamics are

-Longitudinal Dynamics

State Vector : X  u w q  z
T

Input Vector : U   e dT

T

Output Vector : Y  VT  q  h

T

26
  - 0.0698 0.2625 - 2.4799 - 9.7907 0.0001 
 - 0.3688 - 1.9401 39.5234 - 0.6143 - 0.0010
 
State Matrix : A Lon   0.1257 - 2.0038 - 2.4662 0 0 
 
 0 0 1 0 0 
 - 0.0626 0.9980 0 - 40.0000 0 

  0.7190 0.0341
  6.5722 0 

Control Matrix : B Lon   40.7955 0 
 
 0 0 
 0 0 

 0.9980 0.0626 0 0 0 
 0.0016 0.0250 0 0 0 
 
Observation Matrix : C Lon  0 0 1 0 0
 
 0 0 0 1 0
 0 0 0 0  1

-Lateral dynamics

State Vector : X  v p r   T

Input Vector : U   a r 
T

Output Vector : Y   p r   T

 - 0.1425 2.4803 - 39.7495 9.7907 0

 - 0.4447 - 4.7409 0.5970 0 0

State Matrix : A Lat   0.1666 - 0.6307 - 0.4994 0 0
 
 0 1 0.0627 0 0
 0 0 1.0020 0 0

 0 1.6762 
47.5568 17.6965 
 
Control Matrix : B Lat   3.2527  3.3230
 
 0 0 
 0 0 

27
 0.0250 0 0 0 0
 0 1 0 0 0

Observation Matrix : C Lat  0 0 1 0 0
 
 0 0 0 1 0
 0 0 0 0 1

From the state space parameter the characteristics of the aircraft can be determined
by the eigenvalue analysis.
Table 4 Longitudinal Mode Characteristics
Damping Natural Time
Flight Eigen Values Period
Ratio Frequency Constant
Mode    n (rad / sec)
T (sec) (sec)
Short Period -2.1981 ± 8.9107i 0.2395 9.1778 0.7051 -
Phugoid -0.0396 ± 0.3359i 0.1172 0.3382 18.7080 -
Altitude -0.0007 - - - 1417.0626

Table 5 Lateral Mode Characteristics

Damping Natural Time
Eigen Values Period
Flight Mode Ratio Frequency Constant
   n (rad / sec)
T (sec) (sec)
Roll -5.0267 - - - 0.1989
Dutch Roll -0.1723 ± 3.0648i 0.0561 3.0696 2.0501
Spiral -0.0115 - - - 87.2497
Heading 0

3.4 Comparison of Linear and Non-Linear Model

For the elevator input in Figure 8 the outputs shown

Elevator Input
0.15

0.1

0.05

0
degree

-0.05

-0.1

-0.15

-0.2
0 200 400 600 800 1000 1200 1400 1600
seconds

Figure 8 Elevator Input

28
 Total Velocity
40.08
Linear Simout
40.06 Nonlinear Simout

m/s 40.04

40.02

40

39.98

39.96
0 5 10 15
second

Figure 9 Total Velocity

Angle of Attack
3.75
Linear Simout
Nonlinear Simout
3.7

3.65
degree

3.6

3.55

3.5
0 5 10 15
second

Figure 10 Angle of Attack

Pitch Rate
0.4
Linear Simout
Nonlinear Simout
0.2

0
degree

-0.2

-0.4

-0.6
0 5 10 15
second

Figure 11 Pitch Rate

29
 Pitch Angle
3.8
Linear Simout
3.75
Nonlinear Simout
3.7

degree 3.65

3.6

3.55

3.5

3.45

3.4
0 5 10 15
second

Figure 12 Pitch Angle

Altitude
3000.1

3000.05

3000

2999.95
meter

2999.9

2999.85

2999.8

2999.75
0 5 10 15
second

Figure 13 Altitude

For the aileron input in Figure 14 the outputs shown

Aileron Input
0.2

0.1

0
degree

-0.1

-0.2

-0.3
0 200 400 600 800 1000 1200 1400 1600
seconds

Figure 14 Aileron Input

30
 Sideslip Angle
0.3
Linear Simout
Nonlinear Simout
0.2

0.1
degree

-0.1

-0.2
0 5 10 15
second

Figure 15 Sideslip Angle

Roll Rate
2
Linear Simout
1.5 Nonlinear Simout

0.5
degree

-0.5

-1

-1.5
0 5 10 15
second

Figure 16 Roll Rate

Yaw Rate
1
Linear Simout
Nonlinear Simout
0.5

0
degree

-0.5

-1

-1.5
0 5 10 15
second

Figure 17 Yaw Rate

31
 Roll Angle
3
Linear Simout
2 Nonlinear Simout

0
degree

-1

-2

-3

-4
0 5 10 15
second

Figure 18 Roll Angle

Yaw Angle
2
Linear Simout
1
Nonlinear Simout
0

-1
degree

-2

-3

-4

-5

-6
0 5 10 15
second

Figure 19 Yaw Angle

As seen from the figures above, the comparison of the linearized longitudinal
dynamics with the nonlinear model gives better results compared to the linearized
lateral dynamics. This is due to the reason that the lateral stability modes exhibit
more dynamic coupling than the longitudinal stability modes.

3.5 PID Controller Design

Flight control autopilots are the control systems, which take reference desired
commands as input and give the related control surface deflections as output. In this
part of the thesis study, the autopilot design considering both of the decoupled
longitudinal and lateral dynamics will be presented.

32
Although, there are different control methods for the autopilot, basic Proportional
Integral Derivative (PID) controller will be implemented in this study. PID controller
is a three-term controller that has a long history, starting from the beginning of the
last century, in the automatic control field. The “P” action (mode) adjusts controller
output according to the size of the error most effectively at moderate frequencies.
The “I” action (mode) is most effective at low frequencies and primary benefit of this
is the reduction of the steady state error. Finally, the “D” action (mode) is most
effective at high frequencies and primarily helps to improve the responsiveness and
stability of the system. Three-term or PID controllers are probably the most widely
used industrial controller. Even complex industrial control systems may comprise a
control network whose main control building block is a PID control module [11, 12].
Based on a survey of over eleven thousand controllers in the refining, chemicals and
pulp and paper industries, 97% of regulatory controllers utilize PID feedback [24].

PID controller has the transfer function

KI
G C (s)  K P   K Ds
s
The equation for the output in the time-domain is
de( t )
u ( t )  K P e( t )  K I  e( t )dt  K D
dt
where
KP is the proportional gain.
KI is the integral gain.
KD is the derivative gain.
The standard closed loop PID configuration is shown in Figure 20

33
 Figure 20 Generic PID Control Scheme

where R (s) , E (s) , U (s) and Y (s) is the reference, error, control and output signals,
respectively.

The effects of the PID components are different in the control system. Each
component has advantages and disadvantages. K P , proportional gain is typically the
main drive in a control loop. It reduces a large part of the overall error and reduces
the rise time. K I , integral gain reduces the final error in a system. Summing even a
small error over time produces drive signal large enough to move the system toward
a smaller error but it may make the transient response worse. K D , derivative gain
counteracts the KI and KP terms when the output changes quickly. This helps reduce
the overshoot and improve the transient response. It has no effect on final error. A
drawback with derivative control is that an ideal derivative controller has high gain
for high-frequency signals. This means that high-frequency measurement noise will
generate large variations in the control signal. Thus, a pure derivative control term
cannot be implemented due to possible noise amplification in most real time
applications. The effect of measurement noise may be attenuated by implementing a
low-pass filter. By this implementation, the control action acts as a derivative for
low-frequency signals and as a constant gain for high-frequency signals. Instead of
filtering just the derivative, it is also possible to use an ideal controller and filter the

34
measured signal. By combining each controller into a single PID or a PI controller,
one can eliminate most of the disadvantages [11, 12].

In order to obtain the controller gains, the MATLAB/Simulink Control System

Design Toolbox SISO Design GUI is used. As a tuning algorithm ‘Singular
frequency based tuning’ is used. This method implements robust control design
techniques to locate stabilizing PID regions in parameter space. This method also
supports unstable systems. As a performance criterion, integral absolute error (IAE)
is checked. IAE is a performance criteria based on the time response. It tries to
minimize the area under the error curve [8].


IAE   e( t ) dt
0

3.5.1 Altitude Hold Autopilot Design

Altitude hold is an important basic mode, which is present in almost all of the UAV
autopilots in the market today. Especially, when the UAV is in a patrol or
reconnaissance mission, the altitude hold mode is crucial. In this part of the thesis,
the aim is to control the aircraft to automatically fly at a selected altitude. The
altitude-hold mode control loop structure is shown in the Figure 21.

Figure 21 Altitude-hold autopilot

In the altitude hold autopilot, the controller is responsible for controlling the pitch
rate, pitch attitude and altitude. This can be achieved with two inner loops and one
outer loop. The inner loops stabilize the fast rotational dynamic and track the
necessary pitch angle in order to remain the desired altitude. The inner loops also
named as the pitch angle hold autopilot. To simplify the design, a two stage design
approach (successive loop closure) is used.

35
3.5.1.1 Pitch Attitude Hold Autopilot Design

Pitch attitude hold autopilot is normally used when the aircraft is in wings-level
flight and generally used as the inner loop of the other autopilot modes [7]. The
design also includes the inner loop pitch rate control. This inner loop control provide
additional design freedom and to help good short-period damping. The control
architecture of the model can be illustrated as in Figure 22.

Figure 22 Pitch Angle controller block diagram

Using the MATLAB/Simulink Single Input Single Output (SISO) tool, system given
in Figure 22 is converted to compensator control architecture to analyze the system
behavior and to determine gains of the controller. MATLAB’s control and estimation
tools manager allows designer to choose one of its different predefined control
architectures. The suitable architecture to be chosen for the pitch attitude hold
controller design is given in Figure 23.

Figure 23 Control architecture chosen for Pitch Attitude Control

36
Here C1 and C2 are the controller compensators, G1 and G2 are the plants simulating
the dynamic model. According to the control system given in Figure 22, the transfer
functions for the SISO tool’s control architecture given in Figure 23 can be written as

H1  H 2  1
 d e s  q s  
G1   , 
 d e ,comm s  d e s  
s 
G2 
q s 
The controller gain for the pitch angle hold controller is calculated as

Table 6 Pitch Angle Hold Controller Gains

Loop Proportional Gain Integral Gain Derivative Gain

Pitch Rate Hold -1.6072 -3.3671 -
Pitch Angle Hold 0.85179 0.15 -

According to the designed controller parameters the response of the closed system is
shown in Figures 24-26.

Step Response
1.4

1.2

0.8
 (degree)

0.6

0.4

0.2

0
0 5 10 15 20 25 30 35 40 45 50
Time(second)

Figure 24 Step Response of the Pitch Angle Control System

37
 Step Response
0.4

0.2

-0.2
e (degree)

-0.4

-0.6

-0.8

-1

-1.2
0 5 10 15 20 25 30 35 40 45 50
Time(second)

Figure 25 Elevator deflections due to unit step pitch angle command

Step Response
60

40

20
e (degree/second)

0
*

-20

-40

-60
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Time(second)

Figure 26 Elevator deflections rate due to unit step pitch angle command

Note that, in this design the actuator dynamics is also taken into consideration by
 d e s  
using the transfer function   . However, the nonlinear effects such as rate
 d e ,comm s  

38
limiting and saturation of the actuator cannot be included in the linear design. Thus,
after the design in SISO tool, the controller parameters are also evaluated by using
the nonlinear simulation model developed in Matlab/Simulink.

3.5.1.2 Altitude Hold Autopilot Design

The altitude hold configuration is shown in Figure 21. Using the above design’s

results, the loop closed by the altitude loop. The suitable architecture to be
c
chosen for the altitude hold controller design is given in Figure 27.

Figure 27 Control architecture chosen for Altitude Control

In this architecture
H 1
 s  h s  
G , 
  comm s  s  
The controller gain for the altitude hold controller is calculated as given in Table 7.

Table 7 Altitude Hold Autopilot Controller Gains

Loop Proportional Gain Integral Gain Derivative Gain

Altitude hold 0.035282 0.01 0.15

As discussed in section 3.5, instead of implementing pure derivative control term,

derivative control term with a low-pass filter is applied. The transfer function of the
low-pass filter is

1
G f (s) 
f s  1

where  f is the filter time constant and selected as 0.01.

39
According to the designed controller parameters, the response of the closed loop
system is shown in Figures 28-30.

Step Response
1.4

1.2

0.8
Altitude (meter)

0.6

0.4

0.2

-0.2
0 5 10 15 20 25 30 35 40 45 50
Time(second)

Figure 28 Step Response of the Altitude Hold Control System

Step Response
0.5

-0.5
e (degree)

-1

-1.5

-2
0 5 10 15 20 25 30 35 40 45 50
Time(second)

Figure 29 Elevator deflections due to unit step altitude command

40
 Step Response
100

80

60

40
e (degree/second)

20

-20
*

-40

-60

-80

-100
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Time(second)

Figure 30 Elevator deflections rate due to unit step altitude command

3.5.2 Heading Hold Autopilot Design

Heading hold is another important basic mode, which is present in almost all of the
UAV autopilots in the market today. Similar to the altitude hold mode, the mode is
beneficial when the UAV is in a patrol or reconnaissance mission. It is also important
when the aircraft is returning to the base after the mission is completed. This mode is
designed to hold the aircraft on a desired heading. The method of implementing this
autopilot is to close an additional yaw-feedback loop around bank angle control
system. In this part of the thesis, the heading hold autopilot is designed. The heading-
hold configuration autopilot is shown in Figure 31.

Figure 31 Heading hold autopilot

41
3.5.2.1 Bank Angle Hold Autopilot Design

Although the bank angle hold mode is an autopilot control mode itself, it is also used
as an inner loop in a heading angle control system. In the design of the bank angle
hold autopilot roll rate compensation is used in this thesis. The controller architecture
in Figure 27 is used for inner roll rate loop. In the architecture

H 1
  s  ps  
G a , 
  a ,comm s   a s  

Similarly, same architecture is used for the outer loop of the bank angle controller. In
this case
H 1
 ps  s  
G , 
 p comm s  ps  
The controller gain for the bank angle hold controller is calculated as given in Table
8.

Table 8 Table Bank Angle Hold Autopilot Controller Gains

Loop Proportional Gain Integral Gain Derivative Gain

Roll Rate Hold 0.044801 0.1788 -
Bank Angle Hold 0.065074 0.29874 -

According to the designed controller parameters the response of the closed loop
system is shown in Figures 32-34.

42
 Step Response
7

4
 (degree)

0
0 5 10 15 20 25 30 35 40 45 50
Time(second)

Figure 32 Step Response of the Bank Angle Control System

Step Response
0.6

0.5

0.4

0.3
a (degree)

0.2

0.1

-0.1

-0.2
0 5 10 15 20 25 30 35 40 45 50
Time(second)

Figure 33 Aileron deflections due to unit step bank angle command

43
 Step Response
16

14

12

10
a (degree/second)

6
*

-2
0 1 2 3 4 5 6 7 8 9 10
Time(second)

Figure 34 Aileron deflections rate due to unit step bank angle command

3.5.2.2 Heading Hold Autopilot Design

The heading hold configuration is shown in Figure 31. Using the above design’s

results, the loop closed by the yaw angle loop. The suitable architecture to be
c
chosen for the altitude hold controller design is given in Figure 27.

In this architecture
H 1
 s  s  
G , 
  comm s  s  
The controller gain for the heading hold controller is calculated as given in Table 9.

Table 9 Heading Hold Autopilot Controller Gains

Loop Proportional Gain Integral Gain Derivative Gain

Heading hold 1.105 0.11347 0.3

44
Similar to the altitude hold autopilot, derivative control term in heading hold
autopilot is applied with a low-pass filter. The filter time constant,  f is selected as
0.1.

According to the designed controller parameters the response of the closed loop
system is shown in Figures 35-37.
Step Response
1.4

1.2

0.8
 (degree)

0.6

0.4

0.2

0
0 5 10 15 20 25 30 35 40 45 50
Time(second)

Figure 35 Step Response of the Heading Angle Control System

45
 Step Response
0.12

0.1

0.08

0.06

0.04
a (degree)

0.02

-0.02

-0.04

-0.06

-0.08
0 5 10 15 20 25 30 35 40 45 50
Time(second)

Figure 36 Aileron deflections due to unit step heading angle command

Step Response
3.5

2.5
a (degree/second)

1.5

1
*

0.5

-0.5
0 1 2 3 4 5 6 7 8 9 10
Time(second)

Figure 37 Aileron deflections rate due to unit step heading angle command

3.6 Nonlinear Simulation of UAV

The designed attitude and heading hold controllers are implemented in the simulation
environment. For the given step input, the UAV response is shown in Figures 38-40.

46
 Aircraft Attitude States
4

 (degree)
2

-2
0 50 100 150 200 250 300
8
(degree)

2
0 50 100 150 200 250 300
3
(degree)

0
0 50 100 150 200 250 300
Time(second)

Figure 38 Attitude Response of the aircraft

Aircraft Altitude
3007

3006

3005

3004
Altitude(m)

3003

3002

3001

3000

2999
0 50 100 150 200 250 300
Time(second)

Figure 39 Altitude of the aircraft

47
 Control Surface Deflection
2

e(degree)
-2

-4

-6
0 50 100 150 200 250 300

0.4

0.2
a(degree)

-0.2

-0.4
0 50 100 150 200 250 300
Time(second)

Figure 40 Deflection of control surfaces

As shown in the figure the controllers work also on the nonlinear simulation
environment. The fluctuations in longitudinal channel are mainly due to vertical gust.
The control surface deflections values are also within the acceptable region.

48
 CHAPTER 4

SENSOR MODELLING

4.1 Inertial Measurement Unit

Inertial sensors comprise of accelerometers and gyroscopes. An accelerometer

measures specific force and a gyroscope measures angular rate, both without an
external reference. Most types of accelerometers measure specific force along a
single sensitive axis. Similarly, most types of gyros measure angular rate about a
single axis. An inertial measurement unit (IMU) combines multiple accelerometers
and gyros, usually three of each. The sensors are rigidly mounted such that their
measuring axes are orthogonal to each other. Therefore, they produce a three-
dimensional measurement of specific force and angular rate. Other elements of a
typical IMU are an IMU processor, a calibration-parameter store, a temperature
sensor, and associated power supplies [13, 14]. Figure 41 shows the main elements
of a typical IMU.

Figure 41 Schematic of an inertial measurement unit [13]

49
The IMU processor performs unit conversion on the inertial sensor outputs, provides
compensation for the known errors of the inertial sensors, and performs range checks
to detect sensor failure. It may also incorporate closed-loop force feedback or
rebalance control for the accelerometers and/or gyros. Unit conversion transforms the
inertial sensor outputs from potential difference, current, or pulses into units of
specific force and angular rate [13].

An IMU is a self contained system. It does not need any external source of signal. It
can work in almost all environments and cannot be jammed. An IMU can provide
very high rate of outputs, which can track high dynamics of motion of the systems to
be used.

The size, mass, performance, and cost of inertial sensors vary by several orders of
magnitude, both within and between the different technologies. Spinning mass gyro,
depends on the gyroscopic theory, is the oldest way to measure of a body rotation
without using external references. Optical gyroscopes whose principle of operation is
a particular property of light in a rotating frame known as the Sagnac effect. Coriolis
gyroscopes use resonators of different shapes (bars, cylinders etc.).

In a force feedback or pendulous rebalance accelerometers, the movement of the

proof mass by the applied acceleration is counteracted by a restoring force to move
the proof mass back to the null position. The amount of force required to achieve this
is proportional to, and measure of, the applied acceleration. In vibrating beam
accelerometer, the applied acceleration moves a proof mass. This movement places
one resonating element under tension and the other compression, which causes
frequency to increase/decrease respectively. The use of two elements allows a
differential frequency to be obtained.

The ongoing trend in inertial sensor technology development is the use of

micromechanical gyros and accelerometers. MEMS gyros and accelerometers do not
represent a measurement technique but represent another production technique for
the sensor. They are produced in very small sizes and in mass quantities with very
low cost. However the large noises on the sensors degrade the performance.

50
4.2 Inertial Sensor Errors

In the literature more than 20 different types of errors are defined for IMU outputs.
However, from the system point of view, most of these errors are out of concern.
This is because, during the field use of an IMU, combined effect of most of the errors
can not be separated by just observing the raw IMU outputs. To localize each error
sources, some specialized test methodologies (like Allen variance tests) should be
incorporated and obviously this is not possible during the active operation [15].

Major error sources of the inertial sensors are scale factor and bias errors. Scale
factor error is defined as the error in the ratio of a change in output to change in the
input to be measured. Bias is the average over a specified time of sensor output
measured at a specified operating condition that has no correlation with the input
acceleration or rotation. Bias is typically expressed in degree per hour ( o h ) for

gyroscope and m s 2 or g for accelerometer. In general, it is composed of a

deterministic part (bias offset or fixed bias) and a random part (bias drift or bias
instability). Fixed bias refers to the sensor output which is present even in the
absence of an applied input acceleration or rotation. Bias instability is the random
variation in bias as computed over a specified finite sample time and averaging time
intervals [16, 17].

The scale factor asymmetry and non-linearity are other error sources for the inertial
sensors. The scale factor asymmetry is the difference between the scale factor
measured with positive input and that measured with negative input, specified as a
fraction of the scale factor measured over the input range. Scale factor asymmetry
implies that the slope of the input-output function is discontinues at zero input. The
scale factor non-linearity refers to the systematic deviation from the straight line that
defines the nominal input output relationship.

Another error source of the inertial sensor is the axes misalignment. It is an error that
comes from the manufacturing. It is the imperfection of mounting the sensors. It
usually results in a non-orthogonality of the axes. As a result, each axis is affected by
the measurements of the other two axes in the body frame.

51
 Figure 42 Common input/output error types [32]

The noise is an additional signal resulting from the sensor itself or due to the other
electronic equipment that interferes with the output signals. It is usually non-
systematic so it cannot be removed from the data using deterministic models. It can
only be modeled by stochastic processes.

4.2.1 Gyroscope Error Model

The output of an angular rate sensor triad can be characterized as a function of its
input angular rate vector as

GYRO  I  S gyro  S gyro

asym sign (  ) M   b gyro   gyro 
~ gyro
(34)

where
~
GYRO is the angular rate sensor output vector.
 is the rate vector sensed by the angular rate sensor triad.
Sgyro is the angular rate sensor triad scale factor error matrix.
S gyro
x 0 0 
 
S gyro
 0 S gyro
y 0 
 0 0 S gyro 
 z 

Sgyro
asym is the angular rate sensor triad scale factor asymmetry matrix.

52
 S gyro
asym, x 0 0 
 
S gyro
asym  0 S gyro
asym, y 0 
 0 0 S gyro 
 asym, z 

M gyro is the sensor axis misalignment matrix.

m gyro
xx m gyro
xy m gyro
xz

 gyro gyro 
M gyro  m yx m gyro
yy m yz 
m gyro m gyro m gyro 
 zx zy zz 

b gyro is the sensor triad bias vector.

b gyro
x

 gyro 
b gyro  b y 
b gyro 
 z 

 gyro is the random error triad vector.

 gyro
x

 gyro 
 gyro   y 
 gyro 
 z 

4.2.2 Accelerometer Error Model

For the accelerometer, the output of the sensor triad can be characterized as a
function of its input acceleration vector as

a ACC  I  Sacc  Sacc

asym sign (a ) M a  b acc   acc 
~ acc
(35)

where
~
a ACC is the accelerometer output vector.

a is the acceleration sensed by the accelerometer triad.

S acc is the accelerometer triad scale factor error matrix.

53
 S acc
x 0 0 
 
S acc  0 S acc
y 0 
 0 0 S acc 
 z 

Sacc
asym is the accelerometer triad scale factor asymmetry matrix.

S acc
asym, x 0 0 
 
S acc
asym  0 S acc
asym, y 0 
 0 0 S acc 
 asym, z 

M acc is the accelerometer sensor axis misalignment matrix.

m acc
xx m acc
xy m acc
xz

 acc 
M acc  m acc
yx m acc
yy m yz 
m acc m acc m acc 
 zx zy zz 

b acc is the sensor triad bias vector.

b acc
x

 acc 
b acc  b y 
b acc 
 z 

 acc is the random error triad vector.

 acc
x

 acc 
 acc   y 
 acc 
 z 

4.3 Magnetometer

It is well known that a magnetic compass is a measurement device, which is small in

size, light in weight, quick in reaction, reliable in operation and free from drift.
Nevertheless, its application is limited because of its low accuracy [18].

The classification of the magnetic sensors can be done according to the field sensing
range. Low field sensors detect magnetic fields less than 1 μG. Sensors that detect

54
magnetic field with a range of 1 μG to 10 G will be considered Earth’s field sensors
and sensors that detect fields above 10 Gauss will be considered as bias magnet field
sensors. The magnetic sensing technique can also be different for different magnetic
sensors. Figure 43 lists various sensor technologies and illustrates the magnetic field
sensing range. The detailed explanations about the technologies are given in [19] and
[20].

Figure 43 Magnetic Sensor Technology Field ranges[19]

Strapdown magnetometer came into use during the 1980’s. Due to significant
progress in magnetometery, accurate and reliable heading information can be
achieved by employing a small, light weight, low-cost and highly reliable
magnetometer. Three magnetometers which are oriented in orthogonal direction is
used to measure the Bx , B y and B z vector components of a magnetic field. The

heading calculation depends on these three magnetic components from strapdown

magnetometers. Thus to obtain the precise heading angle, first of all, the compass
orientation needs to be mathematically transformed in the navigation frame. The
rotational transformation is achieved by applying the rotation equations [21, 22, 23].

55
B b  1 2 3 B n

B x  1 0 0  cos  0  sin   cos  sin  0 B

B   0 cos  sin    0 1 0   sin  cos  0  0 
 y  
 B z  0  sin  cos   sin  0 cos    0 0 1  0 

where  ,  ,  are the roll, pitch and azimuth angles of the aircraft, respectively. To
solve for the heading equation

1 1
cos  0  sin  1 0 0  B x   cos  sin  0 B
 0 1 0  0 cos  sin   B    sin  cos  0  0 
    y   
 sin  0 cos   0  sin  cos   B z   0 0 1  0 

In this rotation equation the necessary data for roll and pitch angles are obtained
from the IMU. Therefore

 B1   B x cos   B y sin  sin   B z sin  cos    B cos  

     
B 2    B y cos   B z sin     B sin  
 B 3   B x sin   B y cos  sin   B z cos  cos   0 
   

  a tan 2 B 2 , B1  (36)

4.3.1 Magnetometer Error Modeling

Any magnetic sensor used to detect the earth’s magnetic field can be influenced by
several different sources of error, all combining to corrupt the output [23]. The main
error sources of a magnetometer are the scale factor, sensor offsets, sensor non-
orthogonality, misalignments and magnetic deviation. The first four errors are
instrumentation errors where as the magnetic deviation depends on the surrounding
magnetic field anomalies.

Scale error of a sensor is a side effect of varying sensitivities between sensors. The
sensitivity of a sensor serves to scale the output, and as no two sensors will have the
exact same sensitivity it must be determined for each axis independently. The Scale
factor error matrix S can be represent as

56
 S mag
x 0 0 
 
S mag
 0 S mag
y 0 
 0 0 S mag 
 z 

Sensor Offset is a constant offset that shifts output of each sensor. It is modeled as
one bias per axis as

b somag, x 
 
b somag  b somag, y 
 b somag, z 
 

The misalignment error of a sensor comes from the non-orthogonality between

individual sensors in the set. When the sensors in a set are aligned properly, a field
aligned with only one axis is observed by only one sensor. Unfortunately due to
fabrication issues, this may not be the case, introducing misalignment and non-
orthogonality errors in the readings. This error can be modeled as

m mag
xx m mag
xy m mag
xz

 mag 
M mag  m yx m mag
yy m mag
yz 
m mag m mag m mag 
 zx zy zz 

Errors in sensing the Earth’s magnetic field come from the local deviation of the
magnetic vector due to onboard hardware and surrounding magnetic materials. The
magnetization characteristics of the sensor are almost linear and consist of soft and
hard iron parts. The hard iron errors are constant, unwanted permanent magnetic
deviation observed by the magnetometer. It can be modeled as a bias

b mag
hi , x

 mag 
b himag  b hi , y 
b mag 
 hi ,z 

The soft component of the magnetic deviation occurs due to magnetization caused by
the permeability of ferromagnetic compounds interacting with an external field. Soft
iron error affects both the intensity and direction of the magnetic field. The error can
be modeled as

57
  a 11 a 12 a 13 
A  a 21 a 22 a 23 
a 31 a 32 a 33 

The complete error model for the triad magnetometers combines both
instrumentation and magnetic deviation. The following equation gives the complete
model in the sensor frame

~
  
BMAG  I  S mag M mag AB  b hi  b si   mag  (37)

where  mag is the random error vector on triad magnetometer.

4.3.2 Magnetometer Calibration

As discussed in the previous topic, magnetic field distortions occur due to

instrumentation errors and the surrounding magnetic field anomalies. When the
sensor is mounted on a platform, it is subjected to ferromagnetic elements in its
vicinity. The effects of ferrous materials will distort, or bend, the earth’s field which
will alter the magnetometer measurement. This effect can be thought of as a
magnetic field that is added to the earth’s field. If the magnetometer is tightly
mounted in the vehicle, the ferrous effects can be accounted for and removed from
the magnetic readings.

Before looking at the distortions effect, it is beneficial to observe an ideal output

curve with no disturbances. Figure 44 illustrates the Bx and By magnetic readings
when the compass is turning around in a circle in a horizontal plane. When the Bx
and By readings are plotted with respect to each other they form a circle centered
about the origin.

58
 Horizontal Magnetic Vectors
0.25

0.2

0.15

0.1
Magnetic Field Magnitude

0.05
[gauss]

-0.05

-0.1

-0.15

-0.2 x magnitude
y magnitude
-0.25
0 50 100 150 200 250 300 350 400 450 500
Time [second]

Figure 44 Error Free Horizontal Magnetic Field Magnitude

Horizontal Magnetic Vectors

0.2 data 1

0.15

0.1
Y Magnitude [gauss]

0.05

-0.05

-0.1

-0.15

-0.2

-0.25 -0.2 -0.15 -0.1 -0.05 0 0.05 0.1 0.15 0.2 0.25
X Magnitude [gauss]

Figure 45 Error Free Horizontal Magnetic Field Magnitude X vs. Y

In the erroneous case, the scale error and the zero bias affect the measurement of
magnetic field as a scaling and constant shift of the output, respectively. Hard iron
error affects the measurement in a similar manner as zero bias. Misalignment is other

59
error that affects the sensor measurement. Soft iron errors split into combination of a
scale error and a misalignment error. Considering the instrumentation errors -scale
error, sensor offset and misalignment- and the magnetic deviation -hard iron and soft
iron- due to platform that the magnetometer mounts on, if the sensor is turning
around in a circle in a horizontal plane, they form an ellipse centered about the
shifted off the origin. In Figure 46 and Figure 47 the distorted magnetic readings are
shown.

Horizontal Magnetic Vectors

0.7

0.6

0.5
Magnetic Field Magnitude

0.4
[gauss]

0.3

0.2

0.1
x magnitude
y magnitude
0
0 10 20 30 40 50 60 70 80 90 100
Time [second]

Figure 46 Erroneous Horizontal Magnetic Field Magnitude

60
 Horizontal Magnetic Vectors
0.3

0.25

0.2

Y Magnitude [gauss] 0.15

0.1

0.05

-0.05

-0.1

-0.15

-0.2
-0.2 -0.15 -0.1 -0.05 0 0.05 0.1 0.15 0.2 0.25
X Magnitude [gauss]

Figure 47 Erroneous Horizontal Magnetic Field Magnitude (X vs. Y)

In literature there are some calibration methods for the magnetometer errors. Each
method is associated with a specified physical movement of the magnetometer
platform in order to sample the magnetic space surrounding the magnetometer space.
In this thesis, it is assumed that the magnetometer is either isolated from or
compensated for the vehicle magnetic field, so that the hard and soft iron term can be
assumed to be zero. It is also assumed that the compass has been compensated for the
local magnetic field deviation from true north so that m N  m e 0 0 where m e is

the magnitude of Earth magnetic field. Finally, it is assumed that the compass is
mounted such that its axes align with the IMU axes.

61
 CHAPTER 5

ATTITUDE & HEADING REFERENCE SYSTEM

5.1 AHRS Mechanization

Attitude and heading reference system (AHRS) is the avionic system and is a
combination of instruments capable of maintaining an accurate estimate of the
vehicle roll, pitch and yaw attitude [26]. In practice various AHRS system design
approaches are available. In this thesis, we consider gyro outputs are integrated
through the attitude kinematic equations and aided with accelerometer as a vertical
reference and magnetometer as a heading reference.

Angular rate vector is measured by a triad of gyros. These gyro measurements are
integrated through the attitude kinematics, resulting to give the rotation matrix
C BN which is used to compute the Euler angles. The aiding devices, accelerometer and
magnetometer measurements are transformed into the navigation frame to predict the
known earth gravitational acceleration and magnetic field vectors. The difference of
the measurement and the references is used to estimate the attitude error and sensor
calibration factors via Kalman Filter. Figure 48 shows the block diagram for attitude
and heading reference system.

62
 Figure 48 Block Diagram for attitude and heading reference system

5.1.1 Attitude Integration

In order to update the direction cosine matrix C BN , the matrix differential equation 38
must be solved.

C BN  C BN  BNB (38)

Where  BNB is the skew symmetric form of the NB

B
. Here NB
B
can be calculated as
using the earth rate ( IEN ) and transport rate ( EN
N
)

NB
B

 IBB  C BN IEN  EN
N
 (39)

Earth rate is due to the earth rotational velocity with respect to the inertial frame and
known as

  cos(Latitude) 
  
N
IE 0 
 (40)
  sin( Latitude)

where  is the earth rotational velocity 7.292115×10-5 radian per second.

Transport rate EN

N
is the turn of navigation frame with respect to the earth frame due
to the north and east velocity components of the vehicle. This velocity can be
calculated as

63
  VE 

 R T  h  
VN
EN
N
  (41)
 R M  h  
V tan( Latitude) 
 E
 R T  h  
Here R T and R M are the transverse and meridional radii of the curvature of the
earth respectively and h is the height above the ground.

Since in the AHRS mode the position is generally not available and the computed
values are small compared with the error terms, IEN and EN
N
terms can be ignored.

5.2 Kalman Filter

The Kalman filter is a recursive optimal estimation algorithm. It estimates the

parameters of a system that may continually change. The estimates are updated using
measurements that are subject to noise. Kalman filter is optimal in the sense that it
minimizes the mean square of the estimation error. The measurements must be
functions of the parameters estimated, but the set of measurements at a given time
need not contain sufficient information to uniquely determine the values of the
parameters at that time. The filter uses knowledge of the deterministic and statistical
properties of the system parameters and the measurements to obtain optimal
estimates given the information available [13].

In the literature, two types of Kalman filter, direct Kalman filter (total state space)
and indirect Kalman filter (error state space) are used. In the direct Kalman filter, the
states itself are estimated. Thus, the filter must have to maintain the high frequency
state dynamics (e.g. angular and linear dynamics of an aircraft) while suppressing the
noisy measurement data. Since Kalman filter can be considered as a multi-
dimensional low pass filter, this creates an undesirable lag in most of the real-time
applications. Further, it must run at very high rates in real-time operating system in
order to catch the dynamics of the system. On the other hand, since Kalman filter is a

64
linear filter, the nonlinearities in the physical equations must be linearized which
impedes the Kalman filter to work properly for highly nonlinear systems.

In the indirect filter, instead of the system’s total states, the errors of the system
states are estimated. The rationale behind the indirect Kalman filter can be more
easily understood by complementary filter methodology. In Figure 49, a basic
complementary filter is shown.

Figure 49 Basic complementary filter

Suppose that there are two independent noisy measurements (x and y) of the same
signal (z). The noise in y is mostly high frequency and the noise in x is mostly low
frequency. In order to eliminate the high frequency noise in y, a low pass filter G(s)
is used as a filter. If G(s) is low pass, [1-G(s)] is the complementary filter (high pass
filter) which filters out the low frequency noise in x. In the transform domain, the
output is

Ẑ(s)  Z
(s)  N1 (s)1  G (s)   N 2 (s)G (s)
  
Signal Noise Term
Term

The complementary filter can be reconfigured as in Figure 50 In this case the G(s)
operates only on the noise or error in the measurements x and y, not the signal itself.

Figure 50 Equivalent form of complementary filter

65
Feedback and feed forward filters are the two types of the indirect Kalman filter. In
the feed forward type, the system states errors are corrected with the estimated error
states, but the system is unaware of this correction. So for unstable systems, the error
states increase and go away from the linearity assumption. In the feedback indirect
Kalman filter, the estimated errors are fed back to system. Thus, in the feedback
type, the error states are not allowed to grow unbounded.

The main advantages of the Indirect (complimentary) Kalman filter can be

summarized as

1) Linearity requirement of Kalman filter can be satisfied more easily in indirect

Kalman filter.

2) Complimentary Kalman filter maintains the high dynamics of the system

while not introducing undesirable lag in real-time applications.

3) The computational requirement of Indirect Kalman filter is much less than the
direct Kalman filter since it does not need to be run at high rates.

In this chapter, Kalman filter equations are derived. Next the error dynamics of the
attitude and heading reference system is derived. This includes the attitude error
dynamics, accelerometer and magnetometer measurement model which are necessary
for the Kalman filter application.

5.3 Discrete Kalman Filter

In this part of this chapter the discrete Kalman filter in its most universal form will
be presented [27, 28]. The derivations of these equations are given in Appendix A.

Equations 42-46 comprise the kalman filter recursive equation.

x̂ k  x̂ k  K k z k  H k x̂ k  (42)


K k  Pk H Tk H k Pk H Tk  R k 
1
(43)

Pk  I  K k H k Pk (44)

66
x̂ k1   k x̂ k (45)

Pk1   k Pk  Tk  Q k (46)

Here “x” is the state vector, which is the parameter estimated by Kalman filter. “P” is
the error covariance matrix and defines the expectation of the square of the deviation
of the state vector estimate from the true value of the state vector. “z” is the
measurement vector which is a set of measurements of the properties described by
the state vector. R is the measurement noise covariance matrix which defines the
expectation of the square of the measurement noise.

 k is the transition matrix. It defines the change of the states with time as function
of the dynamics of the system modeled by the Kalman filter. Q k is the noise
covariance matrix and defines how the uncertainties of the state estimates increase
with time due to noise sources in the modeled Kalman filter. H k is the measurement
matrix. It defines how measurement vector varies with the state vector. K k is the
Kalman gain matrix and used to determine the weighting of the measurement
information while updating the state estimates. Figure 51 illustrates the Kalman filter
loop.

67
 Figure 51 Kalman Filter Loop

5.4 Linearized Error Dynamics

5.4.1 Attitude Error Dynamics

The estimated direction cosine matrix can be expressed as given in[16];

~
C nb  I   C nb (47)

where  is the skew symmetric matrix that represents the tilt error in the direction
cosine matrix. Thus, the tilt error can be represented as

~ T
  I  C nb C nb (48)

Differentiating this equations

~ n n T ~ n  n T
  C
 b Cb  Cb Cb (49)

68
C nb propagates as a function of the absolute body rate  ibb and the navigation frame

rate  inn as follows

C nb  C nb  ibb   inn C nb (50)

~
Similarly the time differential of the estimated matrix C nb

~ ~ ~ ~ ~
C nb  C nb  ibb   inn C nb (51)

~ ~
Where  ibb and  inn represent the measured body rate and the estimated turn rate of
the navigation reference frame, respectively.

~
Substituting Cnb and C nb

 C
  
b  ib   in C b C b  C b C b  ib   in C b 
~ n ~ b ~ n ~ n nT ~ n n b n n T

~ n ~ b nT ~ n ~ n nT ~ n b nT ~ n nT n
  C
 b  ib C b   in C b C b  C b  ib C b  C b C b  in
~n ~ b
  C
  nT
~ n ~ n nT ~ n nT n
b  ib   ib C b   in C b C b  C b C b  in
b

~
Substituting C nb  I   C nb

 b
~b
  I   C n   b nT ~n

ib   ib C b   in I   C b C b  I   C b C b  in
n nT n nT n

 b
~b
  I   C n   ~n

ib   ib C b   in I     I    in
b nT n

 b
~b
  I   C n   nT

~n ~n
ib   ib C b   in   in    in   in
b n n

Writing

~
 ib   ib   ib
~
 in   in   in

~n
  I   C n  b C n T   n   n  
 b ib b in in in 
~n
  C n  b C n T  C n  b C n T   n   n  
 b ib b b ib b in in in 

Ignoring error product term

69
 in in in 
   n   n   n    n  C n  b C n T
 in b ib b

   n   n    n    n  C n  b C n T
 in in in in b ib b

   n   n    n  C n  b C n T
 in in in b ib b

It can be shown from an element by element comparison that above equation may be
expressed in vector form as

   inn     inn  C nb  ibb (52)

where    Nx  Ny  Nz T
, the misalignment vector and

  
inn    inn
 inn    inn
 ibb    ibb

5.4.2 Accelerometer Measurement Error Model

The reference gravity field vector in navigation frame is used as measurement for the
filter. That is

0
G N
REF   0 
G 

The coordinate transformed navigation frame acceleration vector can be obtained by

transforming the body-frame acceleration vector to navigation frame by the
calculated direction cosine matrix as follows

~N ~ N~B
a  C B a ACC

If we express the measured acceleration vector in terms of the true value as

~B
B
a ACC  a ACC  a ACC

where a ACC is the accelerometer error vector which is composed of

70
a x  a x 0 0  S accx
  0  az a y  m acc
yz
 b acc
x
  acc
x

     acc     acc   acc   acc 
a y    0 ay 0  S y    a z 0  a x  m zx   b y    y 
 a z   0 a z  S acc   a y 0  m acc  b acc   acc 
   0 z   ax xy   z   z 

The calculated direction cosine matrix can be expressed in terms of the true value as

~
C BN  I   C BN

Then the measurement residual is obtained as

~ ~ ~B
a MEA  G REF
N
 a N  G REF
N
 C BN a ACC
 G REF
N
 I   C BN a ACC
B
 a ACC 
 G REF
N
 C BN a ACC
B
 C BN a ACC
B
 C BN a ACC

or

 0  a ACC, Nx   0  a ACC, Nz a ACC, Ny   Nx 

     
a MEA   0   a ACC , Ny    a ACC, Nz 0  a ACC, Nx   Ny   C BN a ACC
G  a ACC, Nz   a ACC, Ny a ACC, Nx 0    Nz 


(53)
B
Here, a ACC is the body acceleration which is composed of sustained acceleration and
vibration.

5.4.3 Magnetometer Measurement Error Model

The reference magnetic vector in navigation frame is used as measurement for the
filter. That is

 B
B N
REF   0 
 0 

The coordinate transformed navigation frame magnetic vector can be obtained by

transforming the body-frame magnetic vector to navigation frame by the calculated
direction cosine matrix as follows

71
~ ~ ~B
B N  C BN BMAG

If we express the measured magnetic vector in terms of the true value as

~B
B
BMAG  BMAG   BMAG

where  BMAG is the magnetometer error vector which is composed of

 m mag
xy

  mag  
B m xz  
 x 0 0  S mag  B y Bz 0 0 0 0   mag
 m yx  
x
  mag  
 0 B y 0  S y    0 0 Bx Bz 0 0   mag 
B x    m yz  
   0 0 B z  S mag
z
 0
  0 0 0 Bx B y   mag  
B MAG  B y    m
 zx 
 B z   m zy  
mag
   
 b hi , x  b so, x   mag x
 
      mag  
 b hi , y   b so, y    y  
  b   b   mag  
  hi ,z   so , z   z  

The calculated direction cosine matrix can be expressed in terms of the true value as

~
C BN  I   C BN

Then the measurement residual is obtained as

~ ~ ~B
BMEA  BREF
N
 B N  BREF
N
 CBN BMAG
 BREF
N B

 I   CBN BMAG  BMAG 
  C B N
B
B
MAG  C BMAG
N
B

or

 0  BMAG , Nz BMAG , Ny   Nx 
 
BMEA   BMAG , Nz 0  BMAG , Nx   Ny   C BN BMAG (54)
 BMAG , Ny BMAG , Nx 0   Nz 
 

72
5.5 AHRS Kalman Filter Design and Simulation

As we derived in section 5.4.1, the attitude errors dynamics satisfy the equation 52.
There  ibb includes various gyro error sources. In the designed Kalman filter only
the gyro biases are modeled as a state of the filter. Actually, the gyro bias is
composed of two error sources, namely run to run bias repeatability and in-run bias
stability. These can be modeled by random constant and Gauss-Markov stochastic
processes, respectively as in the following equations


 BIAS_ OFFSET  0.0

 BIAS _ STAB   1 /  GBIAS BIAS _ STAB  w BIAS _ STAB



where w BIAS _ STAB is white noise process with power spectral density

2 GBIAS
2
Q GBIAS  where  GBIAS
2
is the steady-state variance of the Gauss-Markov
 GBIAS
process. For the sake of complexity, these two process are modeled as a random walk
process


 RWBIAS  w RWBIAS (55)

Here the initial variance of the error state E 2RWBIAS 0 , is taken as the run to run

bias repeatability and the spectral density of the process noise Q RWBIAS is determined
such that the model tracks the in run bias stability.

If the similar approach is applied for the accelerometer, the process model of the
accelerometer is defined as

a RWBIAS  w RWBIAS (56)

where w RWBIAS is the white noise process with spectral density Q RWBIAS .

If the error state vector and process input noise vector are defined as

73
     w ATT 
    
   w ATT 
    w ATT 
   GYRO 
RWBIAS,x   w RWBIAS 
x  RWBIAS, y  w t    w GYRO 
   GYRO
RWBIAS

 RWBIAS,z   w RWBIAS 
 a   w ACC 
 RWBIAS, x   RWBIAS 
 a RWBIAS, y   w ACC
RWBIAS 
 a   ACC 
 RWBIAS, Z   w RWBIAS 

following process model is obtained.

x t   Ax t   Bw t 
(57)
yt   Ht x t   v( t )

0 33  C BN 0 33   C BN  C BN 0 33 

   
where A  0 33 0 33 0 33  ; B   0 33 I 33 0 33 
0 33 0 33 0 33   0 33 0 33 I 33 
 


  gN   0 33 C BN   v ACC 
H ; v  
 B 
N
  0 33 0 33 

 v MAG 

where v ACC  C BN a ACC  a ACC, N and v MAG  C BN B MAG and

with noise densities Ew t w t     Q   Evt vt     R  

Noting that, if the vehicle undergoes a sustained acceleration, for example due to a
prolonged turn, a cutout mechanism is used. The acceleration of the vehicle due to
vibration is compensated with the proper tuning of the measurement noise covariance
matrix, R.

Since Kalman filter is implemented discrete time as it is shown in 5.3, the continuous
time model equation (57) can be converted to discretized at sampling times t k with
uniform sampling interval dt as

74
x k 1   k , k 1 x k  w k

yk  Hk x k  vk

where

x k  x t k 

y k  yt k 

 tk 
 k ,k 1  exp  Ad   I  Adt  0.5A 2 dt 2  
t 
 k 1 
w k is the discrete time process noise sequence with its variance can be obtained as

Q k  Ew k w Tk   Qdt

The inertial sensor rate is simulated as 100 Hz, while Kalman Filter is updated at 1
Hz. In designed Kalman filter, following IMU error model parameters are taken.

Table 10 Table IMU Parameters [29]

Sensor Parameter Specification Units
o
Range +/- 300 /sec
o
Bias (Turn on to Turn on repeatability) 0.08 /sec
o
Bias (In-Run Stability, fixed temp) 0.1 /sec
o
Bias (In-Run Stability, over temp) 0.7 /sec
Angular Rate o
Bias (Short Term Stability) 0.02 /sec
o
Angle random Walk Noise 3.5 /√hour
Scale factor Error 0.5 %
Nonlinearity 0.2 % FS
Range +/- 5 g
Bias (Turn on to Turn on repeatability) 10 mg
Bias (In-Run Stability) 10 mg
Accelerometer Bias (Short Term Stability) 0.2 mg
Noise 0.4 mg/√Hz rms
Scale factor Error 0.5 %
Nonlinearity 0.2 % FS
Range +/-1.2 Gauss
Bias (Turn on to Turn on repeatability) TBD mGauss
Bias (In-Run Stability) 15 mGauss
Magnetometer
Noise 1e-4 mGauss /√Hz
Scale Factor Error 0.7 %
Nonlinearity 0.4 % FS

75
 True Trajectory
100

80

60 

40
Attitude(degree)

20

-20

-40

-60

-80

-100
0 100 200 300 400 500 600
Time(second)

Figure 52 True Trajectory

Attitude Error
20
(degree)

-20

-40
0 100 200 300 400 500 600
50
(degree)

-50
0 100 200 300 400 500 600
50
(degree)

-50 Free IMU Solution

Kalman Filter Solution
-100
0 100 200 300 400 500 600
Time(second)

Figure 53 Attitude Solution Error

As it is shown in Figure 53, the free IMU solution drifts when time goes on due to
sensor errors. In ten minutes, the attitude error becomes very large. However, the

76
Figure 54 shows that, Kalman filter estimates the attitude in the vicinity of the 1
degree error and the error is bounded. The reason is shown in Figure 55 and Figure
56. Kalman Filter estimates both the gyro and accelerometer bias error accurately as
the covariance plots show.

 Error  Error
4 4
 error  error
2 2
Angle(degree)

Angle(degree)
Covariance Covariance

0 0

-2 -2

-4 -4
0 200 400 600 0 200 400 600
Time(second) Time(second)

 Error
5
Angle(degree)

-5
 error
Covariance
-10
0 200 400 600
Time(second)

Figure 54 Kalman filter attitude solution

77
 Acceleroemeter Bias Estimation Error
20

X Acc (mg)
0

-20
0 100 200 300 400 500 600
20
Y Acc (mg)

10

-10
0 100 200 300 400 500 600
20
Z Acc (mg)

10

-10
0 100 200 300 400 500 600
Time(second)

Figure 55 Accelerometer bias estimation error

Gyro Bias Estimation Error

500
X Gyro (deg/hr

-500
0 100 200 300 400 500 600
500
Y Gyro (deg/hr)

-500
0 100 200 300 400 500 600
500
Z Gyro (deg/hr)

-500
0 100 200 300 400 500 600
Time(second)

Figure 56 Gyroscope Bias estimation error

78
 CHAPTER 6

SYNTHESIS OF UAV AUTOPILOT AND NAVIGATION ALGORITHM

In this chapter of the thesis, the designed autopilot and AHRS Kalman Filter are
brought together in a simulation environment as shown in Figure 57.

Figure 57 Simulation Model of the UAV

Simulations are done under two main conditions. In the first one the free IMU
outputs are used as feedback of the system. The second is the AHRS Kalman filter
outputs are used. In both test condition gust are available as a disturbance.

79
From Figure 58 to Figure 63, it is shown that the response of the aircraft using
different measurement, for a given h  3005 meter and   2 degree command
while flying in a trim condition.

Aircraft Attitude
40
Aircraft Attitude
 (degree)

20
Attitude Measurement
0

-20
0 50 100 150 200 250 300
30
(degree)

20

10

0
0 50 100 150 200 250 300
20
(degree)

-20

-40
0 50 100 150 200 250 300
Time(second)

Figure 58 Aircraft attitude with free IMU solution

Aircraft Altitude
3008

3006
Altitude(m)

3004

3002

3000 Aircraft Altitude

Altitude Command
2998
0 50 100 150 200 250 300

2
Altitude Error(m)

-2

-4

-6
0 50 100 150 200 250 300
Time(second)

Figure 59 Aircraft altitude with free IMU solution

80
 Aircraft Heading
10

Heading(degree)
-10

-20
Aircraft Heading
-30
Heading Command
-40
0 50 100 150 200 250 300

0
Heading Error(degree)

-10

-20

-30

-40
0 50 100 150 200 250 300
Time(second)

Figure 60 Aircraft heading with free IMU solution

In Figure 58 it is shown that  measurement is constant, while the aircraft heading

drifts. The reason is that, the  command signal is constant and measured  value
drifts with the rate of gyro bias. The controller generates a constant  command
which steers the aircraft with gyro drift rate but with opposite sign. Thus the
calculated heading by IMU measurements stays constant due to cancelation of gyro
bias but aircraft turns.

It is interesting to see that although the  measurement drifts, the aircraft roll angle
does not. The roll of the aircraft is constant to provide the coordinated turn. In order
to understand these, the steps of the heading hold controller analyzed with the
dominant error sources namely gyro bias in linear simulation environment. In the
analysis, it is seen that the bias on the output heading and outer loop controller
results in a roll  command with a drift rate of the  measurement drift rate. Since
the roll angle hold loop is a negative feedback, the  command and the 
measurement cancel each other. That results in a steady roll attitude.

81
In the altitude hold autopilot, since the altitude is closed as an outer loop the effect of
the pitch angle drift on the measurement cannot change the actual aircraft pitch
angle. The only effect is a small biasing on the altitude. Of course if the autopilot
was only a pitch attitude hold controller without an outer controller loop (altitude
hold), then gyro drift would also make the aircraft pitch angle to drift.

Aircraft Attitude
4
Aircraft Attitude
 (degree)

2
Attitude Measurement
0

-2
0 50 100 150 200 250 300
8
(degree)

2
0 50 100 150 200 250 300
4
(degree)

-2
0 50 100 150 200 250 300
Time(second)

Figure 61 Aircraft attitude with AHRS solution

82
 Aircraft Altitude
3008

3006

Altitude(m)
3004

3002

3000 Aircraft Altitude

Altitude Command
2998
0 50 100 150 200 250 300

2
Altitude Error(m)

-2

-4

-6
0 50 100 150 200 250 300
Time(second)

Figure 62 Aircraft Altitude with AHRS solution

Aircraft Heading
3
Heading(degree)

1
Aircraft Heading
Heading Command
0
0 50 100 150 200 250 300

1
Heading Error(degree)

-1

-2
0 50 100 150 200 250 300
Time(second)

Figure 63 Aircraft heading with AHRS solution

As it can be seen from the Figures 61-63, AHRS Kalman filter prevents the drift on
the attitude of the aircraft. While in the free IMU case the heading errors grow
unboundedly in a short time, in the AHRS mode by using Kalman filter these errors
can be compensated.

83
 CHAPTER 7

CONCLUSIONS

Unmanned air vehicles (UAVs) are performing progressively more complex tasks
than manned air vehicles and thus requiring increasingly robust and accurate heading
and orientation information. As discussed before, inertial measurement units (IMUs)
based on micro electromechanical systems (MEMS) technology are increasingly
becoming important in this extremely challenging area due to their advantages in
terms of small size, low weight, low power consumption, high dynamic response,
availability and cost. However, the large noise characteristics degrade the
performance of the attitude calculations. An IMU using MEMS integrated with the
GPS can provide continuous and reliable solution for the orientation of the UAV. In
a UAV system with low cost IMU and GPS, the error characteristics of the MEMS
IMU can be compensated.
In the case of the GPS cut out, as it was seen from the analysis of the simulation
results, the IMU solution cannot perform the required accuracy. The analysis shows
that in five minutes IMU free solution feedback of the attitude of a commercially
available MEMS IMU causes 35 degrees of drift in attitude of the UAV. However,
simulation results show that using the accelerometer and additional magnetometer
prevents the attitude error growing unboundedly. The accelerometer is used for the
roll and pitch attitude estimation of the UAV and magnetometer is used for the
heading estimation of the UAV. Using both sensors as an aiding, the Kalman Filter
provides accurate pitch, roll and heading estimates during situations that GPS is
compromised or unavailable.

84
In the modeling and simulation studies, following tasks are performed

6 DOF Freedom Equations of motion analysis is done for the UAV including
actuators, engine, atmosphere and gust models.

The nonlinear model is trimmed at selected operational flight condition. Nonlinear

equations of motion for the UAV are linearized at this trim condition. For the
comparison, the nonlinear and linear responses of the aircraft are evaluated for given
elevator and aileron inputs. The open loop response characteristics are investigated.

Two important flight control systems (autopilots) namely, altitude and heading hold
autopilots are designed. The designed autopilot modes are implemented into the
nonlinear simulation environment and their performance are investigated.

The error sources of the inertial sensors and magnetometer are also investigated.
Output of these sensors are modeled and implemented into the nonlinear simulation
environment in order to simulate realistic measurement of the system parameters.

Attitude Heading Reference System Kalman filter equations are derived and the error
state Kalman filter is designed. The inertial sensors and magnetometer measurement
models are derived. The designed AHRS Kalman filter is implemented into the
nonlinear simulation environment.

This thesis has shown the effectiveness of using magnetometer and accelerometer in
AHRS algorithms in attitude calculations and autopilot applications. In this study,
real sensor outputs are simulated by modeling the sensor errors. However, the error
characteristics are affected by the environmental conditions, as well. Thus, the
simulations may not reflect the real sensor error characteristics. Similarly, the
nonlinear model may not be exactly simulating the UAV. Thus, simulations are
always doubtful and do not reflect the real cases unless they are confirmed by the
real flight test data.

85
As a future work, the AHRS mode algorithm can be evaluated by utilizing real
sensor data. Further, other autopilot algorithms can be designed, tested and
confirmed on a real UAV platform along with the existing algorithms.

86
 REFERENCES

[1] The Department of Defense Dictionary of Military and Associated Terms

[2] Roskam Jan, “Airplane Flight Dynamics and Automatic Flight Controls”,
Lawrence, DARcorporation, 1979.

[3] Kargın, Volkan, “Design of an Autonomous Landing Control Algorithm for a

Fixed Wing UAV”, Master Thesis, Ankara : METU, 2007.

[4] Zipfel, H. Peter, “Modelling and Simulation of Aerospace Vehicle

Dynamics”, AIAA Education Series, 2000

[5] Cook, V. Michael, “Flight Dynamics Princible”, Elsevier Aerospace

Engineering Series, Second Edition, 2007

[6] Limbach Flugmotoren website, “http://www.limflug.de/”, Last accessed 15

July, 2010.

[7] Stevens, L. Brian and Lewis, L. Frank, “Aircraft Control and Simulation”,
John Wiley & Sons, Inc. 1992.

[8] Gockenbach, M. S., “Introduction to Sequental Quadratic Programming”

Lecture Notes, Last accessed 10 2010

www.math.mtu.edu/~msgocken/ma5630spring2003/lectures/sqp1/sqp1.pdf

[9] U.S. Standard Atmosphere, 1976, U.S. Government Printing Office,

Washington, D.C.

[10] Goldstein, H.; “Classical Mechanics”, Addison-Wesley 1959

[11] Cheng-Ching Yu, “Autotuning of PID Controllers A Relay Feedback

Approach”, Springer, 2nd Edition, 2006

[12] Michael A. Johnson and Mohammad H. Moradi, “PID Control New

Identification and Design Methods”, Springer, 2005

[13] Groves, D. Paul, “Princibles of GNSS, Inertial, and Multisensor Integrated

Navigation Systems”, Artech House, 2007

[14] Mohinder S. Grewal, Lawrence R. Weill, Angus P. Andrews, “Global

Positioning Systems, Inertial Navigation and Integration”, John Wiley &
Sons, Inc., 2nd Edition, 2001

87
[15] Yüksel, Yiğiter, “Design and Analysis of Transfer Alignment Algoritms”,
Master Thesis, Ankara : METU, 2005

[16] Tittertoon, H. David, Weston, L. John, “Strapdown ınertial Navigation

Technology”, The Institution of Electrical Engineers, Second Edition, 2004

[17] IEEE Standart for Inertial Sensor Terminology, The Institute of Electrical and
Electronics Engineers, Inc., 2001

[18] Liu, Sheng-wu, Zhang, Zhao-nian and Hung C. James, “A High Accuracy
Magnetic Heading System Composed of Fluxgate Magnetometers and a
Microcomputer”, IEEE, 1989

[19] Lenz, E. James, “A Review of Magnetic Sensors”, Proceedings of the IEEE,

Vol. 78, No. 6, June 1990

[20] Caruso, J. M., Bratland, T., Smith H. C., Schneider, R., “A New Perspective
on Magnetic Field Sensing”,

[21] Kayton, M. and Fried, R. Walter, “Avionics Navigation Systems”, John

Wiley & Sons, Inc., Second Edition, 1997

[22] Caruso, M. J., “Applications of Magnetoresistive Sensors in Navigation

Systems”, SAE (Sensors and Actuators), 1997

[23] Koo, W., Sung, S. and Lee, Y. J., “Development of Real time Heading
estimation algoritm using magnetometer”, ICROS SICE International Joint
Conference 2009

[24] L. Desbourough and R. Miller, “Increasing customer value of industrial

control performance monitoring—Honeywell’s experience”, Sixth
International Conference on Chemical Process Control. AIChE Symposium
Series Number 326 (Vol. 98), 2002

[25] Matlab R2008a Release Notes, Mathworks, Inc

[26] Farrell, A. Jay, “Aided Navigation GPS with High Rate Sensors”, Mc Graw
Hill, 2008

[27] Brown, G. R., Hwang Y. C. Patrick, “Introduction to Random Signals and

Applied Kalman Filtering”, John Wiley & Sons ,3rd. Edition , 1997.

[28] Gelb A., “Applied Optimal Estimation”, 1974, MIT Press

88
[29] Technical Product Overview of 3DM-GX1 Gyro Enhanced Orientation
Sensor.

[30] Di Li, Rene Jr. Landry, Philippe Lavoie, “Low-cost MEMS Sensor-based
Attitude Determination System by Integration of Magnetometers and GPS: A
Real-Data Test and Performance Evaluation”, IEEE, 2008

[31] Huang, J. Z. and Fang, C. J., “Integration of MEMS Inertial Sensor-Based

GNC of a UAV”, International Journal of Information Technology, Vol. 11,
No. 10, 2005

[32] Grewal, S. Mohinder, Andrews, P. Angus, “Kalman Filtering: Theory and

Practice Using Matlab”, John Wiley & Sons, Inc., 2nd Edition, 2001

[33] Landou, L. D. and Lifshitz, E. M., “Mechanics”, Pergaman, Press, 1960

89
 APPENDIX A

DISCRETE KALMAN FILTER

A random process to be estimated can be modeled in the form [27]

x k 1   k x k  w k A1

The observation (measurement) of the process is assumed to occur at discrete points

in time in accordance with the linear relation ship

zk  Hk x k  vk A2

where

xk is the (nx1) process state vector at time t k

k is the (nxn) matrix relating x k to x k 1 in the absence of a forcing function (if

x k is a sample of continuous process  k is the usual state transition matrix)

wk is the (nx1) vector – assumed to be white sequence with known covariance

structure

zk (mx1) vector measurement at time t k

Hk is the (mxn) matrix giving the ideal (noiseless) connection between the
measurement and the state vector at time t k

90
vk is the (mx1) measurement error – assumed to be a white sequence with
known covariance structure and having zero cross-correlation with the w k
sequence

The covariance matrices for the w k and v k vectors are given by

ik
 
Q ,
E w k w iT   k
ik
A3
 0,

ik
  R ,
E v k v iT   k
ik
A4
 0,

 
E w k v iT  0, for all k and i A5

Assuming that having an initial estimate of the process at some point in time t k and
this estimate is based on all the knowledge about the process prior to t k . This prior

(or a priori) estimate will be denoted as x̂ k where the “hat” denotes estimate, and the
“super minus” is a reminder that this is the best estimate prior to assimilating the
measurement at t k . It is also assumed that the error covariance matrix associated with

x̂ k is known. That is, the estimation error is defined as

e k  x k  x̂ k A6

and the associated error covariance matrix is

  
Pk  E e k e kT  E x k  x̂ k x k  x̂ k 
T
 A7

With the assumption of a prior estimate x̂ k , the measurement z k is used to improve

the prior estimate. A linear blending of the noisy measurement and prior estimate in
accordance with the equation

x̂ k  x̂ k  K k z k  H k x̂ k  A8

where

x̂ k is the updated estimate

91
Kk is the blending factor (yet to be determined)

Using minimum mean-square error as the performance criterion, finding particular

blending factor K k yields an updated estimate that is optimal in some sense. The
expression for the error covariance matrix associated with the updated (a posteriori)
estimate is formed as

  
Pk  E e k e Tk  x k  x̂ k x k  x̂ k 
T
 A9

Substituting equation A2 into equation A8 yields

x̂ k  x̂ k  K k H k x k  v k  H k x̂ k  A10

Then substituting the resulting expression for x̂ k into equation A9, the result is

  
Pk  E x k  x̂ k   K k H k x k  v k  H k x̂ k  x k  x̂ k   K k H k x k  v k  H k x̂ k 
T

A11

Performing the indicated expectation and noting the x k  x̂ k  is the a priori

estimation error that is uncorrelated with the measurement error v k

Pk  I  K k H k Pk I  K k H k   K k R k K Tk
T
A12

Here Pk is the general expression for the updated error covariance matrix for any

gain K k , suboptimal or otherwise.

To find particular K k that minimizes the individual terms along the major diagonal
of Pk , because these terms represent the estimation error variances for the elements
of the state vector being estimated. The optimization can be done in a number of
ways. Using a straightforward calculus approach, and to do so two matrix
differentiation formulas is to be needed which are

dtrace(AB)
 BT (AB must be square) A13
dA

92
dtrace(ACA)
 2AC (C must be symmetric) A14
dA

Where the derivative of a scalar with respect to a matrix is defined as

 ds ds 
 
 da 11 da 12 
ds  ds ds
  A15
dA  da 21 da 22
 
 
  

Expanding the general form for Pk and rewriting

Pk  Pk  K k H k Pk  Pk H Tk K Tk  K k H k Pk H Tk  R k K Tk A16

Noting that the second and third terms are linear in K k and the fourth term is

quadratic in K k . The aim here is that minimizing the trace of P because it is the sum
of the mean square errors in the estimates of all the elements of the state vector. In
order to do that, differentiating the trace of Pk with respect to K k should be done.

Noting that the trace of Pk H Tk K Tk is equal to the trace of its transpose K k H k Pk , the
result is

dtracePk 
dK k
  
 2 H k Pk  2K k H k Pk H Tk  R k  A17

Setting the derivative equal to zero and solving for the optimal gain

K k  Pk H Tk H k Pk H Tk  R k 
1
A18

This particular K k namely, the one that minimizes the mean square estimation error,
is called the Kalman gain.

The covariance matrix associated with the optimal estimate may now be computed.
Referring equation A16

Pk  Pk  K k H k Pk  Pk H Tk K Tk  K k H k Pk H Tk  R k K Tk A19

93
Substituting of the optimal gain expression in equation A18 in to equation A19 leads
to

Pk  Pk  Pk H Tk H k Pk H Tk  R k  H k Pk

1
A20

or

Pk  Pk  K k H k Pk H Tk  R k K Tk A21

or

Pk  I  K k H k Pk A22

By the use of equation A8 with K k which is set to the Kalman gain as given by
equation A17, the measurement at t k is assimilated. Noting that to accomplish this

x̂ k and Pk are needed for this evaluation. Similarly, in order to make optimal use of

the measurement z k 1 , x̂ k 1 and Pk1 are needed. The updated estimated x̂ k is easily
projected ahead via the transition matrix. We are justified in ignoring the
contribution of w k in equation A1 because it has zero mean and is not correlated
with any of the previous w ’s. Thus

x̂ k 1   k x̂ k A23

The error covariance matrix associated with x̂ k 1 is obtained by first forming the
expression for the a priori error

e k1  x k 1  x̂ k1
  k x̂ k  w k    k x̂ k A24
 k ek  w k

Noting that w k and e k have zero cross-correlation, because w k is the process noise

for the step ahead of t k . Thus it can be written the expression for Pk1 as

  
Pk1  E e k 1e k T1  E  k e k  w k  k e k  w k 
T
 A25
  k Pk  Tk  Q k

94
The quantities at time t k 1 are available and the measurement z k 1 can be
assimilated just as in the previous step.

Equations A8, A18, A22, A23 and A25 comprise the Kalman Filter recursive
equations. The pertinent equations and the sequence of computational steps are
shown pictorially in Figure 51.

95
 APPENDIX B

AERODYNAMIC COEFFICIENTS and DERIVATIVES

This section contains the values of the aerodynamic coefficients and derivatives.
These data is a similar UAV of METU TUAV in geometrical and mass-inertia
properties.
Drag Coefficient
0.4

0.3


0.2
Cd

0.1

0
-0.1 -0.05 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35
 (rad)

Figure 64 C D vs. 

Lift Coefficient
1.5

1

CL

0.5

0
-0.1 -0.05 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35
 (rad)
Figure 65 C L vs. 

96
 Picth Moment Coefficient
1
e = -30 deg

0.5 e = -10 deg

e = -5 deg
e = 0 deg
Cm

e = 5 deg
-0.5 e = 10 deg
e = 30 deg
-1
-5 0 5 10 15 20
 (rad)

Figure 66 C m vs.  different elevator positions

Side Force Coefficient

0.03
r = -30 deg
0.02
r = -10 deg
0.01
r = -5 deg
0 r = 0 deg
Cn

-0.01 r = 5 deg
r = 10 deg
-0.02
r = 30 deg
-0.03
-0.1 0 0.1 0.2 0.3 0.4
(rad)

Figure 67 C Y vs.  different rudder positions

Roll Moment Coefficient

0.015

0.01

0.005

0
Cl

-0.005

-0.01

-0.015
-0.15 -0.1 -0.05 0 0.05 0.1 0.15 0.2
(rad)

Figure 68 C l vs.  different rudder positions

97
 Roll Moment Coefficient
0.1
r = -30 deg

0.05 r = -10 deg

r = -5 deg
0 r = 0 deg
Cl

r = 5 deg
-0.05 r = 10 deg
r = 30 deg
-0.1
-0.1 0 0.1 0.2 0.3 0.4
 (rad)

Figure 69 C l vs.  different aileron positions

Yaw Moment Coefficient

0.01

0.005

0
Cn

-0.005

-0.01

-0.015
-0.2 -0.15 -0.1 -0.05 0 0.05 0.1 0.15
(rad)

Figure 70 C n vs. 

Yaw Moment Coefficient

0.03
a = -30 deg
0.02
a = -10 deg
0.01
a = -5 deg
0 a = 0 deg
Cn

-0.01 a = 5 deg
a = 10 deg
-0.02
a = 30 deg
-0.03
-0.1 0 0.1 0.2 0.3 0.4
 (rad)

Figure 71 C n vs.  different aileron positions

98
 Yaw Moment Coefficient
0.03

0.02 r = -30 deg

r = -10 deg
0.01
r = -5 deg
0 r = 0 deg
Cn

r = 5 deg
-0.01
r = 10 deg
-0.02 r = 30 deg

-0.03
-0.1 0 0.1 0.2 0.3 0.4
 (rad)

Figure 72 C n vs.  different rudder positions

Table 11 Aerodynamic Parametersmeters

Parameter Value
C D,e 0.0741
C Yr 0.11
C Y -0.374
C Yp -0.03
C Yr 0.21
C L ,e 0.4275
C L 1.46
C Lq 3.9
C lr 0.1
C lp -0.47
C lr 0.07
C M -1.5
C M -5.2
C Mq -13.3
Cmo 0.1
C np -0.027
C nr -0.093

99