(Project) Modelling and Control of Autonomous Quadrotor

A A L B O R G U N I V E R SI T E T
Modelling and Control of Autonomous

Quad-Rotor
2nd Semester Project

of the Intelligent Autonomous Systems Master Programme
Group 10833
Faculty of Engineering, Science and Medicine

University of Aalborg, Denmark
June 2010
A A L B O R G U N I V E R SI T E T
Department of Electronic Systems
Fredrik Bajers Vej 7B
9220 Aalborg
Denmark
http://es.aau.dk
Title: Modelling and Control of Autonomous Quad-Rotor
Theme: Modelling and Control
Project period: Spring 2010
Group: 10gr833 Synopsis:
The scope of this project is to design and implement

Group members: a stabilizing and tracking system control for a small
Quad-Rotor platform through a (roll, pitch, thrust, yaw)
Claudia Mary command interface, flying in high precision motion
capture room.
Based on system identification, analysis of the physi-
cal platform, and the fundamental laws of mechanics,
Luminita Cristiana Totu a non-linear model of the system has been designed.
The non-linear model is linearized at hovering operat-
ing point. A state feedback Linear Quadratic controller
with step reference tracking has been implemented in
Simon Konge Koldbæk MATLAB Simulink.
With the implemented control scheme, the Quad-Rotor
is able to hover autonomously and perform small step
movements in all directions. While the evaluated per-
formance did not meet the set requirements, it did show
promising results as proof of concept. Several simple
improvements can be added, such as integral action,
Supervisor: and are expected to significantly improve the perfor-
Flemming Schøler Ph.d. mance.
Copies: 7
Pages: 139
Attachments: 1 CD attached
Completion of project: 01.06.2010
Preface
This report is presented by group 10GR833 from the Institute of Electronic Systems at Aalborg
University. The report documents the 2nd semester master project in relation to the study of
Intelligent Autonomous Systems.
The semester has the purpose of giving the students an intensive and deep understanding of
modelling and control in relation to Intelligent Autonomous Systems. The project concerns the
manoeuvring of the X3D Quad-Rotor using Vicon motion tracking. The goal of the project is to
design and implement a control scheme that allows the platform to fly autonomously in the AAU
Motion Tracking laboratory.
The report is divided into seven main chapters: Introduction, System Analysis, Strategy and
Requirements, System Modelling, Controller design, Fault detection and Project evaluation. All
test journals and general documentation related to the project, are located in the report’s appen-
dices. The relevant simulation models, components data sheets and MATLAB documents are
located on the project CD. The project CD also contains a short video of the X3D Quad-Rotor.
The authors wish to express special thanks to Anders Friis Sørensen for his generous support
during the project.
V of 139
Contents
1 Introduction 1
1.1 Problem Statement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.2 Chapter Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
2 System Analysis 4
2.1 X3D Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
2.2 X3D Remote Control and Manoeuvring . . . . . . . . . . . . . . . . . . . . . . 4
2.2.1 Reference frames . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
2.2.2 X3D Control Inputs . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2.2.3 X3D Manoeuvring . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2.3 Motion Tracking System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.3.1 Motion Tracking Lab . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.3.2 Vicon MX . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.3.3 Tracking precision . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.3.4 Tracking errors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
2.4 Interface Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.4.1 Vicon Link . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.4.2 Quad-rotor command interface . . . . . . . . . . . . . . . . . . . . . . . 15
2.5 Chapter Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
3 Strategy and Requirements 18

3.1 Modelling Strategy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
3.2 Control Strategy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
3.3 Requirements specification . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
3.4 Chapter Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
VII of 139
CONTENTS
4 Modelling 24
4.1 Model Structure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
4.1.1 Internal controller & X3D aerodynamics . . . . . . . . . . . . . . . . . 25
4.1.2 X3D thrust generation . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
4.2 Non-linear Simulation model . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
4.2.1 Non-linear Simulation model Verification . . . . . . . . . . . . . . . . . 28
4.3 Model Linearisation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
4.3.1 Taylor series approximation . . . . . . . . . . . . . . . . . . . . . . . . 31
4.3.2 Rigid Body Dynamics & Kinematics . . . . . . . . . . . . . . . . . . . 31
4.3.3 Thrust generation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
4.4 State Space model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
4.4.1 System States . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
4.4.2 System Equations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
4.4.3 Matrix Derivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
4.4.4 Discrete State Space model . . . . . . . . . . . . . . . . . . . . . . . . . 35
4.5 Chapter Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
5 Controller Design 38
5.1 Linear State Feedback . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
5.1.1 Methods for State Feedback . . . . . . . . . . . . . . . . . . . . . . . . 40
5.2 Linear Quadratic methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
5.2.1 Optimal Problem . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
5.2.2 Linear optimization with quadratic criterion . . . . . . . . . . . . . . . . 41
5.2.3 Infinite Horizon optimization . . . . . . . . . . . . . . . . . . . . . . . . 43
5.3 LQ algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
5.3.1 Dynamic Programming . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
5.3.2 Matrix Calculus . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
Convergence over Infinite Horizon . . . . . . . . . . . . . . . . . . . . . 48
Algebraic Riccati Equation . . . . . . . . . . . . . . . . . . . . . . . . . 48
5.3.3 Pseudocode Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
5.3.4 Tracking reference . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
5.3.5 Choosing the Weight matrices . . . . . . . . . . . . . . . . . . . . . . . 52
5.4 LQ implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
5.4.1 Linear model simulations . . . . . . . . . . . . . . . . . . . . . . . . . . 53
VIII of 139 CONTENTS

CONTENTS
5.4.2 Non-Linear model simulations . . . . . . . . . . . . . . . . . . . . . . . 55

5.4.3 State estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
5.4.4 Online controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57
5.5 Chapter Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
6 Fault Detection 59
6.1 Component analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60
6.2 Failure mode & Effects analysis . . . . . . . . . . . . . . . . . . . . . . . . . . 61
6.3 Fault Detection & Isolation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
6.3.1 Unknown Input Observer . . . . . . . . . . . . . . . . . . . . . . . . . . 64
Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
Verification . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
6.4 Fault Accommodation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71
6.5 Chapter Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71
7 Project evaluation 73
7.1 Results and Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73
7.1.1 Position control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73
7.1.2 Yaw control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74
7.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75
Bibliography 78
A Performance Tests 79
B Performance Tests Results 82
C First Principles model 89
D Vicon Data Management 97
E Non-linear Model Verification - Test flight 98
F Vicon tracking precision 101
G X3D components 114
H Thrust Force Measurement 118
CONTENTS IX of 139
CONTENTS
I Angular Velocity Measurements 125
J Frame Transformation matrices 133
K Software Notes 139
X of 139 CONTENTS
Chapter 1
Introduction
In this chapter an introduction to the project is given in order to set the scene for the reader.
The first section is an overall analysis of possible application domains and project warrant. The
second section presents the project problem statement and specific goals.
UAS (Uninhabited Aerial Systems) have received increasing attention in the last decade as a
replacement for expensive human-piloted systems [9][7][5]. They have become a widely used
platform for many applications for which human operation are considered unnecessary, repeti-
tive, or too dangerous. UAS have an extremely varying application domains that include both
indoor and outdoor applications. Possible UAS applications are enumerated in the list below:
• Aerial Surveillance and Intelligence for Law Enforcement
• Hobbyist and Professional Aerial Photography and Video
• Property Assessment and Real Estate promotion
• Traffic monitoring and Traffic pattern Analysis
• Weed identification in Agriculture
• Educational learning Platforms
The listed application domains require different levels of control and manoeuvrability. Some
applications may need high precision, while others may have a larger tolerance.
An example of UAS is the RQ-11B Raven B that is used by several countries worldwide - includ-
ing Denmark. The Raven illustrates the usefulness of small UAS for reconnaissance and surveil-
lance as a fixed wing platform for outdoor environments, and can cover a large area. Fixed wing
platforms are not intended to be flown at low altitudes, indoors, or in an obstacle prone zones.
For these types of applications, rotor based systems are preferred due to higher manoeuvrability
and hovering capabilities.
High performance combined with an increasing availability and reduced prices have led to a
growing interest in using rotor based platforms. One common task for quad-rotor autonomous
1 of 139
1.1 Problem Statement
Figure 1.1: Picture of the platform used in this project - the X3D Quad-Rotor
flight is a confined environment in presence of multiple types of obstacles. Obstacles may be

stationary like buildings or dynamic like other aerial crafts or human beings. In this project a
Quad-Rotor platform, called X3D, is used as vehicle of designing a control system capable of
executing autonomous flight, hovering, and movement. The X3D Quad-Rotor is illustrated in
figure 1.1.
The application chosen as basis for this project is a flying museum custodian. The objective
of the system is to provide a technological and entertaining alternative to human custodians. The
system is envisioned to guide guests around in a museum environment with multiple obstacles.
The described application will be used as reference throughout the report and will be refereed to
as the project application.
Figure 1.2: Picture of a museum landscape with showcases [www.museodirect.com]
1.1 Problem Statement
In this section the Project Problem Statement will be addressed. The Project Problem Statement
is inferred from the desired functionality of the project application and the necessary control
problems that must be addressed to achieve the desired functionality.
2 of 139 1. Introduction
1.2 Chapter Summary
The goal of this project is to develop and implement a viable control system for the X3D Quad-
Rotor. The control scheme must enable the X3D to perform stable position hovering and trajec-
tory tracking within the limitations of the requirements specification described in more detail in
section 3.3. The process of generating the trajectories for the X3D is not addressed in this project
as it is considered beyond the scope of the project.
In order to enable the X3D to track a predefined trajectory several control problems have to
be addressed and solved. As the X3D system does not feature any means of performing stable
hovering at a predefined way-point1 , a stable hovering controller must be designed and imple-
mented. Subsequently, this controller has to have the ability to manoeuvre the X3D from one
way-point to another. If the results from these first tests are satisfactory, the controller’s ability
to track a continuous trajectory may be investigated.
The collective goals and thus the course of action are described in the following problem state-
ment.
”Is it possible to design and implement a system control that enables stabilization of the X3D
Quad-Rotor position and attitude?”
The development of the collective system control can be divided into several sub-goals. The
completion of the collective sub-goals is necessary in order to develop a viable system control
that will enable the X3D to track the requested way-points form the trajectory controller. The
collective sub-goals of the project are listed below and serve as a course of action for the project.
• Design a viable state space model for the X3D Quad-Rotor
• Design and implement a stable hovering controller for the X3D Quad-Rotor
• Investigate the designed controller’s ability to manoeuvre the X3D Quad-Rotor from one way-point
to another.
• Investigate the designed controller’s ability to track a continuous trajectory.
1.2 Chapter Summary
In this chapter the main objectives of the project have been addressed. The project application
has been presented as a means of validating the development of system control for the X3D
Quad-Rotor. Having set the scene for the reader in relation to the mission of the project and the
encompassed elements, it is considered appropriate to address the nature of the project equip-
ment and methods. The project equipment includes the AAU laboratory MTlab and the X3D
Quad-Rotor. The process of describing these issues will be the main focus of the next chapter -
Project Analysis 2 on the following page. Next, the project modelling and control strategy will
be discussed.
1 Reference point in physical space used for purposes of navigation.
1. Introduction 3 of 139
Chapter 2
System Analysis
In this chapter the project laboratory facilities and equipment are described. The first part of
the chapter deals with the physical design of the X3D and presents the X3D hardware which
as a collective make up the X3D Quad-Rotor. The second part of the chapter deals with the
X3D manoeuvring possibilities, the control inputs and presents an introduction to the physical
forces that effect the X3D. The third part of the chapter deals with the software used in relation to
communicating with the X3D and the motion tracking laboratory - MTLab AAU. The MTLab AAU
is also addressed in a dedicated section where the laboratory set-up is described as well as the
tracking precision. The final objective of this chapter is to enable the establishment of the used
modelling and control strategy. It will also enable the formulation of the project Requirement
Specification which will severe as benchmarks throughout the project.
2.1 X3D Hardware
This section presents the hardware components of the X3D. The X3D has been assembled, main-
tained, and updated by multiple groups in the university laboratory. The platform is based on the
X-3D-BL kit from Ascending Technologies [1] hobby line.
In figure 2.1 the main hardware components of the X3D are illustrated. These are the X-CSM
body frame composed of 4 booms and a structural core, the X-Base main electronic board, X-BL
brushless motors and driver circuits, the radio controller receiver, and the Research Pilot sensor
and control board. Based on the descriptions from the Ascending Technologies manual [14], a
detailed presentation of the components has been compiled in appendix G on page 114.
2.2 X3D Remote Control and Manoeuvring
The X3D is a highly manoeuvrable quad-rotor that enable the execution of precise and rapid
movements in 3D space. In this section a meticulous presentation of the X3D manoeuvring
possibilities is presented. Moreover a series of reference frames used in the process of modelling
the X3D are introduced to better visualize the manoeuvres. This section also contains a small
description of the X3D remote control input.
4 of 139




 



Figure 2.1: Illustration of the X3D Quad-Rotor hardware components
2.2.1 Reference frames
To better understand the manoeuvring of the X3D a series of reference frame are introduced.
In short the reference frames describe the coordinates system or environment in which the X3D
manoeuvres. The coordinates system will be used as a reference in both this section as well as in
the modelling chapter 4 on page 24.
The X3D modelling environment is illustrated in figure 2.2 where E is the earth frame and B
is the body frame. The earth frame E will be the reference frame throughout this chapter. The
frame is related to the MTlab in which the X3D is flown and is orientated so the xy-plane is in
the laboratory floor and the z-axis pointing downwards. The body frame B is related to the X3D
as shown on the figure 2.2. The vectors in the E frame will be referred as E a and those in the B
frame as B a. The X3D is able to rotate around all three body-axis. The sign of the rotations have



 



 

 

Figure 2.2: Illustration of the right-handed coordinate systems as defined in this project
been defined based on the signs related to the input commands from the wireless link. Positive
inputs result in positive right-hand rotations and visa versa. On the physical X3D the front is
marked with red tape to better identify the attitude of the X3D when flown by a human operator.
The x-body axis The B x is always aligned with the front of the rotor. The orientation of the X3D
2. System Analysis 5 of 139





Figure 2.3: Illustration of the coordinate systems xy-body plane view
in the E frame can always be described by three successive right-hand rotations around the three
axes. The rotations used in this project are a 3-2-1 sequence that is read as rotations around
x,y then z axis also called roll, pitch and finally yaw. This sequence of rotation is called 3-2-1
Euler angles and is addressed in detail in appendix J on page 133 along with the transformation
matrices used in the process of modelling the X3D.
2.2.2 X3D Control Inputs
The X3D is an aerial system that uses four propellers to generate lift. The X3D falls under
the definition of a Quad-Rotor that can be classified as a type of helicopter. Most Quad-Rotors
use fixed-pitch blades as opposed to most commercial helicopters. The system is controlled by
varying the rotational velocity of each of the rotors and thereby changing the thrust and torque
produced.

Figure 2.4: Illustration X3D wireless remote control interface
The X3D is designed for manual control i.e. human control with visual feedback and it is thus
not possible to directly control the rotational velocity of the individual rotors. The X3D features
several different modes of control but in the following only the Heading-Hold mode will be
addressed. For a thorough description of the other modes see G on page 114. The X3D flown in
Heading-Hold mode utilises a on-board control circuit that manages the rotational velocity of the
four rotors and adjusts the velocity based on on-board sensory data. The human operator is only
able to control the angular velocities of the X3D along the three body-axis and the generated
thrust. This allows a skilled operator to control the attitude and position of the platform thus
enabling the operator to maintain the X3D in hover. In figure 2.4 the wireless remote control
inputs related to the change in platform attitude are illustrated.
6 of 139 2. System Analysis

Parameter Min Max Nominal range

Roll -2047 2047 [-1500 - 1500 ]
Pitch -2047 2047 [-800 - 800 ]
Yaw -2047 2047 [-800 - 800 ]
Thrust 0 4096 [0-1800 ]
Table 2.1: Wireless link output range [14]
The control signals used by the wireless remote control interface are the same as the ones that are
accessible from the wireless link. The wireless link is the communication channel between the
interface PC and the X3D. The wireless link output range (references for the on-board controller)
is illustrated in table 2.1. The nominal range is based on observation of the reference range
when the X3D is flown in a aggressive behaviour by a human operator. The designed system
control should be well beneath this range as the the operator focused on given the X3D the largest
references possible without crashing. As the X3D is to be modelled it is considered appropriate
to address the manoeuvring of the X3D.
2.2.3 X3D Manoeuvring
Having presented the control Inputs of the X3D and the reference frame terminology the actual
manoeuvring possibilities of the X3D is presented
The X3D generates upward force using four rotors which angular velocity can be controlled
using the wireless control interface 2.4. The force components generated by the four rotors are
illustrated in figure 2.5. As the rotors rotate they are subject to drag caused by the air being
moved. The drag results in a reactive moment on the rotors known as the induced moment. The
induced moments τ1 - τ4 are illustrated on figure 2.5. The induced moment acts on the rotor in the





 





Figure 2.5: Illustration of the angular velocity of the rotors while hovering
opposite direction of the rotors direction of rotation. On a classical helicopter the tail rotor coun-
ters the induced moment caused by the main rotor but on a Quad-Rotor the effects of induced
moment is cancelled out by rotating the motors in pairs in opposite directions as illustrated in
figure 2.5. Rotor 1 and rotor 3 see figure 2.5 are rotating clockwise and the other two rotors, 2
and 4, rotate counter clockwise.

Given are Newtons second law of motion 2.1 and the rotational analogue to the law 2.2. F is
the total applied force to a rigid body, m is the mass of the body and a is the linear acceleration
of the body. τ is the total torque exerted at the body and J is the body moment of inertia and α is
the angular acceleration [6, page 117 ].
∑F = m·a (2.1)
∑τ = J ·α (2.2)
When the X3D is in perfect hover it is in both perfect force and torque balance. As the rotors are
of the same design the equal angular velocity will result in equal thrust and torque. Because of
this the attitude of the X3D is preserved when equal lifting forces are created by each rotor. If the
rotors are rotating at the same angular velocity the total torque acting on the X3D is equal to zero
and the angular acceleration around either axis is exactly zero meaning that the X3D is hovering.
This means that when the X3D is hovering the torque created by the rotors in the x-body axis is
cancelled out by the torque created by the rotors in the y-body axis. The opposing moments puts
immense stress on both the booms and core of the X3D. The materials used for the booms and
the core are thus chosen to withstand harsh treatment. For a complete description of the X3D
hardware view section 2.1.
Roll
Roll is when the X3D performs a rotation around the x-body axis. This is achieved by changing
the angular velocity of the rotors on the y-body axis that is rotor 2 and 4 while maintaining the
same angular velocity on rotor 1 and 3. If the desired rotation is positive the roll is performed
by increasing the angular velocity of rotor 2 and decreasing the angular velocity of rotor 4. This
results in a positive roll or rotation around the x-body axis. The rotor velocities related to this
manoeuvre are illustrated in figure 2.6.

 


 



 


 


Figure 2.6: Illustration of the angular velocities related to positive roll or rotation around the
x-body axis
Pitch
Pitch is when the X3D performs a rotation around the y-body axis. This is achieved by changing
the angular velocity of the rotors on the x-body axis that is rotor 1 and 3 while maintaining the

same angular velocity on rotor 2 and 4. If the desired rotation is positive the pitch is performed
by increasing the angular velocity of rotor 1 and decreasing the angular velocity of rotor 3. This
results in a positive pitch or rotation around the y-body axis. The rotor velocities related to this
manoeuvre are illustrated in figure 2.7.


 

 


 



  


Figure 2.7: Illustration of the angular velocities related to positive pitch or rotation around the
y-body axis
Yaw
Yaw is when the X3D performs a rotation around the z-body axis. This is achieved mismatching
the torque generated by the X3D rotors. During normal flight the net torque acting on the plat-
form body is zero as the rotors along the x-body axis and y-body axis apply the same amount
of torque to the platform. If the torques applied along the axis are mismatched the platform will
perform a rotation around the z-body axis. If the desired rotation is positive the yaw is performed
by increasing the angular velocity of rotor 2 and 4 while decreasing angular velocity of rotor 1
and 3. This results in a positive yaw or rotation around the z-body axis. The rotor velocities
related to this manoeuvre are illustrated in figure 2.8.

 

 

 


 

  


Figure 2.8: Illustration of the angular velocities related to positive yaw or rotation around the
z-body axis

2.3 Motion Tracking System
In relation to the project, it is necessary to continuously track the position and attitude of the X3D
Quad-Rotor. This is achieved in the AAU MTLab, using the installed Vicon MX - a powerful
digital optical system for motion capture. It it possible to track real-time, with high precision,
moving objects within an indoor room.
A specialized motion capture system is not a practical approach for a real deployment, but it
is a very good setting for the initial design and tests of the model and control algorithm, since it
provides highly precise, low noise data for the position and orientation of the flying X3D. This
is considered to be within the project objectives.
2.3.1 Motion Tracking Lab
The AAU MTLab (Motion Tracking Lab) consists of two rooms. The capture room is 5 x 6 x
3 [m] obstacle free room with a multi-point video camera system. The adjacent control room
allows the operator to observe the capture room through a large plexiglas window. The control
room also contains computers and additional hardware related to both autonomous and direct
control of various UAS used at the AAU. Figure 2.9 shows pictures from MTLab as it appeared
in March 2010.
Figure 2.9: Pictures from MTLab AAU March 2010
2.3.2 Vicon MX
The Vicon MX system consists of special cameras, a hardware control module, and a host com-
puter with software to analyze, relay, and present the data. The system can supply the position
and orientation of any tracked object. This information can also be accessed by any station on
the MTLab local network, by directly connecting to the host computer via TCP/IP. Moreover,
Vicon Link is an easy way to use MATLAB Simulink block, build on top of the TCP/IP Vicon
protocol, and is part of the MTLab available software. The Vicon link is the endpoint used in the
project to connect to the Vicon system. It is presented in the Control Software section.
Though the group is not interfacing directly with the Vicon motion tracking system, it is consid-
ered appropriate to give an overall description of the system structure, and highlight two aspects

that need further consideration. In figure 2.10 the general structure of the MTLab is illustrated.
The Vicon MX system works by identifying special markers (reflective spheres). The markers


  
 
 



 
 
 
Figure 2.10: Illustration of the MTLab hardware structure
are mounted on the object and tracked with high precision, at a data rate of 100Hz. The system
supports both single and multi-body systems, where several markers are used to determine the
position of multiple parts of the body. The X3D quad-rotor is fitted with a total of five markers
that allow the determination of both the center mass position, and the attitude. In the capture
room, a configuration of 7 cameras is set in place. At any given time, a marker must be in the line
of sight of at least 4 cameras to be correctly monitored by the system. The tracking zone is specif-
ically designed for aerial objects: the cameras are placed high in the room providing focus both
on the ground level and the on the fly zone in the center of the room, where there are no obstacles.
The Vicon cameras are connected to a controlling hardware module, the Vicon MX box. The
box provides signal synchronization and is connected to the Interface Computer through a Gi-
ganet Ethernet interface. Furthermore, the Interface Computer is connected by an Ethernet hub
to a local area network. The signals from the Vicon MX box are managed on the Interface
Computer using the Vicon iQ and RTE (Real Time Engine). This software provides a GUI and
tools to manage, set-up, record, and process a motion capture session in real-time. An impor-
tant concept is the object model, defined by the user as a collection of markers with additional
properties. Based on the model definition, the object is tracked with high accuracy, even in sit-
uations with complex interactions between multiple objects and large number of markers. The
position information of the markers coming from different cameras is combined and related with
the model definition, making it possible for the software to calculate the center mass position and
the orientation angles.
2.3.3 Tracking precision
Vicon precision in tracking a marker is said to be sub-millimeter on the MTLab website [8]. In
order to get an estimate of the actual precision for the X3D quad-rotor defined as a Vicon iQ
object model with 5 markers, a simple experiment was performed. The experiment measures the

noise in the data when X3D is stationary, and is documented in the appendix F.
It is concluded that the noise is very low, and more than adequate for the project. The recorded
maximum noise tracking the position of the stationary quad-rotor with the motors turned on
(platform is vibrating) is below 1.7 [mm] on each axis, and the maximum recorded noise in the
measured Euler angles is below 1.4 [◦ ].
In the 3D space representation, the position measurements can be been plotted inside a cube with
the sides of 3.4 [mm].
Figure 2.11: View of the position measurements taken with engines on, platform vibrating
2.3.4 Tracking errors
While the Vicon system has a very good precision, it is limited to a relatively small area in the
center of the room that is very well covered by all the cameras. Whenever the object is outside
this area, some of the cameras lose sight of some or all of the markers, and tracking error occur.
Some of the most important type of Vicon errors are discussed below. A precision recording
session for a longer period of time shows level jumps in the signal values. These are caused by

one or more of the camera losing the object, or some of the reflective markers, and the combined
tracking data from the rest of the cameras becomes offseted. In the Vicon case, the jumps in the
signal are very small. In GPS tracking systems, the same type of errors occur, at a larger scale.
The following data has been recorded while the quad-rotor motors were off.
Figure 2.12: Tracking jumps in the position signals
Figure 2.13: Tracking jumps in the orientation signals

An in-flight recording shows other three common Vicon errors: signal-dead zones, spikes, and
noise.
Figure 2.14: Errors in the position signals
Figure 2.15: Errors in the attitude signals

2.4 Interface Software
MATLAB with Simulink is the predilect software environment choice for the project. The ma-
jority of the tasks have been performed under the MATLAB environment: measurements, offline
calculations, simulations, and the online implementation. The software from [8] that is used to
interface with the hardware is in the form of custom Simulink blocks compiled from C++ source
code with mex compiler. The operating system of choice for the project is Windows XP.
2.4.1 Vicon Link
The "Vicon Link" Simulink block is part of the MTLab software and was used without mod-
ifications. It implements a TCP/IP socket connection to the Vicon Interface Computer, where
the Vicon RTE services on the default port 800, providing the tracking data on a selected object
models at a data rate of 100Hz.
The Vicon block has 3 parameters: the IP address of the Interface Computer, the identifier of the
tracked object as defined in Vicon iQ (multiple objects are possible, however this usage is outside
of the of the project scope), and the sample time of 0.01 seconds, equivalent of 100Hz.
Outputs of the Vicon block that are used in the project are Position and Euler signals. There are
3 position signals (x, y, z) measured in meters and 3 Euler angles (roll, pitch, yaw) measured in
radians.
Figure 2.16: Vicon link block
2.4.2 Quad-rotor command interface
It is possible to interface with the X3D quad-rotor through a serial interface. The Research Pi-
lot sensor board has an Universal Asynchronous Receiver/Transmitter (UART) integrated circuit
controller, the key component of the serial communications subsystem. The UART takes bytes
of data and transmits the individual bits sequentially. At the destination (the PC), a second UART
reassembles the bits into complete bytes.
The serial link between the quad-rotor and the control computer is set-up through a pair of small,
wireless RF modules that use IEEE standard 802.15.4 standard to communicate. One of the mod-

ules is connected to the X3D’s UART connector, and the second is connected to the PC via an
USB port.
The AscTec manual describes a simple serial protocol and the format of the packages that can be
exchanged over the serial link to command the X3D quad-rotor and receive sensor information
from the on-board electronics. "X3D interface" is a Simulink block that implements the AscTec
serial protocol. It is used in the project as the quad-rotor communication endpoint. The block
Figure 2.17: X3D Simulink block
takes as inputs the 4 standard commands and 4 corresponding activation flags: pitch, roll, yaw as
references for the angle velocities, and the reference for collective thrust. The command packet
must be send at a rate of minimum 20Hz and a maximum of 200Hz over the serial link.
The output of the block are the raw pitch, roll, and yaw data from on-board X3D from the
electronic gyroscopes, the acceleration rates, and the readings from the 4 R/C channels. It is thus
possible to record the commands from an operator flying session. The output data packet rate
can be set from a minimum of 5Hz to a maximum of 300Hz.
An important note is that throughout the project, the raw sensor data from the X3D on board
instruments ( electronic gyroscopes, accelerometer, pressure sensor) is not used for control. In-
stead, the more accurate Vicon measurements of position and orientation are used. The X3D
interface is used only for command, and was also used during the system analysis and design to
record manual commands for the system identification part of the modelling.
X3D interface is a new MTLab software package, initially available only for the Linux plat-
form. It was required to port the software to Windows operating system. Two main tasks have
been carried out: setting up the build procedure with small changes in the existing code for op-
erating system compatibility, and implementing and integrating the serial interface component,
which is platform specific. More information can be found in the Appendix K.

2.5 Chapter Summary
2.5 Chapter Summary
In this chapter the several main issues of the project has been addressed. The X3D has been
described in relation to physical characteristics, hardware components and maneuvering possi-
bilities. The overall structure of the MTLab AAU has been described as well as the software
related to communicating with both the MTLab AAU and the X3D. As an additional feature the
tracking precision of the MTLab AAU has been investigated and found to be sufficiently precise
for control.
The following chapter will address the strategy related to modelling the X3D and the strategy
for designing the system control.

Chapter 3
Strategy and Requirements
Having described the physical features of the X3D and the AAU MTLab, it is considered appro-
priate to introduce the modelling and control strategy. The modelling and control strategy will
enable the group to identify the main aspects of project and provide a solid starting point for the
further development of the project.
3.1 Modelling Strategy
In order to design a valid control scheme for a system, a representative model is needed.
An often used approach in relation to modelling physical systems is to divide them into subsys-
tems and to define the input and output relations of the subsystems using the laws of physics.
The model should prevail from assumptions such as empirical modelling and fitting of the model
parameters. This approach is often called the First Principles model as it is based exclusively on
the laws of physics.
In relation to the X3D, it is possible to decompose the collective system into subsystems thus
making the system more manageable. The X3D subsystems can be represented as illustrated in
figure 3.1. Using the above decomposition and the established laws of physics, it is possible to
  

    
 
  

Figure 3.1: Illustration of the X3D model sub-blocks
derive the entire model of the X3D system. The opening steps are documented in appendix C
on page 89. However, during this opening analysis of the X3D First Principles model, it became
clear that it was possible to model the Quad-Rotor using different techniques and that using a
First Principles approach would add unnecessary work to the project process.
18 of 139
3.2 Control Strategy
The concept of controlling the X3D has been addressed by previous groups at the University.
Friis, Nielsen, Andersen, Bœnding, Jochumsen and Sœrensen showed that large parts of the
X3D can be modelled using system identification [7]. [7] presented a valid control scheme and
implemented an autonomous hovering control for the X3D using a simplified model based on
system identification. The model included an approximation for both the thrust generated by the
X3D, and the dynamics related to the angular velocities along the three body-axes.
Based on the work done by [7] the group intends to use techniques of System Identification
instead of a complete First Principles approach. It is assumed that this method will lead to a sim-
ple and manageable model that will be representative for the response of the X3D system, and
adequate for control. To this end, some attributes of the platform are approximated, and specific
features of the X3D may be omitted if their contribution to the final model can be considered
insignificant. Below is a list of the assumptions that have been made in relation to the modelling
of the X3D.
• The X3D is modelled in normal operation that is in near hover flight.
• Booms and body are symmetrical along the xy-body axis.
• The X3D structure, booms, body, and rotors, are rigid.
• Any ground effects are assumed to be minute.
• The X3D is modelled in the Heading-Hold mode. View section G on page 117 for description of
Heading-Hold mode.
The listed assumptions enable to disregard several complex phenomenons that effect the X3D,
such as the ground effect. These phenomenons are less significant when the X3D is in near hover
flight at a medium height.
The objective of this project is to stabilize and control the X3D Quad-Rotor in flight. Based on
the application description, three main operational objectives have been identified:
• Autonomous hovering at a static way point
• Autonomous manoeuvring from a static way point to another
• Autonomous trajectory tracking following a dynamic path reference
Different approaches can be used to achieve the performance required by the three modes of op-
eration. There are inherent differences between static way points manoeuvring and following a
dynamic path reference, with the later being more complex than the former.
Figure 3.2 illustrates a controller structure capable of moving between static reference way
points. In this configuration the references consist of static x, y, and z coordinates and yaw
3. Strategy and Requirements 19 of 139

 

    


     
  

     


  
        
      
Figure 3.2: Static position reference controller
angle. To move from its initial position to the newly given way point, the linear controller gen-
erates the pitch, roll, thrust, and yaw commands based on the given reference and the measured
state of the system. In this control scheme there are no constraints on the yaw reference angle.
Although not obvious, free yaw reference requires an operation-point compensation technique.
The linear controller is based on a linearized system model, and will operate correctly only in the
proximity of the chosen operating point. The operating point that fixes several system states, such
as attitudes and translational velocities 4.27, and thus includes the yaw. To support the entire yaw
range, the controller scheme must compensate for the rotation of the reference frames. Changes
in yaw angle will result in the misalignment of the two system frames and special care must thus
be used as the controller commands are issued relative to the Quad-Rotor’s body frame, while the
measured attitude signals are in the Earth reference frame. Appendix J describes in more detail
reference frame transformations. When following a trajectory path, the reference is dynamic and
includes multiple parameters in order to ensure a smooth and precise tracking. In figure 3.3 the
structure of the system control in relation to a possible implementation of a tracking controller is
illustrated.


         





    
  

      

 

         
Figure 3.3: Dynamic position and velocity reference controller
20 of 139 3. Strategy and Requirements

A controller specifically designed for trajectory tracking will most likely be able to achieve ac-
ceptable performance in all three modes of operation, but the design of the trajectory tracking
controller is more complex than that of the static position reference controller. The system con-
tains several reference values and also requires that operating-point compensation be introduced
for more states.
Having described the overall difference between the required modes of operation it is clear that
one of two approaches must be chosen. Designing and developing a trajectory tracking controller
will provide the best performance in relation to the Project application. However, as this is the
group’s first time working with the Quad-Rotor some simplifications in relation to the developed
system control are considered acceptable. The chosen system control is thus designed with the
static waypoint manoeuvring in mind. This decision calls for a series of requirements which will
ease the design of the system control. The requirements are listed below.
• The system control is designed as a Hovering controller
• The system control must feature some static waypoint manoeuvring capabilities
• The X3D yaw angle is maintained at zero meaning that the Earth and body frame are aligned (x-
body axis and x-earth axis are kept parallel)
In figure 3.4 the control scheme used in this project is illustrated. This controller is designed for
hovering and will be tuned include to tracking of step references. It is assumed that an acceptable
response will be achieved using the illustrated structure. The X3D system in this configuration

  

  
  

 

   
   

      


 
        

Figure 3.4: Final control scheme strategy
has a total number of six outputs. The position P and the attitude as Euler angles Θ are all seen
by the Vicon Tracking system that is in the Earth frame. The Sensor management block is used to
estimate additional system variables that are not directly accessible. The structure of the Sensor
management block has not been defined at this point. The reference to the system consists of
the x, y and z position. The operating point for the yaw angle will be fixed to zero. In this first
attempt at designing a system control for the X3D it is assumed acceptable not to include the
use of multiple operating points. The structure illustrated in figure 3.4 will be used as reference
throughout the rest of the report.

3.3 Requirements specification
3.3 Requirements specification
In this section the Project requirements specifications are outlined. The Project requirements
specifications are based on the analysis of typical obstacles in an indoor environment defined by
the project application.
Given the context described in the modelling and control strategy, the performance of the control
is to be evaluated by using two tests. The first test investigates static hovering behavior and the
second position tracking for steps in the x,y, and z axes.
The objective of the X3D is to assume the role of a museum custodian, leading the guests around
the exhibition and supplying information about the exhibited objects. The X3D must navigate
trough the museum with great position precision. In this project, only static obstacles will be
addressed as the scope of the project does not include active obstacles avoidance. In most mu-
seums, the dominating static obstacles are glass display cases or exhibition cases. These usually
rectangular structures prevent the museum’s guests from touching the objects on display while
they still enable the guests to fully view them.And as most display cases are made of glass, it is
highly desirable that the X3D does not come into contact with the fragile structure.
In this context it is know that the X3D will not be able to perfectly maintain its position due to
disturbances and hovering, different requirements are needed in relation to the precision to which
the X3D manoeuvres. Based on the size of the platform and the project application and and work
done by [9] the following precision requirements have been defined.
• During autonomous hovering the Quad-Rotors z-axis position should be kept within ± 0.10 [m].
• During autonomous hovering the Quad-Rotors xy-axis position should be kept within ± 0.05 [m].
• During autonomous hovering the Quad-Rotors angle around the z-axis should be kept within ±
10.00 [◦ ].
• When changing the position 0.5 meter in either the x, y or z axis, the X3D should move to the new
position and remain in stable hover in the new position within 2 seconds.
The angles around the xy-axis do not have any requirements since the quad rotor is hovering and
adjusting pitch and roll to maintain its position. The stated requirements are not meant to be tech-
nical requirements in relation to the designed control scheme but they are considered necessary
requirements if the X3D is to manoeuvre in the project application.
In order to validate the performance of the X3D a series of performance requirements tests have
been devised. The objective of these tests is to validate that the designed system control scheme
meets the formulated requirements. The tests and a thorough description of their execution can
be found in appendix A on page 79. Notice that the tests are divided into two parts of varying
levels of difficulty. This division stems from the division used in the Problem Statement 1.1
where a similar approach has been used in relation to the project sub-goals. It is however noticed
that all the performance requirements tests must be completed and passed in order for the project
to be deemed a complete success.
22 of 139 3. Strategy and Requirements

3.4 Chapter Summary
3.4 Chapter Summary
In this chapter related to modelling the X3D and the strategy for designing the system control
have been presented. Based on the Project application and the X3D limitations addressed in the
chapter a series of Requirement Specifications and Requirement Specifications Test have been
formulated. The following chapter will address the modelling of the X3D system and supply a
valid non-linear system model that can be used in the process of designing and simulating the
system control scheme.

Chapter 4
Modelling
In this chapter, the non-linear dynamic system equations are derived. The used modelling ap-
proach consists in writing mathematical equations that describe the movements and dynamics of
the X3D as derived from the governing laws and principals of classical physics. The final goal of
this chapter is to produce a set of non-linear dynamic system equations that describe the physical
behaviour of the X3D and implement them in MATLAB Simulink.
4.1 Model Structure
As illustrated in appendix C on page 89 the process of modelling the dynamic behaviour of

the X3D using a first principles is not an easy task. In this section the revised system model
structure is addressed. The use of the internal controller removes several of the dynamic features
related to modelling the X3D and enables the revaluation of the system model structure.The
re-evaluated system model structure is illustrated in figure 4.1 As it can be seen from figure
      

 


 
 

Figure 4.1: Illustration of the approximated sub blocks
4.1 the complexity of the system model has been reduced significantly. The dynamics related
to the angular movements of the X3D are managed by the internal controller. The translatory
movements are modelled using a combination of system identification and classical physics. In
the following section the different sub-blocks illustrated in figure 4.1 are addressed in detail.
24 of 139
4.1 Model Structure
4.1.1 Internal controller & X3D aerodynamics
It has been chosen to use system identification for this part of the model. The method presented
by Friis, Nielsen, Andersen, Bønding, Jochumsen and Sørensen entails the use of a high order
transfer function to model the dynamics of the internal controller and X3D aerodynamics [7].
This approach however renders the system equations more complex and thus increases the work
related to generating the system state space model. Based on the work done by Anders Friss1 a
less comprehensive approach has been chosen. Instead of using as high order transfer function
to model the internal controller and X3D aerodynamics a simple scaling factor Kω will be used.
This approach essentially means that the angular velocities requested from the wireless link are
assumed to be executed in scaled value by the internal controller of the X3D. The assumption
assumes that the internal controller of the X3D is sufficiently fast and precise. This claim is
supported by the measurements illustrated in appendix I.
A series of measurements related to the response of the X3D to various inputs has been per-
formed. Based on the measurements illustrated in appendix I the scaling factor Kω illustrated
in equation 4.1 has been found. The experiments and methods related to the derivation of this
scaling factor are described in appendix I.
   
Kφ 0.0011574
   
Kω =  Kθ  = 0.00092612 (4.1)
Kψ 0.0016476
Based on equation 4.1 the generated angular velocities relative to the reference input Sω form the
wireless link can be calculated. The dynamic system equations assumes the structure illustrated
in equation 4.3.
B
ω = Sω · Kω (4.2)
B B B
ωφ = Sφ · 0.0011574 and ωθ = Sθ · 0.00092612 and ωψ = Sψ · 0.0016476 (4.3)
Given are thus the dynamic system equations related to the angular velocity of the X3D.
4.1.2 X3D thrust generation
In this section the thrust generated by the X3D is addressed. Prior to addressing the actual gener-
ation of thrust, 4.4 is introduced where E CB is the direction cosine matrix. The equation relates
the angular and translational velocity of the X3D in the body frame to angular and translational
velocity in the earth frame.
E
v = Ṗ = E CB (Θ
Θ)B v and E
Θ = E HB (Θ
ω = Θ̇ Θ )B ω (4.4)
The total force acting on the X3D can be found as a summation of all external forces. The
summation of the forces yields the equation illustrated in figure 4.2 where B F1 - B F4 is the upward
force generated by the X3D rotors respectively and B Fg is the gravitational force. As it can be
1 Master Student 2010 AAU - Intelligent Autonomous Systems
4. Modelling 25 of 139
4.1 Model Structure
seen from appendix C, the force generated by the X3D is dependent of the angular velocity of
the rotors. The main objective of this project is to enable the X3D to fly in hover mode. When
the X3D is is hover the angular velocity of the four rotors are ideally identical resulting in equal
generated force. The force is always directed along the z-body axis and it is thus considered valid
to replace the four individual force contributions B F1 - B F4 with the summarized force B Frotor .
B
F total = B F1 + B F2 + B F3 + B F4 + B Fg (4.5)
B B
= Frotor + Fg (4.6)
The magnitude of the gravitational force Fg in the body frame can be calculated using Newton’s



 

 


Figure 4.2: Illustration of the considered forces which affect the X3D
second law. The equation is illustrated in equation 4.7 where g is the gravitational acceleration
written in vector form illustrated in equation 4.8 and m is the mass of the X3D.
E B
Fg = m · g and Fg = B CE E Fg ⇒ B
Fg = B CE · m · g (4.7)
' (T
g= 0 0 g (4.8)
Using Newton’s second law it is possible to derive an equation 4.9 for the translational accelera-
tion E v̇ as seen from the earth frame, [6, page 117 ].
EF
E total
F = m·a ⇒ Ftotal = m · E v̇ ⇒ = E v̇ (4.9)
m
The translational velocity E v of the X3D as seen from the earth frame can be written as is in
equation 4.10 where B v is the translational velocity of the X3D in the body frame and E CB (Θ
Θ) is
the transformation matrix from the body frame to the earth frame. The translational acceleration
of the X3D E v̇ as seen from the earth frame can be written as is in equation 4.11.
E
v = E CB (Θ
Θ)B v (4.10)
E E B E B
Θ) v̇ + ĊB (Θ
v̇ = CB (Θ Θ) v (4.11)
26 of 139 4. Modelling
4.1 Model Structure
The derivative of the transformation matrix E CB can be expressed as the cross product illustrated
in equation 4.12, [3, page 24 theorem 4.4 ].
E
Θ) B v = −B CE (Θ
ĊB (Θ Θ)(Bω × B v) ⇒ E
v̇ = E CB (Θ
Θ) B v̇ − B CE (Θ
Θ)(Bω × B v) (4.12)
Equation 4.9 and C.23 are combined yielding the equation illustrated in 4.13. The simplified
equation is illustrated in 4.17.
EF
total
= E CB (Θ
Θ)(Bω × B v) (4.13)
m
EF
E total
Θ)B v̇ =
CB (Θ + B CE (Θ
Θ)(Bω × B v) (4.14)
+E m ,
B
)E B
* B Ftotal B B B
Θ) CB (Θ
CE (Θ Θ) v̇ = CE (Θ Θ) + CE (Θ Θ)( ω × v) (4.15)
m
EF ) *) *
B total
v̇ = + B CE (Θ Θ)B CE (Θ
Θ ) Bω × B v (4.16)
m - ./ 0
I
EF
B total
v̇ = + Bω × B v (4.17)
m
Based on the measurements in appendix H it has been chosen to model the generated force
BF
rotor as a function of the input Sc that is the scalar collective reference Sc from the controller
link. The experiments related to the derivation of the linear function are described in appendix
H. The resulting linear function related to the thrust generated by the X3D rotors is illustrated in
equation 4.18.
 
0
B  
Frotor (Sc ) =  0  (4.18)
−10 3 −6 2 −4
3.2174 · 10 · Sc − 1.9531 · 10 · Sc + 5.4632 · 10 · Sc − 0.6698
Based on equation 4.18 the force generated by the rotor B F rotor relative to the reference input
from the wireless link can be calculated.
However straight forward the use of equation 4.18 also implies that whenever a constant is given
as input to B Frotor (Sc ) the X3D will experience a constant acceleration. This is not the case as
the platform during constant input will not accelerate indefinitely but settle at a constant velocity
after initial acceleration. Based on the work of [7] it has been chosen to add an additional term
to the model, see 4.19. This term emulates the effects of induced inflow through the X3D rotors
during translatory movement along the z-body axis.
B
Fin f low = I f · B vz (4.19)
The induced inflow term will prevent the X3D form accelerating indifferently and thus settling
at a velocity where the B Fin f low is equal B Frotor + B Fg . Notice that I f is a negative scalar. The
introduction of equation 4.18 and the induced inflow term 4.19 results in the dynamic system
4.2 Non-linear Simulation model
equation illustrated in equation 4.21.
B
Ftotal = B Frotor + B Fg + B Fin f low (4.20)
B B B
= Frotor (Sc ) + CE · m · g + I f · vz (4.21)
Induced inflow can be viewed as the total airflow perpendicular to the rotors minus the airflow
generated by the rotation of the rotors. When the X3D is in hover mode the induced inflow will
be zero. When the X3D moves along the z-body axis the term will grow relative to the translatory
velocity. The value of the induced inflow coefficient I f can be estimated by applying steps to the
X3D while flying and sampling the response. This approach has not been used as the manoeuvres
required to perform the experiment are quite demanding in relation to the skill of the operator.
Instead, the induced inflow coefficient I f found by [7] is used. This approach is considered valid
as [7] used an identical X3D Quad-Rotor. While the parameters related to the internal controller
may have been changed the overall design of the X3D has not changed.
In this section the non-linear system equations are implemented in Matlab Simulink in order to
create the non-linear simulation model. The non-linear simulation model is used in the process
of verifying the devised system model. In order to visualize the process the non-linear dynamic
system equations are recapitulated in equation 4.23 to 4.24.
BF
E total
Ftotal = B Frotor (Sc ) + B CE · m · g + I f · B vz and B
v̇ = + Bω × B v (4.22)
m 
Kφ
B  
ω = Sω · Kω and Kω =  Kθ  (4.23)
Kψ
E
v = E CB (Θ
Θ)B v and E
ω = E HB (Θ
Θ)Bω (4.24)
Based on the non-linear dynamic system equations it is possible to derive a non-linear system
simulation model. The process of designing the model is simply to implement equation 4.23
to 4.24 in Matlab Simulink. The resulting non-linear system simulation model is illustrated in
figure 4.3. The transformation matrices used in the model are addressed in detail in appendix J
on page 133. It is noticed that the simulation model operates with a initial state of zero on all
states. This means that when the simulation is initiated the simulated X3D is ”dropped” in free
space at way point [0 0 0] with attitude [0 0 0] and with no angular or translatory velocity. If no
input is given to the model it will fall downwards due to the gravitational force.
4.2.1 Non-linear Simulation model Verification
It is highly desirable to verify that the designed Non-linear Simulation model is representative for
the actual response of the X3D system. This will both improve the group’s understanding of the
model as well as validate the use of the model in relation to designing the system control scheme.
 
 

 
   
    
 
 


  
 
   

 

  
   
  
  

  




Figure 4.3: Illustration of the Simulink implementation of the Non-linear model
The approach of verification chosen in this project requires that a test flight of the X3D is per-
formed. The objective of the test flight is to sample the response of the X3D when it is flown
in normal operation mode, that is in near hovering mode. During the session related to the de-
termination of the Thrust Force H a series of test flights were performed. The guidelines related
to these test flights and the management of the sampled response are addressed in appendix E
on page 98. A total of five test flight where performed and the measurement best fit for use was
found to be flight number three.
The Non-linear Simulation models response to the input related to the test flight is illustrated
in figure 4.4 and 4.5.
From figure 4.4 it can be seen that the response estimated by the model is somewhat repre-
sentative for the measured response of the X3D system. It is noticed that the roll and pitch angles
are drifting initially being quite representative of the measured response of the X3D system but
slowly becoming less and less correct. The estimated angles are drifting away form hovering
state and the X3D model is thus directing the rotor thrust at an angle relative to straight down as
in near hover. This means that there is less downwards force to counter the gravitational force
and that the X3D model is flying sideways instead of hovering. Notice that the estimated roll
and pitch angles are close to 40◦ and 30◦ respectively meaning that the platform is significantly
tilted. Having these results in mind as well as the design of the model with no concept of ground,
the estimated positions illustrated in figure 4.5 are less surprising.
From figure 4.5 it can be seen that when the simulation is initiated, the X3D maintains the initial
position. At approximately 150 [ms] the X3D model begins to fall downwards. Initially this does
not make any sense but looking at the estimated attitude it becomes obvious what is going on.
4.3 Model Linearisation
 




          








          








          

Figure 4.4: Illustration of the estimated attitude of the X3D Θ







          

Figure 4.5: Illustration of the estimated position of the X3D P
The X3D model is significantly tilted and most of the thrust is thus not directed downwards. In-
stead the thrust propels the X3D model sideways in the earth frame while the elevation decreases.
This manoeuvre is equivalent of heavily actuating the roll and pitch simultaneously.
In this chapter the non-linear dynamic system equations derived in chapter 4 are linearised. The
equations are used in the process of designing the system state space model that is to be used
in the process of designing the system control. As the designed control is linear the non-linear
dynamic system equations must be linearised around an specific operating point.
4.3.1 Taylor series approximation
The method used in the process of linearising the non-linear dynamic system equations is Taylor
series approximation. Taylor series approximation consist in representing a given function with a
series of terms calculated from the functions derivatives at a single point known as the operating
point2 To linearise the system equations the first order Taylor approximation is used. This means
that each system variable is substituted with an steady-state value and a small signal gain [see
13, page 60 ]. The Taylor approximation with one variable can be decried as in equation 4.25,
where f(x) is the the function to be approximated at value x and x̄ is the operating point value.
The first order Taylor approximation assumes the structure illustrated in equation 4.26. Notice
that the Lagrange notation is used for the derivative of the function f(x), the zeroth derivative of
f(x) is defined to be f(x) itself and (x − x̄)0 = 0! = 0.
f & (x̄) f && (x̄) f &&& (x̄) f n (x̄)

f (x) ≈ f (x̄) + (x − x̄)1 + (x − x̄)2 + (x − x̄)3 + . . . + (x − x̄)n (4.25)
1! 1 2! 3! n!
1
f (x) ≈ f (x̄) + f (x̄)(x − x̄)11
&
(4.26)
n=1
The linearisation of the non-linear dynamic system equations are performed around the system
normal behaviour operating point. In the case of the X3D the normal behaviour is when the
xy-body plane is parallel to the xy-earth plane equivalent to when the X3D is in perfect hover
and the yaw angle is zero. The system normal behaviour operating point can thus be specified by
setting the system parameters to their constant value associate with the specific operating point.
The system operating point is when the platform is hovering that is when the attitude [ φ θ ψ ]T ≈
[ 0 0 0 ]T . This means that the attitude is maintained close to or equal to [ 0 0 0 ]T . The linearisation
is thus performed about the angles i = 0 + ī | i ε φ, θ, ψ where ī indicates a small angle deviation.
The effect of specifying an operating point on the system parameters are illustrated in equation
4.27.
B B
ω≈0 and v≈0 (4.27)
The first order Taylor series approximation in the operating point entails that the trigonometric
equations can be approximated as illustrated in equation 4.28 to 4.29 in the evaluation point 0.
sin(i) ≈ sin(0) + cos(0) · ī ≈ ī (4.28)

cos(i) ≈ cos(0) + (− sin(0)) · ī ≈ 1
1
tan(i) ≈ tan(0) + · ī ≈ ī (4.29)
(cos(0))2
4.3.2 Rigid Body Dynamics & Kinematics
In this section the non linear dynamic system equations related to the rigid body dynamics and
kinematics are linearised. The non linear dynamic system equations related to the translational
2 If the operating point is zero the Taylor series is called Maclaurin series.
and angular velocity of the X3D are illustrated in equation 4.30.
E
v = E CB (Θ
Θ)B v and E
Θ = E HB (Θ
ω = Θ̇ Θ )B ω (4.30)
The linearisation of the equations in 4.30 are performed using first order Taylor series approx-
imation on the direction cosine matrix E CB (ΘΘ) and the transformation matrix E HB (ΘΘ) respec-
tively. Using the approximations in equation 4.29 the direction cosine matrix E CB (Θ
Θ) assumes
the structure illustrated in equation 4.32.
 
cθcψ sφsθsψ − cφsψ cφsθcψ + sφsψ
E  
Θ) = cθsψ sφsθsψ + cφcψ cφsθsψ − sφcψ
CB (Θ (4.31)
−sθ sφcθ cφcθ
   
1 φθψ − ψ θ + φψ 1 −ψ θ
   
≈  ψ φθψ + 1 θψ − φ ≈  ψ 1 −φ (4.32)
−θ φ 1 −θ φ 1
If the linearised cosine matrix 4.32 is substituted into equation 4.30 the equation for the transla-
tional velocity undertakes the structure illustrated in equation 4.35.
E
v = E CB (Θ
Θ )B v (4.33)
 
1 −ψ θ
 
≈ ψ 1 −φ B v (4.34)
−θ φ 1
   
Bv − ψ · Bv + θ · Bv Bv
x y z x
   
=  ψ · B vx + B vy − φ · B vz  ≈ B vy  = B v (4.35)
−θ · B vx + φ · B vy + B vx Bv
z
The same approach in used in relation to the transformation matrix E HB (Θ

Θ).
     
1 tθsφ tθcφ 1 θφ θ 1 0 θ
E      
Θ ) = 0
HB (Θ cφ −sφ  ≈ 0 1 −φ ≈ 0 1 −φ (4.36)
0 sφ/cθ cφ/cθ 0 φ 1 0 φ 1
If the linearised transformation matrix 4.36 is substituted into equation 4.30 the equation for the
angular velocity undertakes the structure illustrated in equation 4.38.
E
ω =E HB (Θ
Θ )B ω (4.37)
     
1 0 θ Bω + θ · Bω Bω
φ ψ φ
     
≈ 0 1 −φ Bω = B ωθ − φ · B ωψ  ≈  B ωθ  = Bω (4.38)
0 φ 1 Bω + φ · Bω Bω
ψ θ ψ
4.3.3 Thrust generation
In this section the equation from section 4.1 related to the forces affecting X3D will be addressed
and linearised. The equation is linearised using the same method as used in the previous section.
4.4 State Space model
The non-linear dynamic system equation related to the force is illustrated in equation 4.39.
B
Ftotal = B Frotor (Sc ) + B
C ·g·m + I f · B vz (4.39)
- ./ 0 - E./ 0 - ./ 0
Rotor force Gravitational force Induced Inflow
Equation 4.39 consists of a summation of forces and it is thus possible to linearise the compo-
nents individually. The gravitational force B Fg can be expressed as in equation 4.40. Using the
transpose of the linearised direct cosine matrix 4.32 equation for the gravitational force under-
takes the structure illustrated in equation 4.40.
       
0 1 ψ −θ 0 −θ
B B        
Fg = CE · 0 · m = −ψ 1 φ  · 0 · m =  φ  · g · m (4.40)
g θ −φ 1 g 1
Equation 4.41 describes the translational acceleration of the X3D in the body frame B v̇. Using
the equations 4.27 it is possible to rewrite the equation as in equation 4.42.
BF
B total
v̇ = + Bω × B v (4.41)
m
BF BF B B
total rotor + Fg + Fin f low
≈ = (4.42)
m m
When B v̇ is written in vector form, the components B Frotor and B Fin f low are only present in the
z-component of the vector. The collective linearised force equation in vector form undertakes the
structure illustrated in equation 4.43.
   
B v̇ −θ · g
x
B    
v̇ ⇒ B v̇y  =  φ·g  (4.43)
B v̇ Frotor (Sc )+m·g If B
z m + m · vz
The term Frotor (Sc ) + m · g is replaced with the collective force Fz , which describes the collective
force applied to the X3D along the z-body axis. The collective linearised system equations can
thus be written ans in equation 4.44.
B v̇
ẋ = B vx x = −θ · g φ̇ = Sφ · Kφ
B v̇ = φ · g
ẏ = B vy and y and θ̇ = Sθ · Kθ (4.44)
ż = B vz B v̇ = Fz + I f · B v ψ̇ = Sψ · Kψ
z m m z
Given are thus all the equations needed to derive the system state space model. The process of
this will be the main focus of the following section.
In this section the system equations from section 4.3 are converted into state space form. The
state space model is required in order to utilise the desired LQ control strategy. After deriving
the continuous state space model from the linearised system equations the state space model is
discretized using MATLAB.
4.4.1 System States
The states and inputs illustrated in equation 4.45 and 4.46 respectively have been chosen as basis
for the state space model. The vectors in equations 4.45 and 4.46 will be referred to as the state
vector xs and input vector us .
B B B
xs = [ x y z φ θ ψ vx vy vz ]T (4.45)
us = [ Sφ Sθ Fz Sψ ]T (4.46)
Though most of the states have been addressed in previous chapters it is assumed appropriate to
recapitulate their meaning. x, y and z are the position of the X3D’s center of mass Mc . φ, θ and
ψ are the 3-2-1 Euler angles. B vx , B vy and B vz are the translational velocities of the X3D in the x,
y and z direction in the body frame respectively. The vector in equation 4.45 will throughout this
chapter be refereed to as the state vector.
4.4.2 System Equations
The linearised dynamic system equations on which the system state space model are designed
are listed in equations 4.47.
B v̇
ẋ = B vx x = −θ · g φ̇ = Sφ · Kφ
B v̇ = φ · g
ẏ = B vy and y and θ̇ = Sθ · Kθ (4.47)
ż = B vz B v̇ = Fz + I f · B v ψ̇ = Sψ · Kψ
z m m z
4.4.3 Matrix Derivation
The state space model describes the system using differential equations. It consists of two equa-
tions, one related to the states of system and one related to the output parameters of the system.
Notice that x˙s does not refer to the translatory velocity of the platform along the x-axis but a
vector containing the derivative of the states and that A, B, C and D are matrices.
The continues state space model is typically written in the form illustrated in equation 4.48 and
4.49. The model operates using a series of states contained in a state vector xs (t) that are related
to the system input and output through the dynamic system equations. In order to abstract from
the number of states, inputs and outputs, the system variables are expressed as vectors and the
equations are expressed as matrices. In equation 4.48 the equation related to the system output is
illustrated. The output equation contain the system matrices A and the input matrix B.
State derivative System states System input

/0-. /0-. /0-.
xs˙(t) = A
-./0 xs (t) + B
-./0 u(t) (4.48)
System matrix Input matrix
In equation 4.49 the equation related to the system input is illustrated. The output equation
contain the output matrices C and the direct transmission matrix D.
Output matrix Direct transmission term

/0-. /0-.
y(t) = C xs (t) + D u(t) (4.49)
-./0 -./0 -./0
System output System states System input
Using the linearised dynamic system equations it is possible to derive the continues state space
model for the X3D system. By arranging the equations the following A, B and C matrices
are given. Notice that the direct transmission matrix D is the zero matrix as there is no direct
transmission from the system input to the system output.
  
0 0 0 0 0 0 1 0 0 x
0 0 0 0 0 0 0 1 0  y 
  
0 0 0 0 0 0 0 0 1  
  z 
0 0 
 
 0 0 0 0 0 0 0 φ
A xs (t) = 
0 0 0 0 0 0 0 0 0  θ 
 
 (4.50)
0 0  
 0 0 0 0 0 0 0  ψ 
0 0 0 0 0 0 0 0 B 
 −g   vx 
0 0 0 g 0 0 0 0 0 B vy 

If Bv
0 0 0 0 0 0 0 0 m z
 T  
0 0 0 0 0 Kψ 0 0 0 Sφ
0 0 0 0 0 0 0 0 1  Sθ 
B u(t) = 
0
m   (4.51)
0 0 0 Kθ 0 0 0 0   Fz 
0 0 0 Kφ 0 0 0 0 0 Sψ
 
x
  y 
1 0 0 0 0 0 0 0 0  z 

0 1 0 0 0 0 0 0 
0  

 
0 0 1 0 0 0 0 0 0 φ
C xs (t)= 
0
 θ  (4.52)
 0 0 1 0 0 0 0 0  
 ψ 
0 0 0 0 1 0 0 0 0 
B vx 

0 0 0 0 0 1 0 0 0 B 

vy 
Bv
z
Having derived the continues state space model using the linearised dynamic system equations
the model is converted from continues time to discrete time. The process of this discretization is
addressed in section 4.4.4
4.4.4 Discrete State Space model
In this section the continues state space model is discretized. In order for the system control to
be implementable it must be present in discrete time.
In the previous section the continues system was described in the form illustrated in equation
4.53 and 4.54. The equations are the input and output equations respectively.
ẋ = Axs (t) + Bu(t) (4.53)

y = Cxs (t) + Dx(t) (4.54)
A solution to this system can be described as illustrated in equation 4.55 as presented by [see 6,
page 631 ]
!t
A(t−t0 )
xs (t) = e xs (t0 ) + eA(t−τ) Bu(τ) dτ (4.55)
t0
Using equation 4.55 is it possible to obtain the discrete state space representation of the system.
Changing the notation (t = kT + T and t0 = KT) it is possible to rewrite equation 4.55 into equation
4.56.
kT
! +T
AT
xs (kT + T ) = e xs (kT ) + eA(kT +T −τ) Bu(τ) dτ (4.56)
kT
Equation 4.56 is not dependent of the type of hold used as u is specified using its continues time
history u(τ). The discrete model of a continues system can be found using Zero Order Hold
(ZOH) where the input u(τ) is constant throughout the sample interval.
u(τ) = u(kT ) , kT ≤ τ < kT + k (4.57)
To ease the derivation the following notation is introduced.
η = kT + T − τ (4.58)
The use of η enable equation 4.56 to be written i form illustrated in equation 4.59.
 
!T
AT
xs (kT + T ) = e xs (kT ) +  eAη dη Bu(kT ) (4.59)
0
Introducing the notation in equation 4.60 the discrete state space model can be extracted from
equation 4.59.
 
!T
AT
Φ=e and Γ= eAη dη B and H=C (4.60)
0
The discrete state space model undertakes the structure illustrated in equation 4.61 and 4.62
where the discrete version of the D matrix has been omitted.
Γu(k)
xs (k + 1) = Φ xs (k) +Γ (4.61)
y(k) = Hxs (k) (4.62)
4.5 Chapter Summary
The computation of Φ , Γ and H are performed using the dedicated Matlab function c2d(sys,Ts ,zoh).
This function takes the continues system state space model (sys), the sampling time (Ts ) and the
chosen method of sampling (zoh). The found discrete system state space matrices are illustrated
in equation 4.63 to 4.65.
 
1.0000 0 0 0 −0.0005 0 0.0100 0 0
 0 1.0000 0 0.0005 0 0 0 0.0100 0 
 
 0 0 1.0000 0 0 0 0 0 0.0100
 
 0 0 0 1.0000 0 0 0 0 0 
 
Φ=
 0 0 0 0 1.0000 0 0 0 0  (4.63)
 0 0 0 0 0 1.0000 0 0 0 
 
 0 0 0 0 −0.0981 0 1.0000 0 0 
 
 0 0 0 0.0981 0 0 0 1.0000 0 
0 0 0 0 0 0 0 0 0.9914
 
0 −1.5136 · 10−9 0 0
1.8917 · 10−9 0 0 0 
 
 0 0 1.2255 · 10−4 0 
 
1.1574 · 10−5 0 0 0 
 
Γ=
 0 9.2612 · 10−6 0 0 
 (4.64)
 0 0 0 1.6476 · 10 
−5
 
 0 −4.5410 · 10−7 0 0 
 
5.6751 · 10−7 0 0 0 
0 0 2.4476 · 10−2 0
 
1 0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0
 
0 0 1 0 0 0 0 0 0
H=
0
 (4.65)
 0 0 1 0 0 0 0 0
0 0 0 0 1 0 0 0 0
0 0 0 0 0 1 0 0 0
4.5 Chapter Summary
In this chapter the modelling of the X3D has been addressed. The system model has been formu-
lated using both classical modelling techniques as well as system identification. based o the de-
rived non-linear dynamic system equations a non-linear simulation model has been implemented
in Simulink. The a non-linear simulation model has proven to be able to model the response of
the X3D sufficiently. The non-linear dynamic system equations have been linearised around the
system normal operating point, that is close to hover mode. Based on the linearised dynamic
system equations the system state space model has been derived.
The system control is to be based on the system state space model. The process of designing
the system control is the main focus of the following chapter.
Chapter 5
Controller Design
As presented in the Modelling chapter, the quad-rotor system is non-linear and time-variant
in terms of battery power. However, for the project problem of stable hovering and way-point
tracking, the working model is LTI: linearized around the hovering point and considered time-
invariant, as the flight time is limited making it possible to ignore the losses in battery power over
time. The main objectives of this chapter are to present design considerations and implementa-
tion details of the linear optimal controller achieved by minimizing a quadratic performance
function. Specific elements include tracking of step references, and state estimation using pro-
cessing on the Vicon position and attitude data.
All the methods and considerations are discussed for discrete-time.
5.1 Linear State Feedback
The main principle used to control the multiple input-output quad-rotor system is state feedback.
Feedback is a powerful concept, as it can make a system resilient to both external disturbances
and variations in the behavior of the internal parts of the loop, and can create a linear input-output
behavior from non-linear components.
For dynamics described with state models, at each time step, the state vector contains all the
information that is needed to compute the future behavior, allowing the controller to be memo-
ryless, and in the general case a function of the state vector. In state feedback, it is assumed that
all states at each time k are measurable or available from an estimation.
u(k) = F (x(k), k)1 (5.1)
38 of 139
5.1 Linear State Feedback
A controller that is also linear has the form below, where at each time step K(k) is a matrix of
real numbers, making the command a linear combination of the states.
u(k) = −K(k) · x(k); K ∈ Rmxn (5.2)
For time-invariant models, often the designed control is also time-invariant: K(k) = K = ct. The
controller is just a static matrix gain. This is the case of the Linear-Quadratic controller that is
going to be designed in the next sections.
For all practical purposes, the linear feedback also takes into account the reference, and this
aspect is further detailed in section 5.3.4.
u(k + 1) = −K · x(k) + Kr · r(k); K ∈ Rmxn , Kr ∈ Rmxr (5.3)
By creating the feedback loop, the dynamics of the system, governed by matrix A, change from
the open loop (Ad ) to (Ad − Bd · K). An adequate choice for the control matrix K can assign a
desired behavior to the closed loop.

 
  
 


Figure 5.1: OpenLoop
Open loop dynamics:
Γ · u(k)
x(k + 1) = Φ · x(k) +Γ (5.4)
y(k) = Hx(k) (5.5)
Closed loop dynamics:
Φ −Γ
x(k + 1) = (Φ Γ · K) · x(k) +Γ
Γ · Kr · r(k) (5.6)
y(k) = Hx(k) (5.7)
An important assumption is that all the states that define the quad-rotor dynamics are available
for online use in the state feedback controller calculations. This is not the case with real systems,
not all of the states are measured directly with sensors. For the missing states, estimation is used:
filtered measurements are combined with inferences based on the principle of mechanics, or a
1 In real discrete-time systems, there is often a delay of one or more time steps between time of the measured state
and the moment of command. This delay has not been taken into account.
5. Controller Design 39 of 139

5.2 Linear Quadratic methods

 

 
  
 


Figure 5.2: State feedback closed loop with feedback
state-space observer.
5.1.1 Methods for State Feedback
The problem of determining an appropriate control command is the same as determining the ma-
trix K.
For single input systems, it is possible to directly relate the matrix K to the position of the closed
loop eigen values. The method is called pole placement. The main problem is the choice of
closed loop poles locations. Criteria of stability, static and dynamic response requirements, such
as limit offset, rise time, overshoot must be met at a balance with the energy spent on the control
signals.
As discussed in the Optimal Control lecture notes [12], for multiple inputs systems it is not easy
to relate the elements of the control matrix K to the positions of the closed loop poles. There are
more parameters than constraints, and an under-determined system of equations must be solved.
As such, there is no unique solution K for a set of poles, and choosing the optimum K-values is
not trivial. For a large class of multiple input problems, Linear-Quadratic methods are a better
approach.
Another class of solutions that work well for MIMO systems with an LTI approximate model are
H∞ methods, or robust control. These methods are not discussed in the present project.
5.2.1 Optimal Problem
Optimal control deals with the problem of finding the control law u such that a certain optimum
condition is met. The optimum criterion is defined as a cost function H of states and control
40 of 139 5. Controller Design

variables, which must be balanced in a meaningful way to the real system. This formulation is
relevant for a large class of problems, where the goals of control are closely related to quantita-
tive expressions on the system’s inputs, outputs, and other intermediary measured or estimated
parameters of operation.
The general, non-linear, discrete, optimal control problem starts from the system model:
x(k + 1) = G (x(k), u(k), k) (5.8)
A performance function or index, I , is defined as the sum of the performance and cost criterion
H (k) over a time horizon N, starting from t0 = 0. There are two types of problems: finite time
horizon N, and infinite horizon N = ∞.
N−1
I= ∑ H (x(k), u(k), k) + M (x(N)) (5.9)
k=0
∞
I= ∑ H (x(k), u(k), k) (5.10)
k=0
The purpose is to optimize the performance function by manipulating the command u. The opti-
mized performance index will be dependent of the particular system dynamics G and the initial
state x0 . In the case of the finite horizon problem, an additional terminal state cost M is used in
the performance function, to put a specific weight on the state error at the final time step.
5.2.2 Linear optimization with quadratic criterion
The general system G is now assumed to be linear and time invariant.
Γ · u(k);
x(k + 1) = Φ · x(k) +Γ Φ ∈ Rnxn , Γ ∈ Rnxm (5.11)
A particular case is when the performance criterion H takes a quadratic form2 . This is the class
of Linear Quadratic methods.
The performance index has the quadratic sum form below:
N−1
I= ∑ [x(k)T · Q1 · x(k) + u(k)T · Q2 · u(k)] + x(N)T · QN · x(N) (5.12)
k=0
∞
I= ∑ x(k)T · Q1 · x(k) + u(k)T · Q2 · u(k)3 (5.13)
k=0
2A quadratic form is a homogeneous second order polynomial in n variables: pQ (x1 , .., xn ) = ∑ qi j · xi · x j . The
coefficients of the polynomial can be arranged into a nxn symmetric matrix, and the same quadratic form can be
written in the matrix form: pQ (x) = xT · Q · x. Real quadratic forms can always be brought to a diagonal form by a
linear transformation (Jacobi theorem).

Symmetric matrices Q1 and Q2 4 are parameters of the control problem. They can be time-variant,
but this case is beyond the scope of the project and Q1 and Q2 are constant in the following. The
next two restrictions are used in the linear optimization problem. The state weight matrices
Q1 and QN are positive-semidefinite5 , and the actuation weight matrix Q2 is strictly positive defi-
nite6 . The diagonal state weighting matrix can contain factors 0, but the command scaling factors
are always positive.
The advantage is that the resulting controller is a linear function of the states and as such, the
Linear Quadratic optimization yields a linear state feedback solution.
u∗ (k) = −L(k) · x(k); 7 (5.14)
Because of the quadratic form, the optimization is minimization. The solution of the problem,
J = min(I (u)) is dependent of the state dynamics, as defined by constant real matrices (Φ Φ,ΓΓ),
and the initial state x0 . The important result in practical problems is the state-feedback controller
L(k) and the command sequence u∗ (k), while the actual value of the minimum performance in-
dex J (x0 ) is less interesting.
For finite horizon problems, the state feedback gain matrix L(k) is variable with time. In many
practical situations, such as long or undetermined operation-time, a constant gain matrix is pre-
ferred. Receding horizon or the infinite horizon optimizations are two strategies that produce a
time-invariant controller.
The receding horizon strategy is to use a small to medium optimization horizon N, relative to
system dynamics. Matrix weights Q1 and Q2 are chosen to balance the performance and cost of
the dynamic response, while the terminal weight QN puts pressure on the steady state error. At
each time step the issued command is u∗ (k) = −L(0) · x(k), under the assumption that horizon
for control is pushed forward after each time sample. In practical situations, if the horizon time
N is too small, the weights are not well chosen, and the terminal cost is too high, the resulting
optimizing matrix gain can drive the closed loop unstable.
3 The performance function is a real quadratic sum form, in variables x and u

4 Often referred also as Q and R
5 The eigen values of the symmetric matrix Q, or the elements of the equivalent diagonal form, λ >= 0
i
6 The condition on matrix Q is met in the project implementation, but is not necessary for the general linear control
2
problem solution. Q2 can be non-strictly positive definite, and in that case another relaxed condition must be met.
7 Notation u∗ is used for the command sequence that optimizes the performance function. The LQ state feedback
gain is from now on noted as L.

5.2.3 Infinite Horizon optimization
In the project, the infinite horizon controller solution has been used. Infinite time horizon means
that the performance function is an infinite sum of positive terms, due to the quadratic definition.
To minimize the sum, it must converge. The optimum controller sequence u∗ (k) will drive the
performance and cost criterion to zero so as to converge the performance function. Since the
matrices Q1 and Q2 are constant and positive definite, the states x(k) and commands u(k) must
also tend to 0.
∞
J (x0 ) = minu (I (u)) = minu ∑ H (k) = ct =⇒ (5.15)
k=0
lim H (k) = lim (xT (k) · Q1 · x(k) + uT (k) · Q2 · u(k)) = 0, withQ1 = ct, Q2 = ct =⇒ (5.16)
k−>∞ k−>∞
lim x(k) = 0; lim u(k) = 0 (5.17)

k−>∞ k−>∞
The system model must be correctly specified to map to the problem as presented. The hovering
point must fulfill the conditions 5.17. As it will be seen in section 5.3.4, where reference is in-
troduced in the system, only 6 of the states will be weighted in the final problem: position and
attitude. For the position signals, the quantity being weighted in the performance criterion will
be the deviation from the reference: x − rx , y − ry , z − rz , and in this form will fulfill the condition
5.17. Roll, pitch, and yaw states are naturally 0 in the hovering point. The command must also
be zero. The roll, pitch, and yaw commands - which represent angular velocities, are 0 or very
close when hover has been reached. However, the thrust has a constant non-zero value. The
way this problem is approached is that in the linear model, thrust is not system input, but rather
Fz , the resultant vertical force component. The vertical force is 0 in hover point. The controller
computed based on the linear model will output the thrust command in the Fz form, and this is
passed through a non-linear transformation, identified in the thrust measurement modelling, to
the real command range, for the non-linear simulation and the online schema.
The general infinite horizon problem, with a few additional conditions8 , guaranties a unique
command sequence solution to the optimization. Additionally, the optimizing command u∗ (k)
is described by a constant state-feedback gain L(k) = L, and, can be proved, is stable in closed
loop. As such, a single matrix can be computed offline and used for an infinite time horizon of
control, to bring the system from an arbitrary initial state to the minimum cost state space point
of the linear system, 0, against point disturbances.
The time-invariant feedback controller obtained as a result of an infinite horizon optimization

problem is the point of interest in the remainder of the chapter.
8 While not necessary, the following conditions are sufficient: (Φ, Γ) controllable and Q > 0, and (Φ, Q ) observ-
2 2
able. More relaxed criteria exits.

5.3 LQ algorithm
5.3 LQ algorithm
The approach used to find the state feedback gain is a finite-time horizon algorithm that is based
on the optimality principle and backwards induction as described in Dynamic Programming sec-
tion, 5.3.1. The iterations are run with a variable number of steps, until an approximate conver-
gence of the solution is reached.
This section explains the different steps used in the final algorithm, and contains a pseudocode
listing that has been used in the project. The method is based on the course and notes of Opti-
mal Control [12] from this semester, and the description of the Standard Regulator problem for
discrete systems in [2]. Further, the system changes required to introduce the reference point
and guidelines for choosing the weight matrices Q1 and Q2 , parameters of the optimization, are
addressed.
5.3.1 Dynamic Programming
The main idea of this section is the fact that when the controller is optimal in an interval [0;N],
it will be optimal in an interval [k;N] with 0 ≤ k ≤ N. The performance function was introduced
as:
N
I0N = ∑ H (x(k), u(k)) (5.18)
k=0
But the performance only depends on the initial state vector and on the command signal so it can
be written that:
I0N = I0N (x(0), u(0), u(1), ..., u(N)) (5.19)
The other state vectors x(k) are determined by iteration thanks to x(k-1) and u(k-1). Another
notation for the minimum of the performance function will also be introduced:
I0N (x(0)) = minu(0),...u(N) I0N (x(0), u(0), u(1), ..., u(N)) (5.20)
According to the dynamic programming strategy stated in the beginning of the section, the per-
formance function in an interval [k,N] is stated:
N
IkN = ∑ H (x(l), u(l)) (5.21)
l=k
= IkN (x(k), u(k), u(k + 1), ..., u(N)) (5.22)

5.3 LQ algorithm
The next step is to find the minimum of the performance function in this interval.
IkN (x(k)) = minu(k),...u(N) IkN (x(k), u(k), u(k + 1), ..., u(N))
N
= minu(k),...u(N) ∑ H (x(l), u(l))
l=k
N
= minu(k),...u(N) [H (x(k), u(k)) + ∑ H (x(l), u(l))]
l=k+1
N
= minu(k) [H (x(k), u(k)) + minu(k+1),...u(N) ∑ H (x(l), u(l))]
l=k+1
N
= minu(k) [H (x(k), u(k)) + Ik+1 (x(k + 1))] (5.23)
One can see that having the performance function in an interval [k,N], the performance function
in [k-1,N] can be calculated. So to have the performance function in the interval [0,N], each
term will be calculated going backwards from INN (x(N)) to I0N (x(0)) according to the following
algorithm.
• INN = H (x(N), u(N))

u(N) is often set to 0 since it does not influence any state vector and any nonzero value
would increase the performance function.
N (x(N − 1)) = min

• IN−1 N
u(N−1) [H (x(N − 1), u(N − 1)) + IN (x(N))] can be written according
to the equation 5.23 and a minimized command signal u∗ (N − 1) is obtained.
• Step after step we can iteratively calculate the performance function and the minimized
command signal u∗ (N − i).
N (x(N − i)) = min
IN−i N
u(N−i) [H (x(N − i), u(N − i)) + IN−i+1 (x(N − i + 1))]
• Finally the performance function in the interval [0;N] will be obtained.

I0N (x(0)) = minu(0) [H (x(0), u(0)) + I1N (x(1))]
Now, the whole command signal from u∗ (N) to u∗ (0) has been calculated.
Thus with this algorithm, if the initial state is known, it is possible to compute the command
signal u*(0),...u*(N) to bring the plant from an initial state to 0. This method can be applied in
an open loop system but it is not very reliable because of the disturbances and the uncertainties
that can occur in the system. A closed loop control would is preferable, the computation of the
feedback matrix will be addressed in the next sections.
5.3.2 Matrix Calculus
In this section, the specific expression of the LTI system and that of the quadratic performance
criterion will be replaced in the dynamic programming iterations, and explicit expressions for the
LQ problem will be derived.

5.3 LQ algorithm
The quadratic performance index 5.12 is rewritten below with the uniform terminal cost QN = Q1 :
N
I= ∑ [x(k)T Q1 x(k) + u(k)T Q2 u(k)] (5.24)
k=0
• At step N:
uN = 0; JNN (xN ) = xNT Q1 xN (5.25)
• At step N − 1:
N N
JN−1 (xN−1 ) = minuN−1 IN−1 (uN−1 , uN = 0, xN−1 )
T
= minuN−1 (xN−1 Q1 xN−1 + uTN−1 Q2 uN−1 + xNT Q1 xN )
= minuN−1 (ct(uN−1 ) + uTN−1 Q2 uN−1 + (Φ xN−1 + Γ uN−1 )T Q1 (Φ xN−1 + Γ uN−1 ))
(5.26)
To minimize the above expression the constant term of uN−1 can be ignored. Because
of the quadratic form, the local stationary point obtained by solving the equation 5.27, is
guaranteed to be the global minimum.
N
∂IN−1
(u∗ ) = 0 (5.27)
∂uN−1 N−1
To easily perform the multivariable equations derivation, rules of matrix algebra and matrix
calculus will be used, and symmetry of Q1 and Q2 will be taken into account.
(A B)T = BT AT (5.28)
Table 5.1: Matrix derivation
∂y
y ∂x
Ax AT
xT A A
xT x 2x
xT A x A x + AT x
The following relation yields the optimizing command u∗N−1 :

5.3 LQ algorithm
N
∂IN−1 ∂
= (uT Q2 uN−1 + (Φ xN−1 + Γ uN−1 )T Q1 (Φ xN−1 + Γ uN−1 ))
∂uN−1 ∂uN−1 N−1
= 2 Q2 uN−1 + 2 ΓT Q1 Φ xN−1 + 2 ΓT Q1 Γ uN−1
= (Q2 + ΓT Q1 Γ)uN−1 + ΓT Q1 Φ xN−1 = 0
u∗N−1 = −(Q2 + ΓT Q1 Γ)−1 ΓT Q1 Φ xN−1 (5.29)
A condition for existence of the solution is that matrix (Q2 + ΓT Q1 Γ) is non-singular. This
case is covered by the conditions mentioned in 5.2.3.
Performing the calculations for the next iterations will suggest the following notations in
5.29:
u∗N−1 = −LN−1 · xN−1 (5.30)

T T
LN−1 = (Q2 + Γ SN Γ) −1
Γ SN Φ (5.31)
SN = Q1 (5.32)
Again, term (Q2 + ΓT SN Γ) must be invertible, and this is satisfied in the conditions from
5.2.3.
By using the expression of u∗N−1 , the minimized partial performance index could also be
calculated:
N N
JN−1 (xN−1 ) = IN−1 (u∗N−1 , uN = 0, xN−1 )
• By computing the iterations, it can be seen that the matrices Lk and Sk are in the following
form. At step N − i = k:
u∗k = −Lk · xk (5.33)

Lk = (Q2 + ΓT Sk+1 Γ)−1 ΓT Sk+1 Φ (5.34)
T
Sk = Q1 + Φ Sk+1 (Φ − Γ Lk ) (5.35)
JkN (xk ) = IkN (u∗k , u∗k+1 , ..., uN = 0, xk )
= xkT Sk xk
We notice that u(k) is already in a convenient state feedback form, due to the recursion.
• At the final iteration, the minimized performance index yields:
J0N = x0T S0 x0 (5.36)
It can be noticed that relation 5.35 is a complicated recursion, with the boundary for the recursion
that has been taken SN = QN = Q1 . Since Lk also depends on Sk+1 , the Sk recursion can be
rewritten explicitly as:

5.3 LQ algorithm
Sk = Q1 + ΦT Sk+1 [Φ − Γ (Q2 + ΓT Sk+1 Γ)−1 ΓT Sk+1 Φ]

= Q1 + ΦT Sk+1 Φ − ΦT Sk+1 Γ (Q2 + ΓT Sk+1 Γ)−1 ΓT Sk+1 Φ (5.37)
Equation 5.37 is known as a discrete differential equation, that is related to the continuous-time
Riccati differential equation, and in the general case, it is difficult to solve.
The recursive form of Sk and the Lk expression as derived in equations 5.34 and 5.35 are enough
for numerical computations. The sequence of commands uk = −Lk xk can be determined for a
finite time-horizon N.
Convergence over Infinite Horizon
When the horizon N− > ∞, Lk and Sk become time-invariant [12]. An argument comes from
analyzing the form of the performance index.
As mentioned in section 5.2.3 in relations 5.17, the expression of the system states and commands
used in the performance criterion are chosen so as to converge to 0 over time, so as to yield a
performance function that is asymptotically constant when minimizing.
J0N+1 = x0T S0 x0 = ct, N− > ∞

J0N = x0T S1 x0 = J0N+1 =⇒
S1 = S0 (5.38)
The recursive relation 5.37 still applies, and it can be shown that matrix S is constant.
S0 = S1 = Q1 + ΦT S1 Φ − ΦT S1 Γ (Q2 + ΓT S1 Γ)−1 ΓT S1 Φ
S1 = Q1 + ΦT S2 Φ − ΦT S2 Γ (Q2 + ΓT S2 Γ)−1 ΓT S2 Φ
=⇒ S2 = S1 = S0 (5.39)
=⇒ Sk = Sk−1 = ... = S1 = S0 (5.40)
Algebraic Riccati Equation
The equation derived above is the Algebraic Riccati equation.
S = Q1 + ΦT S [Φ − Γ (Q2 + ΓT SΓ)−1 ΓT SΦ] (5.41)

T T T −1 T
= Q1 + Φ S Φ − Φ S Γ (Q2 + Γ S Γ) Γ S Φ
Solving it yields matrix constant S, and subsequently, the constant gain matrix L. The expression

5.3 LQ algorithm
for the command feedback gain:
L = (Q2 + ΓT S Γ)−1 ΓT S Φ (5.42)
In the case of time-invariant system and weights, an analytical solution with matrix exponential
expressions can be found, but it is easier to use the dynamic programming with a large number
of steps until the values of Lk and Sk become stationary within a desired quality range.
5.3.3 Pseudocode Algorithm
With the notations before, the following is a listing for determining the constant feedback gain
by solving testing convergence of the finite-horizon problem.
L=0;
S=0;
do
L=(Q2+B.’*S*B)^(-1)*B.’*S*A;
S_next=Q1+A.’*S*(A-B*L);
conv=S-S_next;
S=S_next;
while (norm(conv)>10e-10);
Notice that in MATLAB environment, there are functions to compute the LQ algorithm and give
the L(0) feedback matrix. Different functions are respectively applied in continuous time and in
discrete time :
lqr and dlqr
But with the introduction of references, these functions do not work anymore so they were not
used in the controller implementation.
5.3.4 Tracking reference
The previous sections were focusing on bringing the system from an initial state to the origin in
the state space meaning to 0. But usually, control problems deal with making the output track a
reference signal and be as close to it as possible with a minimal input signal.
The reference signal r(t) is modeled in the usual way of a state space system with:
x(k + 1) = φr x(k) (5.43)

r(k) = Hr x(k) (5.44)
The initial state x(0) should be known. The resulting system would thus be modeled as showed
in figure 5.3. In the context of the project, the reference will be a step which corresponds to a

5.3 LQ algorithm
 
  

Figure 5.3: Block diagram of an autonomous state space model
constant vector K designed as:
x(k + 1) = x(k) x(0) = K (5.45)

r(k) = x(k) (5.46)
This means that the matrices have the following form:
φr = I (5.47)
Hr = [1..1] (5.48)
In this section, only the implementation of a constant reference will be described. However, there
are ways to implement other reference functions such as ramps or acceleration. These functions
are described in the notes of the optimal course [see 12, page 29].
The system state space model is described as:
xs (k + 1) = φs xs (k) + Γs u(k) (5.49)

ys (k) = Hs xs (k) (5.50)
The system state vector will be augmented by the reference state vector and an error signal can
be introduced. The full system will thus be modeled as:
6 7 6 76 7 6 7
xs (k + 1) φs 0 xs (k) Γs
= + (5.51)
xr (k + 1) 0 φr xr (k) 0
6 7
' ( x (k)
s
e(k) = r(k) − y(k) = −Hs Hr (5.52)
xr (k)
After simplification, the system can be described as
x(k + 1) = Φx(k) + Γu(k) (5.53)

e(k) = Hx(k) (5.54)

5.3 LQ algorithm
The control error is introduced in the performance function, so the function that will be mini-
mized to obtain optimal control is:
N−1
I= ∑ (eT (k)Q1e e(k) + uT (k)Q2 u(k)) + eT (N)QNe e(N) (5.55)
k=0
where Q1e is the weight matrix punishing the error signal. This function can be simplified as :
N−1
I= ∑ ((Hx(k))T Q1e (Hx(k)) + uT (k)Q2 u(k)) + (Hx(N))T QNe (Hx(N)) (5.56)
k=0
N−1
I= ∑ (xT (k)H T Q1e Hx(k) + uT (k)Q2 u(k)) + xT (N)H T QNe Hx(N) (5.57)
k=0
N−1
I= ∑ (xT (k)Q1 x(k) + uT (k)Q2 u(k)) + xT (N)QN x(N) (5.58)
k=0
(5.59)
The weight matrices Q1 and QN have been introduced here.
Q1 = H T Q1e H (5.60)
' (T ' (
= −Hs Hr Q1e −Hs Hr (5.61)
6 7
HsT Q1e Hs −HsT Q1e Hr
= (5.62)
−HrT Q1e Hs HrT Q1e Hr
(5.63)
6 7
HsT QNe Hs −HsT QNe Hr
QN = (5.64)
−HrT QNe Hs HrT QNe Hr
(5.65)
The performance function is thus put in the same form than in the previous sections with the
weight matrices Q1 and Q2 and the LQ algorithm described before can be computed to obtain the
matrices L(k) and S(k). Therefore, the optimal control feedback L(0) can beW obtained and the
input can be calculated.
u(k) = −L(0)x(k) (5.66)

6 7
' ( x (k)
s
= − Ls Lr (5.67)
xr (k)
= −Ls xs (k) − Lr xr (k) (5.68)
The general block diagram of the system can be represented as in [see 12, page 34 ] but in case
the reference is a step the block diagram is represented as in figure 5.4. One can see on the
figure 5.4 that Ls (0) and Lr (0) are two different blocks in the diagram. Indeed, the Ls matrix
corresponds to the feedback of the system without the introduction of a reference. This fact is
proved by a demonstration in [see 12, page 34] but will not be in this section since we do not use

5.3 LQ algorithm
Figure 5.4: Block diagram of optimal control system with a constant reference
it.
Using this method of introducing the reference states into the state vector
5.3.5 Choosing the Weight matrices
As it was seen in the previous sections, optimal control was obtained by minimizing the per-
formance function. This function uses the matrices Q1 and Q2 as parameters in order to have
large/small control signals or large/small states. These two matrices are called weight matrices
and have to be chosen carefully to have a good control design. These matrices if they are well-
chosen can allow the use of some elements more than others in the u(k) vector and they can also
allow more deviation in some of the states.
The weight matrices are quadratic and symmetric, Q1 and QN are positive semidefinite (n x n)
matrices and Q2 is a positive definite (m x m) matrix. It can be said that Q1 influences the current
state, QN the final state and Q2 the input signal.
After the introduction of reference, the Q1 matrix is based on the choice of the error weighting
matrix Q1 e, which is squared sized of the output pxp. The matrices Q1e and Q2 allow to have
p2 + m2 parameters that is a lot to think of. However, by limiting the matrices to diagonal ma-
trices, the parameters that have to be determined can be limited to p + m. Thus, the diagonal
parameters Q1e (i, i) and Q2 ( j, j) respectively weight on ei (k) = ri − yi (k) and u j (k). In fact, if
the control signal u j (k) is too large, Q2 ( j, j) has to be increased and if the output error ei (k) is
too big, Q1e (i, i) has to be increased.
The problem of choosing the appropriate weights for a given control problem is up to the system’s
engineer, and depends on the knowledge of the system’s dynamics. Two guideline rules can be
used: the state-command scaling factor and Bryson’s rule.
First problem to choosing appropriate values for Q1e and Q2 is to find a good scaling factor
between the two. If Q1e = Ip and Q2 = ρ · Im , ρ is the scaling factor.
To scale between different units and ranges that are used for the state, and respectively the com-
mand, Bryson’s rule states suggests that acceptable values for e(k) and u(k) would be to choose

5.4 LQ implementation
the diagonal matrices Q1e and Q2 as:
1
Q1e (i, i) = (5.69)
e2i,max
1
Q2 ( j, j) = (5.70)
u2j,max
where ei,max is the maximum acceptable value for ei (k) and u j,max is the maximum acceptable
value for u j (k), in the corresponding measuring units.
Different values can be tested by trying out the step responses in the linear and non-linear sim-
ulations. After the initial choice, the matrices will be further tunned modified to obtain a good
balance between performance and control effort on the real system.
All the work, offline calculations, simulations and the online controller have been done in MAT-
LAB and Simulink, using additional MTLab interface software to Vicon and the quad-rotor, as
described in 2.4. In the project CD, scripts, models, and simulations can be found. The fre-
quency of the control loop is 100Hz and limited by the Vicon signal rate. The quad-rotor can
theoretically receive command up to 200Hz, see [14] and MATLAB Simulink can also run at this
time-step. Notice that an important parameter is the sampling rate to transform the continuous
system into a discrete one. This sampling frequency has to be the same than the one at which the
controller calculates and send the command.
5.4.1 Linear model simulations
The LQ algorithm runs offline on the discrete linear model (Φ, Γ), derived in chapter 4, and
produces the feedback law L. A few trial and error iterations were run with different weight
matrices to find a good balance for the ρ factor, so that in the linear simulation the values for the
(roll, pitch, thrust, yaw) command are within the practical bounds [−2045, +2045] and [0, 4096].
Step responses, with the final weights that have been tunned in the flying sessions, are shown
below.
Figure 5.5: Step on the X axis

Figure 5.6: Step on the Y axis
Figure 5.7: Step on the Z axis, up
Figure 5.8: Step on the Z axis, down
Figure 5.9: Step of Yaw angle, to align
There is an approximate 10% overshoot on the X and Y position response, and significantly lower,

approximately 2% for the Z-axis control. The quality of the responses is entirely dependent on
the linear controller and the weight matrices, and a better tunning using methods of loopshaping
can be considered for future work.
5.4.2 Non-Linear model simulations
The non-linear simulations graphs are very similar to the linear ones, because the steps are per-
formed around the hovering linearization point. On the X and Y axis steps it can be observed that
the pitch and roll slightly affect the height position, which is to be expected, since some of the
vertical thrust is lost when tilting. Only these graphs are shown below.
Figure 5.10: Step on the X axis
Figure 5.11: Step on the Y axis
5.4.3 State estimation
State feedback requires availability of all the nine system states at each time step for the calcula-
tions of the control law. Only six states are provided by the Vicon sensors, the position and the
attitude. The last three states, the translational velocities, must be estimated from the measured
data.
For this step, a pre-filtering and derivative state reconstructor have been used. The implementa-
tion performed well during flights.

Figure 5.12: State Estimation
The Vicon Transform block changes the signs of the signal data to match the orientation described
in Modelling chapter 4.
The Signal Filter is aimed at correcting three common Vicon errors: dead-zone, spikes, and
noise. The block shown in figure 5.13 below contains a dead-zone detector and a basic correction
algorithm with a zero-order hold of and one size buffer that are implemented in a separate m-file.
The spikes in the signal are reduced using rate limiter block. The low-pass filter has a 10Hz pass
band.
Figure 5.13: Signal Filter
The State Generator block performs the derivation of the position signals, and using the measured
attitude of the quad-rotor, and the reference transformation, to estimate the translational speeds
from the Earth frame to body frame.
Figure 5.14: State Generator
Because the number of estimated states is not high, and the Vicon precision is very good, in

nominal working conditions, the schema performs well.

As future work, another solution that can be employed and that should yield better results, is to
design a deterministic observer, or a stochastic Kalman filter, see [12].
5.4.4 Online controller
The Simulink model for the online controller is presented below.
Figure 5.15: Simulink controller
As reference, the rounded values of the weight matrices and the feedback and reference gains are
shown below.
 
ex ey ez eroll e pitch eyaw
 
2.5 0 0 0 0 0 
 
 0 2.5 0 0 0 0 
 

Q1 e =  0 0 0.5 0 0 0  
0 0 0 0 0 0 
 
 
0 0 0 0 0 0 
0 0 0 0 0 82.054

5.5 Chapter Summary
 
roll pitch Fz yaw
2.5e − 005 0 0 0 
 
 
Q2 =  0 2.5e − 005 0 0 
 
 0 0 0.73463 0 
0 0 0 2.5e − 005
 
x y z roll pitch yaw vx vy vz
 roll 0 −311.42 0 −2625.6 0 0 0 −408.36 0 
−Ls = (
 pitch 311.76 0 0 0 −3048 0 440.22 0 0 )

Fz 0 0 −0.81953 0 0 0 0 0 −0.53911
yaw 0 0 0 0 0 −1784.8 0 0 0
 
x y z roll pitch yaw
 roll 0 311.42 0 0 0 0 
 
 
−Lr =  pitch −311.76 0 0 0 0 0 
 
 Fz 0 0 0.81953 0 0 0 
yaw0 0 0 0 0 1784.8
5.5 Chapter Summary
In this chapter, the control strategy of designing a way-point controller has been developed and
implemented using a constant state feedback gain, calculated with a convergent LQ method for
infinite horizon that guarantees stability. Full state feedback was achieved using signal filter-
ing and state reconstruction, and the control scheme was implemented in MATLAB Simulink
for real-time deployment at 100Hz. As a result, the X3D was able to hoover and perform short
movements, supervised by a human operator in stand-by.
In the next section, elements of fault detection design will be presented.

Chapter 6
Fault Detection
In this chapter the concept of fault tolerant control (FTC) in relation to the project is addressed.
The objective of the implemented fault detection scheme is to render the X3D system tolerant to
system faults. Fault tolerant means that the system is able to maintain performance close to de-
sirable performance and preserve stability even in the presence of actuator and or sensor faults.
In most physical systems there is a possibility of faults developing in the system. Faults can
be viewed as a difference between the measured and the expected behaviour of the system.
Knowledge about the expected behaviour of the system requires deep process knowledge and
a reasonably accurate system model. Most faults, even minor, will effect the performance of the
system and it is thus desirable to both detect, isolate and accommodate the faults as they occur.
In a well-designed fault tolerant system, faults are accommodated with or without performance
degradation and the system insures of that no faults will develop into system failures1 in the sys-
tem or subsystem.
In order for a fault to be accommodated it must be detected and isolated. In this project two
methods of fault detection are addressed and designed. The first method relates to the sensor
fault generated by the Vicon tracking system. In this context sensor faults are distinguished from
measurement noise. The Vicon tracking system generates both sensor faults and measurement
noise. The sensor faults are detected and isolated through a Multi-sensor fault detection scheme.
For the Vicon the scheme evaluates the validity of the measurements data. The considerations
regarding the design and the approach used in relation to the implementation of the Vicon mea-
surement fault detection scheme are addressed in sections 2.3.4 on page 12 and 5.4.3 on page 55.
The noise in the measurements is to be removed using on-line filtering or filtering trough a sys-
tem observer.
The second fault detection method addressed relates to the faults generated in the X3D Quad-
Rotor. The method used for fault detection is based on a system observer which enables the
detection and isolation of faults. The general principle is to monitor changes in the system state.
This is accomplished using the available measurements and system model. Based on these it is
1 Permanent interruption of the systems ability to perform a required function under specific operating conditions.
59 of 139
6.1 Component analysis
possible to generate a residual vector r(t) which contains information of system changes due to
faults, such as changes in magnitude. The residuals will ideally, when the system is operated
normally, be close to zero. Whenever a faulty condition occurs, one or more elements of the
residual vector will become non zero. To detect faults the relevant residual element is compared
to a threshold value. When the threshold is exceeded the fault is detected and corrective actions
can be initiated.
6.1 Component analysis
In this section the components of the X3D system are described. The objective of the Component
analysis is to identify the components of the X3D system and the possible faults related to them.
Notice that the Vicon Motion Tracking system, wireless link, Interface and Control PC are not
considered in the components analysis. The motivation for disregarding the wireless link and the
Interface and Control PC is that no evident means of accommodating faults in these components
exist. Some minor errors may be possible to detect and handle but if a fault occurs in either the
wireless link or the Interface and Control PC system failure is generally unavoidable . In figure
6.1 the main components of the X3D are illustrated. Based on figure 6.1 it is possible to identify




 



Figure 6.1: The components of the X3D
several of the components of the X3D system. The components that will be taken into account in
the further analysis are illustrated in figure 6.2. Notice that the Internal Controller, Internal Sen-
sors, DC-motor Driver and CMS are not taken into account in the further analysis. The grounds
for this decision is that the group’s knowledge about these four blocks are insufficient. Any anal-
ysis including the four blocks will thus be based purely on assumptions. The only place where
complete knowledge about the component input exist is in relation to the DC-motor block. At
this point it is known that the input is DC voltage. This input may contain certain faults but their
origin in the preceding system blocks are not known. In the following it will be assumed that a
total of four faults in the input signal to the DC-motor block is possible dur to each one of the
rotors. The generation and migration of these faults in the preceding system blocks will not be
addressed.
Having disregarded multiple of the system blocks it is evident that the only blocks left are the
DC-motor and Rotor block. In the following an opening Failure mode and Effects analysis for
60 of 139 6. Fault Detection

6.2 Failure mode & Effects analysis
      


  

  
 
  




Figure 6.2: The main components of the X3D
these two blocks will be implemented. The objective of this analysis is not to provide a conclu-
sive list of all the possible fault modes and effects but to investigate some of the most evident
faults modes and their effects. In figure 6.3 the X3D’s DC-motor fitting and Rotor are illustrated.
The fitting is the piece of the motor axle on which the rotor is mounted.

 

Figure 6.3: Picture of the X3D DC-motor, Driver, fitting and Rotor
To identify the possible Failure modes of the system a Failure mode and Effects analysis is im-
plemented. The final objective of the analysis is to identify faults that must be detected and the
appropriate accommodation of these faults.
In order to isolate any particular fault, some hypothesis regarding the observable effects from
the fault must be present. Obviously this requires a deep knowledge about the dynamic char-
acteristics of the system as well as the fault modes that may occur in the system. However no
method can guarantee a complete description of all possible fault modes of a system [4, page 18
]. In this analysis it has been chosen to focus on the DC-motor and Rotor configuration in general
not performing the analysis for all four motors and matching rotors. The effects caused by the
various fault modes only differ slightly and it is assumed acceptable only to address the possible
fault modes and effects in the general case. Thus the considered block are the DC-motor with the
DC voltage as input and the fitting of the rotor as output and the Rotor block with the fitting as
input and the thrust as output.
6. Fault Detection 61 of 139

In tables 6.1 and 6.2 the different fault modes and effects related to the DC-motor and the Rotor
are illustrated respectively.
Effect ⇒ Angular speed reduced Angular speed Increased No Angular speed

Fault ⇓
Input Fault Low voltage High voltage No voltage
Wire loose Driver fault Wire broken
Output Fault Rotor deformed Rotor broken off Rotor obstructed
Fitting worn Fitting loose Fitting broke
Component Fault Motor overheated Motor broken
Table 6.1: Fault and Effect analysis related to the DC-motor
Effect ⇒ Thrust reduced Thrust Increased No Thrust

Fault ⇓
Input Fault Low Angular speed High Angular speed No Angular speed
Fitting worn Fitting broke
Output Fault Turbulence Ground effect
Component Fault Rotor deformed Rotor deformed

Rotor missing
Table 6.2: Fault and Effect analysis related to the Rotor
Notice that some crude assumptions about the behaviour of the internal controller are made as it
is assumed to behave quite moderately. The group has no knowledge about the behaviour of the
internal controller during actuator fault. The internal controller may try to compensate for the
lack in thrust on the affected rotor using the other rotors but no conclusive knowledge is present.
Based on table 6.1 and 6.2 it is possible to identify the potential failure modes that have the
highest effect on the system. Failure modes are errors in the collective system that effect the
overall performance of the system. All faults modes will in term result in two measurable effects
meaning changes in the position and attitude. In fact, whenever there is a fault in the generation
of thrust on any of the four rotors it will effect the attitude of the X3D. In table 6.4 the collective
effects of decreased, increased and lack of thrust are illustrated. The increase in a parameter rela-
tive to the expected value is illustrated in red marking, the decrease in the parameter in green, the
absence of parameter in blue and no effect as blanks. Notice that it is assumed that only one fault
occurs at a time. Multiple faults in combination with each other are thus not taken into account.
Even though confusing, Table 6.4 actually provides information regarding the measurable ef-
fects of faults related to the generation of thrust. It is noticed that a decrease in thrust and the
complete absence of thrust will result in the same effects. Based on this observation it is not

6.3 Fault Detection & Isolation
    

 






Figure 6.4: Effects of the faults related to Thrust generation
possible to differentiate between these two faults. In relation to the severity of the different fault
modes the most critical fault is considered to be the loss of thrust on either one of the X3D motors
meaning that one of the motors stops. If the thrust is lost the internal controller has no means of
supplying the reference values requested by the wireless link and the system will become utterly
unstable. As little knowledge is known about the internal controller’s behaviour when one or
more actuators are subjected to fault no reliable means of recovery can be implemented. The
obvious accommodation in relation to actuator faults would be to switch to a controller capa-
ble of landing the X3D using only a limited number of actuators. As the scope of this project
is limited the accommodation to identified and isolated faults will be switching to a dedicated
controller. The development of this dedicated controller will not be addressed beyond guiding
considerations.
Based on the project application and table 6.4 it has been chosen to focus on the detection of
loss on thrust on rotor number 1. As the X3D in it’s final configuration will fly with the front
of the frame ( rotor 1 ) directed in the direction of flight. Rotor1 will thus be especially exposed
to faults caused by physical damage to the rotor. Faults developed internally in the platform that
may migrate to any of the three other motors are not taken into account. As the X3D is flown
indoors and in a confined space the danger of flying into an obstacle and thus damaging rotor1.
The X3D has been flown for 2 years in the MTlab AAU without experiencing faults that can
be identified to originate from within the system. All faults experienced can be identified to be
caused by external influence, - Walls, wind, controller faults etc.
The process of detecting faults in rotor1 is addressed in the following section 6.3.
Using a model-based approach for Fault Detection & Isolation (FDI) requires that the input and
output of the monitored system are available and that the dynamic characteristics of the system
are known with a reasonable degree of precision. The precision of the system model is vital as
the information about the system faults, contained in the residuals, are highly dependent on the
system model. In this project a reasonably accurate model of the X3D has been derived in 4 on
page 24.

6.3.1 Unknown Input Observer
In order to generate the system residual r an Unknown Input Observer (UIO) is used. The gen-
eral thought behind UIO, is that the states affected by the disturbance are decoupled from the
unknown input. By defining the disturbance as an unknown input the effects of the faults do not
need to be modelled, and it can change freely without effecting the model. An UIO is defined by
the following statement:
An observer is defined as an unknown input observer for the system described by equation 6.1,
if its state estimation error e(t) approaches zero asymptotically, regardless of the presence of the
unknown input (disturbance) in the system [10].
ẋs (t) = Axs (t) + Bu(t) + Ed(t)

(6.1)
y(t) = Cxs (t)
Where xs (t) ∈ Rn is the state vector, y(t) ∈ Rm is the output vector, u(t) ∈ Rr is the known input
vector and d(t) ∈ Rq is the disturbance vector. A, B, C and E are the system equations derived
in previous sections.
A full-order UIO can be described as illustrated in equation 6.2.
ż(t) = Fz(t) + TBu(t) + Ky(t)

(6.2)
x̂s (t) = z(t) + Hy(t)
where z(t) is the state if the UIO, x̂s (t) is the estimate of the state vector xs (t) and F, T, H and
K are the matrices of the UIO. The structure of the UIO is illustrated in figure 6.5. Applying the


 

 
 

 
   

 

Figure 6.5: Conceptual structure of an UIO [4]
UIO illustrated in figure 6.5 to the system illustrated in equation 6.1 the estimation error e(t) = xs
- x̂s will be governed by the structure illustrated in equation 6.3.
ė(t) = [A − HCA − K1 C] + [F − (A − HCA − K1 C)z(t)] + . . .

. . . + [K1 − (A − HCA − K1 C)]y(t) + [T − (I − HC)]Bu(t) + (HC − I)Ed(t) (6.3)

Where K = K1 + K2 . If the system can be designed so that the following holds true.
(HC − I)E = 0 (6.4)

T = I − HC (6.5)
F = A − HCA − K1 C (6.6)
K2 = FH (6.7)
The state estimation error will be ė = Fe(t). If the poles of F are stable the estimation error will
approach zero asymptotically. That is the estimated state vector x̂s will approach the actual state
vector xs .
In order to design the UIO the design algorithm presented by Chen & and Patton [10] is used.
The design algorithm is illustrated below.
1. Check the rank condition for E and CE: If rank(CE) += rank(E), an UIO does not exist.
2. Compute H, T and A1
H = E[(CE)T CE]−1 (CE)T T = I - HC A1 = TA
3. Check for observability: If (C, A1 ) is observable, a UIO exists and K1 can be computed using pole
placement.
4. Compute F and K:
F = A1 − K 1 C K = K1 + K2 = K1 + FH
Notice that the algorithm is presented in a reduced form as not all steps presented by Chen &
and Patton [10] are relevant to this project Using this approach it is possible to derive the UIO
matrices. The process of this as well as the implementation of the UIO in Simulink is addressed
in the following section.
Implementation
In this section the derivation of the UIO matrices and the Simulink implementation of the UIO
are addressed.
In accordance with the design algorithm proposed by Chen & and Patton [10] the rank of the
matrices E and CE are found.
  
0

 1 0 0 0 0 0 0 0 0 0 
 0
0 1 0 0 0 0 0 0 0 1
0 0 1 0 0 0 0 0 0  
rank (CE) = rank  0 0 0 1 0 0 0 0 0  
 1 = 1 (6.8)

 0 0 0 0 1 0 0 0 0 1
  
 0 0 0 0 0 1 0 0 0 0
0
0
+' (T ,
rank (E) = rank 0 0 0 1 1 1 0 0 0 =1 (6.9)
From equation 6.8 and 6.9 it is clear that the first condition for the existence of a UIO is fulfilled.
The next step is to calculate H, T and A1 . The matrices are calculated using the formulas 2

from the design algorithm. The resulting matrices are illustrated in equation 6.10, 6.11 and 6.12
respectively.
 
1 0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0
0 0 1 0 0 0 0 0 0
0 0 0 0.6667 0 0 0

 −0.333 −0.333
T = I − HC = 0 0 0 −0.333 0.667 −0.333 0 0 0 (6.10)
 
0 0 0 −0.333 −0.333 0.667 0 0 0
0 0 0 0 0 0 1 0 0
 
0 0 0 0 0 0 0 1 0
0 0 0 0 0 0 0 0 1
 
0 0 0 0 0 0
0 0 0 0 0 0 
0 0 0 0 0 0 
0 0 0 0.333 0.333

0.333

H = E[(CE)T CE]−1 (CE)T = 0
 0 0 0.333 0.333 0.333
 (6.11)
0 0 0 0.333 0.333 0.333
0 0 0 0 0 0 
 
0 0 0 0 0 0
0 0 0 0 0 0
 
0 0 0 0 0 0 1 0 0
0 0 0 0 0 0 0 1 0 
0 0 0 0 0 0 0 0 1 
0 0 0 0 0 0 0 0 0

 
A1 = TA = 0 0 0 0 0 0 0 0 0  (6.12)
 
0 0 0 0 0 0 0 0 0 
0 0 0 0 −9.8066 0 0 0 0 
 
0 0 0 9.8066 0 0 0 0 0
0 0 0 0 0 0 0 0 −0.8604
In order to investigate the second condition for the existence of an UIO the observability matrix
for (A1 , C) is found using the Matlab command obs(). The matrix resulting from this equation
is a 54 × 9 matrix. The observability matrix for (A1 , C) is found to have full column rank and
thus an UIO exits.
 
C
 
 CA1 

obs(A1 , C) =  .   and rank(obs(A1 , C)) = 9 (6.13)
 .. 
CAn−1
1
Based on the preceding work it has now been proven that an UIO exists. The next step is to
calculate the gain matrix K1 . The objective is to find the gain matrix K1 that enables the F
matrix to stabilise the fault. The poles of F must thus be chosen in the left half plane to insure
stability. The current poles of A1 can be found using the Matlab command eig() which supplies
the eigenvalues of A1 that is the poles. The found poles are illustrated in equation 6.14.
' (T
eig(A1 ) = 0 0 0 0 0 0 0 0 −0.8604 (6.14)

From equation 6.14 it can be seen that the system is marginally stable. In order to render the
system completely stable the collective poles are placed in the left half plane. The poles are
chosen arbitrarily but sufficiently small so that the values of K1 will be reasonable in size. The
chosen poles are placed using the Matlab command K1 = (place(A&1 , C& ,Poles)’ resulting in the
gain matrix K1 illustrated in equation 6.15 [see 11, page 70 ].
' (T
Poles = −20.1 −20.2 −20.3 −20.4 −20.5 −20.6 −20.7 −20.8 −20.9
 T
40.900 0 0 0 0 0
 0 31.000 0 0 0 0 
 0 0 39.939 0 0 0 
 0 0 0 20.900 0 0 


K1 =  0 0 0 0 20.500 0  (6.15)
 
 0 0 0 0 0 20.400
418.180 0 0 0 −9.806 0 
 
0 218.160 0 9.806 0 0
0 0 381.707 0 0 00
Subsequently the additional matrices K and F can be calculated 6.17. The matrices are calculated
using the formulas 6.16 from the design algorithm.
F = A1 − K1 C K = K1 + K2 = K1 + FH (6.16)
 
−40.9000 0 0 0 0 0 1.0000 0 0
 0 −31.0000 0 0 0 0 0 1.0000 0 
 0 0 −39.9396 0 0 0 0 0 1.0000 
 0 0 0 0 0 0 0 0

 −20.9000 
F=  0 0 0 0 −20.5000 0 0 0 0 
 
 0 0 0 0 0 −20.4000 0 0 0 
−418.1800 0 0 0 0 0 0 0 0 
 
0 −218.1600 0 0 0 0 0 0 0
0 0 −381.7070 0 0 0 0 0 −0.8604
 
40.900 0 0 0 0 0
 0 31.000 0 0 0 0 
 0 0 39.939 0 0 0 
 0 0 0 13.933

 −6.967 −6.967
K=  0 0 0 −6.834 13.667 −6.833 (6.17)
 
 0 0 0 −6.800 −6.800 13.600 
418.180 0 0 0 −9.806 0 
 
0 218.160 0 9.806 0 0
0 0 381.707 0 0 0
Having calculated all the relevant matrices of the UIO it is now possible to implement the UIO
in MATLAB Simulink. In figure 6.6 the implementation of the UIO is illustrated. From figure
6.6 the approach used in relation to generating the residual can be seen. The estimated state x̂s (t)
is multiplied with the output matrix C and the estimated position and Euler angles are subtracted
from the measured position P and Euler angles Θ . The value of the resulting residual is ideally
zero when the system operates normally. When a faulty condition occurs, one or more elements
of the residual vector change to non-zero.


 

 
 
  

 

  



Figure 6.6: Illustration of the Matlab Simulink implementation of the UIO
Verification
As a means of testing the designed UIO’s ability to respond correctly to faults in the system the
operation of the UIO is verified through a series of tests.
The first test involves testing the UIO’s ability to estimate the translatory velocities of the X3D.
This is tested by giving the designed LQ controller the reference values (1,1,-3), describing the
final position of the X3D and compare the estimated velocities to the ones derived in the model.
In fact, in order to get the translatory velocities of the X3D the approach used in the initial testing
of the designed LQ controller consisted in generating the elements of the state vector which are
not measured (the translatory velocities) simply by differentiating the filtered measured position
P. This approach has proven to supply sufficiently accurately values allowing preliminary testing
of the designed LQ controller’s ability to control the position of the X3D. For more information
regarding this test see section 5. In figure 6.7 both the derived and the estimated translatory ve-
locities are illustrated. As it can be seen from figure 6.7 the derived and the estimated translatory
velocities are convincingly similar. For this test the Non-linear Simulink model was used. It
is assumed that the small deviation is caused by the method with which the derived translatory
velocities are generated.
The second test involves testing the UIO’s ability to generate a detectable change in the residual
vector when a fault in rotor1 is introduced. In figure 6.8 the residual during simulation of posi-
tion control is illustrated. The reference value for the simulation is again (1,1,-3). From figure
6.8 it can be seen that the magnitude of the residual elements are close to zero during position
control with reference value [1,1,-3]. As a means of testing whether the UIO is able to generate
detectable changes in the residual vector when faults are introduced to the system, a series of test
were conducted. The system faults are introduced as additive steps on the system actuators that is
the internal controller input vector Sω . The sign and location of the introduced steps are adjusted








      








      




 




      

Figure 6.7: Illustration of the derived and the estimated translatory velocities









 




      
Figure 6.8: Illustration of the residual during simulation of position control
so that they will emulate the effect of an actuator fault in the system. In the following, focus
will be placed on detecting and identifying faults in rotor1 as this actuator has been identified
to be more vulnerable to faults. Should a fault occur (thrust failure) in rotor1 the effects will
be equivalent to a decrease in the rotational velocity ωθ see table 6.4. It is therefore considered
valid to emulate the fault by introducing a negative step to the internal controller reference ωθ
(negative pitch). At the time of writing the report no other means of introducing the fault to the
system have been identified even though several may exist.
The step size used to emulate faults has been estimated trough trial and error until achieving

a response that is believed to be representative for a fault in rotor1. In this case the step given
is approximately 1000 [-] resulting in an initial angle deviation of approximately 15 [◦ ]. It is
noticed that the LQ control will try to compensate for the introduced offset and the fault will
thus emulate the effects of a faulty rotor that produces less thrust than expected. The fault is
introduced after seven seconds that is after the reference position [1,1,-3] has been reached. In
figure 6.9 the simulated position of the X3D during simulation of a fault in rotor1 is illustrated.
In figure 6.10 the residuals during the simulation are illustrated.









          
Figure 6.9: Illustration of the position during simulation of position control with a simulated
fault in rotor1 at t = 700 [ms]




 






          
Figure 6.10: Illustration of the residual during simulation of position control with a simulated
fault in rotor1 at t = 700 [ms]

6.4 Fault Accommodation
From figure 6.10 it is obvious that the UIO is able to generate a detectable change in the residual
when a fault occurs in rotor1. As expected there is only noticeable changes in the residuals re-
lated to the X3D attitude. It is now known that the fault is in fact detectable however detectability
does not insure that the fault can be isolated. In order to investigate the response of the system,
similar faults in the additional rotors (2,3,4) are simulated. In table 6.11 the response of residuals
in relation to simulation of faults in the four rotors are illustrated.
    




Figure 6.11: Illustration of the change in the residuals during simulation of rotor faults
As it can be seen from table 6.11 (parameter increase in red and decrease in green) the change in
the residuals are characteristic to faults in the respective rotors. Based on this it can be concluded
that faults isolation can be achieved. Determination of the introduced thresholds and decision
rules will not be addressed further as the thresholds are dependent of the magnitude of the intro-
duced fault that is the specific type of thrust degradation. The decision rules used to isolate and
identify the specific actuator fault can easily be designed based on the values in table 6.9. As
the faults in the system have been detected and identified some preventive steps must be taken
in order to prevent further system performance degradation. The process of accommodating the
detected and isolated fault is addressed in the following section 6.4.
6.4 Fault Accommodation
In the case of a detected fault some corrective action must be taken. Faults in one subsystem
may migrate to other subsystems and in long term cause system failure. One approach often
used is to let the system return to a safe state (state freezing) or switch to a different controller
that can operate with a reduced number of sensors or actuators depending on the fault’s type.
Using a dedicated controller will most often result in a degradation of system performance but
will allow the system to function until the fault can be repaired. In this project the use of a
dedicated controller will be the remedy for faults in rotor1. The design of this controller will not
be addressed as the Quad-Rotor has not been modelled with one faulty rotor. In addition, there is
no knowledge about the behaviour of the internal controller during operation with a faulty rotor.
The primary objective of the dedicated controller would be to perform an emergency landing
sequence and return the X3D to the ground unharmed so that maintenance can be performed.
6.5 Chapter Summary
In this chapter the Fault Detection scheme used in this project has been addressed. A reduced
FEMA system analysis has been implemented as a means of identifying the possible fault modes
of related to the X3D. Notice that only a reduced FEMA analysis has been used as the knowledge

6.5 Chapter Summary
about the internal working of the X3D is limited. Based on the analysis critical fault modes have
been identified as they have their effects on the system. Using a geometric approach a system
observer has been designed as a means of performing FDI specifically for the detection of faults
in rotor 1. As a means of accommodating the detected and isolated fault in rotor 1 a proposal
for the development of a dedicated controller has been formulated. The primary objective of this
controller is to return the X3D to the ground if a fault in rotor 1 occurs.

Chapter 7
Project evaluation
The chapter summarizes the results of the performance tests, presents analysis and reflections,
as well as ideas for future work.
7.1 Results and Conclusion
With the implemented control scheme, the Quad-Rotor is able to hover autonomously and per-
form step movements in all directions. Several testing flying session have been performed for
tunning the weight matrices of the LQ controller and to carry out performance tests. The recorded
data from the tests is outlined in Appendix B. The requirements from section 3.3 on page 22 are
reviewed below.
• During autonomous hovering the Quad-Rotors z-axis position should be kept within ± 0.10 [m].
• During autonomous hovering the Quad-Rotors xy-axis position should be kept within ± 0.05 [m].
• During autonomous hovering the Quad-Rotors angle around the z-axis should be kept within ±
10.00 [◦ ].
• When changing the position 0.5 meter in either the x, y or z axis, the X3D should move to the new
position and remain in stable hover in the new position within 2 seconds.
While autonomous flight was achieved, the recorded performance did not meet the above require-
ments. In the following a short summary of the most evident results and the possible solutions
are presented.
7.1.1 Position control
As mentioned before it was not possible to achieve an acceptable system performance in relation
to the precision of the position tracking. Based on the performance test B on page 82 it is quite
obvious that several problems exist in relation to position tracking. The problems are illustrated
using the measurement related to the test of diagonal movement in figure 7.1 where a step from
(0,0,-0.5) to (0.5,-0.5,-1.5) is performed.
73 of 139
7.1 Results and Conclusion













         

Figure 7.1: The measured position of the X3D during (0,0,-0.5) to (0.5,-0.5,-1.5)
From figure 7.1 ii can be seen that the x and y axis are suffering from overshoot and the z-axis
is not able to reach the reference position. It is believed that both of these problems could be
removed by implementing integral action or a better system model.
7.1.2 Yaw control
The only requirement that has been met is the requirement for the yaw angle. Based on the
collective results of the performance test B on page 82 it is possible to identify the largest angle
deviation on the yaw angle to be approximately 7 [◦ ]. This not coincidental as the root of the
problem has been identified to be caused by system model. In figure 7.2 the Yaw command and








      











      

Figure 7.2: Illustration of the Yaw command and measured Yaw angle
74 of 139 7. Project evaluation

7.2 Future Work
measured Yaw angle are illustrated. The measurements are related to the measurement of a step
from (0,0,-1) to (1,0,-1) which is illustrated in figure B.1, B.2 and B.3. The system model for the
angular velocities assumes that all controller commands are mapped directly to the X3D using
the scaling factor Kω . This is however not the case as there seems to be a dead zone in the internal
controller for small commands (0-200) which can be seen from figure 7.2. As the LQ controller
features no integral action there is no build up of error and thus no increase in the command
signal. As the yaw drifts it eventually reaches an angle error which results in a command that
is higher than the internal controller dead zone. The command is executed and the yaw angle
is returned towards the reference value. Notice that there is clear offset in the angle the Yaw is
returned to indicating that the model is insufficient as well.
7.2 Future Work
This section presents open points and the most practical improvement ideas in the current context
of the project. Some of these have been also mentioned previously along different chapters.
During the experiments related to the thrust measurements described in appendix H on page 118
a series of problems were encountered. The thrust generated by the X3D was found to be substan-
tially lower than expected for a given input Sc . During the final stages of the project a possible
flaw in the used test set-up was pointed out by the project supervisor. For a complete descrip-
tion of the problem and the proposed solution see H on page 118. It is believed the test set-up
could account for a decrease in the measured thrust however further investigation is needed to
substantiate this claim. Another recurring problem with the thrust measurement setup, that has
not yet been fully understood, is an acceleration in the motors angular speed even as the thrust
input is kept constant. This made the measurement session difficult, as it seem occur unrelated
to possible expected causes.
In relation to the implementation of the controller, the state estimation method using signal filter-
ing and reconstruction with derivation is very sensitive to tracking errors in the Vicon system. For
example, even relatively short periods of tracking loss cause the Quad-Rotor to destabilize, as the
current zero-order hold correction is not adequate for the control. It is more reliable to design a
system observer to estimate the system states. The observer can be deterministic in the first step,
and a Kalman filter implementation can be attempted in the second stage. An observer is the-
oretically proven to be a better method for state estimation with guaranteed performance margins.
For periods of time when tracking is lost, the on-board gyroscope and accelerometer data can
be received by the wireless link channel. But the on-board data is more noisy than the Vicon, and
it would require a mandatory Kalman filtering. By using fused sensor data, it is possible to fly
more reliably outside of Vicon optimum tracking range.
From the performance tests it can be seen that there is a significant steady state error in rela-
tion to the reference, on all three axes. Implementation of integral action onto the current control
scheme implementation should dramatically increase the performance. Since integral action is
able to correct constant disturbances, other range of problems are compensated at the same time
7. Project evaluation 75 of 139

7.2 Future Work
improving the overall tracking precision: ground effect for stable low altitude flight, better com-
pensation of battery discharge, wind.
During the tunning session, it was noticed that introducing higher weights for the state caused
undesired dynamics in the roll and pitch, with high oscillations. It is reasonable to assume that
the dynamics of the roll and pitch command are not well identified with just a constant, and that
a higher order transfer function should produce better approximation of internal controller. This
will hopefully also decrease the problems related to the dead zones currently in the internal con-
troller which is not included in the simple model.
Except for the integration of the on-board gyroscope and accelerometer sensor data, in the view
of having a working system that has been analyzed and familiarized, all the above improvement
ideas require only little work effort and should provide significant performance improvement in
relation to the requirements. In the event of that further development of the system control is
decide upon the items in the future work section should be considered.
76 of 139 7. Project evaluation

Bibliography
[1] Ascending technologies. www.asctec.de.
[2] Brian D.O. Anderson and John B. Moore. Optimal Control: Linear Quadratic Methods.
Prentice Hall, 1990.
[3] T. Bak. Lecture Notes - Modelling of Mechanical Systems. AAU, 1th edition, 2002.
[4] Mogens Blanke. Fault tolerant control - an engineering approach. Technical report, De-
partment of Control Engineering, Institite of Elecronic Systems, Frederik Bajers Vej 7,
DK-9220 Aalborg , Denmark.
[5] Samir Bouabdallah. Design and control of Quad rotors with Application to Autonomous
flying. PhD thesis, Lausanne, 2007.
[6] Gene F Franklin, J. David Powell, and Abbas Emami-Naeini. Feedback control of dynamic
systems. Prentice Hall PTR, Upper Saddle River, NJ, USA, 5th edition.
[7] J. Friis, E. Nielsen, R. F. Andersen, J. Bønding, A. Jochumsen, and A. F. Sørrensen. Au-

tonomous Landing on a Moving Platform. AAU, 1th edition, 2009.
[8] Mobile Robotics Group. Vicon motion tracking lab.

http://www.es.aau.dk/projects/robotics/labs/motion_tracking_lab.
[9] Daniel Gurdan, Jan Stumpf, Michael Achtelik, Klaus-Michael Doth, Gerd Hirzinger, and
Daniela Rus. Energy-efficient Autonomous Four-rotor Flying Robot Controlled at 1 khz.
In ICRA, pages 361–366. IEEE, 2007.
[10] R.J. Patton Jie Chen. Robust model-based fault diagnosis for dynamic systems. Technical
report, Kluwer Academic Publishers.
[11] Anh Tuan Kieu and Karsten Bolsmand Rasmussen. Fault Detection and Isolation for a
Supermarket Refrigeration System. AAU Esbjerg, June 2009. Master Thesis.
[12] Palle Andersen Ole Sø rensen. Optimal control, lecture notes. Technical report, Department
of Control Engineering, Institite of Elecronic Systems, Frederik Bajers Vej 7, DK-9220
Aalborg , Denmark, 6. February 2010.
77 of 139
BIBLIOGRAPHY
[13] Tom S. Pedersen Palle Andersen. Modeldannelse. Technical report, Department of Con-
trol Engineering, Institite of Elecronic Systems, Frederik Bajers Vej 7, DK-9220 Aalborg ,
Denmark, 7. February 2007.
[14] Ascending Technologies. X-3D-BL Research Pilot Users Manual. Paperback Booklet.
78 of 139 BIBLIOGRAPHY
Appendix A
Performance Tests
The final goal of this project is to achieve a valid control scheme that enables the X3D to hover
autonomously and meet the requirements stated in section 3.3 on page 22. For this reason, a
series of performance requirements tests have been devised as a means of verifying that the col-
lective system meets the Performance Requirements.
Below is a description of the collective performance requirements tests. It is noticed that the
designed system control is required to pass all the described tests to be deemed a complete suc-
cess. However, the group takes the right to make a reserve : the project is deemed as partially
successful if at least the hovering test is passed.
Hovering
This test is inspired by the work done by [9] and the objective is to test the precision of the X3D
in autonomous hovering. Below is a description of the test scenario.
• The X3D is equipped with a newly charged battery and placed at the approximate position (0,0,0).
• The VICON system, see 2.3 is started and the position and attitude of the X3D is sampled at 100
[Hz].
• The X3D system is started and flown by a human pilot to an approximate position of (0,0,-1)1 .
• The control scheme is given the reference way-point (0,0,-1) and the controller is allowed to control
the X3D.
• After approximately 30 seconds the control is overtaken by the human pilot and the X3D is landed.
• The VICON system sampling is stopped and the test is completed.
In order to pass this test the control scheme must be able to maintain the X3D position with a ±
0.05 [m] tolerance on the z-axis and a ± 0.01 [m] tolerance on the xy-axis relative to (0,0,-1).
The yaw-component of the X3D attitude must be kept within ± 10 [◦ ]
1 The negative value of the z-axis is caused by the orientation of project coordinate system, see2.2.1. The position
is in fact in the center of the MTlab, see2.3.1
79 of 139
Axial movements
The objective of this test is to verify the Quad-rotor’s ability to change its position and stabilize
within the required time. The test is performed on all three axes even though only the y-axis is
described in the test scenario. The used reference way-point are respectively (0.5,0,-1), (0,0.5,-1)
and (0,0,-1.5).
Below is a description of the test scenario for the y-axis.
• The VICON system is started and the position and attitude of the X3D is sampled at 100 [Hz].
• The X3D system is started and flown by a human pilot to an approximate position of (0,0,-1).
the X3D.
• When 10 seconds have passed the control scheme is given a new reference way-point (0,0.5,-1) and
the controller brings the X3D to the new position.
In order to pass this test the control scheme must be able maintain the position of the X3D at
the initial reference position (0,0,1) in accordance with the hovering performance requirements,
control the X3D to the new position and achieve stable hovering within 2 [s].
Diagonal movements
The objective of this test is to verify the Quad-rotor’s ability to change its position and stabi-
lize within the required time. This test differs form the axes movements described above as far
as diagonal movements require intricate actuation of the Quad-rotor’s multiple degrees of free-
dom. The test is performed on all three planes (xy, yz and xz) even though only the xy-plane
is described in the test scenario. The used reference way-point are (0.5,0.5,-1), (0,0.5,-1.5) and
(0.5,0,-1.5) respectively.
Below is a description of the test scenario for the xy-plan.
• The VICON system is started and the position and attitude of the X3D is sampled at 100 [Hz].
• The X3D system is started and flown by a human pilot to an approximate position of (0,0,-1).
the X3D.
• When 10 seconds have passed the control scheme is given a new reference way-point (0.5,0.5,-1)
and the controller brings the X3D to the new position.
80 of 139 A. Performance Tests

The completion of this test is not directly contained in performance requirements but it is as-
sumed that the diagonal movements will be more complicated than the axial ones. In order to
pass the test, the control scheme must be able to maintain the position of the X3D at the initial
reference position (0,0,1) in accordance with the hovering performance requirements, control the
X3D to the new position and achieve stable hovering within 2 [s].
Appendix Summery
In this appendix the Performance Tests have been described in detail. These tests are the most
important part of the project validation as their objective is to examine to what degree the de-
signed system control scheme meets the Performance Requirements. The Performance Tests are
divided into two tests and each one has different degrees of difficulty. The first test seeks to ex-
amine the control scheme’s ability to maintain (stationary hovering) the position of the X3D at a
way-point. The second test focuses on the control scheme’s ability to control the X3D from one
way-point to another. This test includes both way-points in the same plane and combinations.
A. Performance Tests 81 of 139

Appendix B
Performance Tests Results
In this appendix the results related to the Performance Tests A on page 79 are documented. The
objective of this appendix is not to evaluate nor discuss the achieved results in detail but to
merely present the results in raw form. For a thorough description of the Performance Tests see
appendix A on page 79
Below are the results of the performance tests. There is some variation in relation to the ap-
proach described in the Performance Tests A on page 79. This is due to limited time in the
laboratory and it has thus been considered acceptable to combine several of the test thus optimiz-
ing the use of flight time. Most of the results are presented in graphical form as it is assumed to
better illustrate the context in which the results have been achieved. A first observation is that
here is a drift in the quad-rotor causing the Yaw rotate counter clockwise if not compensated.
The yaw compensation works good as manages to keep the drift in a manageable range.
Hovering
The objective of this test is to examine the designed control schemes ability to maintain the X3D
at a specific way point as well as its ability to manoeuvre the X3D from one way point to another.
The used reference where (0,0,-1) and (1,0,-1) In figure B.1, B.2 and B.3 the measured position,
attitude and input commands are illustrated respectively.
Axial movements
within the required time. The references used for the first test where (0,0,-1) and (0,-0.5,-1). In
figure B.4, B.5 and B.6 the measured position, attitude and input commands related to this first
test are illustrated. A second test was conducted to test the vertical precision of the X3D control
scheme. The references used for this test where (0,0,-0.5) and (0,0,-1.5). In figure B.7, B.8 and
82 of 139




 








      

Figure B.1: The measured position of the X3D during (0,0,-1) to (1,0,-1)




 






      

Figure B.2: The measured attitude of the X3D during (0,0,-1) to (1,0,-1)
B.9 the measured position, attitude and input commands are illustrated respectively.
Diagonal movements
within the required time. This test differs form the axes movements described above as far as
diagonal movements require intricate actuation of the Quad-rotor’s multiple degrees of freedom.
The references used for this test where (0,0,-0.5) to (0.5,-0.5,-1.5). In figure B.7, B.8 and B.9 the
B. Performance Tests Results 83 of 139






 






      

Figure B.3: The sampled input commands form the X3D controller during (0,0,-1) to (1,0,-1)




 







         

Figure B.4: The measured position of the X3D during (0,0,-1) to (0,-0.5,-1)
measured position, attitude and input commands are illustrated respectively.
Appendix Summery
In this appendix the results of the Performance Tests have been presented. The results deemed
most interesting are addressed in detail in the project evaluation 7 on page 73.
84 of 139 B. Performance Tests Results





 






         

Figure B.5: The measured attitude of the X3D during (0,0,-1) to (0,-0.5,-1)












         

Figure B.6: The sampled input commands form the X3D controller during (0,0,-1) to (0,-0.5,-1)











         

Figure B.7: The measured position of the X3D during (0,0,-0.5) and (0,0,-1.5)




 







         

Figure B.8: The measured attitude of the X3D during (0,0,-0.5) and (0,0,-1.5)













         

Figure B.9: The sampled input commands form the X3D controller during (0,0,-0.5) and (0,0,-
1.5)













         

Figure B.10: The measured position of the X3D during (0,0,-0.5) to (0.5,-0.5,-1.5)





 








         

Figure B.11: The measured attitude of the X3D during (0,0,-0.5) to (0.5,-0.5,-1.5)





 






         

Figure B.12: The sampled input commands form the X3D controller during (0,0,-0.5) to (0.5,-
0.5,-1.5)

Appendix C
First Principles model
In this appendix the First Principles model of the X3D system is addressed. The objective of
this appendix is not to derive a conclusive First Principles model but to address the physical
principles that affect the X3D as a means of better understanding the fundamental workings of
the X3D. As the project uses the internal controller in Heading-Hold mode, most of the X3D
dynamics are not relevant from a control conditional viewpoint and no collective First Principles
model will thus be derived.
The X3D system model and the respective inputs and outputs are illustrated in figure C.1. The
 
 
 
 
Figure C.1: Inputs and outputs X3D System model
X3D system consists of a series of subsystems that as a collective make up the X3D system. For
a full description of the X3D hardware and software see G on page 114. In order to simplify
the examination of the X3D, the complex collective structure of the X3D is broken down into
subsystems. It is assumed that the use of subsystems will render the modelling of the X3D more
manageable. The structure of the X3D model is illustrated in figure C.2. The model illustrated
    

      

 
   
    

Figure C.2: Illustration of the X3D model structure
in figure C.2 will serve as the basis of the X3D first principles system modelling. In the follow-
ing the inputs and outputs from the different system sub-blocks will be described as well as the
general introduction to the principles involved in modelling the blocks.
89 of 139
Internal controller & Thrust generation
The first block in the X3D system model is the internal controller that implements the on-board
angular velocity controller and thrust generation. The X3D contains a state-of-the-art internal
controller that allows the controller link to request angular velocities around the three body-axis
x,y,z and a collective generation of thrust by the four rotors. The thrust is not actively controlled
in the same way than the angular velocities as the scalar input Sc simply is a reference offset
on all the rotors. This results in a collective generated thrust where the distribution among the
four rotors may change as the angular velocities are changed. The collective generated thrust is
however always in accordance with the scalar Sc . The thrust generation will not be addressed
further in this section as it is not a direct part of the internal attitude controller.
The Internal controller block takes the angular velocity reference vector Sω illustrated in equation
C.1 as input. The output form the block is the vector illustrated in equation C.2 that contain the
DC voltages used as input for the DC motors.
' (T
Input Sω = ωφ ωθ ωψ (C.1)
' (T
Output V = V1 V2 V3 V4 (C.2)
The structure of the internal attitude controller is known from [9] who designed the internal con-
troller of the X3D as well as a hover and position control scheme. Using the Xcontrol software,
the internal controller parameters are accessible and changeable. The structure of the internal
attitude controller is illustrated in figure C.3 and the parameters currently implemented on the
platform are listed in table C.1. The parameters are accessible through the Xcontrol software.
 
    
   
  


 
 


Figure C.3: Illustration of the X3D internal controller structure[9]
Axis Kp Kd K stick K stick expo K stick Direct

Pitch 230 28000 200 25 40
Roll 230 28000 200 25 40
Yaw 200 18000 - - -
Table C.1: Measurement Data from the from the Thrust Force Measurement
90 of 139 C. First Principles model

It is noted that the the only parameters accessible through the Xcontrol software related to the
yaw is Kp and Kd . It is assumed that the yaw controller is a simplified version of the controller
illustrated in figure C.3.
The output from the block that is the voltage vector V is not controlled directly as the resulting
voltages are a result of the internal controllers distribution of angular velocities. The internal
controller bases the distribution on both the input Sω and the scalar Sc and the means of calcu-
lating the distribution is not known. Using the controller structure illustrated in figure C.3 and
the parameters in table C.1 it is possible to derive the transfer function for the X3D internal con-
troller without taking the thrust generation into account. However since the model is not used in
the process of designing the project system control scheme the block is not investigated further
as there is no incentive to derive the transfer function for the internal controller.
DC-motor Dynamics
The next block in the system model is the DC-motor Dynamics block that implements the dy-
namics of the four X3D DC-motors. The input to this block is a vector V illustrated in equation
C.3 that contain the input voltages for the four DC-motors 1 - 4. The output from the block is a
vector Ω illustrated in equation C.4 that contains the angular velocity of the four rotors 1 - 4.
' (T
Input V = V1 V2 V3 V4 (C.3)
' (T
Output Ω = Ω1 Ω2 Ω3 Ω4 (C.4)
In the following, the lower-case i indicates that the parameter is related to the ith of the four
DC-motor. The process of designing the model of a DC-motor has been addressed in several
places in the literature including [13, page 8 ]. The version used in this project differs from the
common model where the motor input voltage Vi must be related to the generated motor torque.
In this model, only the electrical characteristics of the motor will be considered as the output
from the model is to be the angular velocity of the motor shaft Ωi . The motor’s electrical char-
acteristics can, and are often modelled as a resistor Ri and an inductor Li in series with a voltage
contribution VEMF,i . The electrical model of the motor can be modelled as illustrated in figure
C.4. The voltage contribution VEMF,i is related to the rotational velocity of the motor shaft Ωi
and is caused by back EMF (Electromagnetic force) from the motor. Using the voltage drops
  
 
 
 
Figure C.4: DC-motor model [13, page 8 ]
in the motor’s electrical equivalent diagram, it is possible to write an equation for the rotational
velocity generated by the motor relative to the input voltage.
C. First Principles model 91 of 139

If a dedicated model of each of the X3D four motors is to be derived, the motor’s parameters
have to be determined through an analysis of the motors. However since the motor model is not
used in the system model, the process of designing a dedicated motor model for each motor is
not addressed further.
Rotor & CSM Aerodynamics
The Rotor & CSM Aerodynamics block is the most complex part of the system model as it con-
tains several advanced physical aspects. This block contains the aerodynamics related to the X3D
rotors, core and booms. Notice that the core and booms are denoted CSM when refered to as a
collective.
The input to this block is a vector Ω illustrated in equation C.5 that contains the angular velocity
of the four rotors 1 - 4. The output is the two vectors Frotor and τ rotor illustrated in equation C.6
and C.7. The two vectors contain the force and torque generated by the four rotors respectively.
' (T
Input Ω = Ω1 Ω2 Ω3 Ω4 (C.5)
' (T
Output Frotor = F1 F2 F3 F4 (C.6)
' (T
Output τ rotor = τ1 τ2 τ3 τ4 (C.7)
In this preliminary system analysis only one aerodynamic force is considered. The group is aware
that several other aerodynamic forces exist, though only the Ground effect will be addressed. The
term ground effect is a collective term used for several effects that may occur when the X3D is
close to the ground. The evident ground effect is known as the Wing In Ground effect (WIG)
which refers to a reduction in wing drag. The rotor lift is generated by the difference in air
pressure on the upper and the lower rotor’s surface. When the X3D is flying close to the ground,
the air pressure on the lower part of the rotor surface is increased thus creating what is known as
a cushion effect. The effect means that less power is needed to create the same thrust compared
to when flying in mid air.
Forces & Torques
This block serves as a summation block and takes the input vectors Frotor , τ rotor and converts
them into collective force and torque contributions Ftotal , τtotal . Notice that additional forces and
torques contributions are also added here to find the total force and torque exerted on the CSM
center of mass Mc .
Forces
The total force acting on the X3D can be found as a summation of all external forces. The sum-
mation of the forces yield the equation illustrated in figure C.8 where B F 1 - B F 4 are respectively

the upward forces generated by the X3D rotors and B F g is the gravitational force.
B
FTotal = B F 1 + B F 2 + B F 3 + B F 4 + B F g (C.8)
Notice that the forces B F 1 - B F 4 consist of negative components along the z-body axis as il-



 

 


Figure C.5: Illustration of forces affecting the X3D
lustrated in figure C.5. The magnitude of the gravitational force Fg in the body frame can be
calculated using Newton’s second law 2.1. The equation is illustrated in C.9 where g is the
gravitational acceleration written as in equation C.10 and m is the mass of the X3D.
E B
Fg = m · g and Fg = B CE E Fg ⇒ B
Fg = B CE · m · g (C.9)
' (T
g= 0 0 g (C.10)
Torque
Using the same approach than in the section above, the total amount of torque on the X3D can be
found by the summation of all external torques along the three body-axes. The concept of torque
can be described using figure C.6 where a single force F is acting on a rigid body. The effect of
the force in point O depends on the location of the force’s point of application P. In the illustration
the torque is applied at an angle relative to the point O and a trigonometric function is applied
to transform the actual set-up to the standard force times arm set-up. The torque experienced at
point O can be calculated as in equation C.11.
τ = Force × arm ⇒ τ = F ·d ⇒ τ = F · r · sin(θ) (C.11)
In relation to the summation of the torques applied to the X3D there is no need for the trigono-
metric function as the force from the X3D rotors are always perpendicular to r that is the platform
booms. The torques summation along the three body-axes are illustrated in equations C.12 - C.14


   

Figure C.6: Illustration of torque
and are based on positive rotations along all body-axes.
B
τ x,Total = Bτ x,rotor + Bτ x,gyro (C.12)
B B B
τ y,Total = τ y,rotor + τ y,gyro (C.13)
B B
τ z,Total = τ z,rotor (C.14)
Bτ is the total torque experienced along the x,y,z-body-axes. Bτ i,rotor is the torque contri-
i,Total
bution generated by the rotor thrust along the x,y,z-body-axes. Bτ i,gyro is the gyroscopic torque
caused by the rotational movement of the rotors placed on the x,y,z-body-axes. Notice that there
are no gyroscopic torque contributions along the z-body-axis as all of the X3D rotors are in the
xy-body plane. B F 1 - B F 4 are the forces respectively generated by the X3D rotors and r is the
distance from the X3D center of mass Mc to the force point of application that is the motor axis.
The platform is assumed to be symmetric and the distance r can thus be used for all the force
contributions.
B
τx = BF 4 · r − BF 2 · r ⇒ B
τx = (B F 4 − B F 2 ) · r (C.15)
B
τy = BF 1 · r − BF 3 · r ⇒ B
τy = (B F 1 − B F 3 ) · r (C.16)
B B B B B
τ z = ( τ2 + τ4 − τ1 − τ3 ) (C.17)
The gyroscopic effect B τi,gyro along the x,y-body axes can be expressed as in equation C.18 -
C.19 where Ωi is the angular velocity of individual rotors respectively and Jrotor is the assumed
identical inertia of the rotors. ωy and ωx are the angular velocities of the X3D respectively around
the x and y-body axis. The sign of the velocities can be inferred from the velocities related to the
individual rotors [see 5, page 18 ].
B
τx,gyro = Jrotor · ωy · (Ω1 + Ω3 − Ω2 − Ω4 ) (C.18)
B
τ y,gyro = Jrotor · ωx · (Ω2 + Ω4 − Ω1 − Ω3 ) (C.19)
Rigid Body Dynamics & Kinematics
In this section the rigid body dynamics and kinematics of the X3D are addressed. Prior to ad-
dressing the inputs to this block the concept in equation C.20 is introduced where E CB is the
direction cosine matrix.The equation relates the angular and translational velocity of the X3D in

the body frame to angular and translational velocity in the earth frame.
E
v = Ṗ = E CB (Θ
Θ)B v and E
Θ = E HB (Θ
ω = Θ̇ Θ )B ω (C.20)
The input to the rigid body dynamics and kinematic block consists of the collective torque τtotal
and collective force Ftotal . The collective torque on the X3D center of mass Mc gives rise to
angular acceleration and the generated thrust gives rise to translational acceleration. As the
model C.2 contain two inputs related to the translational and angular acceleration a total of two
system equations will be derived.
Translational acceleration
It is possible to describes the translational acceleration of the X3D with respect to the body frame
as a function of the applied force. Using Newton’s second law of motion it is possible to derive
C.21 describing the for the translational acceleration E v̇ of the X3D (body frame) as seen from
the earth frame, [6, page 117 theorem 5.2 ].
EF
E total
F = m·a ⇒ Ftotal = m · E v̇ ⇒ = E v̇ (C.21)
m
The translational velocity E v of the X3D as seen from the earth frame can be written as is in
equation C.22 where B v is the translational velocity of the X3D in the body frame and E CB (Θ
Θ)B is
the transformation matrix from the body frame to the earth frame. The translational acceleration
of the X3D E v̇ seen from the earth frame can be written as is in equation C.23.
E
v = E CB (Θ
Θ)B v (C.22)
E E B E B
Θ) v̇ + ĊB (Θ
v̇ = CB (Θ Θ) v (C.23)
in equation C.24, [3, page 24 theorem 4.4 ].
E
Θ) B v = −B CE (Θ
ĊB (Θ Θ)(Bω × B v) ⇒ E
v̇ = E CB (Θ
Θ)(Bω × B v) (C.24)
Equation C.21 and C.24 are combined yielding the equation illustrated in C.25. The simplified
equation is illustrated in C.29.
EF
total
= E CB (Θ
Θ)(Bω × B v) (C.25)
m
EF
E total
Θ)B v̇ =
CB (Θ + B CE (Θ
Θ)(Bω × B v) (C.26)
+E m ,
B
)E B
* B Ftotal B B B
Θ) CB (Θ
CE (Θ Θ) v̇ = CE (Θ Θ) + CE (Θ Θ)( ω × v) (C.27)
m
EF ) *) *
B total
v̇ = + B CE (Θ Θ)B CE (Θ
Θ ) Bω × B v (C.28)
m - ./ 0
I
EF
B total
v̇ = + Bω × B v (C.29)
m

Given is thus an equation relating the translational acceleration of the X3D (body frame) as a
function of the applied force.
Angular Acceleration
In this section an equation for the angular acceleration with respect to the body frame as a func-
tion of the applied torques is derived. Using the rotational equivalent of Newton’s second law of
motion it is possible to derive an equation C.33 for the angular acceleration E v̇ as seen from the
body frame. The collective torque τtotal can be expressed as the derivative of angular momentum
L well as the product of the rigid body inertia J and angular acceleration ω̇, [6, page 340 theorem
11.11 ]
τtotal = L̇ = Jω̇ (C.30)
BC Θ) is the transformation matrix from the earth frame to the body frame and B L is the angular
E (Θ
momentum of the platform in the body frame.
E
τtotal = E L̇ = E CB (Θ
Θ) B L̇ + E ĊB (Θ
Θ) B L (C.31)
in equation C.32, [3, page 24 theorem 4.4 ].
E
Θ) B L = −B CE (Θ
ĊB (Θ Θ)(Bω × B L) (C.32)
B B B
Θ)( ω × J ω )
= − CE (Θ (C.33)
Equation C.33 is substituted into C.31 yielding the equation illustrated in C.34. The simplified
equation is illustrated in C.38.
E
τ Total = E CB (Θ
Θ) JBω̇
ω − B CE (Bω × JBω ) (C.34)
B
) *
Θ)E τ Total = E CB (Θ
CE (Θ Θ) E CB (Θ
Θ) JBω̇
ω − B CE (Bω × JBω ) (C.35)
B
) *
Θ)E τ Total = JBω̇
CE (Θ ω − B CE (ΘΘ)B CE (Θ
Θ) (Bω × JBω ) (C.36)
B
τ Total = JBω̇
ω − (Bω × JBω ) (C.37)
Bτ + (Bω × JBω )
B Total
ω=
ω̇ (C.38)
J
Given is thus an equation relating the angular acceleration of the X3C (body frame) as a function
of the applied torque.
Appendix Summary
In this appendix the First Principles model for the X3D has been addressed. Using the governing
laws of physics, a series of the X3D’s dynamical features has been examined. The objective of
this appendix was not to design a conclusive First Principles model but to better understand the
behaviour of the X3D as a dynamic system.

Appendix D
Vicon Data Management
In this appendix the transformations used in relation to the Vicon Motion Tracking system are
addressed. In order to insure that the data from the Vicon Motion Tracking system is compatible
with the system model a transformation of the captured data is introduced.
As described in section 2.2 on page 4 the two reference frames E and B used in this project
can be illustrated as in figure D.1. E and B refer to the Earth and Body reference frames respec-
tively. The default reference frame used by the Vicon Motion Tracking system is illustrated in



 
 

 


 




 

Figure D.1: Illustration of the reference frame as defined in this project
figure D.1 as the V frame reffering to the Vicon reference frame. It is quite obvious that the E
and V frames are different and thus can not be directly mapped. In fact, a transformation of the
output form the Vicon Motion Tracking system is needed.
Based on figure D.1 it is evident that the transformation undertakes the structure illustrated in
equation D.1.
   
1 0 0 1 0 0
E    
Θ) = 0 −1 0  and E TV (P) = 0 −1 0 
TV (Θ (D.1)
0 0 −1 0 0 −1
Using the transformation described in equation D.1 on the data form the Vicon Motion Tracking
system that is the measured distance and angles the measurement will fit the system model.
97 of 139
Appendix E
Non-linear Model Verification - Test flight
In this appendix the measurements related to the test flight performed as part of the verification
of the Non-linear Model is presented. The management and sampling of the data is not addressed
in detail as it is similar to the methods used in appendix I on page 125
Obtaining Measurement Data
The objective of this appendix is to measure the response of the X3D when flying close to hover.
The purpose of the test flight is thus to maintain the X3D as stable as possible actuating the
angular velocity input Sω as little as possible. It is noticed that the used operator was a novice
and that a smother flight may be obtained if a more experienced operator was used.
Test flight procedure
The test flight procedure is similar to the procedure used in appendix I on page 125. The main
difference is that the operator focuses on maintaining the X3D as stable as possible. The steps re-
garding the initialization of the Vicon system and the sampling of the X3D response are identical
to the steps used in appendix I on page 125.
Measurement Data and Management
The sampled response of the X3D is managed in order to filter out the faulty measurements from
the Vicon system as well as the noise in the measurements. The methods used in relation to
filtering the measured data are identical to the methods used in appendix I on page 125.
The managed data related to the wireless link, the measured Euler angles and the measured
position are illustrated in figure E.1, E.2 and E.3 respectively. From figure E.2 it can be seen
that the roll and pitch are fluctuating around 0◦ as expected during normal operation. It is also
noted that the yaw angle is drifting during the flight. Based on the input values it can be seen
that the drifting in the yaw angle is not caused by input commands as this channel is more of
98 of 139
 








          

Figure E.1: The input channel during the test flight










          

Figure E.2: The measured Euler angles Θ from the test flight
less silent. The yaw angle will not be addressed in the validation as the measured response is not
caused by active inputs from the remote control. The drifting is assumed to be caused by the fact
that the X3D’s frame has been modified with an extension on one of the booms to enable better
positioning of the Vicon Motion Tracking marker. This modification will not be addressed in the
model as it was introduced later in the project process by other groups using the X3D. As it can
be seen form figure E.2 and E.1 the response of this particular test flight is representative of the
normal operation mode as the platform attitude is close to hover during the entire test flight. It is
also noticed that the elevation achieved during the test flight is lower than when it is considered
E. Non-linear Model Verification - Test flight 99 of 139



 





          

Figure E.3: The measured position P from the test flight
normal1 . The reason that less elevation is considered acceptable is because this elevation is quite
representative of the elevation used in relation to the determination of the Thrust Force H.
Appendix Summary
In this appendix the measurement related to the verification test flight has been presented. The
objective of this appendix was not to address in detail how the data were collected or managed
but to present the managed data.
1 The position of the X3D during the performance tests are specified to be around 1.0 [m]
100 of 139 E. Non-linear Model Verification - Test flight

Appendix F
Vicon tracking precision
To evaluate the tracking precision of the Quad-Rotor using the Vicon tracking system two setup
measurements where performed in both cases, the aim is to record and analyze the variation
in the measured data when the Quad-Rotor is in a static position. The final objective of this
appendix is to derive and estimate the confidence interval for point measurements. The analyzed
output data consist of 6 signals sampled at frequency of 100Hz: the (x, y, z) position and the
(roll, pitch, yaw) orientation.
Scenario 1
The quad-rotor is powered off, placed on a marker in the capture room, and tracked for about 10
seconds. Since the quad-rotor is completely still, all the variation in the measured data is caused
by the Vicon system.
The sample mean values of the 6 recorded signals give an estimation of the accuracy or the deter-
ministic offset in the measurements. The precision, or noise, is given by evaluating the variance
in the mean centered measurements.
The position of the quad-rotor on the capture room was (0, 0, 0) following the pre-existing mark-
ers on the floor. Referring to the attitude, it was horizontally leveled and aligned with the x-axis,
which corresponds to roll, pitch and yaw angles of (0, 0, 0).
Measurement analysis
We assume that each measured data point has 3 components. s is used to denote any of the 6
signals:
s = abs + o f f set + noise (F.1)
101 of 139
The absolute value abs and the measuring o f f set are deterministic components, constant for the
entire sample. Noise is the stochastic component, a summation of disturbing energies that affect
the measurement. It is additive to the measurement. Is is characterized by properties such as
mean, variance, and distribution. It is assumed that the expected value, or mean of the noise is
zero.
E[noise] = 0 (F.2)
The absolute value is known, it is part of the measurement set-up. For all signals, the position
and orientation, it is 0.
abs = 0 (F.3)
Using the collected data, we need to estimate the offset as a deterministic value, and the distribu-
tion parameters of the noise.
To isolate the offset from the noise, the expected value operator is applied to equation F.1:
E[s] = 0 + o f f set + E[noise] = o f f set (F.4)
Next, the expected value of the signal is approximated using the sample mean.
∑ni=0 si
o f f set = s̄ = (F.5)
n
To isolate the noise, the variance operator is applied to equation F.1:
VAR[s] = 0 +VAR[o f f set = ct] +VAR[noise] = VAR[noise] (F.6)
The variance of the signal is approximated using the sample variance.
∑ni=0 (si − s̄)2

VAR[s] = (F.7)
n−1
From the data, the sample mean position is (-1.190 mm, -0.511 mm, 0.002 mm), and the sample
mean orientation is (0.025◦ , −0.523◦ , 1.724◦ ). The sample standard deviation of the position
data is below 0.03 [mm] for each of the three axis signals, and the standard deviation in the
orientation angles is below 0.02 [◦ ]. More detailed results are compiled in the table below.
Table F.1: Scenario 1: Data Summary
Signal Sample Mean Sample Standard Deviation Maximum noise Unit

x −1.1898 0.022078 0.08330 [mm]
y −0.5109 0.021505 0.06974 [mm]
z 0.0023 0.016697 0.05498 [mm]
φ 0.0248 0.017929 0.06214 [◦ ]
θ −0.5226 0.012766 0.06734 [◦ ]
ψ 1.7241 0.009276 0.02728 [◦ ]
102 of 139 F. Vicon tracking precision

Figure F.1 provides a good representation of the measurements’ accuracy (offset) and precision
(noise) for the position.
Figure F.1: Scenario 1, Position data. Offset: mean(s) − s0(= 0) & noise: s − mean(s)
A first observation is that the offset is quite large compared with the noise.
A possible conclusion would be that there is a constant deviation in the data from Vicon in
relation with the position of the tracked object in the capture room. The are a series of factors
which can account for the such a constant deviation. The most evident factors are listed below.
• The Vicon reference for the center of the room is not perfectly aligned with the markers on
the floor, not at the resolution of the resulted offset
• There are imprecisions in definition of the center mass point relative to the 5 object mark-
ers, as defined in the Vicon iQ system. The imprecisions translate directly into a constant
deviation from the perspective of the human operator.
A simple way to reduce the offset is to implement a subtraction of the offset constants in the data
processing stage.
However, it is also likely that the set-up done by the operator is not accurate according with the
reference for the center of the room. The markers on the floor are pieces of black tape, approx-
imately 1.5 [cm] wide. During the experiment set-up the X3D was positioned approximately in
the center of the room however it is considered more likely that this placement was careless or
the position of the tape is offset than the Vicon system is inaccurate. In either case introduction
of an offset correction is not justified.
F. Vicon tracking precision 103 of 139

The next observation is related to the actual offset values being small: −1.190 [mm] on the x-
axis, −0.511 [mm] on the y-axis, and the 0.002 [mm] on the z-axis. These deviation values are
below the accuracy of a human operator.
Other observation is that the offset on the z-axis is significantly lower than for the other direc-
tions, and close to 0. Also, we would expect the center mass of the quad-rotor to be placed in a
low position on the structure, close to the battery, but higher than the floor level. This indicates
both that the center mass point in the Vicon iQ object definition is too low, and consequently, that
the z-axis offset from the real center mass position is larger than the 0.002mm value.
Figure F.2 contains close up graphics of the noise in the position measurements.
Figure F.2: Scenario 1, Position data. Noise: s − mean(s)
It is also interesting to see a 3D graphical representation for the distribution of the measured
position points. The measurements form a compact cloud that has been plotted in figure F.3
inside a cube with the sides of 0.1 [mm].
The accuracy and precision in the measurement of the orientation angles are especially important
for the control. The data shows that offsets for the roll and pitch angles are both below half a
degree, and the maximum recorded noise was below 0.1◦ . This can be seen in figure F.5. Since
the quad-rotor was placed on a leveled surface, the expected angles are exactly (0,0). But no
offset correction was previewed in the data processing because the values are accurate enough.
The yaw angle has a higher offset, and this can be attributed to operator’s inaccuracy. The
maximum recorded noise in the session was below 0.1◦ , as seen in the close up figure F.6.

Figure F.3: View of the position measurements taken with engines off, no vibration
Figure F.4
Figure F.5: Scenario 1, Orientation data. Offset: mean(s) − s0(= 0) & Noise: s − mean(s)

Figure F.6: Scenario 1, Orientation data. Noise: s − mean(s)
Statistic inferences for Scenario 1
The sample mean and sample variance are not deterministic, but also random variables. The used
estimates, the sums in F.5 and F.7, converge towards the mean and variance as the simple size
grows, but without further assumption, it is not possible to evaluate the confidence intervals of
these estimation.
The needed assumption is about the distribution of the noise. It was expected that the distribu-
tion of the would match a Gaussian profile, and the 95% confidence intervals for offsets and the
variance could be determined.
Figures F.7 and F.8 show the histograms of the normalized position and orientation signals, (s −
s̄)/σs .
The histograms are well centred around the 0 mark, the maximum probability value is close to
the 0.399, and have a bell-shaped aspect. However, MATLAB lillietest and jbtest return
a significance level less than 5% for the normal distribution matching. As such, the confidence
interval estimates cannot be evaluated, since the distribution profiles do not match.
In conclusion, the offset values are very small, and below the accuracy of a human operator.
Without further measurement sessions, there is not enough justification to do offset correction
to the Vicon signals in the data processing. The variation/noise in the data is also very low, the
precision of Vicon has been successfully evaluated to be sub-millimeter.

Figure F.7: Scenario 1, Position histogram
Figure F.8: Scenario 1, Orientation histogram
Scenario 2
In the second scenario, the rotors are powered on and driven at the low end of the throttle control.
The quad-rotor is not airborne, its position is static and stable, but the platform and the reflecting
markers are vibrating, which will cause larger variations in the measured data. This scenario
better approximates the measuring variation when the tracked quad-rotor is in flight.
From the data, the sample mean position is (−1.000mm, −0.610mm, −0.103mm), and the sam-
ple mean orientation is (−0.03◦ , −0.72◦ , 1.79◦ ). The sample standard deviation for the position
data can be approximated to be below 0.4mm in each of the three axis signals, and the standard
deviation in the orientation angles is below 0.4◦ . The detailed results are compiled in the table
below.

The increase in data variation indicates that the quad-rotor platform is not completely rigid,
vibrations from the moving rotors induce fluctuations in the calculated position of the platforms
center of mass.
Table F.2: Scenario 2: Data Summary
Signal Sample Mean Sample Standard Deviation Maximum noise Unit

x -0.999 0.3521 1.634 [mm]
y -0.6105 0.3375 1.602 [mm]
z -0.1028 0.2340 0.736 [mm]
φ -0.0317 0.2928 1.0362 [◦ ]
θ -0.7177 0.3056 0.9555 [◦ ]
ψ 1.7932 0.2375 0.9226 [◦ ]
Figure F.9 contains the graphic of the offset and noise.
Figure F.9: Scenario 2, Position data. Offset: mean(s) − s0(= 0) & noise: s − mean(s)
Figure F.10 contains a close up of the noise.

Figure F.10: Scenario 2, Position data. Noise: s − mean(s)
In the 3D space representation, the measurements form a cloud which exhibits concentration
areas and a more scattered distribution than in the first scenario. The data has been plotted inside
a cube with the sides of 3.4mm.
Figure F.11: View of the position measurements taken with engines on, platform vibrating
Figure F.13 shows the offsets for the roll and pitch angles, which are both under one [◦ ]. The yaw
angle has a higher offset, and again, and this can be attributed to operator’s inaccuracy. While the
noise has increased, it can be seen that the offset values for both position and orientation angles

are in the same range as in Scenario 1.
Figure F.12
Figure F.13: Scenario 2, Orientation data. Offset: mean(s) − s0(= 0) & Noise: s − mean(s)
Figure F.14 contains close up of the noise in the orientation angles.
Figure F.14: Scenario 1, Orientation data. Noise: s − mean(s)

Statistic inferences for Scenario 2
The following figures show the histograms of the position and distribution data, same as in Sce-
nario 1.
Figure F.15: Distribution of position signals with engines on, platform is vibrating
Figure F.16: Distribution of orientation signals with engines on, platform is vibrating
The distribution of the signals deviates even further from the expected normal noise contribution.
Possible causes are related vibrations, the placing of the reflective markers in an asymmetrical

configuration related to the rigid body structure, and the internal calculations of the Vicon system.
Data Summary
A summary of the standard deviation and maximum noise in the data, in the two scenarios:
Table F.3: Standard deviation 1-Motors OFF , 2-Motors ON
Scenario 1 Scenario 2 Unit

x 2.2078 ·10−5 3.7917·10−4 [mm]
y 2.1505·10−5 3.7357·10−4 [mm]
z 1.6670·10−5 2.6594·10−4 [mm]
roll 0.01793 0.2930 [◦ ]
pitch 0.01277 0.3056 [◦ ]
yaw 0.0093 0.2375 [◦ ]
Table F.4: Maximum noise 1-Motors OFF , 2-Motors ON
Scenario 1 Scenario 2 Unit

x 0.08330 1.634 [mm]
y 0.06974 1.602 [mm]
z 0.05498 0.736 [mm]
roll 0.062145 1.0362 [◦ ]
pitch 0.067342 0.9555 [◦ ]
yaw 0.027284 0.9226 [◦ ]
Figure F.17: Scatter of the position noise deviation between Scenario 1 and 2

Chapter Summary
In this appendix the precision of the Vicion tracking system has been examined. Based on exper-
iments related to tracking the X3D with both motors off and on a rough estimate of the precision
of the Vicion tracking system has been presented. It was hoped to present a confidence interval
for the position and attitude of the X3D however since the present measurement could not be
profiled to a Gaussian distribution this has not been possible.
The experiment have given a good estimation of the Vicon precision in the best tracking zone
range, which is centred the middle of the room. Depending of the camera configuration and line
of sight orientation, Vicon can lose tracking towards the corners and edges of the room.

Appendix G
X3D components
X-CSM frame
The frame of the X3D consists of four light weight carbon fiber booms, and a magnesium core
structure. The use of light weight but sturdy materials renders the platform durable and rigid.
The magnesium core improves the structural integrity and houses the electronics and the battery.
The X3D is capable of enduring small crashes without permanent damage. The figure G.1 shows
one of the carbon fiber booms, along with the magnesium core.
Figure G.1: Picture of one of the Quad Rotor carbon fibre booms and the magnesium base
X-Base
X-Base is the main electronic component of the X3D. The unit manages power control and
distribution, and issues commands to the electronic drivers, or controllers, of the motors. It
manages external communications, the R/C channel, and the connectivity with additional internal
modules, such as the sensor board. The X-Base circuit is shown in figure G.2.
Battery
The battery is Lithium-ion polymer type with multiple elements, is relatively light, has a capacity
of 910[mA/h] and is able to supply a maximum of 15A continuously. The battery is rechargeable
114 of 139
Figure G.2: Picture of the X-Base
and special care must be taken since it will explode if overcharged. However, it will leak and not
burst into flames.
Figure G.3: Battery
X-BL
The X-BL is the term used to refer the motor controllers and the brushless DC motors. The
platform features four specially designed brush less motors and the dedicated drive circuits. The
motors operate at a high control frequency of 1KHz, allowing for rapid changes of the rpm. One
of the four driver circuits along with the brushless motor are shown in figure G.4.
Figure G.4: Electronic driver and the DC motor
G. X3D components 115 of 139

Spectrum Radio Control link
X3D is remotely controlled by wireless radio link. In figure G.5 depicts the receiver/transmitter
mounted on top of the platform core.
Figure G.5: Picture of the Spectrum Radio receiver and transmitter
Fun Pilot sensor and control board
Fun Pilot is an integrated sensor board. It has three electronic gyroscopes that allow the measure-
ment of the platform’s attitude. The FunPilot is also the main control board, and it houses the
algorithms used for flight stabilization. It is currently upgraded with the ResearchPilot firmware.
The board features a UART controller and a serial interface. It can be used to connect to a PC
though a USB cable and configure the control algorithm parameters using the AscTec X-Control
software. The serial interface can also function as a real-time command input (alternative to the
radio control) and sensor output channel, when connected to the PC via an wireless link. This is
further described in the Control Software section.
In figure G.6, the Fun Pilot board is shown.
Figure G.6: Fun Pilot “brain” board
Wireless Serial Link
The wireless set-up link is made by using a pair of low-cost, low-power wireless RF modules that
have a serial data interface. The underlying communication standard is IEEE 802.15.4, which is
also the basis for the more known ZigBee protocol.
One module connects directly at the FunPilot serial interface by exposed wires. The second mod-
ules connects o the computer via an USB cable. The programming serial interface is available
116 of 139 G. X3D components

Figure G.7: XBee
through USB by a set of drivers K
Flight modes
The X3D is able to operate in three modes. Each mode has different features, and thus, different
areas of usefulness.
Heading Hold
In the project, only Heading Hold mode is used. Heading Hold takes as commands the angular
velocities for the three axes: pitch, roll, and yaw, and a proportional thrust input. The internal
controller uses only the gyroscopic sensor data for flight stabilization. When left unsupervised,
that is if the radio control is released, X3D maintains its last orientation.
X-ACC
The X-ACC is an Extension board to the X-Base and features a three axis accelerometer. The
accelerometer is a MEMSIC accelerometer, model MXR9500GIM 1 . In X-ACC mode both gy-
roscope and accelerometer data are used by the internal controller, allowing a more robust flight
where the platform will try maintain the requested absolute angles. When left unsupervised this
mode will try bring the platform to a horizontally aligned orientation that is a hovering position.
This mode features a fail safe that allow the platform to attempt an emergency landing if the radio
control is lost. In figure G.8 the X-ACC board is depicted.
Figure G.8: Picture of the X-ACC board Extension
X-ACC with Height Control

This mode is the X-ACC mode with height control. Using the on-board pressure sensor the
platform is able to estimate and maintain its height by adjusting the thrust.
1 look at www.memsic.com or further information
G. X3D components 117 of 139

Appendix H
Thrust Force Measurement
This appendix contains the process and methods related to the model’s identification of the X3D
thrust generation. The Matlab scripts and related data sets are located on the project CD in the
Thrust Measurements folder.
In order to identify the thrust generation model a series of data sets must be acquired. The data
are used to establish the connection between the collective thrust input Sc and the collective
downwards-generated force B Frotor .
The test is conducted by attaching the X3D to a heavy mass and actuating the reference input Sc
while measuring the resulting downwards-generated force. The force is evaluated by measuring
the decrease in the set-up’s weight and then inferring the generated force from the weight-loss.
In accordance with Newton’s second law H.1 the weight-loss is proportional to the applied force
provided that the acceleration is constant.
B
Frotor = m · a (H.1)
The generated force B Frotor is in [N], the mass m is the [kg] and the acceleration a is in [m/s2 ].
The weight of the X3D is used as a critical parameter throughout this appendix. During the
mounting of the used test set-up the weight of the Quad-Rotor was measured to be 0.3273 [kg].
The weight of the batteries was found to be 0.079.5 [g].
Test procedure
Below is a short description of the procedure used in relation to the test and the data measurement.
• The control PC is started with Matlab Simulink
118 of 139
• The X3D is equipped with a fully charged battery and placed in the test set-up illustrated in figure
H.1.
• The X3D is attached to the mass using heavy duty plastic strips
• The cone elevates the X3D relative to the scale and ground so that no ground effects will interfere
in the measurement.
• The initial weight of the X3D and test set-up is noted and a series of constant inputs is given to the
X3D.
• At each constant input the mass of the test set-up is read using the scale.
During the initial testing of the test set-up it was discovered that the X3D reacted somewhat
unexpectedly to a constant input. Whenever the input Sc was set to a constant value, the rotational
velocity of the X3D rotors would increase over time leading to the increase of the generated
thrust. This problem only occured when the group applied a constant input and while measuring
the input signal using the wireless link. After extensive investigation of the problem, it was
chosen to use a slightly noisy signal as input. At the time of writing this appendix the group
has no explanation for this strange behaviour. As the devised control scheme will never give a
constant value for long periods of time, the problem is assumed to have no effect on the validity
of the devised model.




Figure H.1: The test set-up used in the process of sampling the response of the X3D
Measurement Data
In this section the raw measurement data from the experiment are presented. The results of
the experiment have been gathered in table H.1 where the measured mass along with the input
reference Sc are illustrated. Notice that only parts of the input range of Sc has been investigated.
This is because of the fact that the X3D will never be flown in the upper end of the input range. It
has thus been considered more relevant to fit the model to the lower and center part of the input
range. The process of managing the measured data from table H.1 is addressed in the following
of this appendix.
H. Thrust Force Measurement 119 of 139

Input Signal Experiment I
0 3.160 [g]
500 3.111 [kg]
1000 3.025 [kg]
1500 2.890 [kg]
2000 2.750 [kg]
2500 3.580 [kg]
3000 3.370 [kg]
3500 3.300 [kg]
4000 3.295 [kg]
Table H.1: Measurement Data from the from the Thrust Force Measurement
Measurement Data Management
The measured data must be managed in order to determine the generated force Frotor . The amount
of generated force can be inferred from equation H.2 where ∆m = minitial − mmeasured . minitial is
the weight of the X3D with additional weight and mmeasured is the measured values relative to the
input signal.
Frotor = g · ∆m (H.2)
Based on the initial weight of the measurement set-up and the weight-loss at the various input
values it is possible to derive the generated force Frotor .



 










        

Figure H.2: The generated lift relative to the input signal Sc
As a means of investigating the validity of the measurements, the weight-loss ∆m is plotted

along with the known mass of the X3D in figure H.2. It is noticed that the weight lifted by the
120 of 139 H. Thrust Force Measurement

X3D intercepts the mass of the X3D at Sc ≈ 2000. This is not in accordance with the expected
value as it is known that the X3D will take off at Sc ≈ 1700 when situated on the ground.
In order to investigate this problem, a series of test flights at low altitude where performed and
sampled. The altitude was approximately the same as the one used in the process of measuring
the generated thrust. During these flights it became clear that the X3D is indeed able to fly at
Sc ≈ 1700. As the elevation during the thrust measurement and the test flights where more or less
identical the group believes that some unknown factor has affected the thrust measurements.
Based on this observation it has been chosen to manipulate the data set related to the weight
lifted by the X3D. In short this means that a artificial offset is introduced to the measurements.
This enable the group to move the take off value down thus hopefully resulting in a better repre-
sentation of the actual take-off value. The introduced offset has been estimated to 0.032 [kg] to
create a take-off value in the region of Sc ≈ 1700. The new data set is illustrated in figure H.2 as
the revised weight lifted.
System Identification
Based on the data from figure H.2 and the input reference values that is Sc it is possible to derive
a polynomial relationship between the input and output of the system. As the data can quite
obviously be represented fairly precisely using a low order polynomial, a third order polynomial
has been chosen as the used model.
In order to find the polynomial parameters the Matlab function polyfit() is used. This function
supplies the best fit for the data series illustrated in figure H.2 in the least-squares sense. The
resulting force polynomial is illustrated in equation H.3.
 
0
B  
Frotor (Sc ) =  0  (H.3)
3.2174 · 10−10 · Sc3 − 1.9531 · 10−6 · Sc2 + 5.4632 · 10−4 · Sc − 0.9150
In figure H.3 the generated force Frotor and the polynomial B Frotor (Sc ) is illustrated.
Model Validation
Validating the model for the thrust is difficult since the most obvious idea would be to compare
the measured response of the X3D moving just along the z axis to the simulated one. But as it is
only possible to measure the response of an actual flight of the X3D, a series of approximations
must be made. The test flight used for the validation is the test flight from appendix E. Indeed,
from appendix E it can be seen that there are large fluctuations in both the attitude and the
position of the X3D. Ideally the test flight would only feature changes along the z-axis in the
earth frame. This behaviour would however call for an extremely precise hovering control with
a skilled operator. But since this perfect flight can not be done, it is chosen to use only the
thrust input Sc and the changes in the z-earth axis in the validation. This approach assumes that















        

Figure H.3: The revised generated force Frotor relative to the input signal Sc
all other parameters are zero and that the X3D is a point mass on which is applied an upward
force Ftotal . The size of Ftotal can be calculated using the equation H.4. Where B Frotor (Sc ) is the
derived thrust polynomial, B CE · m · g is the gravitational force in the body frame and I f · B vz is
the induced inflow term.
B
Ftotal = B Frotor (Sc ) + B CE · m · g + I f · B vz (H.4)
The input Sc from the test flight E.1 on page 99 is applied to the Non-Linear system model
illustrated in figure 4.3 on page 29. Notice that the simulation is initiated when the X3D is in
mid air simulating the environment at which the performance specifications tests are defined.
The initial position of the X3D is based on the first position measurement from the test flight.
Also notice that not the entire data set from the test flight is used. The used interval is defined to
[1000:2000] thus the middle part of the test flight is considered.
The resulting response of illustrated in figure H.4. From figure H.4 it can be seen that the two
curves are quite alike.
Appendix Summary
In this appendix the model used in relation to the X3D force generation has been derived. The
derivation of the model’s parameters has been based on system identification and measurements
of the X3D response to various reference commands Sc . The model has been verified using mea-
surements taken from a test flight. The similarity between the estimated and measured response
indicates that the derived model for the generated rotor force Frotor is sufficient.
During the management of the measurement data it became clear that the measurements where
not consistent with the expected take off value. The problem was investigated by allowing the










          

Figure H.4: Illustration of the estimated z position of the X3D
platform to fly at the hight used during the thrust measurements. The test flight showed that the
true take off value was indeed lower than the measured one. It was chosen to directly manipulate
the measured data in order to get the take off vale that was known to be true. This approach is not
considered truly valid as the correct solution would be to investigated the root of the measure-
ment inconsistency further. However due to limited time this approach is considered legitimate
providing that it yields a response truly representative for the actual response of the X3D. The
derived model B Frotor (Sc ) have not been validated directly but is validated as part of the collective
non-linear simulation model validation in section 4.2.1 on page 28.
 
 
 
Figure H.5: The used test set-up and the modified one

Experiment Follow-up
Close to the ending of the project a possible flaw in the used test set-up was pointed out by the
project advisor. The flaw in the test set-up is illustrated in figure H.5 to the left. In figure H.5 it
can be seen that in the current test set-up some of the generated thrust is actually pushing down
on the scale thus increasing the weight of the platform set-up. This problem could be countered
using a modified test set-up that eliminates the effect of air flow on the scale. The modified
test set-up is illustrated in figure H.5 where a wooden crate in mounted on to of the scale thus
shielding it form the generated air flow. The X3D is situated on a cone still and the cone is
placed on the scale using a small opening in the wooden crate. It is believed that this modified
test set-up will provide more valid measurements and thus the correct take off value (Sc ≈ 1700).
However due to the late introduction of the modified test set-up it has been chosen not to redo
the experiment and maintain the initially found model. This approach is considered valid as the
group is highly convinced that the measurement inconsistency was in fact caused by the air flow
generated by the X3D.

Appendix I
Angular Velocity Measurements
This appendix contains the process and methods related to identifying the model of the X3D
internal controller. The Matlab scripts and related data sets are located on the project CD in the
System Identification folder.
In order to identify the values of the angular velocity model, a series of data sets must be ac-
quired. The data are used to identify the connection between the input to the internal controller
Sc and the body rates of the X3D Θ̇Θ.
The measurements are obtained by sampling the response of the X3D during a test flight. The
only means of measuring the response of the X3D is during flight when it is controlled by a hu-
man operator. The human operator focuses on performing rotations around the three body-axes
x,y and z individually. It is important that only one input channel is actuated at a time so that
the response to the specific input can be assumed to be uncorrelated with other inputs. A skilled
operator is needed to perform these challenging manoeuvres.
The structure of the internal controller and the parameters used for the test flight are illustrated in
figure C.3 on page 90. Using the controller block diagram it is possible to derive the structure of
the internal controller transfer function. This approach has not been used as the illustrated struc-
ture does not reveal any information about the dynamics of the system that is the aerodynamics
of the X3D.
Test flight procedure
Below is a short description of the procedure used in relation to the test flight and the data
measurement.
• The Vicon box is turned on
• The interface PC is turned on and wireless link is started.
125 of 139
• The control PC is started with Matlab Simulink
• The X3D is equipped with a fully charged battery and placed on the floor in the MTLab approxi-
mately at way point [ E x E y E z ]T = [ 0 0 0 ]T .
• The Euler angles and internal controller reference Sω is sampled and saved in a Matlab file.
• The X3D is flown manually while actuating the input Sω as much as possible. That is the actuation
of the angular velocities ωφ , ωθ and ωψ .
• When all three body-axes x, y and z have been actuated the X3D is landed and the data is evaluated.
The test flight was conducted several times in order to obtain satisfactory measurements where
a clear and individual actuation of the three inputs was visible. A total of two data sets where
collected in order to use one set as working data and the other set as validation. The working data
set consisted of 5856 samples with a sample rate of 0.01 second that is a complete experiment
time of 58.56 seconds or approximately one minute. Only 3500 of the samples where used in
this appendix as the beginning and the end of the test flight are not interesting in relation to the
process of system identification. The validation data set consisted of 7500 samples that is 75
seconds.
Measurement Data
In this section the raw measurement data is presented. In figure I.1 the measured Euler angles
from the working data set are illustrated. The measured Euler angles from the validation data
set are illustrated in figure I.2. Notice that the measurements contain small amounts of noise
and faulty measurements. The process of removing these problems is addressed in the following
section.






      







      








      


Figure I.1: The measured Euler angles Θ from the working data in degrees
126 of 139 I. Angular Velocity Measurements







       










       







       

Figure I.2: The measured Euler angles Θ from the validation data in degrees
Preparing Data for System Identification
In order to use the measured data for system identification it must be adapted so that the system
input [ Sφ Sθ Sψ ]T and output [ φ θ ψ ]T can be directly compared. This includes differentiating
the measured Euler angles to achieve the Euler rates and then transforming the rates to the body
frame. The measured data in figure I.1 also contain noise that must be filtered out. The process
of preparing the measured data for system identification will be addressed in the following.
The measured data contains three types of noise. One type of noise is caused by the Vicon
system 2.3.1 on page 10 and gives rise to high frequency noise. The other is caused by tracking
errors in the Vicon system causing the Euler angles to go to [ φ θ ψ ]T = [ 0 0 0 ]T . This error occurs
several times and therefore a filtering algorithm is implemented. The algorithm works as a zero-
order hold filter and checks if all of the measured angles are equal to zero and replaces the zero
with the last known valid measurements on the three axis respectively. The zero-compensation
used for the filtering is illustrated below.
for i=1:length(data);
if data(i)==[0 0 0] ;
data(i)= data(i-1);
end
end
This approach in effect means the last valid measurements are simply maintained until the Vicon
system is able to track the X3D again. This approach is assumed valid as the duration of each
tracking loss is in general quite brief.
From figure I.1 it can be seen that the measurement data contain high frequency noise. This noise
is assumed to be caused by the Vicon system tracking and must be removed before transforming
I. Angular Velocity Measurements 127 of 139

the measured Euler angles to Euler rates. Any fluctuations in the measurement data will be
magnified when the data are differentiated when transforming Euler angles to Euler rates. So
in order to filter out the noise, an averaging filter of 10 samples is used. The use of the filter
smoothes out the effect of the measurement noise. The resulting data after the filtering and zero
compensation is illustrated in figure I.3 and I.4. From figure I.2 it can be seen that a third type
of error is present in the data sets. This error causes the Vicon to mix up the orientation of the
platform and to rapidly change the orientation of the platform from 180◦ to −180◦ or the other
way around. At the time of this appendix no valid means of compensating for this error has been
derived. The used solution has simply been to avoid using intervals where the error was present.
Since the only measurements of interest are the measurements where the individual channels



 


      




 


      





 


      


Figure I.3: Illustration of the filtered working data
are actuated smoothly there is no reason to work on the collective measurements. Based on the
appearance of the data, three intervals of special interest have been identified. The intervals are
listed in table I.1. The intervals give rise to the data series illustrated in figure I.5 and I.6 The
Parameter Working Data Validation Data

Roll [1400:1900] [700:1300]
Pitch [1:500] [2200:3600]
Yaw [2500:3000] [3200:4200]
Table I.1: Intervals of special interest
measured data consist of the Euler angles and must thus be differentiated to supply the Euler
Θ are then transformed to body rates Bω using the transformation matrix
rates. The Euler rates Θ̇




 


       











       




 


       

Figure I.4: Illustration of the filtered validation data






          







          








          


Figure I.5: Illustration of the intervals of special interest for the working data set
B H (Θ
E Θ ). The used transformation is illustrated in equation I.2.
    
ωφ 1 0 −sθ φ̇
    
ω =  ωθ  = 0 cφ sφcθ  θ̇  (I.1)
ωψ 0 −sφ cφcθ ψ̇
= B HE (Θ
Θ)Θ̇
Θ (I.2)
Θ to body rates ω the collective measurements

Notice that in order to transform the Euler rates Θ̇
are used. This means that in order to find the angular velocity around i.e. the y-body axis ωφ







      










       







          

Figure I.6: Illustration of the intervals of special interest for the validation data set
the measurements on all three axis are used, see equation I.2. As the intervals of special interest
have been only defined on the appearance of the actuated axis there is no guarantee that the
corresponding measurements on the other two axis are fit for use. The problems related to this
fact has not been investigated in detail as the resulting body rates ω follow the input reference Sω
quite convincingly, see figure I.8.
System Identification
Based on the data from figure I.3 and the input reference values that is Sω it is possible to derive
a linear relationship between the input and output of the system. As it has been chosen to use a
constant gain for the three velocities the process of finding the best fit solution is quite simple and
can be done fairly precise by trial and error. However since the process of finding the right gains
that map the input values to the response of the platform is time-consuming and trivial Matlab is
used. Matlab contains a system identification toolbox that is able to find system models based on
system input and system output.
The system identification toolbox is able to identify complex models based on system input and
output data. The transfer function for the used model is illustrated in equation I.3.
G(s) = Ki · exp(−Ti · s) (I.3)
The model contains only a gain factor Ki and a time delay Ti . The time delay will not be used in
the final model that will consist of only the derived gain but it is used in the system identification.
Indeed, the use of a time delay in the identification process is assumed to enable Matlab to find a
better gain factor as some time delay is present in the response of the system. The found constants

are illustrated in equation I.4 - I.6.
Kφ = 0.0011574 and Tφ = 0.021 (I.4)

Kθ = 0.00092612 and Tθ = 0.021 (I.5)
Kψ = 0.0016476 and Tψ = 0.151 (I.6)
It can be seen form equations I.4 - I.6 that the delay in the response in along the xy-body axis
is almost 2 samples which is quite high. The response of the yaw that is the rotation around the
z-body axis is 15.1 samples and is thus even slower. It is also noted that the difference between
the pitch and roll constants are quite similar. This is expected at the platform is symmetric and
the internal controller parameters are identical.
The resulting scaling factor Kω can thus be written as in equation I.7.
 
0.0011574
 
Kω = 0.00092612 (I.7)
0.0016476
The input reference and output response used in the system identification process are illustrated
in figure I.7. Notice that the input reference signal has been multiplied with the scaling factor Kω
to illustrate the estimated response of the system. As it can be seen form the figure the estimated
response and the measured response are somewhat similar. In the section I the derived model is
validated using verification data.

 




          



 

          





 


          

Figure I.7: Working data reference Sω scaled by Kω and the measured system response

Model Validation
In this section the system model derived in the preceding section will be validated using the
validation data set. The objective is to investigate whether the derived model is able to model the
response of the system in a satisfactory manner.
The input reference and output response used in the model validation are illustrated in figure I.8.
Notice that the input references have been multiplied with the scaling factor Kω to illustrated the
estimated response of the system. As it can be seen form the figure the estimated response and
the measured response are somewhat similar. It can be seen on the figure I.8 that the estimated





      




 


       




 


          

Figure I.8: Validation data reference Sω scaled by Kω and the measured system response
angular velocities are matching the measured ones quite well. Thus, the model is considered
validated.
Appendix Summary
In this appendix the model used in relation to the generation of the platform’s angular velocity Bω
has been derived. The derivation of the model parameters has been based on system identification
and the measurements of the quad-rotor’s response to various reference commands Sω .
The derived model Kω has been validated using validation data and has proven to be sufficient in
the sense of modelling the response of the X3D to the input reference commands Sω . The found
model Kω is repeated in equation I.8.
 
0.0011574
 
Kω = 0.00092612 (I.8)
0.0016476

Appendix J
Frame Transformation matrices
In this appendix the transformation matrices used in the Modelling chapter 4 on page 24 are
derived. The need for transformation matrices will be addressed using a simplified example that
illustrates the problems related to navigating in multiple coordinate systems in the same time.
Imagine a situation where a helicopter pilot has lost all navigation instruments and is essen-
tially flying blind. In this situation a ground official must assume the task of talking the pilot
trough the process of landing. The ground official is only able to measure the position of the
helicopter in the Earth frame E and the helicopter pilot is only able to move relative to direction
vectors in the Body frame B. The helicopter pilot does not know his position but he knows his
attitude. Even though the ground official knows the position of the helicopter and the target desti-
nation the process of navigating a rotating body from another reference frame can be quite tricky.
The problem will be illustrated using two positions that is the target position E P(E x,E y) and the
current position of the helicopter E O(E x,E y). The direction vector E d illustrates direction from
the current position of the helicopter to the target position P relative to the Earth frame E. The
direction vector B d illustrates direction from the current position of the helicopter to the target
position B P(B x,B y) relative to the Body frame B. In figure J.1 the described situation is illustrated
in two-dimensional space. The use of two-dimensional space instead of three-dimensional space
will render the visualization of the example more apparent.
From the ground official it is quite easy to calculate the direction vector E d. The ground of-
ficial is able to measure the current position of the helicopter O(16,9) and the target position
P(4,13). Based on these two coordinate sets the direction vector E d can be calculated as P - O -
that is [4 − 16, 13 − 9]T = [−12, 4]T . The most apparent solution for the ground official is to send
the direction vector E d to the helicopter pilot and let him execute the movement. This will how-
ever cause problems as the helicopter pilot’s coordinate system is rotated relatively to the ground
official’s coordinate system, meaning that they are in different frames. If the helicopter pilot uses
direction vectors described in the Earth frame, he will not end up at the correct position. This
problem is illustrated in figure J.1 where E d̂ is the execution of the direction vector E d in the
Body frame. It is apparent that the pilot will not end up at position P even though the direction
vector from the ground officials point of view is valid.
133 of 139

 

  

 



 

 

 


  
Figure J.1: Illustration of the Helicopter example
The problem occurs as the frames are rotated with the angle θ to each other. If θ = 0 there is no
problem but when θ += 0 problems arise. Luckily for the helicopter pilot there is a simple way
of compensating to the rotating frame. The solution is to introduce a transformation or rotation
on the direction vector E d so that it is described in the body frame B instead of the earth frame
E. The new direction vector B d will describe the direction from point B O to point B P in the body
frame B. The structure of the clockwise rotation B AE (θ) about the body frame origin is illustrated
in equation J.2.
B
d = B AE (θ) E d (J.1)
6 7 6 76 7
Bx cos θ sin θ E x
B y = − sin θ cos θ Ey (J.2)
If the rotation angle θ is π/4 ≈ 45◦ the direction vector B d can be derived as illustrated in equation
J.4.
6 7 6 76 7
Bx cos θ sin θ E x
B y = − sin θ cos θ Ey (J.3)
6 76 7 6 7
cos 45◦ sin 45◦ −12 ≈ −5.6
= = (J.4)
− sin 45◦ cos 45◦ 4 ≈ 11.3
If the helicopter pilot executes the movement in accordance with the direction vector B d he will
end up at position P. The only thing the ground official has to do to compensate for the rotation
of the body frame is perform the transformation B AE (θ). Thus in order to guide the helicopter
down the ground official must know only the position of the helicopter, the helicopter attitude
and the target position.
134 of 139 J. Frame Transformation matrices

Euler Angles
The same problem appears when the Quad-rotor is to be controlled. The role of the ground
official is overtaken by the Vicon system that is able to track the position and attitude of the
Quad-Rotor in the Earth frame. The role of the helicopter pilot is overtaken by the designed
control scheme. Even though the frames are now in 3D the rotation of the Quad-Rotor, the body
frame gives rise to exactly the same problem as in the previous example. As in the previous
example there is a simple solution, indeed the orientation of one Cartesian coordinate system
with respect to another can always be described by three successive rotations. The orientation of
the X3D in the E frame can thus be described by three successive right-hand rotations around the
three axes. The rotations used in this project are a 3-2-1 sequence that is read as rotations around
the x axis (roll), around the y axis (pitch) and finally around the z axis (yaw). This sequence of
rotation is called 3-2-1 Euler angles. The sequence describes the rotation about the x-axis, then
a rotation about the new y-axis and finally a rotation about the new z-axis as illustrated in figure
J.2. The sequence of rotations can be defined using to transformation matrices as in equation J.5
  

 

 

 
  
  
Figure J.2: Illustration of the 3-2-1 Euler angle rotations [see 3, page 16 ]
- J.7 [see 3, page 15 ].

 
1 0 0
 
Cx (φ) = 0 cos(φ) sin(φ)  (J.5)
0 −sin(φ) cos(φ)
 
cos(θ) 0 − sin(θ)
 
Cy (θ) =  0 1 1  (J.6)
sin(θ) 0 cos(θ)
 
cos(ψ) sin(ψ) 0
 
Cz (ψ) = −sin(ψ) cos(ψ) 1 (J.7)
0 0 1
Using equations J.5 - J.7 it is possible to describe a transformation matrix B CE (Θ

Θ) from the E
frame to the B frame. This transformation is often called the direction cosine matrix. Notice
that cos, sin and tan is denoted c, s and t respectively. The direction cosine matrix B CE (Θ
Θ) is
J. Frame Transformation matrices 135 of 139

illustrated in equation J.9.
B
Θ) = Cx (φ)Cy (θ)Cz (ψ)
CE (Θ (J.8)
 
cθcψ cθsψ −sθ
 
= sφsθsψ − cφsψ sφsθsψ + cφcψ sφcθ (J.9)
cφsθcψ + sφsψ cφsθsψ − sφcψ cφcθ
In figure J.3 the MATLAB Simulink implementation of the B CE (Θ Θ) cosine matrix is illustrated.
In order to go from the B frame to the E frame the transformation matrix from the B frame to the

 

  


  


 
 

 


Figure J.3: Simulink implementation of the transformation matrix from Euler angles to body
angles.
E frame is needed that is E CB (Θ

Θ). The transformation matrix B CE (ΘΘ) is orthonormal meaning
E Θ) can be fond as illustrated in equation J.10.
that the inverse transformation CB (Θ
E
Θ) = B CE (Θ
CB (Θ Θ)−1 = B
Θ)T
CE (Θ (J.10)
Using equation J.10 the inverse direction cosine matrix that yields the transformation from the B
frame to the E frame can be describe as illustrated in equation J.12.
E
Θ) = B CE (Θ
CB (Θ Θ)T (J.11)
 
cθcψ sφsθsψ − cφsψ cφsθcψ + sφsψ
 
= cθsψ sφsθsψ + cφcψ cφsθsψ − sφcψ (J.12)
−sθ sφcθ cφcθ
In figure J.4 the MATLAB Simulink implementation of the cosine matrix B CE (Θ

Θ) is illustrated.


 

  


   

 
 
 

 


Figure J.4: Simulink implementation of the transformation matrix from body angles to Euler
angles.
Euler Rates
In order to map the Euler rates that is the angular velocity Θ̇ of the X3D in the earth frame E to
body rates Bω a third transformation matrix is needed. This transformation relates the derivative
Θ to the angular velocity of the body frame Bω
of the measured Euler angles that is Euler rates Θ̇
[see 3, page 25 ].
       
ωφ φ̇ 0 0
       
ω =  ωθ  = 0 + Cx (φ) θ̇ +Cy (θ)  0  (J.13)
ωψ 0 0 ψ̇
  
1 0 −sθ φ̇
  
= 0 cφ sφcθ  θ̇  (J.14)
0 −sφ cφcθ ψ̇
= B HE (Θ
Θ)Θ̇
Θ (J.15)
Where B HE (Θ
Θ) is the transformation matrix that relates the Euler rates Θ̇
Θ to the angular velocity
of the body frame Bω in the body frame B. The same approach than the one used to get the
transformation matrix E CB (Θ
Θ) can be used when finding the transformation matrix from body
B Θ that is E HB (Θ
rates ω to Euler rates Θ̇ Θ).
E −1
Θ) = B HE (Θ
HB (Θ Θ) (J.16)
 
1 tθsφ tθcφ
 
= 0 cφ −sφ  (J.17)
0 sφ/cθ cφ/cθ
Θ = E HB (Θ
Θ̇ Θ)ω
ω (J.18)
In figure J.5 the MATLAB Simulink implementation of the transformation matrix is illustrated.
J. Frame Transformation matrices 137 of 139



   
 






Figure J.5: Simulink implementation of the transformation matrix from body rates to Euler
rates.
Appendix Summary
In this appendix the rotation and transformation matrices used in the modelling chapter 4 on
page 24 have been addressed. The rotation matrix used in relation to the measured angles is the
3-2-1 Euler angle rotation matrix. The use of the rotation matrix enables the transformation of
angles from one frame to another. In relation to the Euler rates a second transformation matrix is
used. This matrix enables the transformation of angles rates from one frame to another.

Appendix K
Software Notes
The control software package has been developed for Windows OS. It is based on a Simulink
mdl file and additional Windows platform specific files. MATLAB is needed on the deployment
control PC. Two user-defined S-function blocks are included in the Simulink model: the Vicon
link and the X3D serial interface. Their code is contained in the platform dependent MATLAB
executable files, with the .mexw32 extension.
Virtual COM Port Drivers
The Virtual COM drivers from ftdichip.com need to be installed on the control PC. They “cause
the USB device to appear as an additional COM port available to the PC”. The application soft-
ware can then access the USB device in the same way as it would access a standard COM port,
using the system’s standard serial device API.
Developer notes on the X3D serial link interface
The X3D interface Simulink user-defined block implements the serial AscTec protocol and com-
municates with the X3D quad-rotor. It is a C++ program with 4 layers (Figure K.1), compiled
with MATLAB’s mex build script and an underlying C++ compiler. The block’s inputs and out-
puts have been described in the Control Software section of the report 2.4.2.
  
 


Figure K.1: Code components of the X3D interface program
139 of 139

(Project) Modelling and Control of Autonomous Quadrotor

Hochgeladen von

Dokumentinformationen

Originalbeschreibung:

Originaltitel

Copyright

Verfügbare Formate

Dieses Dokument teilen

Dokument teilen oder einbetten

Freigabeoptionen

Stufen Sie dieses Dokument als nützlich ein?

Sind diese Inhalte unangemessen?

Copyright:

Verfügbare Formate

(Project) Modelling and Control of Autonomous Quadrotor

Hochgeladen von

Copyright:

Verfügbare Formate

A A L B O R G U N I V E R SI T E T

Modelling and Control of Autonomous

2nd Semester Project

Faculty of Engineering, Science and Medicine

Title: Modelling and Control of Autonomous Quad-Rotor

Theme: Modelling and Control

Project period: Spring 2010

Group: 10gr833 Synopsis:

The scope of this project is to design and implement

3 Strategy and Requirements 18

VIII of 139 CONTENTS

5.4.2 Non-Linear model simulations . . . . . . . . . . . . . . . . . . . . . . . 55

B Performance Tests Results 82

C First Principles model 89

D Vicon Data Management 97

E Non-linear Model Verification - Test flight 98

F Vicon tracking precision 101

G X3D components 114

H Thrust Force Measurement 118

I Angular Velocity Measurements 125

J Frame Transformation matrices 133

K Software Notes 139

• Aerial Surveillance and Intelligence for Law Enforcement

• Hobbyist and Professional Aerial Photography and Video

• Property Assessment and Real Estate promotion

• Traffic monitoring and Traffic pattern Analysis

• Weed identification in Agriculture

• Educational learning Platforms

flight is a confined environment in presence of multiple types of obstacles. Obstacles may be

Figure 1.2: Picture of a museum landscape with showcases [www.museodirect.com]

1.1 Problem Statement

• Design a viable state space model for the X3D Quad-Rotor

• Investigate the designed controller’s ability to track a continuous trajectory.

1.2 Chapter Summary

1 Reference point in physical space used for purposes of navigation.

2.1 X3D Hardware

2.2 X3D Remote Control and Manoeuvring

Figure 2.1: Illustration of the X3D Quad-Rotor hardware components

2.2.1 Reference frames

2. System Analysis 5 of 139

2.2.2 X3D Control Inputs

Figure 2.4: Illustration X3D wireless remote control interface

6 of 139 2. System Analysis

Parameter Min Max Nominal range

Table 2.1: Wireless link output range [14]

2.2.3 X3D Manoeuvring

2. System Analysis 7 of 139

8 of 139 2. System Analysis

2. System Analysis 9 of 139

2.3 Motion Tracking System

2.3.1 Motion Tracking Lab

Figure 2.9: Pictures from MTLab AAU March 2010

10 of 139 2. System Analysis

Figure 2.10: Illustration of the MTLab hardware structure

2.3.3 Tracking precision

2. System Analysis 11 of 139

2.3.4 Tracking errors

12 of 139 2. System Analysis

Figure 2.12: Tracking jumps in the position signals

Figure 2.13: Tracking jumps in the orientation signals

2. System Analysis 13 of 139

Figure 2.14: Errors in the position signals