technical documentation autonomous unmanned aerial vehicle · in automatic control project course,...

Technical DocumentationAutonomous Unmanned Aerial Vehicle

Version 0.3

Author: Jan BetshammarDate: 18th May 2006

Status

ReviewedApproved

Course name: Control Project E-mail: uav [email protected]

Project group: dUVAn Document responsible: Jan Betshammar

Course code: TSRT71 Author’s E-mail: [email protected]

Project: AUAV Document name: technicaldoc.pdf

Abstract

In Automatic Control Project Course, TSRT71 a project named dUVAn was carried out.The goal of the dUVAn was to develop an autonoumous unmanned model aeroplane thatwas to be able to follow a predefined trajectory. This document describes the function ofthe system and the different subsystems. The system uses measurements from a GPS andan IMU to estimate the position, velocity, acceleration, angular velocity and the attitudeof the aeroplane with an Extended Kalman Filter. With this information the rudderswere controlled with a PID and LQ scheme.

Project Identity

Group E-mail: uav [email protected]: http://www.edu.isy.liu.se/∼henoh509Orderer: David Tornqvist, Linkoping University

Phone: +46 709-600447, E-mail: [email protected]: Rickard Karlsson, Linkoping University

Phone: +46 13 281890 , E-mail: [email protected] Responsible: Anders Hansson, Linkoping University

Phone: +46 13 281681, E-mail: [email protected] Manager: The three managers of the sub-groups togetherAdvisors: Jeroen Hol, Linkoping University

Phone: +46 13 282803 , E-mail: [email protected] Sjoberg, Linkoping UniversityPhone: +46 13 282803 , E-mail: [email protected]

Group Members

Name Responsibility Phone E-mail(@student.liu.se)

Control Group

Sara Lundin Project Manager 073-6297277 sarlu871Henrik Ohlsson Design Responsible 070-9206665 henoh509Jonas Kohlstrom Document Responsible 073-6307330 jonko288Magnus Johansson Test Responsible 070-6686134 magjo518Marten Hedborg Quality Responsible 073-0638150 marhe866Rikard Falkeborn Customer Responsible 073-6456972 rikfa098

Hardware Group

Per Jonsson Project Manager 070-5642016 perjo542Dejen Meless Design Responsible 070-8742417 dejme948Robert Isaksson Document Responsible 070-5864188 robis579Giuseppe Franco Test Responsible 070-1493535 giufr446

Positioning Group

Carl Svard Project Manager 070-3925303 carsv595Anders Jormgard Design Responsible 070-2665656 andjo454Jan Betshammar Document Responsible 073-9235603 janbe729Tina Erlandsson Document Responsible 070-6236579 tiner501Karl Andersson Test Responsible 070-9130601 karan583Kristina Jersenius Quality Responsible 073-5690333 ingje224Nina Wollinger Customer Responsible 070-6579522 ninwo463

Version Date Changes made Sign Reviewer

0.1 2006-04-04 Converted Design Specification1.2

JB -

0.2 2006-05-14 New chapters JB, TE KA, MH, RI0.3 2006-05-17 Some corrections + chap 6 JB TE, HO





Contents

1 Introduction 1

1.1 Involved Parties . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1

1.2 Goals . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1

1.3 Definitions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1

2 System Overview 2

2.1 Aeroplane . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

3 Hardware System 4

3.1 General Description of the Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

3.2 Interface Towards Other Subsystems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

3.3 Gatekeeper . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

3.3.1 The Microcontroller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

3.4 Devices . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

3.4.1 GPS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

3.4.2 IMU . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

4 Positioning System 9

4.1 General Description of the Positioning System . . . . . . . . . . . . . . . . . . . . . . . . . 9


4.3 Design of the Positioning System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

4.3.1 Block Diagram for the System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

4.3.2 Measurement Instruments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

4.3.3 State-Space Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

4.4 Hardware Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

4.4.1 GNU Scientific Library, GSL . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

4.5 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

5 Control System 16

5.1 General Description of the Control System . . . . . . . . . . . . . . . . . . . . . . . . . . . 16


5.3 Design of the Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

5.3.1 The LQ Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

5.3.2 The PID Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

5.3.3 The Module for Choosing Reference Point . . . . . . . . . . . . . . . . . . . . . . . 18

5.4 Hardware Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

5.5 Visualisation and Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

5.6 The Software - Duvan Visualisation Studio . . . . . . . . . . . . . . . . . . . . . . . . . . 19

6 Results and Conclusions 22

A Coordinate Systems 24

B Quarternions 25

C Measurements from the IMU 26

C.1 Angular Velocity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

C.2 Acceleration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

C.3 Compass . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27

D Interface 28

E Data Logging 29

F Aeroplane Model 30

F.1 Introduction to Aeroplane Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30

F.2 Interface . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30

F.3 Input Signal . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30

F.4 States . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30

F.5 The Structure of the Aeroplane Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

F.5.1 Aerodynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

F.5.2 Engine Block . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

F.5.3 Gravitation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

F.5.4 Rigid Body Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

F.5.5 Linearizing of Equation of Motion . . . . . . . . . . . . . . . . . . . . . . . . . . . 34

F.6 Adaptation of the Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35

G Linux Computer 38

G.1 Special Considerations Regarding Software Environment . . . . . . . . . . . . . . . . . . . 38

H Extended Kalman Filter 39

I Gatekeeper 41





AUAV 1

1 Introduction

The purpose of the project, dUVAn, was to develop a positioning and control systemfor an autonomous aeroplane. The user was supposed to be able to steer the aeroplanemanually or choose the autonomous mode once the aeroplane was in mid-air.

The positioning system is based on a GPS, a 3-axial accelerometer, a 3-axial gyro and anelectronic compass. Information from the positioning system and the control system areprocessed by an embedded computer running on Linux. The computer controls two of thethree rudders. For safety reasons the aeroplane engine is controlled manually.

This document describes the system and the implementation in hardware and software.It also includes results of the project.

1.1 Involved Parties

The customer is Rickard Karlsson and the orderer is David Tornqvist from the departmentof Automatic Control at Linkoping University. The project has been carried out bya group of 17 students in their fourth year of studies attending the course AutomaticControl Project Course, TSRT71.

1.2 Goals

The goal of the project was to construct an autonomous unmanned aerial vehicle (AUAV)which should be able to follow a pre-defined trajectory, given as a number of predefinedpoints, using GPS and inertial navigation.

1.3 Definitions

AUAV Autonomous Unmanned Aerial Vehicle.

IMU Inertial Measurement Unit, a unit for determining the AUAV’s attitude and mo-tion.

GPS Global Positioning System, a unit for determination of the AUAV’s position.





AUAV 2

2 System Overview

The following chapter gives an overview of the system and its components.

The purpose of the project was to modify a model aeroplane to autonomously follow agiven trajectory. It was to be possible to switch from manual mode to the autonomouswhile in mid-air.

The model aeroplane is equipped with a Linux computer, a GPS, a gyro, a compass andan accelerometer.

The system is divided into the three subsystems:

1. Hardware system

2. Positioning system

3. Control system

The different subsystems and their dependencies are visualised in Figure 1. The position-ing and control boxes symbolise packages of code running on the Linux computer aboardthe plane. The hardware system is all the hardware including the model aeroplane, sen-sors and the Linux computer. The positioning system gets sensor data from the hardwaresystem and provides the control system with state data. The control system producescontrol signals that are used by the hardware system to steer the rudders.

Linux Computer

Positioning

Control

GPSIMUAircraft

Figure 1: Overview of the system.





AUAV 3

2.1 Aeroplane

The aeroplane model is a Tiger Trainer 60 with a motor named Thunder Tiger Pro .61.It is made especially for beginners because it is stable and has a big wing span. The wingis on top of the plane which makes it appear like a Cessna which is a popular full sizeaeroplane.

weight: 3.75 kgwing span: 1.85 mmean chord: 0.32 mwing platform area: 0.59 m2

Table 1: Aeroplane data.

Figure 2: Tiger Trainer N60TT.





AUAV 4

3 Hardware System

3.1 General Description of the Hardware

This chapter describes the hardware, see Figure 3 and 4. The hardware system consistsof:

• Gatekeeper

• GPS unit

• IMU

• Linux computer

Linux Computer

Positioning

Control

GPSIMUAircraft

Figure 3: An overview of the system.

Linux Computer

Gatekeeper

Servo system

Radio reciever Hand control

Figure 4: A view of the hardware within the dashed part in previous figure.

3.2 Interface Towards Other Subsystems

See Appendix F.2.





AUAV 5

Figure 5: Overview of the system components.

3.3 Gatekeeper

To increase reliability of the aeroplane a module called Gatekeeper are implemented. Thefunction of the Gatekeeper is to control some of the servos, see Figure 6. The Gatekeeperis able to control the aileron and the elevator servos. It is possible to implement codeto control four servos and measure seven channels. If the mode is set to manual theGatekeeper will provide the servos signals from the radio receiver, and if the mode is setto autonomous the servos will be controlled by the Linux computer. If the Linux computercrashes the Gatekeeper is still functional and the user is able to maintain manual controlof the aeroplane. There are more resources for other processes on the Linux computerwhen a Gatekeeper is designated to control the servos.

The Gatekeeper consist of a microcontroller and necessary components for voltage trans-formation. A separate IC (Integrated Circuit) board is used for the Gatekeeper. Theboard is constructed in Eagle Layout Editor by CadSoft. Visit http://www.cadsoft.de/for more info and download. See appendix I for layout and schematic.

3.3.1 The Microcontroller

The microcontroller that has been chosen is an ATmega16 from Atmel [1]. ATmega16 isan 8-bit RISC device. The microcontroller will run on a +5V power source and a clockfrequency of 8Mhz. The code is developed in AVR Studio from ATMEL and the code iswritten in C.





AUAV 6

Communication with the Linux computer is performed through SPI (Serial Pe-ripheral Interface) of the computer. The SPI interface on the Linux computer is basedon +3.3V therefore an IC that transforms the level from +3.3V to +5V and vice versaare used. This IC is MAXIM’s MAX3390E. The gatekeeper is the slave and the Linuxcomputer is the Master. See also Appendix G

To read the servo values the computer sends a data request message on the SPI followedby four bytes. The data request message indicate that the computer wants to read servopositions. The four bytes that are followed are to get the four bytes from the gatekeeperthat contains the positions of the servos. To write a servo position the computer sends abyte with a specific prefix and value for the servo that should be set, see table 2. The bytewhich is send consists of both value and prefix. The most significant bits is the prefix andthe other 6 bits is the value for the servo angle.

Prefix Description00 Mode01 Aileron10 Elevator11 Throttle

Table 2: Prefix for SPI communication.

Communication with the servos will be performed using PWM (Pulse Width Mod-ulation). When the plane is in manual mode the servos will communicate with the radioreceiver. The Gatekeeper will track the behaviour of the servos and send that informationto the computer. In autonoumous mode elevator and aileron servos are possible to controlfrom the computer. During test flights it was shown that only a little angle on the servoswas used by the pilot. The gatekeeper only measure half of its possible full width to geta better resolution.

Init

Read servo input

Manual?

Put out servo data

Calculate servo data Read/Write SPI data

Store SPI data

Wait for interrupt

Return from interrupt

Y es

No

Figure 6: Flow chart of Gatekeeper function.

3.4 Devices

In order to estimate the position of the plane two devices has been used: a GPS moduleand the IMU.

These two devices have complementary functions, not only because they provide differentkind of information. The GPS returns very inaccurate data with a constant error while





AUAV 7

the IMU returns reliable values, unfortunately they are drifting. The GPS position updatefrequency is quite low, 4 Hz, while the IMU has a good one up to 512 Hz.

In both the cases serial ports on the on board computer have been used, the COM1 wasassigned to the IMU, the COM2 to the GPS. Standard kernel driver are used in orderto deal with the low level communication. Further programs to elaborate raw data havebeen used.

All the software was then inserted in a general framework whose scope is to initialize,handle and log the whole communication, besides other functions.

3.4.1 GPS

The GPS module has been used to return the position, as latitude, longitude and height,the dilution of position and time. In order to contain costs the normal GPS standard willbe used, as the Differential GPS is a pay service.

As the GPS unit SAM-LS support different protocols it has been decided to use theNMEA 0183 as it is an international standard. The NMEA 0183 Interface Standarddefines electrical signal requirements, data transmission protocol and time, and specificsentence formats for a 4800-baud serial data bus.

Data from the GPS receiver are extrapolated from a number of NMEA messages: RMC,VTG, GGA, GSA, GSV, GLL and ZDA. For the goal of the project only RMC, in orderto get position, and GSV, in order to get dilution of position are used. These are strictlyneeded since the frequency of getting new data is so low that it can not read and log allthese other data in order to verify their consistency and have a number of information on”flight environment”.

The standard latitude and longitude position are represented in the format degrees, min-utes (decimal, fractions of minutes).

The dilution of position is a function expressing the mathematical quality of solutionsbased on the geometry of the satellites, the accuracy of any measurement is proportion-ately dependent on the DOP value. This means that if the DOP value doubles, the errorin determining a position increases by a factor of two.

Three different dilutions are saved: Position dilution of precision (PDOP), the most com-mon such value, has a best case value of 1, higher numbers being worse. A low numberof DOP (2) is good, a high number (>7) is considered to be bad. The best PDOP wouldoccur with one satellite directly overhead and three others evenly spaced about the hori-zon. HDOP and VDOP are used to calculate the precision of the measurement on thehorizontal plane and on the vertical dimension. In the test flights the values returned wereusually under 2. For other information about messages name and content please refer to[9].

As already referred, a program was used to read the device and obtain these data. Thisprogram is GPSLogger, which has been chosen among the others because it is quite smalland so easy to manipulate and customise. The program was written in C++ so it hadto be translated in C in order to be compatible. A few rows of code have been added inorder to get the three dilution of position, as in the original version the GSV message wascompletely ignored and no dilution was returned.

An issue was the synchronisation between the GPS and the IMU, unfortunately the GPSdelay due to time spent in position calculation is in the order of 100-200 ms and it isvariable depending on the number of satellites being tracked, this was found out only atthe end of the project so nothing was done to synchronise the two devices.





AUAV 8

3.4.2 IMU

The IMU is a complete miniature inertial measurement units with integrated 3D magnetometers (3D compass), with an embedded processor capable of calculating roll, pitch andyaw in real time, as well as outputting calibrated 3D linear acceleration, rate of turn (gyro)and (earth) magnetic field data. It was used to calculate the orientations in Quaternion,calibrated 3D linear acceleration, 3D angular velocity (gyro) and 3D magnetic field datais in sensor-fixed coordinate system.

Also in this case the communication task was accomplished with the use of externalsoftware in this case provided by the device producer. As for the GPS this program hadto be translated from C++ to C and customised in order to get the already described setof information, and to adjust the baudrate.

Before using the manufacturer software a low-level function for read and write on theserial port was tried but the communication was quite difficult, in particular it was notpossible to change mode of operation while receiving data. Even though the IMU has thepossibility to operate at higher frequencies it was decided to operate at 75 Hz to avoidweighing down the on board computer. For more information see [11].





AUAV 9

4 Positioning System

4.1 General Description of the Positioning System

The purpose of the positioning system is to provide the control system with data necessaryfor controlling the aeroplane so that a predefined trajectory can be followed. The position-ing is performed using GPS and inertial navigation. The measurements from the inertialsensor are very accurate, but have errors that have a tendency to drift in time. The GPSon the other hand gives unbiased measurements. The system combines the different typesof measurement information with a model describing the dynamics of the aeroplane. Byusing suitable signal processing estimations of the position, velocity, attitude, accelerationand angular velocity are obtained.

The measurements units, i.e. the IMU and the GPS, provide data in two different frames.The GPS coordinates are given in Earth Frame (E) and the data from the IMU in IMUFrame (I), see Appendix A


Figure 7 shows how the Positioning System interacts with the other two subsystems.

Hardware

Positioning Control

Actuators IMU GPS

Figure 7: The interaction of the Positioning System.

The Positioning System receives three vectors called Imu, Gps and Actuators from theHardware System. The vector Imu contains the three-dimensional acceleration, angularvelocity and earth magnetic field measured by the IMU in the IMU frame, (I), with respectto the Locally level Frame (L). The vector Gps contains three GPS coordinates (longitude,latitude and altitude) in the Earth frame (E). Information about the angle of the ruddersand the level of the throttle is found in the vector Actuators.

The Positioning System delivers a vector called Pos State containing position in an earth-fixed coordinate system (L) and velocity, acceleration, attitude and angular velocity in abody-fixed coordinate system (B) to the Control System. See also Appendices A and D.

4.3 Design of the Positioning System

The angles of the rudders and the level of the throttle are used as input signals to amodel and estimations of position, velocity, attitude, acceleration and angular velocityare calculated. With aiding measurements of the position from the GPS and acceleration,angular velocity and magnetic field from the IMU, the estimates from the model arecalculated. The main idea behind this procedure can be found in [2].





AUAV 10

4.3.1 Block Diagram for the System

A block diagram for the Positioning System is shown in Figure 8.

IMU

Model

GPS

Coord. transform.

Coord. transform.

h

Σ

Σ K

xt|t−1

z − h(xt|t−1)

Error

Actuators +

+

−

+

+

xt|t

EKF

Figure 8: Block diagram for the Positioning System.

In the block Coord. Transform. for the GPS the longitude and latitude obtained from theGPS are transformed to the position of the aeroplane, expressed in metres in the Locallylevel Frame (L). The block Coord. Transform. for the IMU transforms the measurementsfrom the IMU from the IMU Frame (I)to the Body Frame (B). See Appendix C.

The positioning system uses an Extended Kalman Filter (EKF) to process measurementdata, therefore linearization will be done in every point according to [6]. The time updateoccurs in the block Model and the measurement update in the block K. Because the GPSmeasurements are updated less often than the IMU measurements K includes two separatemeasurement updates. See Appendix H.

The states of the state space model and the EKF are position expressed in the Locallylevel Frame (L), velocity, acceleration and angular velocity in the Body Frame (B) andattitude. All quantities are three dimensional, except for the attitude, which is representedin quaternions and therefore is four dimensional, see Appendix B.

Input signals to the block Model are rudder angles and level of throttle provided bythe Hardware System. The output signal xt|t−1 contains position, velocity, acceleration,attitude and angular velocity. The errors in xt|t−1 are estimated using transformed mea-surements from the GPS and the IMU in the block K. Finally the estimated error is addedto the output signal of the block Model.

4.3.2 Measurement Instruments

The measuring instruments involved are a GPS and an IMU.

The IMU provides the positioning system with the acceleration and angular velocity inthree dimensions, and also a compass vector. The IMU measurements are updated tentimes per second. The measurements of acceleration and angular velocity given by theIMU are considered to have bias errors. This means that:

xmeasured = xtrue + bias

where xmeasured is the measured quantity and xtrue is the true value. These errors are con-





AUAV 11

sidered to be constant during flight. In the calibration phase estimations of the bias errorsare achieved by calculating the mean value of an appropriate numbers of measurements.See also [4].

The GPS provides the positioning system with the position expressed in latitude, longi-tude and altitude. The factors HDOP and VDOP, which describe the uncertainties ofthe horizontal and vertical measurements, are also obtained from the GPS. For furtherinformation about HDOP and VDOP see [10]. The GPS measurements are updated twotimes per second. The errors of the position measurements from the GPS are assumed tobe white noise.

When the GPS delivers positioning data there will be a time delay. It means that thepositioning data for a certain position will be delivered after the position already havebeen passed. This time delay could become a problem for the synchronisation of theIMU and the GPS. According to Stephane Vincent, Customer Support u-blox, this timedelay varies between 100 to 200 ms depending on how many satellites that are being used.Since the project group got the information rather late it has not applied this in theimplementation. But considering the speed of the aeroplane this could be something tolook into further during the continuation of this project.

The measurements from the GPS are not so accurate in altitude as in longitude andlatitude. As shown in figure 10 the estimations of altitude of the EKF are not as good asthe estimations of the x- and y-components of the position. To obtain estimations withless noise in altitude an extra height sensor can be used.

4.3.3 State-Space Model

A model describing the dynamics of the aeroplane is used for time updating the EKF.The aeroplane model is described in Appendix F. The state vector is:

x = ( x y z u v w ax ay az e0 e1 e2 e3 p q r )T

with the components described in Table 3.

States Coordinate system RepresentationPosition Locally level Frame (L) x, y, zVelocity Body Frame (B) u, v, wAcceleration Body Frame (B) ax, ay, az

Attitude - e0, e1, e2, e3

Angular velocity Body Frame (B) p, q, r

Table 3: The components of the state vector.

The attitude of an aeroplane can also be expressed in Euler angles. An advantage ofusing quaternions is that there are no singularities which can become a problem whenusing Euler angles. Another advantage is that the differentiation of the quaternions givesimpler calculations and expressions than the differentiation of the Euler angles. TheEuler angles can be found to give a more intuitive interpretation of the orientation thanthe quaternions. See also Appendix B.

The input signals are provided by the Hardware System in the vector Actuators. Thecomponents can be found in Table 4. See also Appendix D.

In an early stage of the project the components of the angular velocity were not statesbut used as input signals to the model. The angular velocity can be calculated from





AUAV 12

Input signals RepresentationAileron δa

Elevator δe

Throttle γ

Table 4: The input signals to the system.

information about the rudders, which means that using both rudder data and angularvelocity as inputs to the model would give an over-determined equation system. Whenusing the components of the angular velocity as states instead of input signals the noiseof the measurements can be taken into consideration.

The aiding measurement signals are used to estimate the errors in the estimated states.They are provided by the Hardware subsystem and delivered as the vectors Gps and Imu,see also Appendix D. They consist of:

Measurement signals Coordinate system RepresentationPosition from GPS Earth Frame (G) long, lat, altAcceleration from IMU IMU Frame (I) 3dim vectorAngular velocity from IMU IMU Frame (I) 3dim vectorCompass direction from IMU IMU Frame (I) 3dim vector

Table 5: The measurement signals of the system.

Output signals from the model are the corrected estimates of the state vector. Theyare given to the Control subsystem and delivered as a vector called Pos State, se alsoAppendix D. The output signals are shown in Table 6:

Output Signals Coordinate system RepresentationPosition Locally level Frame (L) 3dim vectorVelocity Body Frame (B) 3dim vectorAcceleration Body Frame (B) 3dim vectorAttitude - e0, e1, e2, e3

Angular velocity Body Frame (B) 3dim vector

Table 6: The output signals of the system.

4.4 Hardware Implementation

The Positioning System has been developed in MATLAB but is implemented in C on theLinux computer.

4.4.1 GNU Scientific Library, GSL

For the implementation several matrix operations where needed. To get predefined func-tions for vectors and matrices GSL was used. (For more information about GSL see[5])

4.5 Results

The positioning system was developed in Matlab and is implemented in C. It consistsmainly of an Extended Kalman Filter which receives information from a GPS and an





AUAV 13

IMU and uses these measurements and an aeroplane model to estimate the position, thevelocity, the acceleration, the attitude expressed in quaternions and the angular velocityof the aeroplane. All the estimations are stored in a log. Figure 9 shows the estimateddata and Figure 10 shows a comparison between the position measured by the GPS andthe position estimated by the positioning system.

It is difficult to decide how good the estimations are since the true trajectory of theplane is unknown. This also makes it hard to determine if additional sensors are needed.However, an additional height sensor would probably be useful since the measurements ofthe GPS are more uncertain vertically than horizontally.

It can be seen in Figure 10 that the filter does not function completely satisfactory.Sometimes the predictions between the GPS measurements updates are good and resultin a smooth curve between the GPS measurements. In other parts of the figure it can beseen that the filter predicts that the plane moves in a direction that is opposite to the oneindicated by the GPS measurements.

One possible source of error is that the aeroplane model needs to be corrected. Theaeroplane model is designed for a plane that flies forwards at a constant velocity and at aconstant height and there can be a need for different types of models designed for differenttypes of flying. It can also be good to check if the parameters of the aeroplane should beadjusted. Other possible sources of error are a need for higher resolution for the rudderdata and incorrect IMU measurements caused by vibrations. The performance of the filtercan possibly be improved by using unfiltered GPS signals instead of filtered ones since theKalman Filter is designed for white measurement noise.





AUAV 14

500 1000 1500 20000

100200

N

x [m

]

500 1000 1500 2000−200

0200

Estimated state vector

Ny

[m]

500 1000 1500 2000−80−60−40−200

20

N

z [m

]

500 1000 1500 2000203040

N

u [m

/s]

500 1000 1500 2000−10

01020

N

v [m

/s]

500 1000 1500 2000−20−10

0

N

w [m

/s]

500 1000 1500 2000−10

010

N

a x [m/s

2 ]

500 1000 1500 2000−20−10

010

N

a y [m/s

2 ]

500 1000 1500 2000

−200

20

N

a z [m/s

2 ] 500 1000 1500 2000

−0.50

0.5

N

e 0

500 1000 1500 2000

−0.50

0.5

N

e 1

500 1000 1500 2000−0.5

00.5

N

e 2

500 1000 1500 2000

−0.50

0.5

N

e 3

500 1000 1500 2000−5

05

10

N

p [r

ad/s

]

500 1000 1500 2000−10−5

05

N

q [r

ad/s

]

500 1000 1500 2000−6−4−20

24

N

r [r

ad/s

]

Figure 9: The estimated state vector of the EKF.





AUAV 15

50 100 150 200 250 300 350 400 450 500

0

100

200

Comparison between estimated position and gps data

N

x [m

]

Gps dataFilter data

50 100 150 200 250 300 350 400 450 500−100

0

100

200

N

y [m

]

50 100 150 200 250 300 350 400 450 500−250

−200

−150

−100

−50

0

N

z [m

]

Figure 10: Comparison of the estimated position and the position given by the GPS.





AUAV 16

5 Control System

5.1 General Description of the Control System

The purpose of the control system is to set the rudders in such a way that the AUAVfollows a set of predefined position points. This was solved by first creating a model ofthe aeroplane and secondly by a control design based on this model.

The controller and the model was developed in Matlab/Simulink but the controller wasalso implemented in C to be able to run on the onboard computer. As a help in thedevelopment of the control system a tool for analysis and visualisation was created.


The input signals for the control system are the predefined GPS points (saved in thememory of the onboard computer) and the estimated states of the AUAV. With the helpof these signals an output is created as angles on the different rudders.

Input signals/vectors:

• Pos State set by the Positioning system.

• The predefined position points

Output signals/vectors:

• Rudders used by the Hardware system.

The predefined position points will be decided before takeoff and stored in the memory ofthe computer.

For more information about the input and output vector see Appendix D and E.

5.3 Design of the Controller

The control system was developed in three steps; a LQ design from angular velocitiesto rudder angels, a set of PID controllers from reference position to angular velocitiesand a module deciding which waypoint to be the reference position. Figure 11 shows theSimulink implementation of the control system.

5.3.1 The LQ Design

To be able to separate heavy cross coupling a LQ design from angular velocities (p and q)to rudder signals was used. More specific, in the LQ design a deviation from a referencevalue in p and q were punished and together with the velocities (u, v, w) used as an input.The dynamics between angular velocities and the rudders was seen as much faster thenthe dynamics between reference point and reference angular velocities and therefore werethe reference angular velocities seen as constants.

The model of the aeroplane was non linear and therefore had to be linearized, see AppendixF, to be able to carry out the LQ design.

To be able to run on the onboard computer the controller had also to be discretisized.A plot of the sensitivity function using the discretisized LQ controller and the linearized





AUAV 17

Figure 11: An overview of the control system in a Simulink test bench.

Figure 12: The sensitivity function.





AUAV 18

discretisized model of the aeroplane are given in Figure 12. As seen in the plot, the designsuppress model errors of low frequency. Noise is also handled in a good way i.e., suppressedfor high frequencies, which can be seen in the complementary sensitivity function, whichplot was chosen to not be include in this document.

5.3.2 The PID Design

To handle the slower dynamics a PID setup was used. The PID structure uses the dif-ference in position, corrected current position and reference position, to set the angularvelocities. See Figure 13 for the Simulink implementation.

Figure 13: The PID structure.

5.3.3 The Module for Choosing Reference Point

This module uses the predefined waypoints and the current position point to decide areference point to aim for. The module start to send out the first waypoint in the list ofpredefined waypoints as a reference. A change of reference point is done when the distanceto the reference point is less then 10m, the new reference point will then be the next inthe list. See Figure 14 for an overview of the module and also the dUVAn CD for thefunction blocks in the Simulink block model.





AUAV 19

Figure 14: The module for choosing reference point.

5.4 Hardware Implementation

The controller was implemented in C to be able to run on the onboard computer, see thedUVAn CD.

5.5 Visualisation and Analysis

To visualise and analyse the results, a visualisation software has been produced. Howeversince no real flight is available, the software now uses simulated data instead and the fol-lowing is a explanation of how it works. See Figure 15 for a screen shot of the visualisationof the AUAV.

5.6 The Software - Duvan Visualisation Studio

The software requires the following additional programs and toolboxes to work properly:

• Matlab 7

• FlightGear ver. 0.9.6

• MyGPS

• Aerosim Blockset

• Realtime Blockset

Once these are acquired the software is started by typing Duvan in the Matlab window.The user will then be prompted for a waypoint file from MyGPS which defines the planned





AUAV 20

Figure 15: Visualization of the AUAV. The red ball symbolizes a waypoint.

flight. In MyGPS any map can easily be calibrated if one knows the latitude and longitudeof two points on that map. On the map only waypoints must be used and then exportedto file through MyGPS. When the software is given this file it immediately saves thefirst waypoint and treats it like the origin while also saving all other points to variablevectors. After that the points are first transformed to geocentric coordinates with the useof equations that can be obtained from Lantmateriet. The points are then transformedto the local system by the use of a transformation matrix for the current origin.When inthe local system, the points can be used for navigation in our simulated flight and thethe user is promted to press enter to start the simulation with our model and controller.The simulation runs in the background until the plane is within one meter of the finalwaypoint.

After this step the simulated data that is given in latitude and longitude is transformedto the local system by the use of the transformations stated above. With the data inthe local system the user can produce a 3D-plot as well as an error plot showing theminimum distance from waypoints. The simulated flight data in latitude and longitudeis still preserved as the user may choose to export this data to a MyGPS file. This filecontains not only the waypoints but also the path flown by the plane in the simulation.There is also another reason to keep the simulated flight data in this format, and that isto let the user see the flight in FlightGear.

Before starting up Flightgear with a bat-file configured to receive data from Simulink theuser may choose to produce a file that will also show the waypoints as red balls duringthe flight. This file is produced with a pre-defined name and the user is instructed tocopy this file into a specific folder in FlightGear directory. For this to work, the imagefiles waypoint.ac and red.bmp must also be copied there. When this step is completed





AUAV 21

the user can see the flown path in FlightGear. When the user no longer wishes to see thesimulation, the software will exit and the visualisation is complete.

The following Matlab files is used by the software:

• Duvan.m - The main file from which all other files are called.

• init duvan.m - Initialises the model and the controller.

• appendiz a.m and parametrar.m - Model Initialisation.

• getFx.m and getFu.m - Model Linearization.

• duvan diskret wind.mdl - The Simulink model file containing the aeroplane modeland the controller.

• Flight data intake.m - Writes the flight data to a MyGPS file.

• errorplot.m - Produces the errorplot from flight and waypoint data.

• plot3d.m - Produces the 3D-plot from flight and waypoint data.

• coord trans.m - Transforms flown route and waypoints in the local systems to lati-tude and longitude format.

• coordinattransformation.m - Transforms points in geocentric coordinates to latitudeand longitude format.

• inv coordinattransformation.m - Transforms latitude and longitude points to geo-centric coordinates.





AUAV 22

6 Results and Conclusions

Three separate subsystems with a well defined interface have been developed.

The hardware system is able to control the rudder servos. It receives information fromthe IMU and GPS and sends it to the positioning system. It also provides the positioningsystem with information about the rudders.

The positioning system processes data received from the hardware system and estimatesthe positon, velocity, acceleration, attitude and angular velocity of the aeroplane. It passesthis information to the control system.

The control system has been implemented to control the aeroplane. It has never beentested in reality , but works well in simulations. It can also visualize the flight route.

The purpose of the project was to make a model aeroplane able to follow a predefinedtrajectory. It has never been tested of the aeroplane can fly autonoumously, since thereare parts of the subsystems that are not working well enough.

To reach the goal of flying autonoumously more testing has to be done to be able to findand correct the parts that are not working satisfactory.





AUAV 23

References

[1] Atmel Corporation. ATmega16(L) Complete Datasheet, l edition, June 2006.

[2] P Y.C Brown, R G & Hwang. Introduction to Random Signals and Applied KalmanFiltering. John Wiley & Sons, Inc, 1997.

[3] Averil B. Chatfield. Fundamentals of high accuracy inertial navigation. Reston, Va.: AIAA, cop., 1997.

[4] Rikard Falkeborn. Manual Autonomous Unmanned Aerial Vehicle. dUVAn, May2006.

[5] GNU, http://www.gnu.org/software/gsl. GNU Scientific Library, a numerical libraryfor C and C++ programmers, version 1.7 edition, September 2005.

[6] T. Kailath, A. Sayed, and B. Hassibi. Linear Estimation. Prentice Hall, Inc., 2000.

[7] Robert C. Nelson. Flight stability and automatic control. WCB/McGraw-Hill, 2:ndedition, 1998.

[8] Mattias Svensson. Aircraft trajectory restoration by integration of inertial measure-ments and gps. Master’s thesis, Department of Electrical Engineering, LinkopingUniversity, 1999.

[9] u-blox AG. ANTARIS Protocol Specification, gps.g3-x-03002 edition.

[10] u-blox AG. SAM-LS GPS Smart Antenna Module Data Sheet, June 2005.

[11] Xsens Technologies B.V., www.xsens.com. MTi and MTx User Manual and TechnicalDocumentation Document MT0100P.

[12] Xsens Technologies B.V., www.xsens.com. MTi and MTx Low-Level CommunicationDocumentation, revision d edition, September 2005.





AUAV 24

APPENDIX

A Coordinate Systems

The different quantities will be expressed in four coordinate systems [8]:

Earth Frame (E) The Earth Frame is fixed in earth. The zE-axis is pointing throughthe north pole and the xE-axis is directed along the Greenwich meridian. The GPS givesthe position in longitude, latitude and altitude, height above the Mean sea level, in theEarth Frame (E).

Locally level Frame (L) The origin of the Locally level Frame (L) is placed at a fixedpoint on the ground. The xL-axis is pointing towards the north, the yL-axis towards theeast and the zL-axis towards the centre of earth. The position of the plane estimated bythe positioning group will be expressed in this frame.

Body Frame (B) The Body Frame (B) is attached in the centre of mass of the aeroplaneand rotates with the aeroplane. The zB-axis is pointing downwards according to theaeroplane, the xB-axis is pointing through the nose and the yB -axis is directed along theright wing. The B-frame is related to the L-frame with an attitude and a distance. Theestimations of velocity, acceleration and angular velocity of the aeroplane relative groundwill be expressed in this frame.

IMU Frame (I) The IMU Frame (I) is attached to the IMU. Like (B) and (G) theIMU Frame rotates with the aeroplane. The axis of the I-frame are not directed in thesame directions as the axis of the B-frame. Measured data from the IMU will be given inthis frame.





AUAV 25

B Quarternions

This appendix will describe how a vector in cartesian coordinates is represented in quater-nions [3],[8]. It will also explain how calculations like translation and rotation from acoordinate system a to b is performed.

Basic Definitions

A quaternion e ∈ Q = {R4 : eeT = 1}, is a 4-tuple of real numbers is denoted bye = (e0, e1, e2, e3). Alternatively it is denoted by e = (e0, e), where e0 is called the scalarpart and e the vector part of a quaternion. For quaternions the following operators willbe used:

addition p + e = (p0 + e0,p + e)multiplication p ⊙ e = (p0e0 − p · e, p0e + e0p + p × e)conjugation ec = (e0,−e)

A cartesian vector x ∈ R3 is represented in quaternions as (0,x) ∈ R4.

Translation

The translation from the b to the a coordinate system of ea ∈ Q is defined byea = eb + va,

where va = (0,v) and v is the vector from the orgin of coordinate system a to the orginof coordinate system b, expressed in coordinate system a.

Rotation

A rotation matrix from the b to the a coordinate system, Rab, in Cartesian coordinates isdefined by

Rab =

xx xy xz

yx yy yz

zx zy zz

.

This rotation matrix can be represented in a quaternion vector, eab, with the follow-ing elements:

e0 =

√xx+yy+zz+1

2

e1 =

√xx−yy−zz+1

2 , where e1 is negative if zy − yz is negative and otherwise positive.

e2 =

√−xx+yy−zz+1

2 , where e2 is negative if xz − zx is negative and otherwise positive.

e3 =

√−xx−yy+zz+1

2 , where e3 is negative if yx − xy is negative and otherwise positive.

The rotation from the b to the a coordinate system, Rab, of a position, x, is defined usingquaternion algebra by

xa = Rab(xb) ≡ eab ⊙ xb ⊙ (eab)c.





AUAV 26

C Measurements from the IMU

C.1 Angular Velocity

The IMU measures the angular velocity in the IMU Frame. It is related to the angularvelocity in the Body Frame by the rotation matrix RIMU , i.e.

ωmeasIMU = RIMU ∗ ω

where ωmeasIMU is the angular velocity measured by the IMU.

C.2 Acceleration

The IMU gives the acceleration of the point in which the IMU is attached to the aeroplanei.e. the origin of the IMU Frame. The measured acceleration also includes the gravity.For the acceleration of the centre of mass the following relation is valid:

aIMU = a + gravity + α × rIMU + ω × (ω × rIMU )

where aIMU denotes the acceleration of the point where the IMU is attached and a denotesthe acceleration of the centre of mass. rIMU is the vector that points from the IMU tothe centre of mass. α and ω is the angular acceleration and angular velocity respectively.The angular acceleration is unknown and considered to be small, hence it will here beneglected. With α neglected the equation can be expressed like:

aIMU = a + gravity + ω × (ω × rIMU )

where all quantities must be expressed in the same coordinate system. The vector rIMU isunknown but considered to be small, which means that the last term also can be neglected.The equationen will be expressed liked:

aIMU = a + gravity

The gravity is directed along the zE-axis of the Locally level Frame, i.e. toward the centreof earth. It can be written in the Body Frame as:

gravity = RGS ∗

00−g

where RGS is the rotation matrix between the L-frame and the B-frame and g is theconstant of gravity.

The aIMU is measured in the IMU Frame, but the acceleration of the centre of mass isexpressed in the Body Frame, hence a rotation matrix is needed between the two framesis needed. It means that:

ameasIMU = RImu ∗ aIMU = RImu ∗ (a + gravity)

Totally everything can be sumerized as:

ameasIMU = RImu ∗ (a + RGS ∗

00−g

)





AUAV 27

C.3 Compass

In the calibration phase a compass vector is stored, magcal. The compass vector measuredduring the flight relate to the calibrated compass vector as:

magmeas = RGS ∗ magcal

where magmeas is the compass vector measured by the IMU and RGS is the rotationmatrix between the L-frame and the B-frame.





AUAV 28

D Interface

Set by Hardware System, Used by Positioning System

Vector name: Gps

• GPS-position xyz (long, lat, z, V DOP,HDOP )

Vector name: Imu

• Acceleration (x, y, z)

• Angular velocity (p, q, r)

• Compass (x, y, z)

All data are unprocessed, ”raw”, and therefore given in the coordinate frame given bythe different measurement units. The Imu-vector will be in the IMU frame (I) and theGps-vector in an Earth frame (E), see also Appendix A.

Set by Hardware System, Used by Positioning System, Control System

Vector name: Actuators

• Aileron (δa)

• Elevator (δe)

• Throttle (γ)

Scalars symbolizing the angles of the rudders and the level of the throttle.

Set by Positioning System, Used by Control System

Vector name: Pos State

• Position (x, y, z)

• Velocity (u, v, w)

• Acceleration (ax, ay, az)

• Attitude (e0, e1, e2, e3)

• Angular velocity (p, q, r)

The positioning is given in a local level coordinate system (L) while all the others aregiven in a body-fixed system (B).





AUAV 29

E Data Logging

Set by Hardware System

Vector saved: Pos State

In lack of memory the positioning data in Pos State has highest priory.





AUAV 30

F Aeroplane Model

This appendix describes the aeroplane model.

F.1 Introduction to Aeroplane Model

The purpose of the model is to simulate an aeroplane with aerodynamic forces, engineforces, gravitation force and rigid body dynamics. The model will be used in the devel-opment of the control- and positioning system.

F.2 Interface

Figure 16 shows the input and output signals of the aeroplane model. A closer descriptionof the signals can be found in the following sections.

Aeroplane modelActuators States

Figure 16: Aeroplane model.

F.3 Input Signal

Inputs to the aeroplane model are the two vectors Rudders and Throttle.

Actuators consist of the elements:

Elevator describes the angle of the elevator, (δe), in radians.

Aileron describes the angle of the aileron, (δα ), in radians.

Throttle describes the level of the throttle, (γ ), between 0 and 1.

F.4 States

The output from the model consists of a vector, Pos State with 16 elements.

Position Three elements describing the position of the aeroplane in an inertial system(L).

Velocity Three elements describing the velocity of the aeroplane in a body-fixed system(B).

Acceleration Three elements describing the acceleration of the aeroplane in a body-fixedsystem (B).

Attitude Four elements describing the orientation of the aeroplane expressed in quater-nions.

Angular velocity Three elements describing the angular velocities of the aeroplane ina body-fixed system (B).





AUAV 31

F.5 The Structure of the Aeroplane Model

The aeroplane model is divided into four separate blocks, aerodynamics, gravitation, en-gine and rigid body dynamics. A block diagram can be seen in figure 17.

Aerodynamics

Gravitation

Engine

Rigid body dynamics

Rudders

Angular velocity

V elocity

Attitude

maeroplane

g

Throttle

Faero

Maero

Fg

CGpos

Fdrag

States

Figure 17: Block diagram of the aeroplane model.

The following section will describe these blocks in detail.

F.5.1 Aerodynamics

This block calculates the aerodynamic forces and moment that affect the aeroplane. Tobe able to perform these calculations the block uses the control signals, velocities andangular velocities of the aeroplane.

Inputs:

Rudder A vector containing two elements, describing the angles of the elevator and theaileron.

Angular velocity A vector containing three elements, (p, q, r), describing the angularvelocity of the aeroplane expressed in a body-fixed system.

Velocity A vector containing three elements, (u, v, w), describing the velocity of theaeroplane expressed in a body-fixed system.

Outputs:

Faero A vector containing the resulting aerodynamic forces, (X,Y,Z), expressed in abody-fixed system (B). X, Y , and Z are scalar and X points in xB direction, Y inyB and Z in xB.

Maero A vector containing the resulting aerodynamic moment, (L,M,N), expressed ina body-fixed system (B). L, M and N are scaler and L are moment around xB , Marund yB and N around zB .





AUAV 32

The input signals Angular velocity and Velocity are feedback signals from the rigidbody dynamics block. The vector Rudder is one of the two input signals to the aeroplanemodel. Small deviations about a steady flight condition are assumed. Then the equationsare:([7] page 107)

X/m = Xu(u − u0) + Xw(w − w0) + Xδe(δe − δe0

)

Xu = ∂X∂u

Xw = ∂X∂w

Xδe= ∂X

∂δe

Y/m = Yv(v − v0) + Yp(p − p0) + Yr(r − r0)

Yv = ∂Y∂v

Yp = ∂Y∂p

Yr = ∂X∂r

Z/m = Zu(u − u0) + Zw(w − w0) + Zq(q − q0) + Zδe(δe − δe0

)

Zu = ∂Z∂u

Zw = ∂Z∂w

Zq = ∂Z∂q

Zδe= ∂Z

∂δe

L/Ix = Lv(v − v0) + Lp(p − p0) + Lr(r − r0) + Lδα(δα − δα0

)

Lv = ∂L∂v

Lp = ∂L∂p

Lr = ∂L∂r

Lδα= ∂L

∂δα

M/Iy = Mu(u − u0) + Mw(w − w0) + Mq(q − q0) + Mδe(δe − δe0

)

Mu = ∂M∂u

Mw = ∂M∂w

Mq = ∂M∂q

Mδe= ∂M

∂δe

N/Iz = Nv(v − v0) + Np(p − p0) + Nr(r − r0) + Nδα(δα − δα0

)

Nv = ∂N∂v

Np = ∂N∂p

Nr = ∂N∂r

Nδα= ∂N

∂δα

To simplify the model we will use constants that we have estimated from the shape of theaeroplane instead of the different derivatives. Ix, Iy and Iz are mass moments of inertia.

F.5.2 Engine Block

This block consists of a simple model translating the input signal, Throttle, to drag force(Fdrag). Any moment caused by the engine is neglected.

F.5.3 Gravitation

This block computes the gravitation forces in a body-fixed system (B). The gravitationforce is assumed to be constant. The input signals are the orientation of the aeroplaneexpressed in quaternions, the mass of the aeroplane and the gravitation constant. Theoutput signal is a vector containing the three forces, Fgx, Fgy and Fgz.

Fgx

Fgy

Fgz

B

=

e20 + e2

1 − e22 − e2

3 2(e1e2 + e0e3) 2(e1e3 − e0e2)2(e1e2 − e0e3) e2

0 − e21 + e2

2 − e23 2(e2e3 + e0e1)

2(e1e3 + e0e2) 2(e2e3 − e0e1) e20 − e2

1 − e22 + e2

3

︸︷︷︸

R

00mg

where R is the rotation-matrix using quaternions. The quaternions aree = (e0, e1, e2, e3).





AUAV 33

F.5.4 Rigid Body Dynamics

This block uses the output from the three previous blocks. These signals together withthe equations of motion are used to calculate the velocity, acceleration and position of theaeroplane.

Total acceleration

Total moment

Equation of motion

Faero

Fg

Fdrag

Maero

Faero

CGpos

Acceleration

Moment

Position

V elocity

Acceleration

Angular velocity

Attitude

Figure 18: Block diagram of the rigid body dynamics model.

The rigid body dynamics consists of three subsystems; total acceleration, total momentand equation of motion. Figure 18 shows the signals and subsystems of the block.

Total Acceleration

Inputs:

• Faero

• Fg

• Fdrag

Output:

• Acceleration - A vector containing three elements, (ax, ay, az), describing the accel-eration of the aeroplane in a body-fixed system (B)

Total Moment

Inputs:

• Maero

• Faero

• CGpos - Center of gravity

Output:

• Moment - The total moment of the aeroplane relative CGpos





AUAV 34

Equation of Motion

Inputs:

• Acceleration

• Moment

Outputs:

• Position

• Velocity

• Acceleration

• Angular velocity

• Attitude

To calculate the outputs we use a set of non-linear equations. ([7] page 105)

u = −qw + rv + X/m + Fgx/mv = −ru + pw + Y/m + Fgy/mw = −pv + qu + Z/m + Fgz/m

p =(Iy−Iz)qr

Ix+ L

Ix

q = (Iz−Ix)rp

Iy+ M

Iy

r =(Ix−Iy)pq

Iz+ N

Iz

e0

e1

e2

e3

= 12

0 −p −q −rp 0 r −qq −r 0 pr q −p 0

e0

e1

e2

e3

+ λ

e0

e1

e2

e3

λ = 1 − (e20 + e2

1 + e22 + e2

3)

xyz

L

= RT

uvw

R =

e20 + e2

1 − e22 − e2

3 2(e1e2 + e0e3) 2(e1e3 − e0e2)2(e1e2 − e0e3) e2

0 − e21 + e2

2 − e23 2(e3e2 + e0e1)

2(e1e3 + e0e2) 2(e3e2 − e0e1) e20 − e2

1 − e22 + e2

3

F.5.5 Linearizing of Equation of Motion

To use the model in the project the equation of motion must be linearized. Linearizeingaround a point (x0, u0) gives: x = f(x, y) ≈ f(x0, uu)+Fx(x0, u0)(x−x0)+Fu(x0, u0)(u−u0)





AUAV 35

From equation in F.5.4 we have the expression for f(x, u). To save space, some states areshown together. Derivating with respect to x gives:

Fx,x

Fx,y

Fx,z

=

0 0 00 0 00 0 0e20 + e2

1 − e22 − e2

3 2(e1e2 + e0e3) 2(e1e3 − e0e2)2(e1e2 − e0e3) e2

0 − e21 + e2

2 − e23 2(e2e3 + e0e1)

2(e1e3 + e0e2) 2(e2e3 − e0e1) e20 − e2

1 − e22 + e2

3

2(e0u − e3v + e2w) 2(e3u + e0v − e1w) 2(−e2u + e1v + e0w)2(e1u + e2v + e3w) 2(e2u − e1v − e0w) 2(e3u + e0v − e1w)2(−e2u + e1v + e0w) 2(e1u + e2v + e3w) 2(−e0u + e3v − e2w)2(−e3u − e0v + e1w) 2(e0u − e3v + e2w) 2(e1u + e2v + e3w)0 0 00 0 00 0 0

T

Fx,u

Fx,v

Fx,w

Fx,e0

Fx,e1

Fx,e2

Fx,e3

=

0 0 0 0 0 0 00 0 0 0 0 0 00 0 0 0 0 0 0Xu −r Zu + q 0 0 0 0r Yv −p 0 0 0 0Xw − q p Zw 0 0 0 02e2g −2e1g 2e0g 0 p/2 q/2 r/22e3g −2e0g −2e1g −p/2 0 −r/2 q/22e0g 2e3g −2e2g −q/2 r/2 0 −p/22e1g 2e2g 2e3g −r/2 −q/2 p/2 00 Yp + w −v −e1/2 e0/2 e3/2 −e2/2−w 0 Zq + u −e2/2 −e3/2 e0/2 e1/2v Yr − u 0 −e3/2 e2/2 −e1/2 e0/2

T

Fx,p

Fx,q

Fx,r

=

0 0 00 0 00 0 00 Mu 0Lv 0 Nv

0 Mw 00 0 00 0 00 0 00 0 0Lp r(Iz − Ix)/Iy q(Ix − Iy)/Iz + Np

r(Iy − Iz)/Ix Mq p(Ix − Iy)/Iz

q(Iy − Iz)/Ix + Lr p(Iz − Ix)/Iy Nr

T

F.6 Adaptation of the Model

To use the model in the project the model has to be adapted to our aeroplane. To dothis there are 24 different c-parameters to determined. The first approximation of the c-parameters was taken from a similar project in Stanford. They had a bit larger aeroplanebut it should be close to the truth. These are:





AUAV 36

CL = 0.763 CD = 0.029 CLα= 2.82

Cmα= −3.19 CLq

= 4.64 Cmq= −11.48

CmM= 0 CLδe

= 0.384 Cmδe= −1.018

Cyβ= −0.396 Cyp

= 0.107 Cyδα= 0

Clβ = −0.027 Cnβ= 0.197 Clp = −0.482

Cnp= −0.044 Clδa

= 0.135 Cnδa= −0.027

Some parameters was taken from the aeroplane Navion. These are:

CLα= 4.44 CDα

= 0.33 Cmα= −0.683

CLM= 0 CDM

= 0

and one is a approximation of a propeller-driven aeroplane:

CTu= −CD

This C-parameters are used to calculate the different values in F.5.1 as follows:

Xu =−(CDM

+2CD)QS

muo+

CTuQS

muoXw =

−(CDα−CL)QS

muo

Zu =−(CLM

+2CL)QS

muoZw =

−(CLα+CD)QS

muo

Zq =−CLq QSc

2muoZδe

=−CLδe

QS

m

Mu =CmM

MoQSc

uoIyMw =

Cmα QSc

uoIy

Mq =Cmq QSc2

2uoIyMδe

=Cmδe

QSc

Iy

Yβ =Cyβ

QS

mYp =

Cyp QSb

2muo

Yδα=

CyδαQS

mLβ =

ClβQSb

Ix

Lp =ClpQSb2

2uoIxLδα

=Clδα

QSb

Ix

Nβ =Cnβ

QSb

2uoIzNp =

CnpQSb2

2uoIz

Nδα=

CnδαQSb

IzYv =

Yβ

uo

Lv =Lβ

uoNv =

Nβ

uo

where m(mass) = 3.5 kg, S(wing planform aera) = 0.59 m2, Q(dynamic pressure) =403 Pa, uo(reference flight speed) = 25 m/s, c(mean chord) = 0.32 m, Mo(Mach nr) =0.0753, b(wing span) = 1.8 m





AUAV 37

When testing the model we discovered that it differs a bit from the real aeroplane. Itshould depend on the c-parameters that are taken from the aeroplane Navion and thatthe rudders from the real aeroplane not are converted and adapted to the model. Unfor-tunately the time ran out but we think we are pretty close to a good model.





AUAV 38

G Linux Computer

G.1 Special Considerations Regarding Software Environment

It is quite important to be careful when running code on an embedded system. Duringinitial research of the capabilities of the system, it was found that it is crucial to use acorrectly configured toolchain when building code for the system.

The toolchain, in this case gcc and glibc, must be built with the knowledge that thecomputer lacks a floating point operations unit in the processor. If an improperly con-figured toolchain is used, the performance of code using floating point operations will beextremely bad.





AUAV 39

H Extended Kalman Filter

An Extended Kalman Filter is a Kalman filter where the non-linear functions f and h inthe following system are linearized around the estimated values of x:

x = f(x(t), u(t)) + v(t)

y = h(x(t), u(t)) + v(t)

The process noise v(t) and the measurement noise e(t) are white noise.

The system is approximated by:

x ≈ f(x, u) + Fx(x, u)(x(t) − x) + v(t)

where Fx is ∆xf(x).

A discretization of the system results in:

x(t + T ) = x +

∫ T

0

eFx(x,u)τdτf(x, u)

where eAt = I + At + A2 t2

2! + A3 t3

3! + ... .

The Extended Kalman Filter equations for the system consist of the following equations:

Time-update:

xt+T |t = xt|t +

∫ T

0

eFx(xt|t,u)τdτf(xt|t, u)

Pt+T |t = eFx(xt|t,u)T Pt|teFx(xt|t,u)′T + Qt

where Pt|t = E((xt|t − x(t))(xt|t − x(t))′) and Cov(v(t)) = Qt.

Measurement-update:xt|t = xt|t−T + Kt(yt − h(xt|t−T ))

St = HtPt|t−T H ′t + Rt

Kt = Pt|t−T H ′tS

−1t

Pt|t = Pt|t−T − KtStK′t

where Ht is ∆xh(x)|x=xt|tand Cov(e(t)) = Rt.

By selecting a Q that is large in comparison to R a fast filter that is sensitive to noiseis acquired.The Kalman Filter will pay more attention to the measurements than to themodel. If instead R is large in comparison to Q the model will matter more and the filterwill be slower and less sensitive to noise. If the initial value of x(t) is known P0|−T = 0.

In this project there are three noise covariance matrices; RIMU for the measurementsof the IMU, RGPS for the measurements of the GPS and Q for the process noise. Thematrices below where used when the figures 9 and 10 were produced.

RIMU = diag(

10−4 10−4 10−4 10−3 10−3 10−3 5 · 10−3 5 · 10−3 5 · 10−3)

The diagonal components of the RIMU matrix correspond to the variances of the mea-surement noise of the IMU measurements found in:





AUAV 40

yIMU = ( p q r ax ay az magx magy magz )T

where p, q and r are the components of the angular velocity, ax, ay and az are thecomponents of the acceleration and magx, magy and magz are components of the compassvector measured by the IMU.

The magnitudes of the variances have been estimated by looking in [12] and by estimatingthat the variance of the angular velocity is approximately ten times smaller than thevariance of the acceleration.

RGPS = diag(

1 1 10)

The diagonal components of the RGPS matrix correspond to the variances of the mea-surement noise of the x, y and z components of the GPS position.The RGPS matrix variesover time. Its diagonal components are multiplied by HDOP and VDOP, which are facorsdescribing the uncertainties of the horizontal and vertical measurements obtained fromthe GPS.

Q = diag(

10 10 1000 10−5 10−5 10−5 1 1 1 10−4 10−4 10−4 10−4 10−2 10−2 10−2)

The diagonal components of the Q matrix correspond to the variance of the process noiseof the components in the state vector:

x = ( x y z u v w ax ay az e0 e1 e2 e3 p q r )T

The main focus when these magnitudes were chosen was to get unnoisy estimations of theposition,the velocity and the quaternions.

The initial condition of the state vector is zero for all components except the quaternione0 that is one. Because all of these initial values are known, except the ones for thequaternions, the P matrix is initialized as shown below:

P = diag(

0 0 0 0 0 0 0 0 0 103 103 103 0 0 0 0)





AUAV 41

I Gatekeeper

Figure 19: Layout for the gatekeeper.

Component Value QuantityCrystal 8MHz 1Reset switch 1Capacitance 100nF 2Capacitance 100uF 1Capacitance 22pF 2Contact list for connecting SPI,supply and servosResistance 4.7kohm 1Plinth for AVR 1ATmega16 1Laminate 7.5x10cm 1Jumpers 4Level transformation +3.3,+5V 1

Table 7: Components on the gatekeeper.





AUAV 42

Figure 20: Schematic for the gatekeeper.





technical documentation autonomous unmanned aerial vehicle · in automatic control project course,...

Documents