Full Text 01

Degree Project in Technology
First cycle 15 credits
Quadcopter Control System Design

and Implementation with LQR
assisted by Kalman Filter: An
Experimental Study
LUDWIG HORVATH & MARTIN PETRÉ
Stockholm, Sweden 2023

TRITA – ITM-EX 2023:77
www.kth.se
Quadcopter Control System Design and
Implementation with LQR assisted by
Kalman Filter: An Experimental Study
Ludwig Horvath & Martin Petré
Bachelor’s program in Vehicle Engineering

Date: 2023-06-05
Supervisor: Daniel Frede
Examiner: Daniel Frede
School of Engineering Sciences
Stockholm, Sweden 2023

1 Abstract
This study stems from the audacious assumption that two bachelor students could successfully con-
struct a functional quadcopter from scratch, covering both software and hardware aspects. The project
begins by establishing a foundation for modeling the quadcopter dynamics, including derivation and
linearization techniques for implementing a linear quadratic regulator (LQR). To validate the model,
a MATLAB simulation environment is developed, providing valuable qualitative insights for hardware
selection and a holistic understanding of the quadcopter system.
Moving beyond simulation, the implementation phase involves selecting hardware components based
on simulation results, consultation, and practical considerations. CAD modeling is utilized for 3D
printing, post-processing, and assembly, resulting in a physical drone. Extensive experimentation and
programming are conducted to enhance the robustness of sensors, while the challenges posed by radio
modules are addressed through message splitting techniques and the development of a software archi-
tecture that leverages the computational power of a laptop.
With the fundamental functions in place, efforts focus on approximating the idealized model. While
the BNO055 effectively approximates six of the twelve states, accurate estimation of the remaining six
states, hence position and velocity in all three degrees of freedom, proves challenging. Incorporating a
GPS, barometer, and sensor fusion techniques is explored but does not yield satisfactory results due
to velocity drift and slow position updates.
Although this report does not demonstrate the performance of a fully functional flying quadcopter, it
provides practical solutions to the challenges encountered during the construction process. These so-
lutions offer valuable insights and compensate for the initial ambitious goals. The subsequent sections
of the report analyze the encountered issues, draw conclusions about quadcopter theory, and propose
avenues for future development.
4
2 Referat
Denna studie utgår från det djärva antagandet att två kandidatstudenter framgångsrikt skulle kunna
konstruera en fungerande quadcopter från grunden, inklusive både mjukvaru- och hårdvaruaspekter.
Projektet inleds med en förklaring som lägger grunden för modelleringen av quadcopterns dynamik,
inklusive härledning och linjäriseringstekniker för att implementera en linjär kvadratisk regulator
(LQR). För att validera modellen utvecklas en MATLAB-simuleringsmiljö som ger värdefulla kval-
itativa insikter för val av hårdvara och en helhetsförståelse för quadcoptern som system.
Utöver simuleringen genomförs en implementationsfas som innebär att välja hårdvarukomponenter

baserat på simuleringsresultat, konsultation och praktiska överväganden. CAD-modellering används
för 3D-utskrift, efterbehandling och montering, vilket resulterar i en fysisk drönare. Omfattande
experimentering och programmering utförs för att förbättra robustheten hos sensorerna, medan ut-
maningarna med radiosändarmodulerna hanteras genom meddelandesplittringstekniker och bristen på
beräkningskraft och minne hanteras genom att utföra reglertekniska beräkningar hos datorn istället
för hos drönaren.
När de grundläggande funktionerna är på plats lades ansträngning på att så nogrannt som möjligt nå
den idealiserade modellen. Även om BNO055 effektivt approximerar sex av de tolv tillståndsvariablerna
visar sig noggrann uppskattning av de återstående sex, det vill säga position och hastighet i alla tre
frihetsgrader, vara utmanande. Att inkludera GPS, barometer och tekniker för sensorfusion undersöks
men ger inte helt tillfredsställande resultat på grund av hastighetsdrift och långsamma positionsupp-
dateringar.
Även om denna rapport inte visar prestandan hos en fullt fungerande flygande quadcopter, ger den
praktiska lösningar på de utmaningar som uppstod under konstruktionsprocessen. Dessa lösningar
erbjuder värdefulla insikter och kompenserar för de ursprungliga ambitiösa målen. De efterföljande
avsnitten i rapporten analyserar de påträffade problemen, drar slutsatser om quadcopter-teori och
föreslår möjliga utvecklingsvägar framåt.
5
3 Acknowledgements
We express our gratitude to our supervisor, Daniel Frede, for granting us the opportunity to undertake
this project and for his continuous availability and guidance throughout the process.
Furthermore, we extend our appreciation to Lei Feng for his assistance in tuning the LQR controller
and the Kalman Filter aswell as his affirmation of the system design.
Lastly, we would like to acknowledge Seshagopalan Thorapalli Muralidharan for his early consultation
on the system design and the project as a whole during the initial planning phase.
6
4 Preface
Quadrotors or quadcopters have gained popularity in recent years since they started appearing as a
consumer product. Since the 1930’s pioneers have attempted to construct VTOL (Vertical takeoff and
landing) vehicles with the design principles of a quadcopter having the presumption that it would
result in a system which is highly controllable and maneuverable. However, this was not achieved by
the pioneers in practice due to the absence of computational aid which led to immense pilot work load.
Today due to the existence and development of computational aid as well as light weight construction
materials that concept is very well applicable.
Quadcopters are often refereed to as drones since it is obvious which sort of drone people are referring
to, it is important to note however that a drone is a broader category in which a quadcopter count as
a multi-rotor drone. Another common term is UAV which stands for unmanned aerial vehicle - the
quadcopter designed during this thesis is therefore also rightfully a UAV.
A number of important design principles are presented such that the reader feels prepared for the
formal sections. Typically a quadcopter consists of a stiff body which at its outer ends has elements
for propulsion. A common setup is a cross or plus configuration which is symmetric and in which the
propulsion is realized by a BLDC-motor (Brushless DC ) motor which rotates a propeller. The pro-
peller creates two essential phenomena: Drag - the propellers experiences resistance in the transversal
direction from the fluid through which it rotates. The other effect is of course: Thrust - the propeller
creates thrust by pushing the air downwards by use of its geometry and motion. With four independent
actuators creating thrust and drag, the drone can move to an arbitrary point while assuming an arbi-
trary orientation in the horizontal plane - given that this orientation is stable. To give a more detailed
understanding, presented below are two figures. Figure 1 a) illustrates the configuration together with
the essential definitions to begin the modeling. Figure 1 b) shows how an actuator creates drag and
thrust. The torque τr is a reactive torque due to the drag which it has to overcome in order to create
thrust upwards - thrust which compensates for the effects of the gravitational force, indicated by g.
(a) (b)
Figure 1: (a) Configuration of Quadcopter in terms of its body fixed coordinate system and direction
of actuators angular speeds. (b) Illustration of propeller, the forces and torques acting.
7
To further explain how a quadcopter is maneuvered. The quadcopter creates a net-force vector in the
desired direction by changing its orientation while producing thrust. The thrust from each propeller
as well as the orientation has to be given as instructions from a controller which determines and sends
these instructions as inputs given the earlier output of the quadcopter’s state as well as its defined
goal. The controller does this by rigorously calculating values in real-time which in effect scales the
rotational speed of the propellers. Part of the theoretical basis for these calculations commence in the
clever concept by which the quadcopter rotates and about its every axis.
Below, the least obvious technique is illustrated, how it produces yaw. The yaw angle Ψ has to
be controlled in order to maintain the face-first orientation, suitable if one wants to attach a camera.
The yaw angle is changed by creating a net torque about the corresponding axis, hence the z-axis.
The origin of this torque was illustrated above, now imagine four origins that work in pairs. These
motors can produce a net-torque about the z-axis which alignes with the mass center of the quadcopter
(ideally). Rotating one pair of propellers faster CW (clockwise) than the pair rotating CCW (counter
clockwise) will produce a net reaction torque on the entire body rotating it in the positive z-direction
giving rise to change in yaw which is CCW 2 a). The opposite strategy creates the opposite effect,
hence if Ω2 and Ω4 overpower Ω1 and Ω3 , the quadcopter will rotate CW 2 b).
(a) (b)
Figure 2: (a) Configuration of Quadcopter in terms of its body fixed coordinate system and direction
of actuators angular speeds. (b) Illustration of propeller, the forces and torques acting.
To move the drone in the horizontal plane it tilts itself resulting in a net-force pointing in the desired
direction. The vertical component of the thrust cancels out the weight. Meanwhile its horizontal
equilibrium is achieved after reaching a speed which implicates a drag-force, that once the desired
speed is reached, will cancel out the net thrust in that direction 3.
Figure 3: Standard sensor-fixed reference frame SBN O for Bno055 with angular velocities defined.
8
Contents
1 Abstract 4
2 Referat 5
3 Acknowledgements 6
4 Preface 7
5 Introduction 15
6 Pilot study 16
7 Mathematical modeling 18
7.1 Definitions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
7.2 Configuration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
7.3 State Space Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
7.4 Linearization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
8 Control strategy 22
8.1 System overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
8.2 LQR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
9 Simulation 24
10 Hardware 26
10.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
10.2 Microcontroller - Arduino Uno . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
10.3 Inertial Measurement Unit - BNO055 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
10.4 GPS - Adafruit Ultimate GPS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
10.5 Barometric Pressure Sensor - BMP280 . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
10.6 Radio Module - nrf24L01 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
10.7 Powertrain . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
10.8 Basis for Powertrain Choice . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
11 Position and Velocity Estimation 32

11.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
11.2 Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
12 Implementation 38
12.1 Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
12.2 Filters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
12.3 States . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
12.4 Tuning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
12.5 Construction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
12.6 Performance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
12.7 Model vs Reality . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47
13 Conclusions and future developments 48
A Kinematics derivation 49
B Kinetics derivation 52
9
C Actuator dynamics derivation 53
D Performance of Power System for remaining Setups 54
E Source Code 57
10
List of Figures
1 (a) Configuration of Quadcopter in terms of its body fixed coordinate system and di-
rection of actuators angular speeds. (b) Illustration of propeller, the forces and torques
acting. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2 (a) Configuration of Quadcopter in terms of its body fixed coordinate system and di-
rection of actuators angular speeds. (b) Illustration of propeller, the forces and torques
acting. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
3 Standard sensor-fixed reference frame SBN O for Bno055 with angular velocities defined. 8
4 Flowchart of the project and its stages ranging from looking up how drone moves until
there is a working drone. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
5 An overview of the setup, quadcopter piloted from the computer - they communicate
wirelessly via radio. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
6 Figure depicts the configuration which is used in the mathematical model, observe the
symbols which were defined earlier. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
7 Block schematic over the idealized full state LQR controlled system. . . . . . . . . . . . 23
8 Simulink model of the simulated system. Notice the manual control inputs. . . . . . . . 24
9 Simulink model of the simulated system. Notice the manual control inputs. . . . . . . . 25
10 The inputs of a dysfunctional system, notice they plumit below 0. . . . . . . . . . . . . 25
11 An overview of the drone’s hardware components . . . . . . . . . . . . . . . . . . . . . . 26
12 Standard sensor-fixed reference frame SBN O for BNO055 with angular velocities defined. 27
13 Standard definition of Euler angles provided by Bno055. The yaw angle ψ is defined in
negative z-direction relative to magnetic north. . . . . . . . . . . . . . . . . . . . . . . . 27
14 Motor voltage, current and total Power requirements for Setup 1. . . . . . . . . . . . . . 30
15 Green line represents consumed energy [J] as a function of time [s]. The red dotted line
shows the energy threshold at which the battery is depleted. . . . . . . . . . . . . . . . . 31
16 The earth is displayed along with GCS and the inertial reference frame as well as the
estimated range of the different GCS coordinates. . . . . . . . . . . . . . . . . . . . . . . 32
17 The earth is displayed along with GCS and the inertial reference frame as well as the
estimated range of the different GCS coordinates. . . . . . . . . . . . . . . . . . . . . . . 33
18 Block diagram over Kalman filter as an observer with optimal gain K. . . . . . . . . . . 34
19 Computational algorithm and block diagram over discrete Kalman filter. . . . . . . . . . 37
20 Architecture over the drone’s software. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
21 Figure depicting a successful transmission utilizing acknowledgments. . . . . . . . . . . . 39
22 State-machine of the radio-protocol on computer side. (SoS - Send over Serial Port) . . 40
23 Digital second order Butterworth filter. T denotes the delay variable which equals the
sample time of the filter. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
24 Power Spectral Density and Frequency response function of the second order Butter-
worth filter. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42
25 Controls for piloting the quadcopter. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
26 The quadcopter separated from it shell and mounted on a testing rig for tuning. . . . . 45
27 A sample from a attitude tuning test locking all DOF except for pitch Θ, notice the
spikes. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
28 (a) Rotation about X axis misaligning the initial body frame with the latter. (b) Rota-
tion about Y. (c) Rotation about Z. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
30 Energy Consumption plot for setup 2. Green line represents consumed energy [J] as a
function of time [s]. The red dotted line shows the energy threshold at which the battery
is depleted. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
11
32 Energy Consumption plot for setup 3. Green line represents consumed energy [J] as a
function of time [s]. The red dotted line shows the energy threshold at which the battery
is depleted. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
12
List of Tables
1 The table contains most of the definitions which are used and referred to during the
mathematical derivations to come. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
2 Specification of the Bno055’s measured output . . . . . . . . . . . . . . . . . . . . . . . 28
3 Drive-train components for each Setup. . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
4 Unit table for parameters in table 3 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
5 Flight-performance for each setup when flying the described elliptical path. . . . . . . . 31
6 Chosen data size in bytes and decimal precision for each variable in the desired state
that is sent over radio . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
7 Mapping between keys and affected states when pressing them. . . . . . . . . . . . . . . 43
8 The authors contentment regarding different parts of the project . . . . . . . . . . . . . 48
13
List of acronyms and abbreviations
BLDC Brushless Direct Current
CAD Computer Aided Design
CRC Cyclic Redundancy Check
ESC Electric Speed Controller
GPS Global Positioning System
I2C Inter-Integrated Circuit
IIR Infinite Impulse Response
IMU Inertial Measurement Unit
LQR Linear Quadratic Regulator
PWM Pulse Width Modulation
14
5 Introduction
This report presents a comprehensive study on the development, analysis, and control of a quadcopter
system, acknowledging the challenges encountered during the design and implementation phases. The
primary objective of this research project was to construct a functional quadcopter, incorporating both
software and hardware aspects, while overcoming various obstacles along the way. The study focuses
on the modeling and control aspects of the quadcopter, with particular emphasis on the utilization
of a linear quadratic regulator (LQR) for control optimization. Additionally, the report discusses the
complexities associated with sensor integration, the evaluation of hardware components, and the chal-
lenges of maintaining stability in the presence of disturbances.
The project commenced with a detailed investigation into the dynamics of the quadcopter, resulting
in the derivation and linearization of a mathematical model. A simulation environment was created
using MATLAB to validate the model and gain qualitative insights into system behavior. The sim-
ulation results provided valuable guidance for hardware selection and enhanced understanding of the
quadcopter’s operation.
The implementation phase involved several stages, including CAD modeling, 3D printing, post-processing,
and assembly, to transform the theoretical design into a physical quadcopter. Selecting and calibrating
sensors proved to be challenging, requiring extensive experimentation and programming to improve
their reliability. Additionally, the integration of radio modules necessitated the development of a trans-
mission protocol to mitigate their inherent unreliability.
Addressing the need for accurate state estimation, the study explored the integration of GPS, barom-
eter, and sensor fusion techniques to approximate the quadcopter’s twelve states. Despite significant
efforts, precise estimation of position and velocity in all three degrees of freedom proved to be a
formidable task.
The report also discusses the presence of analog disturbances and faulty hardware, resulting in spikes
in the system response. These disturbances, characterized by high-frequency energy content, presented
stability challenges that requires further hardware improvements and filtering techniques in the future.
Although the project did not achieve the ultimate goal of a fully functional flying quadcopter within
the given timeframe, the report offers valuable insights into the design, construction, and control as-
pects of quadcopter systems. The challenges encountered provide valuable lessons, shedding light on
the complexities of translating theoretical knowledge into practical implementation.
To conclude the introduction properly, our research questions are the following.
1. Is LQR sufficient for controlling a quadcopter?
2. If possible under what conditions? If not, then why?
15
6 Pilot study
Before investing time into the project it was nessecary to at least qualitatively evaluate whether the
task seemed feasable or not. Thus, it was proposed that a flowchart should be made and from that it
was possible to get an overview and a better understanding of the risks involved 4 .
Figure 4: Flowchart of the project and its stages ranging from looking up how drone moves until there
is a working drone.
16
A quick walkthrough of the flowchart is hereby presented. As little was known about exactly how a
quadcopter changes its orientation a small study was conducted. When that was understood it was
time to move on to modeling the dynamics of the drone in which several topics were new, specifically in
the kinematics. This led to a lot of time invested into studying 3D dynamics, the generalized concepts
allowed for a deeper understanding. With the newly obtained knowledge it was time to move on to
specifically connecting the theory to the physical behavior of the quadcopter.
A number of master thesis’s were read, the ones that covered less complicated control strategies were
mostly targeted. One study discussed the possibility of using LQR (Linearized Quadratic Regulator )
for auto-piloting the drone. Since LQR was a known concept it was decided that it was possible to
create a quadcopter given that LQR was used.
As the model had been linearized and defined in matlab together with other functions it was seen
that the drone worked in theory - hence according to the mathematical model. A bit of tuning high-
lighted some important points and effects which will be covered later in the paper.
It was then considered that the simulation could be used to further prepare before moving on to
implementation. With theory from electronics and a lot of product specifications a number of sim-
ulations were made not only to observe the mechanical behavior but also the electrical parameters.
Current and voltage was carefully analyzed to make sure that the demanded input was realistic, from
that point and forward a number of different hardware setups was compared.
It was also understood that if the mathematical model, which was very idealized, were to reflect
the reality - then an effort should be made to make the reality as idealized as possible. This is mani-
fested in the presence of the filters: IIR filter and kalman filter. The kalman filter makes the yielded
position very exact by combining the attributes from the GPS and the IMU and exploiting their re-
spective strengths letting them compensate for each others weaknesses.
These filters are referred to as digital, that together with the implementation of the radio module
and a demand for fast real time calculations led to a lot of programming both in the Arduino IDE
as well as in python. Simultaneously some 3D-modeling was conducted in Solid Edge to realize the
physical model as the software was ready to be implemented and tested. The tests gave opportunity
for valuable data collection which was used to tune the quadcopter to fit its purposes and ultimately
fixing bugs and improving performance. The setup is, while stylized and simplified, by large measures
also representative of the total system 5.
Figure 5: An overview of the setup, quadcopter piloted from the computer - they communicate wire-
lessly via radio.
17
7 Mathematical modeling
7.1 Definitions
Before introducing the formal derivation of the dynamical system and its control algorithm, a handful
of definitions are listed. See table 1.
SR0 (X0 , Y0 , Z0 ) Initial body frame p Angular velocity about x [rad/s]
Sb (x, y, z) Body fixed frame q Angular velocity about y [rad/s]
SRG (X, Y, Z) Earth local inertial frame r Angular velocity about z [rad/s]
Ω1 Angular velocity rotor 1 [rad/s] CW u Velocity in x direction [m/s]
Ω2 Angular velocity rotor 2 [rad/s] CW v Velocity in y direction [m/s]
Ω3 Angular velocity rotor 3 [rad/s] CCW w Velocity in z direction [m/s]
Ω4 Angular velocity rotor 4 [rad/s] CCW x Position in X [m]
Φ Roll angle, angle about X [rad/s] y position in Y [m]
Θ Pitch angle, angle about Y [rad/s] z position in Z [m]
Ψ Yaw angle, angle about Z [rad/s] ft Total thrust in z direction [N]
Rx Rotation matrix about X [-] ft Total thrust in z direction [N]
Ry Rotation matrix about Y [-] τx Roll torque about x [Nm]
Rz Rotation matrix about Z [-] τy Pitch torque about y [Nm]
RZY X Rotation matrix (3-2-1) [-] τz Yaw torque about z [Nm]
m Mass of drone [kg] ⃗x State vector [-]
l Dist. between BLDC and mc [m] ⃗u Input vector [-]
Ixx Moment inertia about x [kg · m2 ] A System matrix [-]
Iyy Moment inertia about y [kg · m2 ] B Input scaling matrix [-]
Izz Moment inertia about z [kg · m2 ] C Output scaling vector [-]
Table 1: The table contains most of the definitions which are used and referred to during the mathe-
matical derivations to come.
18
7.2 Configuration
An illustration of the drone together with its defined rotor speeds, its body fixed reference frame as
well as its center of mass and the crucial dimension l, see figure 6. Please note that the construction
is symmetric, therefore it would be redundant to specify the length of the two remaining arms.
Figure 6: Figure depicts the configuration which is used in the mathematical model, observe the
symbols which were defined earlier.
To briefly explain the coordinate systems. SR0 is the initial body frame which aligns with the inertial
frame from the IMU’s (Inertial Meassurement Unit) point of view. The IMU is not guaranteed to be
aligned with the local inertial frame which acts as the coordinate system for the GPS.
19
7.3 State Space Model
The state space model is the state space representation of the differential equations derived starting
from Newtons law for rotation and translation. The essential steps in the derivation are captured in
appendix, see section: Kinematics, Kinetics and Actuator Dynamics. From these sections a total of
twelve differential equations are obtained which are presented in this subsection, each one defining one
state explicitly.
⃗ which stores the state variables showcased as below.

Let the state vector be denoted X
⃗ = [Φ Θ Ψ p q
X r u v w x y z] (1)
The purpose of this small section is to summarize the equations presented in (22) and (13) to fully
define the dynamic state of the quadcopter in a state space representation. An intermediate result is
a system of twelve unique equations which are presented below where the state space variables have
been solved explicitly from the equations in the referenced systems.



 Φ̇ = p + rCΦ Sθ + qSΦ tθ
Θ̇ = qCΦ − rSΦ




Ψ̇ = rCθ−1 CΦ + qSΦ Cθ−1




ṗ = Iz −Iy + τx



 Ix Ix
 Iz −Ix τy



 q̇ = Ix + Iy
I −I
ṙ = xIx y + τIzz


(2)


 u̇ = rv − qw − gSθ




 v̇ = pw − ru + gSΦ Cθ
= qu − pv + gCθ CΦ



 ẇ

ẋ = w[Sθ SΨ + CΦ CΨ Sθ ] − v[CΦ SΨ − CΨ SΨ Sθ ] + u[CΨ Cθ ]





ẏ = v[CΦ CΨ + SΦ SΨ Sθ ] − w[CΨ SΦ + CΦ SΨ Sθ ] + u[Cθ SΨ ]





ż = w[C C ] − u[S ] + v[C S ]]
Φ θ θ θ Φ
Also it is hereby stated that.
m⃗v˙ = R
⃗ f⃗B = mg e⃗z − ft R
⃗ e⃗3 (3)
Expanding the expression and writing out its components yields.

−ft
ẍ = m [SΦ SΨ + Cθ CΨ Sθ ]

−ft
ÿ = m [CΦ SΨ Sθ − CΨ SΦ ] (4)
z̈ = g − fmt [CΦ Cθ ]


To formulate the final state space representation in a desired linear expression it is necessary to linearize
the equations presented in (26) and (28) about a stationary point. The stationary point is an arbitrary
point in space when the quadcopter hovers in the horizontal plane, hence the pitch and roll angles are
zero.
20
7.4 Linearization
Since the desired shape of the expression is.
⃗˙ = AX
X ⃗ + B⃗u (5)
As the left hand side suggests X ⃗ has to be acquired. B is a matrix which scales the input to different
output state derivatives. The input ⃗u is manifested by the square of the angular speeds of the four
actuators. These rotors implicate the quantities of ft , τx , τy and τz as (23) and (24) shows. Therefore
the coefficients which these quantities scale are collected in the B-matrix. A relates the derivative of
different states to the states themselves and only store constants.
⃗˙ = [ Φ̇ Θ̇ Ψ̇ ṗ q̇
X ṙ u̇ v̇ ẇ ẋ ẏ ż ] (6)
By use of small angle approximations for the roll and pitch euler angles the following matrices are
acquired which makes expression (29) complete.
 
0 0 0 1 0 0 0 0 0 0 0 0
0 0 0 0 1 0 0 0 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 0 0 0 0 0 0 0 0
 
A=  0 0 0 0 0 0 0 0 0 0 0 0
 (7)
0 −g 0 0 0 0 0 0 0 0 0 0
 
g 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 0 0 0 1 0 0 0 0
0 0 0 0 0 0 0 0 1 0 0 0
Identification of terms controlled by input are scaled by elements stored in B which after the small
angle approximation becomes.
0 0 0 0
 
 0 0 0 0

0 0 0 0


 1 
 0 Ix 0 0
1
 

 0 0 Iy 0
1 
 0 0 0
B= Iz  (8)

 0 0 0 0

 0 0 0 0
− 1 0 0 0
 m 
 0 0 0 0
 
 0 0 0 0
0 0 0 0
The linearization of the model is hereby finished and the state space model can be used for simulating
and testing controllers as well as filters of different types - it describes the system of the quadcopter
when it flies close to a stationary point in terms of its orientation. Based on the system matrix A
an LQR controller can be obtained which has the ability to scale the input such that the system
remains close to the stationary point although there is in fact wind, air resistance and other kinds
of disturbances. Much of the derivation is inspired or taken from [7], some slight variations occur
however.
21
8 Control strategy
8.1 System overview
The system which is to be controlled is the quadcopter which has four identifical rotors but in which
crossed pairs spin in the opposit direction. These rotors have a lift and a drag coefficient as presumed
earlier. The system was earlier modeled and it is apparent that the mass and the moment of inertia are
important physical parameters which affects the system behavior - therefore these must be obtained.
The system is described by twelve different states, these states are in a stationary point satisfying the
following homogeneous solution to (29).
⃗ = [0 0 0 0 0 0 0 0 0 x y z]
X (9)
Hence, the stationary point is an arbitrary point in space where the quadcopter hovers in the horizontal
plane and does not rotate about its x- nor its y-axis in Sb .
It is in a practical sense important to know the relationship between the physical quantities that
manifest the input and the actual input quantity which can be directly controlled. As (23) and (24)
shows, these relations are linear given that the rotational speed is squared of each rotor. The solution
of the equation below gives the required squared rotational speed of each rotor.
     1
ft b b b b Ω1
τx  −bl 0 bl 0  Ω22 
 =   (10)
τy   0 −bl 0 bl Ω23 
2
τz −d d −d d Ω4
Solving (34) for Ω⃗ 2 yields a unique solution for each rotor since every rotor has a pre-determined
direction of rotation.
22
8.2 LQR
The controller used for determining the inputs is mainly an LQR (Linear Quadratic Regulator ) which
is obtained as an optimal solution to a quadratic cost function. In this case a full-state feedback LQR
is used, meaning all twelve states are included in the calculation of the state feedback.
Assuming that the real-life quadcopter can measure all twelve states accurately, the following block
scheme will resemble the overall system 7.
Figure 7: Block schematic over the idealized full state LQR controlled system.
⃗xref is the reference which in addition to the arbitrary stationary point (33) produces the state which
is desired for the quadcopter at that instance. Please overlook the sign before L as its purpose is a
mere algebraic correction.
In this project, when developing the LQR controller, the System & Control toolbox in MatLab was
used extensively. The proof of the solution to the LQR problem is not provided anywhere in this
report, it does not reflect the goal neither was there any attempt to solve the equation analytically.
As figure 7 suggests, the full state feedback is a vector product of the state error: ⃗e and the neg-
ative of the L-matrix. The L matrix is the obtained result of importance when solving the LQR
problem and is referred to as the optimal gain. The calculations are easily done in MatLab given the
system matrix: A, and the weight matrices Q and R which correspond to the outputs and inputs re-
spectively. The initial design of the LQR controller was conducted in MatLab for simulation purposes,
however as the project progressed and the physical quadcopter was tested a testing protocol in Python
meant that the same calculations had to be conducted in Python, this was done with a similar library
to the one used in MatLab see Appendix E for further details.
The weight matrices Q and R are of different sizes. Q is 12x12 and is also a diagonal matrix in
which the diagonal holds elements which correspond to the different state variables. The weights scale
the cost of these state variables, the higher the cost the higher importance in this case. Therefore a
large weight indiciate that the optimal gain is adjusted such that those states are prioritized. In the
context of inputs however, the interpretation differs - the R matrix is a 4x4, also diagonal, matrix
which holds weights for the different inputs. Scaling the cost of the diagonal elements indicates that
the input is more expensive and less can be exploited to regulate for errors. Since every actuator is
identical, in its effort and purpose, every element in the R matrix is identical and identically scaled.
To conclude this section, tuning the LQR is an iterative process where simulations are evaluated
as a result of different weight matrices Q and R. Eventually it narrows down to a result which is close
to the desired one in terms of the displayed performance.
23
9 Simulation
To verify the many assumptions and understand how the system, in theory was to be controlled, many
simulations were made. A simulation environment was programmed in MatLab where different ref-
erence tracks could be generated as desirable trajectories for the quadcopter. One arbitrary elliptic
track for overall agility, two others which were implemented later in Python for real-life testing. The
code is available in the Appendix E
The software enables an iterative process where the performance can be visualized in a 3D plot,
highlighting the difference between the desired reference and the calculated state from the simulated
system. This model only considers gravity as a disturbance, and the primary objective is to identify
which state variables should be manipulated and which ones should not.
Below is an overview of the Simulink model, which facilitates testing of manual control and refer-
ence tracking. The simulated model is derived from the linear model, which also serves as the basis for
the regulator. It’s important to note that the model does not accurately reflect reality, as it neglects
factors such as reaction torque on the propellers, air resistance, and its accuracy is limited to the
vicinity of the linearized stationary point.
Figure 8: Simulink model of the simulated system. Notice the manual control inputs.
Additionally, the electric model representing the coupling between the BLDC (brushless DC) motor
and battery is simplified. Altogether what was extracted from studying the simulation was the behavior
of the regulator for which one could identify potential phenomena that may occur if it is improperly
tuned.
24
To put weight into this argument, see the plot below which presents an example of a functioning
regulator all assumptions considered 9. The track is en ellipse around which two turns are made - this
can be understood from the X, Y and Z coordinates.
Figure 9: Simulink model of the simulated system. Notice the manual control inputs.
If one were to have to low margins on the potential thrust generated from the motors and or use
propellers with a low drag coefficient then the following results will be obtained from the simulation.
Figure 10: The inputs of a dysfunctional system, notice they plumit below 0.
When plotting the states as shown in Figure 10, the results remain unchanged because the regulator
continuously increases the input to meet its requirements. However, the valuable insights are derived
from Figure 10. As previously mentioned, the rotation direction is fixed and cannot change during
flight. In this case, the propellers exhibit such changes, leading to imaginary solutions, which explain
why the PWM signals appear to vanish from the real plane. This is one important insight that was
gained from the simulation, with this in mind the hardware that was picked would later also not exhibit
this behavior.
25
10 Hardware
10.1 Introduction
This chapter describes the hardware components of the drone. An Arduino Uno, microcontroller, is
used to enable communication between itself and each sensor, make necessary computations and send
the drone’s state to an additional micro controller on the ground by radio. The purpose, measurement,
and limitations of each sensor used in the drone will be explained aswell as the choice of drivetrain
components. Everything on the drone is powered by a LiPo battery, including the microcontroller that
receives regulated voltage through a switch voltage regulator.
Figure 11: An overview of the drone’s hardware components
10.2 Microcontroller - Arduino Uno

Two Arduino Unos microcontrollers are utilized for this project. One situated in the drone and one
connected to the computer via USB. The drone’s microcontroller communicates with the BNO055 and
BMP280 using the I2C protocol whereas the GPS sends data over hardwareserial, i.e. RT and TX.
The Arduino connected to the computer communicates to other software using the Serial port. The
arduinos provide an acceptable amount of memory and computing power for this project.
10.3 Inertial Measurement Unit - BNO055

The BNO055 is a 9-axis IMU that provides the user with absolute orientation through Euler angles by
enabling sensor fusion between the 3-axis gyroscope, 3-axis accelerometer, and 3-axis magnetometer.
Apart from raw sensor data the unit also outputs lowpass filtered body-fixed linear acceleration where
the influence of gravity has been removed from the raw acceleration data.
The sensor data will be used to measure some of the system’s state variables. This includes: Euler
angles (Θ, Φ, Ψ); body-fixed angular velocities (p, q, r) and body-fixed acceleration values (aX , aY , aZ ).
26
The sensor measurements are defined according to the figure below, see figure 12 and 13. The state
variables of interest, their units, sample frequencies and respective ranges can be found in the table,
see table 2.
Figure 12: Standard sensor-fixed reference frame SBN O for BNO055 with angular velocities defined.
Figure 13: Standard definition of Euler angles provided by Bno055. The yaw angle ψ is defined in
negative z-direction relative to magnetic north.
27
Measurement Notation Unit Sampling Frequency [Hz] Range
◦
Euler (Φ, Θ, Ψ) 100 (-180,180); (0,360)
◦
Gyro (p, q, r) /s 100 ±1000
Linear acceleration (ax , ay , az ) m/s2 100 ±4 · 9.82
Table 2: Specification of the Bno055’s measured output
Now, the sensor’s coordinate system SBN O is not equivalent with the body-fixed coordinate system of
the drone, see figure 6 and 28. Therefore, a linear transformation is needed to obtain the body-fixed
acceleration and angular velocities in the correct frame. The following transformation accomplishes
the desired change.
    
p 1 0 0 ωx
⃗ B = q  = 0 −1 0  ωy 
ω
r 0 0 −1 ωz
  
1 0 0 ax
⃗aB = 0 −1 0  ay 
0 0 −1 az
The provided raw Euler angles by the sensor exacly equates to the orientation angles defined for the
drone.
Calibration of the Bno055 is of great importance for robust sensor readings. By removing potential
offsets in angular velocity and linear acceleration, the calibration ensures that the sensor reads minimal
values when stationary. Additionally, the calibration removes influence of hard iron magnetic sources.
However, the impact of soft iron magnetic sources is harder to reduce by calibration because of the
magnetic source’s changing magnitude and direction [8]. More about how these effects were taken into
account during design can be found in the implementation chapter.
10.4 GPS - Adafruit Ultimate GPS

This GPS unit, which is built on the MTK3339 chip-set, is small in size and can provide location up-
dates at a rate of up to 10 Hz. The unit provides precise longitude, latitude, and altitude measurements
with respect to the Earth’s geographical north pole, these quantities can then be used to calculate the
position relative to a earth’s local inertial reference frame. According to the unit’s datasheet, the
sensor has a position accuracy of 3 m and a horizontal speed accuracy of 0.1 m/s [4].
10.5 Barometric Pressure Sensor - BMP280

The BMP280 sensor is a absolute and relative barometric pressure sensor that allows for relative
altitude change readings. The BMP280 measures relative pressure with an accuracy of ±0.12 hPa.
which equates to an accuracy of ±1 m in altitude [1].
10.6 Radio Module - nrf24L01

Two nrf24L01 radio modules were used to establish communication between the airborne drone’s Ar-
duino and the Arduino connected to a computer. The module provides an air data rate ranging between
250 kB/s to 4 MB/s with each radio packet consisting of maximum 32 bytes data. To ensure that a
message has been sent and obtained correctly, the receiving module sends back an acknowledgment
to the transmitter for each package. There is also opportunity to fit an additional message inside the
28
acknowledgement which in turn opens up possibility for simple but effective two-way communication
[3].
10.7 Powertrain
The drone’s powertrain consists of a lipo battery which provides voltage to 4 electronic speed controllers
(ESC) in parallel. Each ESC is paired with a brushless direct current (BLDC) motor and uses the
PWM signal from the Arduino to control the motor voltage and current. Lastly, the Arduino is also
supplied with regulated voltage from the battery through a switch regulator.
Motor
The motor utilized is a XNOVA 2208 Lightning V2N Series 2500KV. As is stated in the name, the
motor has a KV-rating of Kv = 2500 RMP/V which means that the motor, under no load, will rotate
with a rotational speed of 2500 RPM for every Volt applied.
ESC and Battery

The electronic speed controller FMS Predator ESC 40A has a maximum current rating of 40 A and
is compatible with LiPo batteries with 2-4 serially connected cells, i.e. a nominal voltage from 7.4 V
to 14.8 V. The chosen LiPo-battery has 4 serially connected battery cells, 14.8 nominal voltage and a
charge capacity of 5000 mAh.
Voltage Regulator
The voltage regulator is of a switch-type which is capable of regulating a direct current supply from
3-40 V to 1.25-35 V and has a maximum current rating of 2.5 A. The Arduino Uno is specified to have
a maximum current draw of 200 mA which is well below the current rating of the voltage regulator.
10.8 Basis for Powertrain Choice

Simulations of the drive-train circuit, with different setups, were conducted. The setups investigated
were all taken from a drone-testing database, namely Tyto Robotics Database [9]. The database
provided us with exact specifications of motors, ESCs and propellers and three of these setups were
chosen to get an understanding of what parameters are of importance when choosing the hardware for
the system. The table below summarizes the three setups.
Table 3: Drive-train components for each Setup.
Setup Motor ESC Propeller

T-Motor LF40 T-Motor F35A GEMFAN5040R
1 Kv = 2305 Ic = 35 b = 2.35e − 6
Kt = 0.0050 Imax = 45 d = 3.39e − 8
RCX RS1407 Flycolor XCross Blheli32 HQPROP3040
2 Kv = 4100 Ic = 50 b = 3.37e − 7
Kt = 0.0021 Imax = 60 d = 3.67e − 9
DYS Samguk Wei Flycolor XCross Blheli32 HQPROP5040
2 Kv = 2600 Ic = 50 b = 2.35e − 6
Kt = 0.037 Imax = 60 d = 3.39e − 8
The lift-coefficient b as well as the drag coefficient d were computed using empiric test-data provided
by the database [9].
29
Table 4: Unit table for parameters in table 3
Parameter Kv Kt Ic , Imax b d
N Nm
Unit RPM/V Nm/A A (rad/s)2 (rad/s)2
Each setup was provided with a 3s 11.1 V LiPo Battery with a charge capacity of 5000 mAh and the
drone with a mass of m = 1 kg was simulated to fly in a 10 by 20 meter ellipse, making 10 laps. The
resulting motor voltage, current, total power requirement and consumed energy for setup 1 are shown
in the figures below, see figures 14 and 15. To avoid permanently damaging a LiPo Battery, one should
only let it discharge about 80% of its capacity. Hence the capacity of the battery is set to 4000 mAh
in the simulation.
Figure 14: Motor voltage, current and total Power requirements for Setup 1.
30
The figure illustrating total power requirement and consumed energy is presented below, see figure 15.
Figure 15: Green line represents consumed energy [J] as a function of time [s]. The red dotted line
shows the energy threshold at which the battery is depleted.
The figures of performance for the remaining setups can be found in the apendix, see Appendix A.
Lastly, the flight-performances using each setup are summarized in the table below:
Table 5: Flight-performance for each setup when flying the described elliptical path.
Setup Flight-time [min] Max Motor Current [A] Max Motor Voltage [V] Rated Power [W]
1 18 13 6 150
2 7 23 8 385
3 17 11 7 160
From the results in table 5, it is evident that setup 1 and 3 perform better than setup 2 in every
metric. This is mostly related to the high Kv value of setup 2 together with the small propeller size
and low blade-pitch, that is the angle of the propeller blades relative to the plane of rotation. The low
drag and lift-coefficients b and d respectively, results in the motors needing to provide high RPM to
generate enough lifting force. However, the required voltage is still be kept at a low rate because of the
high Kv -rating of 4100 RPM/V which on the other hand results in higher motor current. For a given
torque-load, a motor with high Kv will have a low Kt -rating (because of their inversely proportional
relationship) and therefore naturally require a large motor current and high rated power. A setup such
as number 2 could potentially be fitting for a light weight racing-drone that requires high power for
rapid manoeuvrability and agility however, it does not serve the purpose of this project.
Setup 1 and 3 perform on similar levels with flight times almost reaching 20 minutes and rated powers
of at most 160 W while remaining constant z. This lead to the decision of choosing a setup similiar
to 1 and 2. Four motors with KV-ratings of KV = 2500 RPM/V were each paired with an ESC with
max current ratings of 40 A and a LiPo battery with nominal voltage of 14.7V and charge capacity
of 5000 mAh. The battery chosen is of larger size than the one used in simulation. However, despite
being heavier, it allows for longer flight time.
31
11 Position and Velocity Estimation
11.1 Introduction
As stated in previous control derivation chapters, the system’s state space vector ⃗x contains six un-
T
measurable state variables. Namely the body-fixed velocity, ⃗vB = u v w and the inertial position,
T
⃗x = x y z . These state variables are to be estimated in real time during flight by fusing the sensor
data of the the bno055, Adafruit Ultimate GPS and bmp280. The fusion is done with a Kalman filter.
The derived state-space system will require inertial acceleration measurements from the imu. However,
the imu only provides linear acceleration relative to the sensor-fixed reference frame. To transform the
sensor-fixed acceleration ⃗aB to inertial acceleration ⃗a one utilises the rotation matrix Rzyx as discussed
in the kinematics chapter.
⃗a = Rzyx⃗aB (11)
Where Rzyx is given by (32).
Moving on to the GPS, those coordinates are acquired in longitude, latitude and altitude. Altitude
corresponds well to the inertial frame but longitude and latitude are presented in degrees and minutes.
The horizontal components of the GPS coordinates must therefore be transformed. Presented below
is an illustration of planet earth, it showcases how the inertial frame is positioned relative to the GCS
(Geographic Coordinate System).
Figure 16: The earth is displayed along with GCS and the inertial reference frame as well as the
estimated range of the different GCS coordinates.
With the illustration as basis one was able to derive the following expressions to find the coordinates
in the local inertial frame with coordinates tangent to north and west like displayed in 16.

Xi = rsin(lati − lat0 )

Yi = −rcos(lati )(longi − long0 ) (12)

Zi = alti − alt0

32
Onward one must transform the coordinates from the current local inertial frame to the initial body
frame which is the desired reference for all coordinates in the future calculations. To understand how
the systems mentioned are related see 17.
Figure 17: The earth is displayed along with GCS and the inertial reference frame as well as the
estimated range of the different GCS coordinates.
To relate the systems information of the relative angle about the z-axis had to be known. Another
important factor was that the magnetometer measured the yaw angle relative to the magnetic north
pole. Since north in the local inertial frame points to geometric north and the ambition is to relate
the two displayed cartesian frames the following holds true 13.
θ = Ψ0 − θD + 90 (13)
Where Ψ0 is the first sampled yaw-angle and θD is the estimated declination which is almost constant
due to the scale of the earth relative to the movements of the quadcopter. Finally as the angle θ has
been obtained one must conduct the final transformation of the components Xi , Yi and Zi to get the
corresponding coordinates in the initial body frame 14.
 
Cθ −Sθ 0
RItoB0 =  Sθ −Cθ 0 (14)
0 0 −1
This concludes the derivation of how the GPS coordinates were transformed and from this point
forward it is possible to use the contribution from the GPS and the accelerometer to create a sensor
fusion of them two.
33
11.2 Kalman Filter
A linear time invariant system that is influenced by measurement noise v as well as process noise w is
written on state space form as:
⃗x˙ = A⃗x + B⃗u + w

⃗
(15)
⃗y = C⃗x + ⃗v
In our case, ⃗x is a column vector containing unmeasurable inertial position values and velocities.
Although the variables in question is considered unmeasurable, we will be utilizing GPS’s relative
position in the Inertial xy − plane and the relative altitude z from the bmp280 as measurements.
However, since sensors alone does not provide a sufficient level of accuracy, one must incorporate the
estimates from the IMU to enhance the accuracy of quadcopter’s position. The state-space outputs,
inputs and matrices are defined below:
T T
⃗x = x y z ẋ ẏ ż ⃗u = ax ay az (16)
   
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
A=
0
 B=  C = I6 (17)
 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
Here, ⃗u is the input vector containing inertial acceleration measurements provided by transforming
the imu’s linear acceleration measurements. System output ⃗y is measured by the gps affected by mea-
surement noise ⃗v . The Kalman filter will be utilized to estimate the true state of the system, denoted
by ⃗x, by accounting for potential process noise represented by w.
⃗
We can naturally introduce an observer to our system and estimate our state by utilizing feedback
between the model of our system and the observer’s estimate error. The estimate’s error ⃗y˜ = ⃗y − ⃗yˆ =
ˆ is attenuated by the Kalman gain K, as can be seen in figure 18, which is computed iteratively
⃗y − C ⃗x
based on the statistics of the process disturbances and measurement noise.
Figure 18: Block diagram over Kalman filter as an observer with optimal gain K.
34
In the following discrete model, we replace all vector notations (such as ⃗x or ⃗u) with the same notations
but without arrowheads (i.e., x or u). This is done to help reduce clutter and improve the readability
of the text.
The system is now to be discretized with a sample time T and express a clear computation algorithm
for the Kalman gain and state estimate. Using the backward Euler method we can discreteize the
continous state space system (15) to (18):
xn = Ad xn−1 + Bn un + Gd wn
(18)
yn = Cd xn + vn
The Euler backward method is an unconditionally stable method where the derivative is approximated
based on the current state xn , potential current inputs un and process noise wn .
xn − xn−1
≈ Axn + Bun + wn ⇐⇒ xn = (I − T A)−1 xn−1 + (I − T A)−1 T Bun + (I − T A)−1 T wn
T
Comparing this result to (18) one obtain the corresponding matrices Ad , Bd , Gd for the discrete system.
  


 1 0 0 T 0 0
0 1 0 0 T 0 

  


  


−1
0 0 1 0 0 T 
−

 A = (I T A) =
 d
  

 0 0 0 1 0 0 

  
0 0 0 0 1 0

  

  




 0 0 0 0 0 1




  2 
T 0 0





 0 T2 0 

  


0 T 2
  

−1
0
Bd = (I − T A) T B =    (19)


  T 0 0 

0 T 0

  



0 0 T








  
T 0 0 T2 0




 0
0 T 0 0 T2 0 

  



0 T 2
 


 −1
0 0 T 0

 Gd = (I − T A) T = 
0 0 0 T 0 0



  

0 0 0 0 T 0

  

  

 0 0 0 0 0 T
The system includes covariance matrices R and Q for the measurement noise vn and the process noise
w = Gd wn respectively. These are assumed to be positive definite and diagonal, i.e. co-variances
between different variables are set to zero, see (20).
35
  
2


 E[wt,x ] 0 0 0 0 0
2
 0 E[wt,y ] 0 0 0 0 

  



2
 

  0 0 E[wt,z ] 0 0 0 
Q = E[wt wtT ]


 =
 0 2

0 0 E[wt, ẋ ] 0 0 



  
2

 0 0 0 0 E[wt, ] 0
  
ẏ 
2
(20)


 0 0 0 0 0 E[wt, ż ]




  
2
E[vt,1 ] 0 0




R = E[vt vtT ]
 2
= 0 E[vt,2 ] 0 

  


2

 0 0 E[vt,3 ]
E[wt wtT ] is the notation for the covariance matrix for the process noise vector w containing the error
for each variable in the state-space (18) at time t, ranging from x, ..., ż. The primary source of process
noise is propagated by the error of measured acceleration and as a result is challenging to quantify
in terms of covariance. Consequently, to identify a proper covariance matrix Q, the matrix is tuned
based on the filter’s performance.
Unlike Q, R is considerable easier to determine. The matrix is assumed to be nonchanging over time
with the first two diagonal elements in R related to the GPS’s position accuracy and the third element
related to the accuracy of the pressure sensor. These values are mathematically expressed as the
2
expected value of E[vt,i ] : i ∈ [1, 2, 3] which, assuming zero-mean, can be expressed as the variance of
V (vi ) (21), based on theorem 5.6 in [2].
2
E[vt,i ] = E[vi2 ] = V (vi ) − [E(vi )]2 = {E(vi ) = 0} = V (vi ) = σi2 . (21)
Where σi is the standard deviation of the measured sensor value yi . As stated in the hardware chapter,
the uncertainty of the GPS is about 3 m which equates to a standard deviation of 3/1.96m ≈ 1.53 m
assuming a normal distribution with a confidence interval of 95 %. With the same assumptions for the
altitude measurement, the corresponding standard deviation becomes σ3 = 1/1.96m ≈ 0.51 m since
the barometer has a relative altitude uncertainty of 1 m. With the covariance matrices sorted out, we
can now start estimating our state vector x.
Now, given an initial state estimate x̂0 , one can compute the priori state estimate x̂1|0 . This estimate
can be thought of as the guess based on the systems last behavior, without knowledge of the measured
input or output. The priori estimate can be generalized as:
x̂n|n−1 = Ad x̂n (22)

Suppose a Kalman gain matrix K is given and measurement data yn is available. This would allow
for the computation of the priori state estimate error and lastly computation of the posteriori state
estimate x̂n , this by including the measured acceleration input un . The posteriori state estimate x̂n is
expressed below:
x̂n = x̂n|n−1 + Bd un + K(y − Cd x̂n|n−1 ) (23)

Where the error (y − Cd x̂n|n−1 ) can be compared to our previous expression for the continuous ob-
server, namely: (⃗y˜ = ⃗y − C ⃗x
ˆ).
36
This is all illustrated in the block scheme below, see figure 19.
Figure 19: Computational algorithm and block diagram over discrete Kalman filter.
It can be shown that the optimal Kalman gain that minimizes the posteriori state estimation error
en = x̂n − xn , where xn is our true state, is given by:
Kn = Pn|n−1 Cd (Cd Pn|n−1 CdT + R)−1 (24)

Where Pn|n−1 is the priori error covariance matrix and R is the proccess noise covariace matrix as
seen previously. Along with the Kalman Gain, the priori error covariance will be updated recursively,
see equations (25) below.
Pn = (I − Kn Cd )Pn|n−1
(25)
Pn+1|n = Ad Pn ATd + Q
Given plausible initial conditions P0 and x̂0 , the Kalman gain Kn and posteriori covariance matrix
Pn will converge for constant system matrices Ad , Bd and Cd . Hence, it is possible to compute the
converged Kalman gain K ”offline” before flight or testing of the position estimator and then adjust
the covariance matrices according to results.
The posteriori state estimate x̂n is an estimation of the drone’s position and velocity at instance n.
After a sampling period T has passed, the drone will have obtained new sensor-data in terms of GPS
position and linear acceleration and the process of estimating the next state x̂n+1 begins anew.
The theoretical basis for the discrete Kalman filter, derived here, can be seen in a video lecture by
Prof. Harry Asada, MIT [6].
37
12 Implementation
12.1 Software
Drone
The drone’s software is written in Arduino Ide which utilizes the c++ programming language. It
consists of four main sections: Initialization, sensor-reading, tranceive State and Receive pwm, see
figure 20. The protocol also includes a safety measure where if no pwm signal, i.e. radio communication
is stopped, for three seconds, the motors are stopped. The specific implementation of the code is left
for curious readers and can be found in Appendix E.
Figure 20: Architecture over the drone’s software.
38
Radio Tranceiver Protocol
A protocol for the two-way radio transmission had to be implemented to ensure stable communication
, dealing with corrupted and non-received messages. After the part Kalman-filtered state has been
read by the sensors, the drone has to send it’s complete state to the ground computer. As stated in
the Hardware chapter, see 10.6, the radio can transmit 32 bytes each message at a data rate of atleast
250 kB/s. The drone’s state consists of 12 variables, where each variable size in terms of bytes need to
be decided to determine how the transmission will be excecuted. The table below depicts the desired
number of decimals and datasize of each variable, see 6.
Table 6: Chosen data size in bytes and decimal precision for each variable in the desired state that is
sent over radio
Variable Φ Θ Ψ p q r u v w z
Data size [bytes] 3 3 4 7 7 7 5 5 5 5
Decimal Precision 0 0 0 2 2 2 1 1 1 1
The nrf24l01 can only transmit character arrays, therefore the float variables needs to be converted
and copied to a character array. However, because of the byte limit of 32 bytes per message, the state
is divided into to packets. The first packet contains Φ, Θ, Ψ, p and q and the second packet naturally
contain the rest of the variables. As a result, the first packet contains of 24 bytes and the second 28
bytes.
To know if the message has been sent and received successfully, the radio utilized acknowledgements,
a feature that can be enabled when initializing the radio. When transmitting a message, the packet
might potentially be received and if so, a substantially smaller acknowledgement message will be sent
back to the transmitter. If the acknowledgement is received, the transmission is called successful and
the next packet can be sent, see 21. The nrf24 modules utilizes a message corruption detection called
CRC (Cyclic Redundancy Check) in combination with the acknowledgment check. This way, if the
message is corrupted, an acknowledgement is not sent back and the transmission can be retried [5].
Figure 21: Figure depicting a successful transmission utilizing acknowledgments.
39
After the two packets have been received by the radio and Arduino on the ground, the drone’s state is
then sent via Serial communication to the control algorithm in python. It is here of crucial importance
that the packages are sent in the correct order, which is made sure by an additional packet verification
in the protocol.
The packet verification is possible by adding a unique one letter, 1 byte, key to each packet, notifying
the computer of the packets content which can guarantee that the packages are received and stored in
the correct character arrays sent over the Serial port. The letter ’L’ is added to packet one and the
’Q’ letter is added to packet two. In practice, if the read packet is expected to contain a certain key
and a miss-match occurs, the computer-radio is made to look for the first packet by returning to stage
0. The complete protocol as a state-machine is visible in figure 22. Once the machine is on stage 2. it
has successfully received the two packets and is now ready to send it over the Serial port (SoS).
Figure 22: State-machine of the radio-protocol on computer side. (SoS - Send over Serial Port)
After being transferred through the serial communication, the Arduino ensures that Python has re-
ceived the string before looking for PWM-signals. The raw state is then processed by Python and
in accordance to the chosen control algorithm it returns required PWM-signals back to the Arduino
using the Serial interface.
Furthermore, upon receiving the PWM-signals, the Arduino sends the said string by radio using a, so
called, acknowledgement payload that can accommodate for a total of 32 bytes. In comparison, the
PWM-string itself is 15 bytes in size.
Regardless of if the acknowledgment package containing the PWM signals is received or not, the drone
will continue to sample the next state and prepare for transmission. This measure prevents the possi-
bility of lost acknowledgement-payloads affecting the transmission and ensures stable communication.
However, if multiple messages, acknowledgments or state-packets, are lost for at least half a second,
the drone’s motors are turned off in accordance to the safety protocol, see the bottom of figure 20.
40
12.2 Filters
Low Pass IIR Filter
In order to handle the circumstances which are introduced by reality, digital filters are used. The states
most sensitive to high frequency noise are the angular velocities p, q and r. Since the primary object
of the LQR controller is to provide stability around the linearized point it is important to maintain
the orientation of the quadcopter. It was expereienced that a well designed low pass filter produced
a noticable difference in keeping the drone stable. For designing the digital filters MatLab was used -
a number of filters were designed but only one was seriously considered. A second order Butterworth
low-pass filter with a cut-off frequency at 10 Hz, this filter was compared to a few other candidates
which failed to impress.
The implemented filter is referred to as an IIR (Infinite Impulse Response) filter because the out-
put is recursively superimposed to produce desired change on the sampled data which leads to an
infinite impulse response. Below is a figure which illustrates the schematic of a second order butter
worth filter 23.
Figure 23: Digital second order Butterworth filter. T denotes the delay variable which equals the
sample time of the filter.
From the block diagram it is apparent that the equation relating a filtered and an unfiltered state can
be written as 26 suggests.
⃗yn = g(b0 ⃗xn + b1 ⃗xn−1 + b2 ⃗xn−2 ) − a0 ⃗yn−1 − a1 ⃗yn−2 (26)
⃗yn holds the filtered states while ⃗yn stores the new unfiltered states p, q and r. For reasons of symmetry
the filter is identical for all these states. The purpose of the filter is to dampen the influence of high
frequency disturbances or noise which exceeds the highest possible frequency for which the mechanical
system with its actuators can compensate for. Thus the selection of a cutoff frequency of 10 Hz is
not based on the drone’s sampling rate of 20 Hz (approximately 50 Hz), but rather on the physical
limitations that prevent it from effectively tracking rapid variations.
41
The low-pass IIR filter was designed in MATLAB. To evalutate its performance, a white noise signal
was passed through it and the resulting PSD (Power Spectral Density) of the filtered signal as well as
the filters frequency response can be seen in figure 24.
(a) Power Spectral Density of a white noise signal passed (b) Frequency response function of the filter.
through the filter
Figure 24: Power Spectral Density and Frequency response function of the second order Butterworth
filter.
42
12.3 States
The choice of states and their manipulation by the pilot in the flight controller largely govern the
range of functionalities achievable by the quadcopter. Depending on the states that are accessible and
accurately measured, the pilot can either track a desired path using coordinate information within the
inertial frame or may be compelled to allocate control inputs solely for adjusting velocities.
In the section for the simulation this particular choice was presented but not thoroughly explained.
Below is a figure which depicts all the nessecary controls to go between all the states of the quadcopter’s
state machine 25.
Figure 25: Controls for piloting the quadcopter.
Two distinct configurations were examined. Initially, it seemed logical to employ push buttons as
integrators, generating a linearly increasing gain that could scale an external input to any relevant
state. This concept persisted until implementation and proved effective using the PyGame module in
Python, which provided iterative boolean values based on the execution of code lines when buttons
were pressed. For further details, please see Appendix.
Regarding which buttons connect to which states there were some more considered alternatives and
only one remained after some testing and consideration.
Table 7: Mapping between keys and affected states when pressing them.
W A S D E Q SPACE C H L R
True +u -v -u +v +Ψ -Ψ +Z -Z *HOVER* *LAND* *Track*
False - - - - - - - - - - -
To briefly explain the choice, initially it was considered to make W, A, S, D integrate vector magnitudes
for vectors which are aligned with the inertial axis but this is hard to implement accurately and was
thought to result in awkward controls - something which was also experienced from testing in MatLab.
Therefore, to make it more intuitive and responsive (given that there is a good measurement of velocity)
the W, A, S, D will instead control the body frame velocities u, v and w. The yaw angle however is
not controlled through its derivative as table 7 suggests, the same goes for the altitude. H dampens
all the velocities such that the quadcopter can end up in a hover mode which occurs in an arbitrary
point in space which as earlier established counts as a stationary point. When in hover mode L and
R becomes available options, if the quadcopter is close to the ground one can drop it by pressing L. If
the quadcopter is expected to conduct a measurement of its own performance then it is supposed to
begin this reference track as R is pressed from hover mode.
43
12.4 Tuning
Kalman Filter
In order to allow for manual control using body-fixed velocities and inertial z-coordinate as reference
input, the Kalman filter will need to provide a satisfactory estimation of body-fixed velocities and
altitude. As stated in 11.2, the covariance matrix R can already be expressed using their uncertainty
provided by the sensors’ data-sheets provided by the manufactures. The covariance matrix Q for
process noise includes variances of position and velocity propagated by an error in the acceleration
readings. An error, in terms of variance that was estimated to be σa2 = 2.34 × 10−4 m/s2 based on it
having an uncertainty of about 0.03 m/s2 . Because of the discrete integration of the linear acceleration,
a factor of T 2 and T can be included in the variances of the position and velocity respectively. Moreover,
by also including two tuning factors k1 and k2 a rough estimation of Q can be expressed according to
(27).
k1 T 2
 
0 0 0 0 0
 0
 k1 T 2 0 0 0 0  
2 0
 0 k1 T 2 0 0 0 
Q = σa   (27)
 0 0 0 k2 T 0 0  
 0 0 0 0 k2 T 0 
0 0 0 0 0 k2 T
Firstly, a sufficient altitude estimation was obtained for k1 = 0.1, k2 = 0.3 and setting an uncertainty
of 1.8 m instead of 1 m. This way, the altitude estimation reacted well to quick movements and oscil-
lations, relying on the accelerometer but still reducing drift.
While tuning the Kalman gain such that the body-fixed velocity estimation gave non drifting ap-
propriate values, the filter did not behave as expected. The velocity reading had large degree of
uncertainty and a resulting inconsistent performance. This was due to a miss-implementation of the
GPS transformation in the Arduino software but the issue could not be dealt with before the report
was due.
44
12.5 Construction
The construction adheres to the planned design, meticulously crafted using Solid Edge software. The
primary objective was to achieve a symmetrical structure characterized by a well-balanced configura-
tion and a low moment of inertia. A visual representation of the model is featured on the front cover
of this report, showcasing its shell. Additionally, Figure 26 provides a glimpse of the setup used for
fine-tuning the quadcopter’s ability to maintain a stable horizontal orientation.
During the testing phase, the quadcopter’s state variables were systematically sampled and recorded in
plots. These data-rich plots served as valuable resources for subsequent analysis, enabling informed ad-
justments to the controller. By carefully scrutinizing the recorded information, necessary refinements
and modifications could be made to enhance the overall performance of the quadcopter.
Figure 26: The quadcopter separated from it shell and mounted on a testing rig for tuning.
45
12.6 Performance
The quadcopter is not capable of flying but it is probably capable of hovering a bit and yaw while
balancing itself in the air - it is however not safe and could very well crash and therefore it was not
attempted. The reasons for its bad performance became quite clear when the testing began. One
plot will be shown to explain the phenomena which diminished the capabilites of the quadcopter, this
then led to making it impossible to test its performance and sample data that allows for a rigorous
quantitative analysis. See figure 27 below which in part depicts the pitch angle and its derivative with
respect to time.
Figure 27: A sample from a attitude tuning test locking all DOF except for pitch Θ, notice the spikes.
The occurrence of the blue spikes is not attributed to sampling issues, programming bugs, errors,
or deficiencies in the PWM conversion process. Instead, they appear to originate solely from faulty
hardware or other analog disturbances. Periodically, the motors would experience involuntary twitches,
and these spikes became more pronounced with higher current levels. These spikes resemble Dirac
pulses, which possess a substantial amount of high-frequency energy content capable of destabilizing
the system.
46
While the system manages to maintain stability for low PWM signals, the disturbance eventually be-
comes too significant for the actuators to handle. This limitation arises due to the torque constraints
of the actuators and the reaction time imposed by the sampling frequency of the feedback loop.
It is important to acknowledge that these spikes pose a significant challenge to the system’s over-
all stability. The presence of high-frequency energy content within the disturbance can have adverse
effects on system dynamics. As a result, careful consideration must be given to mitigating these distur-
bances through hardware improvements or employing appropriate filtering techniques to ensure stable
and reliable operation.
12.7 Model vs Reality

To evaluate the model against reality, a comparison can be made between the MATLAB plot obtained
in Figure 9 and the spikes depicted in Figure 27 just above. In reality, numerous disturbances exist,
and these disturbances possess energy that must be countered. This energy can manifest in different
forms, such as the expenditure of time, computational resources, or financial investment in superior
hardware. Even with the presence of digital filters, analog disturbances in the wiring or faulty hard-
ware can still pose challenges. Constructing a quadcopter and achieving success within a few months,
especially for first-time builders, is no simple task.
Another reality check arises from the sensors used. While it is often acknowledged that system identi-
fication is the challenging aspect in control theory, it is essential to recognize that when transitioning
from theory to practical implementation, unforeseen challenges arise, which are often more difficult to
debug. For instance, obtaining accurate velocities that are devoid of zero frequency components is a
formidable undertaking without prior experience.
Navigating the complexities of building and controlling a quadcopter encompasses a range of ob-
stacles that extend beyond theoretical understanding. It requires perseverance, adaptability, and a
willingness to overcome unforeseen difficulties to achieve satisfactory results.
47
13 Conclusions and future developments
To begin the conclusions that can be drawn from this project, answering the research question or
questions that arose, a table containing our satisfaction with different parts of project is shown below
in table 8.
Table 8: The authors contentment regarding different parts of the project
Part Contentment
Velocity estimation Not satisfied
Altitude estimation Almost satisfied
LQR performance on rig Satisfied
Hovering capabilities Not satisfied
Radio communication Satisfied
Powertrain performance Not satisfied
The research question focused on the suitability of LQR as a control method for a drone, and the
findings indicate that it is a viable approach. However, the evaluation was limited to the analysis of
attitude control, and the full assessment of the twelve states in the linearized state of the drone was not
feasible, as six of these states needs to be estimated. To enable manual control of the drone using LQR
with velocity and altitude as reference inputs, accurate estimation of these states becomes essential.
To address this, a Kalman filter was implemented to estimate the states. However, the performance
of the Kalman filter fell short of the desired standard. While the altitude estimation proved almost
satisfactory, further enhancements are needed to improve the accuracy of velocity estimation.
The most effective and significant improvement that can be made to the drone is to reduce its weight,
replace the ESC, install a new battery, and use smaller propellers to reduce torque and the amount of
current drawn from the ESC, which helps prevent voltage spikes. Additionally, the wires connecting
the battery to the ESC should be shortened. Upon achieving a nonspiking powertrain, further tuning
of the LQR controller is recommended, along with improving the accuracy of velocity and altitude
estimation. This can be achieved through the use of higher quality sensors or the implementation
of a full-state Kalman filter. Although the latter method requires more computational power, it is a
more effective means of estimating state variables than the simple Kalman filter discussed in this report.
48
A Kinematics derivation
Kinematics describe geometrical relationships between units in space in terms of position, velocity and
acceleration. A clear description of the kinematics is essential in order to map between effects which
act in different frames to one single frame where for instance the reference state is defined.
To relate the fixed-body reference frame Sb : (e⃗1 , e⃗2 , e⃗3 ) and the inertial reference frame Sb0 : (e⃗x , e⃗y , e⃗z )
we use Euler angles: (Φ, Θ, Ψ) which are defined above in 1. Presented below are the three individ-
ual rotation matrices which transforms vectors between frames which are rotated accordingly. See 28
below. Presented next are the rotation matrices.
(a) (b) (c)
Figure 28: (a) Rotation about X axis misaligning the initial body frame with the latter. (b) Rotation
about Y. (c) Rotation about Z.
The rotation matrices are the following.
 
1 0 0
Rx = 0 CΦ −SΦ  (28)
0 SΦ CΦ
 
CΘ 0 −SΘ
Ry =  0 1 0  (29)
−SΘ 0 CΘ
 
CΨ −SΨ 0
Rz =  SΨ CΨ 0 (30)
0 0 1
A reader with some experience can probably verify this intuitively by looking at figure 28: a) to c).
Given these matrices one can produce any orientation in the R3 by doing three consecutive rotations.
This then equates to multiplying the matrices in the following way, given that we use a (3-2-1) rotation
sequence the following statement holds true.
Rzyx (Φ, Θ, Ψ) = Rz (Ψ)Ry (Θ)Rx (Φ) (31)
Substituting eq. 1 to 3 into eq. 4 eventually produces the following matrix.

 
CΘ CΨ SΦ SΘ CΨ − CΦ SΨ CΦ SΘ CΨ + SΦ SΨ
Rzyx =  CΘ SΨ SΦ SΘ SΨ + CΦ CΨ CΦ SΘ SΨ − SΦ CΨ  (32)
−SΘ SΦ CΘ CΦ CΘ
49
Given the matrix from (5), it is possible to project any vector described in terms of unit vectors from
the body-fixed coordinate system to the initial body frame, hence the inertial frame as the IMU knows
it. For instance, if the velocity ⃗vB is known, then by matrix-vector multiplication we obtain ⃗v compo-
nents in the inertial reference frame.
On to relating the angular velocities in the body frame to the inertial frame. From three consec-
utive rotations in a (3-2-1) rotation sequence, one can state that the resulting angular velocity is given
by (6).
ω ⃗2 + ω3 e⃗1
⃗ = ω3 e⃗z + ω2 N (33)
Where e⃗z is a unit vector in the inertial reference frame, e⃗1 is the corresponding unit vector but
in the body fixed frame which after the last rotation is aligned with latter z-axis. To complete the
kinematic relationship the ambition is to write (6) in terms of body-fixed unit vectors (e⃗1 , e⃗2 , e⃗3 ).
Before presenting this derivation, realize that the following is true.
⃗ e⃗x N⃗1 ⃗ N⃗1

 ′  ′  ′′   ′    ′′ 
N 1 N 1 e⃗1
 ⃗′ T  ⃗′   ⃗′′  T  ⃗′ T  ⃗′′ 
N2  = R3 ey  N2  = R2 N2   e⃗2 = R1  N2 
 (34)
⃗
N3
′ ⃗
ez′ ⃗
N3
′′ ⃗
N3
′ e⃗3 ⃗
M3
′′
With the relations stated in (7) one obtains.


ωB · e⃗1 = −ω3 SΘ + ω1

ωB · e⃗2 = −ω3 CΘ SΦ + ω2 CΦ (35)

ωB · e⃗3 = −ω3 CΦ CΘ + ω2 SP hi

The components of ω
⃗ are ω1 , ω2 and ω3 . It is now apparent that the following is true:

ω1 = Φ̇

ω2 = Θ̇ (36)

ω3 = Ψ̇

Substitution gives the final result which has been gathered in the matrix notation below.
    
ωB1 1 0 −SΘ Φ̇
ωB2  = 0 CΦ CΘ SΦ  Θ̇ (37)
ωB3 0 −SΦ CΘ CP hi Ψ̇
This result (10) is considered important for future calculations, therefore it is noted that.
 
1 tΘ SΦ tΘ CΦ
ω
⃗ = T ω⃗B T = 0 CP hi −SΦ  (38)
CΦ CΦ
0 CΘ CΘ
Finally, before summarizing the final result it is reconciled that.

   
u ẋ
⃗v = Rzyx v⃗B v⃗B =  v  ⃗v = ẏ  (39)
w ż
50
Therefore one can now state the following kinematic relationships.


 ẋ = u(CΘ CΨ ) − v(CΦ SΨ − CΨ SΦ SΘ ) + w(SΦ SΨ + CΦ CΨ SΘ )

ẏ = u(CΘ SΨ ) + v(CΦ CΨ − SΦ SΘ SΨ ) − w((CΨ SΘ − CΦ SΘ SΨ )





ż = −u(S ) + v(C S ) + w(C C )
Θ Θ Φ Φ Θ
(40)


 Φ̇ = p + q(S Θ tΘ ) + r(CΘ tΘ )
θ̇ = q(CΘ ) − r(SΘ )




 SΦ CΦ
Ψ̇ = q( C ) + r( C )

Θ Θ
This concludes the section of the kinematics, the latter system of equations (13) will later be assembled
with the six equations which are derived from dynamics and specifically Newtonian mechanics, together
they form the total number of equations necessary to describe the majority of the influential dynamics
of the quadcopter.
51
B Kinetics derivation
This section presents the dynamics of the quadcopter, hence it aims to relate forces to the movement
of the quadcopter. The model is derived from the following two formulas.
0 b
X ∂ R0
Fi = ma⃗g = m( v⃗G + R0 ω⃗B × R0 v⃗G ) (41)
∂t
∀i
In order to clarify equation (14). F⃗i denote the i:th force acting on the stiff body. Obviously m denotes
the mass. a⃗G is the acceleration vector for the center of mass. The prescript of the differential informs
that the velocity of the center of mass was differentiated with respect to a body fixed frame where
only the relative acceleration is captured, hence acceleration which exists within the fixed body frame.
R0 is the initial body frame, which is a true inertial frame. vG is the velocity of the mass center and
it is defined in the inertial frame like ωB which is the angular acceleration of the body.
4
X
MG,i = I⃗G · ω⃗˙B + ω⃗˙B × H⃗G (42)
∀i
The structure in equation (15) strongly resembles the one explained in (14). The left hand side is the
vector sum of every i:th moment Mi acting about the mass center of the drone. As Newtons second
law of rotation states the right hand side equals the cross product of the inertia tensor I⃗G and the
angular acceleration a⃗G of the body. The angular acceleration of the body is however represented by
a kinematic expression which stems from the time derivative of the angular velocity ω⃗B . H⃗G denotes
the angular momentum which is a vector product presented below in equation (16).
HG = I⃗G ω⃗B (43)

Given these relations a number of helping definitions are introduced.
X
f⃗B = Fi (44)
∀i
X
m⃗B = MG,i (45)
∀i
Given the definitions in (17) and (18) and substituting for the declared variables the following result
is obtained, notice the already defined physical quantities from table 1. The vectors are described in
component form.
f⃗B = m[(u̇ + qω − rv)e⃗1 + (v̇ − pω + ru)e⃗2 + (ω̇ + pv − qu)e⃗3 ]



        

 Ixx 0 0 ṗ p Ixx 0 0 p
(46)
m⃗ = 0 I 0 q̇ + q × 0 I 0 q
       

 B  yy       yy  

0 0 Izz ṙ r 0 0 Izz r

It is worth mentioning that the inertia tensor is diagonal since the construction is assumed to satisfy
this idealization due to its symmetry. What is left of this derivation is the description of the forces
acting on the quadcopter.
52
The forces and torques acting on the drone are presented below. Consider the presence of gravity, this
implicates a downward force always acting along the negative Z-axis of the inertial system: R0. The
thrust generates an upward force which instead acts along the body fixed z-axis in system: b. The
forces induced by air resistance is neglected in this model. It is understood from the preface that some
considerable torques also arise in this model which have to be included in the model. The torques are
generated by differences in the applied thrust to each rotor and are denoted τB
f⃗B = mg R
⃗ T e⃗z − ft e⃗3 (47)
m⃗B = τB (48)
All these vector components are represented in the body fixed frame: b. By computing the results
with substitution of RT and dividing the result into components, as well as substituting these results
as the left hand side of equation (19) one obtains.

−mg(Sθ ) = m(u̇ + qω − rv)


mg(Cθ Sθ ) = m(v̇ − pω + ru)




mg(C C ) − f = m(ω̇ + pv − qu)
θ θ t
(49)


 τx = ṗIx − qrIy + qrIz
τy = q̇Iy − prIx + prIz





τz = ṙIz − pqIx + pqIy

τi , i = {x, y, z} and ft have to me modeled with basis in how the actuators work.
C Actuator dynamics derivation

Assuming that there is a constant coefficient b such that the following holds true.
ft = b(Ω21 + Ω22 + Ω23 + Ω24 ) (50)
And for every torque generated about each axis in the body fixed system there is a constant coefficient
d such that. 
2 2
τx = bℓ(Ω3 − Ω1 )

τy = blℓ(Ω24 − Ω22 ) (51)
 2 2 2 2
τz = d(Ω2 + Ω4 − Ω1 − Ω3 )

The lift coefficient: b, relates the produced thrust to the squared angular velocity of the rotor. The
drag coefficient: d, relates the produced yaw torque to the difference of the squared rotor speeds be-
tween the CW and CCW actuator pairs.
These coefficients can be obtained from open source test measurements where setups consisting of
a propeller, BLDC and a force sensor of some sort have been utilized to give plots of how the men-
tioned physical quantities are related throughout a range of different angular velocities.
53
D Performance of Power System for remaining Setups
54
Figure 30: Energy Consumption plot for setup 2. Green line represents consumed energy [J] as a
function of time [s]. The red dotted line shows the energy threshold at which the battery is depleted.
55
Figure 32: Energy Consumption plot for setup 3. Green line represents consumed energy [J] as a
function of time [s]. The red dotted line shows the energy threshold at which the battery is depleted.
56
E Source Code
The source code for the drone’s Arduino microcontroller, the computer’s Arduino microcontroller, the
Python script for controlling the drone, and the MATLAB code containing the design and implemen-
tation of the IIR and Kalman filters, are all available on GitHub.
GITHUB - https://gits-15.sys.kth.se/mapetr/LQRDrone
57
References
[1] Adafruit BMP280 I2C or SPI Barometric Pressure & Altitude Sensor. https://www.adafruit.
com/product/2651. accessed 2023.
[2] Blom. Sannolikhetsteori och statistikteori med tillämpningar. Lund, 2017.
[3] Last Minute Engineers. Getting Started with NRF24L01 Transceiver Module. 2023. url: https:
//lastminuteengineers.com/nrf24l01-arduino-wireless-communication/.
[4] Adafruit Industries. Adafruit Ultimate GPS Breakout. 2021. url: https://www.adafruit.com/
product/746.
[5] Nordic Semiconductor. NRF24L01+ Product Specification. Datasheet. 2011. url: https://www.
sparkfun.com/datasheets/Components/nRF24L01_prelim_prod_spec_1_2.pdf.
[6] dLab Robotics MIT. dLab Robotics @ MIT - Episode 12 - Introduction to Robot Dynamics. https:
//www.youtube.com/watch?v=E0lnf9k8Neo&list=PL8y36K17Be3jNvBkDxIEae7z0CLn7NoU6&
index=12&ab_channel=dLabRoboticsMIT. 2017.
[7] Francesco Sabatino. “Quadrotor control: modeling, nonlinear control design, and simulation”.
PhD thesis. KTH Royal Institute of Technology, 2020. url: https://www.kth.se/polopoly_
fs/1.588039.1600688317!/Thesis%20KTH%20-%20Francesco%20Sabatino.pdf.
[8] Bosch Sensortec. BNO055 Intelligent 9-axis absolute orientation sensor datasheet. 2014. url:
https://www.boschsensortec.com/media/boschsensortec/downloads/datasheets/bst-
bno055-ds000.pdf.
[9] Tyto Robotics. Drone Database. n.d. url: https://database.tytorobotics.com/motors.
58

Full Text 01

Uploaded by

Copyright:

Available Formats

Full Text 01

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Full Text 01

Uploaded by

Copyright:

Available Formats

Degree Project in Technology

First cycle 15 credits

Quadcopter Control System Design

Stockholm, Sweden 2023

Ludwig Horvath & Martin Petré

Bachelor’s program in Vehicle Engineering

Stockholm, Sweden 2023

Utöver simuleringen genomförs en implementationsfas som innebär att välja hårdvarukomponenter

11 Position and Velocity Estimation 32

13 Conclusions and future developments 48

D Performance of Power System for remaining Setups 54

2. If possible under what conditions? If not, then why?

SR0 (X0 , Y0 , Z0 ) Initial body frame p Angular velocity about x [rad/s]

Sb (x, y, z) Body fixed frame q Angular velocity about y [rad/s]

Ω1 Angular velocity rotor 1 [rad/s] CW u Velocity in x direction [m/s]

Ω2 Angular velocity rotor 2 [rad/s] CW v Velocity in y direction [m/s]

Ω3 Angular velocity rotor 3 [rad/s] CCW w Velocity in z direction [m/s]

Ω4 Angular velocity rotor 4 [rad/s] CCW x Position in X [m]

Φ Roll angle, angle about X [rad/s] y position in Y [m]

Θ Pitch angle, angle about Y [rad/s] z position in Z [m]

Ψ Yaw angle, angle about Z [rad/s] ft Total thrust in z direction [N]

Rx Rotation matrix about X [-] ft Total thrust in z direction [N]

Ry Rotation matrix about Y [-] τx Roll torque about x [Nm]

Rz Rotation matrix about Z [-] τy Pitch torque about y [Nm]

RZY X Rotation matrix (3-2-1) [-] τz Yaw torque about z [Nm]

m Mass of drone [kg] ⃗x State vector [-]

l Dist. between BLDC and mc [m] ⃗u Input vector [-]

Ixx Moment inertia about x [kg · m2 ] A System matrix [-]

Iyy Moment inertia about y [kg · m2 ] B Input scaling matrix [-]

Izz Moment inertia about z [kg · m2 ] C Output scaling vector [-]

⃗ which stores the state variables showcased as below.

Also it is hereby stated that.

Figure 11: An overview of the drone’s hardware components

10.2 Microcontroller - Arduino Uno

10.3 Inertial Measurement Unit - BNO055

Table 2: Specification of the Bno055’s measured output

10.4 GPS - Adafruit Ultimate GPS

10.5 Barometric Pressure Sensor - BMP280

10.6 Radio Module - nrf24L01

ESC and Battery

10.8 Basis for Powertrain Choice

Table 3: Drive-train components for each Setup.

Setup Motor ESC Propeller

⃗x˙ = A⃗x + B⃗u + w

x̂n|n−1 = Ad x̂n (22)

x̂n = x̂n|n−1 + Bd un + K(y − Cd x̂n|n−1 ) (23)

Kn = Pn|n−1 Cd (Cd Pn|n−1 CdT + R)−1 (24)

Figure 20: Architecture over the drone’s software.

Figure 21: Figure depicting a successful transmission utilizing acknowledgments.

⃗yn = g(b0 ⃗xn + b1 ⃗xn−1 + b2 ⃗xn−2 ) − a0 ⃗yn−1 − a1 ⃗yn−2 (26)

Figure 25: Controls for piloting the quadcopter.

12.7 Model vs Reality

Table 8: The authors contentment regarding different parts of the project

(a) (b) (c)

The rotation matrices are the following.

Substituting eq. 1 to 3 into eq. 4 eventually produces the following matrix.

⃗ e⃗x N⃗1 ⃗ N⃗1

With the relations stated in (7) one obtains.

Finally, before summarizing the final result it is reconciled that.

HG = I⃗G ω⃗B (43)