tracking control of a drone using the aerospace blockset

Tracking control of a drone using the Aerospace Blockset

G.J.C. Schotman

Bachelor Final Project4WC00

DC 2019.062

Coach: dr. ir. A.A.J. Lefeber

Department of Mechanical EngineeringDynamics and Control Group

Eindhoven University of TechnologyThe Netherlands

Eindhoven, June 22, 2019

Summary

The main objective of this research is to extend a state feedback controller into an output feedbackcontroller using the controllers developed in the Dynamics and Control group of the Technical Universityof Eindhoven and implement both controllers in the Simulink model of the Parrot Mambo developed bythe Simulink Support Package for an arbitrary trajectory, so not only for hovering. Both controllers aresimulated in this report using the ode45 solver and both the state feedback controller and the outputfeedback controller show convergence towards their set points. The state feedback controller has beenimplemented in the Simulink model of the Parrot Mambo as well and shows the same convergence ingeneral except for some disturbances in the (x, y)-plane, especially at higher fly altitudes. A possiblecause of this effect is the bad representation of the optical flow data. Now, although the output feedbackcontroller shows stability in the ode45 solver, a large deviation occurs when implementing the outputfeedback controller in the Simulink model of the Parrot Mambo. This deviation occurs because theoutput feedback controller requires discrete integrators to solve the proposed output feedback controllerin the Simulink model of the Parrot Mambo. It is concluded that these discrete integrators only givethe same results as the continuous integrator with the ode45 solver, when the sample time is set at leastequal to or smaller than 1e−5 seconds. The Simulink model of the Parrot Mambo, however, is workingwith a smallest sample time of 5e−3 seconds, which causes that the outcome goes to infinity. Potentialfurther research can investigate how to change the sample time in the Simulink model of the ParrotMambo for the discrete integrators, but without changing the sample time elsewhere in the model.

Contents

Page

1 Introduction 11.1 Modeling of the drone using the Aerospace Blockset . . . . . . . . . . . . . . . . . . . . . 11.2 Problem formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.3 Objectives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21.4 Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

2 Preliminaries 32.1 Norm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32.2 Euler angles and quaternions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32.3 Special Orthogonal Group SO(3) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42.4 Lyapunov stability . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

3 System Dynamics 63.1 Dynamic model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63.2 Translational dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73.3 Attitude dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73.4 Reference dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73.5 Sensor dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 93.6 Estimators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103.7 Concluding remarks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

4 State feedback control 124.1 Problem formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124.2 Altitude controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124.3 Attitude controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 154.4 Concluding remarks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

5 Output feedback control 185.1 Problem formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 185.2 Altitude controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 185.3 Attitude controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 215.4 Concluding remarks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

6 Simulations 246.1 State feedback control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 246.2 Output feedback control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 256.3 Concluding remarks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

7 Conclusions and recommendations 277.1 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 277.2 Recommendation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27

References 28

Appendices 29

A Derivatives and derivations 29A.1 Reference dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29A.2 Tracking error . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29A.3 Estimated tracking error . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30A.4 Control law . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31A.5 Reference thrust . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31A.6 Thrust . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31A.7 Desired thrust . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

B Lyapunov Stability Criterion 33

B.1 Lyapunov function of the altitude output feedback controller . . . . . . . . . . . . . . . . 33B.2 Lyapunov function of the attitude output feedback controller . . . . . . . . . . . . . . . . 33

C Simulation in the Simulink model of the Parrot Mambo 35

List of Symbols

Symbol Descriptiond(R1, R2) Distance between two rotation matrices in the SO(3) group

e3[0 0 1

]Tf Input thrustfd Desired thrust directiong Gravitational accelerationm Mass of the droneu Virtual control law||q|| Unit quaternion||x|| Euclidean norm of a vector x||x|| Derivative of the Euclidean normJ Mass moment of inertia tensorR Rotation matrices relating the inertial frame to the body-fixed frameRd Desired rotation matrixRe Attitude tracking errorRr Reference rotation matrix

R Estimated attitude tracking errorS(a) Skew-symmetric matrix of a vector aV Lyapunov function candidateφ, θ, ψ Pitch angle, roll angle and yaw angle respectively

φ, θ, ψ Pitch rate, roll rate and yaw rate respectivelyν, νr Body-fixed linear velocityνe Tracking velocity error in the body-fixed frameνe Estimated tracking velocity error in the body-fixed frameρ, ρr Position center of the drone relative to the inertial frameρe Tracking position error in the body-fixed frameρe Estimated tracking position error in the body-fixed frameσ(e) Saturation function of an error eτ Input torqueω, ωr Body-fixed angular velocitiesωd Desired body-fixed angular velocityωe Body-fixed angular velocity tracking errorω Estimated body-fixed angular velocity tracking error

1. INTRODUCTION

1 Introduction

Recently, more and more applications of drones arise. The drones could be helpful in the working fieldof search and rescue for example. Inaccessible areas for humans may arise during natural or nucleardisasters. Consequently, when it is achieved that a drone is able to manoeuvre autonomously, it canfly through a particular disaster area as described above to help locate victims and help to deliver lifesupporting attributes. Another interesting occasion drones could be of help in is safety surveillance.The drone should be able to recognize criminal activities such as drugs and migrant smuggler at borderareas [1], after which the local safety authority can be alarmed much more efficiently.

In order to achieve these tasks, a controller needs to be developed [2]. Recently, the Technical Universityof Eindhoven developed nonlinear controllers for this purpose in [3], [4] and [5]. These controllers needto be validated by means of simulation. The recently introduced Aerospace Blockset by Matlab can beused for this validation. This Blockset provides Simulink blocks for modeling, simulating and analyz-ing aerospace vehicles. Next to this, the Blockset incorporates environment models to represent flightconditions and increase fidelity as well as a visualization of the drone in a simulation environment [6].Subsequently, Simulink Support Package developed a model for parrot minidrones, and in particular thetype named Parrot Mambo, which lets on design and build flight control algorithms for these type ofminidrones. The Dynamics and Control Group of the Technical University of Eindhoven is currentlytesting with the AR Drone 2.0, however, this type of drone is out of stock. Therefore, the Dynamicsand Control Group wants to know if this new tooling in Matlab can be used for designing, tuning andtesting controllers.

1.1 Modeling of the drone using the Aerospace Blockset

The Simulink model of the Parrot Mambo generates a user-defined reference trajectory, which is sub-sequently sent to the flight control system. The reference states are compared to the measured statesin the flight control system after which the control actions are determined. The control actions are theinput thrust f ∈ R and input torque τ ∈ R

3. These control actions are thereafter converted to anelectrical signal, which is sent to the actuators of the rotors, in this way the motor speed is controlled.Now, the system dynamics are modeled in Simulink using the six-degrees-of-freedom (DOF) equation ofmotion block of the Aerospace Blockset as can be seen in [7]. The inputs of this block are the thrust andtorque around the respective axis and therefore the electric signal, which is sent to the actual actuatorsis converted back to a force and torque (this conversion is only done in order to model the system dy-namics in the simulation environment). The 6 DOF equation of motion block converts the input thrustand torque to translational and rotational states (positions, velocities and accelerations) of the droneat a particular moment in time by using the translational and rotational equations of motions in thebody-fixed coordinate frame. These states are used to model the influences of the environment on theflight path. Besides this, these states are used as input for the sensor dynamics, which incorporate theequations that describe the behaviour of the measurement equipment stored on the Parrot Mambo andprocesses the ideal states of the drone that follow from the 6 DOF motion block into simulated, morerealistic, measured states with added noise and biases for example. These measured states are used ascontroller input next to the reference states as mentioned above. In this way the closed-loop system ofthe Parrot Mambo is obtained.

1.2 Problem formulation

The current Simulink model of the Parrot Mambo provided by Matlab generates a reference trajectory tohover at a certain height. The flight control system consists of both an estimator and a linear controller,which are designed separately. The estimator uses a complementary filter to estimate attitude and aKalman filter to estimate position and velocity. For control, the model uses [8]:

• A PID controller for pitch/roll control;

• A PD controller for yaw control;

• A PD controller for position control in North-East-Down coordinates.

1

1. INTRODUCTION

It turns out that this linear controller design is highly unstable, because even when the trajectory, whichis hovering by default, is only changed a little the drone crashes in the simulation environment. Thisresearch therefore focuses on replacing these linear controllers by nonlinear controllers.

1.3 Objectives

The ultimate goal will be to fly in synchronization with multiple drones such that they can perform certaintasks together. In order to accomplish this ultimate goal, this is subdivided in different objectives. Thedifferent objectives that are looked into in this research are:

• Implementation of a nonlinear state feedback controller developed by the Dynamics and ControlGroup of the Technical University of Eindhoven in Matlab with the ode45 solver for an arbitrarytrajectory and illustration of stability of this state feedback controller by means of simulation;

• Extension of this state feedback controller to an output feedback controller in Matlab with theode45 solver for an arbitrary trajectory using combined controllers and observers obtained from theDynamics and Control Group of the Technical University of Eindhoven for filtering measurementsand illustration of stability of this output feedback controller by means of simulation;

• Implementation of both the state feedback controller and output feedback controller in the Simulinkmodel of the Parrot Mambo provided by the Simulink Support Package and a comparison betweenthe results of the ode45 solver and Simulink model of the Parrot Mambo;

• Comparison between saturated PD-term at once and separately saturated P-term and D-term andchoosing the one with the smallest average position tracking error and fastest settling time to thesteady-state error.

1.4 Outline

This report firstly introduces some theorems and mathematical expressions that are used throughout thereport in Section 2, which presents the preliminaries. Section 3 derives the drone dynamics using Newton’ssecond law and shows how to obtain the reference dynamics from these drone dynamics for an arbitrarytrajectory. Furthermore, this section discusses how the sensors are modeled in the Simulink model ofthe Parrot Mambo using the Aerospace Blockset and lastly presents the main working principles of theobservers in the model to obtain all possible states, which are used to design an appropriate controller.Section 4 introduces the tracking problem and illustrates how to solve this problem using state feedbackcontrol. Next to this, the stability of the state feedback controller is illustrated by means of simulation.Subsequently, Section 5 discusses a proposed output feedback controller that uses combined controllerand estimators to solve the tracking problem while showing stability of the closed-loop system as well.This section is followed by Section 6, which evaluates the ability to follow a set point, when the initialconditions of the actual states of the drone differ from the initial conditions of the reference states of thedrone. This evaluation is achieved by using the ode45 solver in Matlab as well as the Simulink model ofthe Parrot Mambo. Besides this, a comparison between the results of the ode45 solver and the Simulinkmodel of the Parrot Mambo is given. Finally, the conclusions and recommendations in Section 7 closethe report.

2

2. PRELIMINARIES

2 Preliminaries

This section discusses some theorems, definitions and mathematical notations, which are used in theremainder of this report. Firstly, the norm and its derivative are discussed and in particular the naturalRiemannian metric. Secondly, the shortcomings of rotation matrices using Euler angles or quaternionsare discussed. This is followed by introducing the Special Orthogonal Group SO(3) in combination withthe properties of the skew-symmetric matrix. Lastly, the Lyapunov stability theorem is presented, whichis used extensively to illustrate stability of the proposed output feedback controller in this report.

2.1 Norm

The norm of a vector x ∈ Rn is defined by:

||x|| =√xTx =

√x21 + ...+ x2n. (2.1)

The derivative of the norm is now given by:

||x|| = 1

2(xTx)−

12 · (xTx+ xT x) =

xT x√xTx

, x �= 0. (2.2)

This derivative is used extensively in the derivation of the reference dynamics and controller design.Furthermore, the distance of a rotation matrix R1 with respect to a rotation matrix R2 is described byusing the natural Riemannian metric [3]:

d(R1, R2) = || log(R1RT2 )||. (2.3)

This is a useful tool to define for example the error measure of the tracking problem.

2.2 Euler angles and quaternions

One way of describing the rotation matrix R is as a product of successive rotations about the principalcoordinate axis in a specific order [9]. The rotation about the x-axis is denoted as roll angle (φ), rotationabout the y-axis is denoted as pitch angle (θ) and lastly, the rotation about the z-axis is denoted asyaw angle (ψ) in this report. These angles are referred to as Euler angles [10]. The body-fixed frameis chosen such that it is right-handed and the x-direction is pointing in the direction along the noseof the quadcopter, the y-direction is pointing to the right of the drone and the z-direction is pointingdownwards [8]. Furthermore, the dynamics in this report are described in the moving body-fixed frame,because it is easier to measure the actual states in this frame. Therefore, the rotation order is given by:

R = Rx,φRy,θRz,ψ

=

⎡⎣cφ −sφ 0sφ cφ 00 0 1

⎤⎦⎡⎣ cθ 0 sθ

0 1 0−sθ 0 cθ

⎤⎦⎡⎣1 0 00 cψ −sψ0 sψ cψ

⎤⎦ , (2.4)

where cα equals cos(α), sα equals sin(α) and tα equals tan(α) in which α represents one of the Eulerangles as described above. The relationship between the derivative of these Euler angles and body-fixedangular velocities is presented in [11] and is given by:

⎡⎣φθφ

⎤⎦ =

⎡⎣1 sφtθ cφtθ0 cφ −sφ0 sφ/cθ cψ/cθ

⎤⎦⎡⎣pqr

⎤⎦ . (2.5)

Now singularities occur, if θ = π/2+kπ, because then cθ = 0 and tθ becomes undefined. This singularityproblem is also known as gimbal lock. Most controllers developed so far are based on these Euler anglesand it is tried to overcome this issue by describing the controllers in quaternions. A unit quaternionrotation matrix is given by [11]:

R =1

||q||

⎡⎣||q||

2 − 2(q2y + q2z) 2(qxqy − qzqr) 2(qxqz + qyqr)2(qxqy − qzqr) ||q||2 − 2(q2x + q2z) 2(qyqz − qxqr)2(qxqz − qyqr) 2(qyqz + qxqr) ||q||2 − 2(q2x + q2y)

⎤⎦ , (2.6)

3

2. PRELIMINARIES

in which the unit quaternion can be written as:

q = qr + qxi+ qyj + qzk = cos(γ

2) + i · (x · sin(γ

2)) + j · (y · sin(γ

2)) + k · (z · sin(γ

2)), (2.7)

where γ denotes the angle around the vector the drone is rotating. A unit quaternion means that itsatisfies the following condition:

||q|| = qq =√q2w + q2x + q2y + q2z = 1. (2.8)

Since q yields the same rotation matrix as q, this leads to ambiguities, when using quaternions forcontroller design [11]. Both the singularity problem and ambiguity problem can be avoided by introducingthe Special Orthogonal Group denoted by SO(3) as is discussed in Section 2.3.

2.3 Special Orthogonal Group SO(3)

The tracking control is considered using the SO(3) group as stated in [3] to rotate a moving frame ontoa fixed frame or the other way around. The definition of this Special Orthogonal Group is as follows:R ∈ SO(3) =

{R ∈ R

3x3 | RT = R−1, RTR = I, det R = 1}. This means that the columns of R ∈

SO(3) are mutually orthogonal and each column of R ∈ SO(3) is a unit vector. This SO(3) group is lateron extensively used to describe among others the reference dynamics and tracking error dynamics. Thederivative of the rotation matrix in this group is described by [3]:

R = RS(ω), (2.9)

where S(ω) denotes the skew-symmetric matrix, which is used to easily calculate the outer productbetween two vectors, since a× b = S(a)b, and is defined as:

S(ω) = −S(ω)T =

⎡⎣ 0 −r qr 0 −p−q p 0

⎤⎦ . (2.10)

Furthermore, throughout the report the following properties of the skew-symmetric matrix are used [4]:

S(a)b = −S(b)a (2.11a)

RS(a) = S(Ra)R (2.11b)

aTS(b)c = cTS(a)b = bTS(c)a (2.11c)

(RS(a))T = −S(a)RT , (2.11d)

in which a, b, c ∈ R3 and R ∈ SO(3).

2.4 Lyapunov stability

Lyapunov stability analysis is used to study the stability of motion described by systems of ordinarydifferential equations (ODEs), i.e. a dynamical system that satisfies [12]:

x = f(x, t) x(t0) = x0 x ∈ Rn. (2.12)

A system is stable in the sense of Lyapunov, if, for any given ε > 0, one can find a δ > 0 such that:

||x(t0)|| < δ =⇒ ||x(t)|| < ε, ∀t ≥ t0. (2.13)

If the state is not only stable, but in the limit as t −→ ∞, ||x(t)|| −→ 0, the system is said to beasymptotically stable as well. An illustration of this is shown in Figure 2.1.

4

2. PRELIMINARIES

Figure 2.1: Representation of the definition of Lyapunov stability criterion [13]

For the drone, this means that the error converges towards zero as time goes to infinity, if the trajectoryis feasible and the initial conditions of the trajectory satisfy the above stated condition. Lyapunov’smethod can be seen as a measure of the total energy stored in the system at any instant and if thisstored energy is no longer changing, the system must be at rest [14]. In case of the closed-loop system ofthe drone, when the stored energy is no longer changing, this implies that the drone is exactly followingits reference trajectory. A time-invariant Lyapunov function V (x) has the following properties:

• V(0) = 0;

• positive definite: V (x) > 0, ||x|| �= 0;

• V is continuous and has continuous derivatives with respect to all components of x;

• negative definite: V (x) = ∂V∂x x = ∂V

∂x f(x) < 0 along trajectories of the equation.

Now, if V (x, t) is positive definite, V (x, t) is negative definite and radially unbounded, then the originof the system is globally asymptotically stable (GAS). Radially unbounded means that if V (x) −→ ∞ as||x|| −→ ∞ [9]. It is uniformly globally asymptotically stable (UGAS), when also [15]:

• V (x, t) ≥W1(x) > 0, with W1(x) being a positive definite function;

• V (x, t) ≤ −W2(x) < 0, with W2(x) being a positive definite function,

is satisfied. For the definition of UGAS, see also [3] and [10]. The system is uniformly almost globallystable (UaGAS), if it is UGAS, except for initial conditions in a set of measure zero [3]. These propertiesmake the Lyapunov function a useful tool to prove and illustrate stability of derived closed-loop dynamics.

5

3. SYSTEM DYNAMICS

3 System Dynamics

First some insights in the dynamics of the system have to be gained in order to set up a proper controllerdesign. Therefore, in this section there is looked into how the dynamic model of the drone can be derivedby using a schematic overview of the drone. Subsequently, the drone dynamics can be subdividedin both translational dynamics and attitude dynamics with the corresponding differential equations.Consequently, the results of these drone dynamics are used to derive the reference dynamics of anarbitrary trajectory. It is discussed how the Parrot Mambo measures its actual states and how thesecan be modeled using the Aerospace Blockset. Lastly, it is explained how estimators are used to obtainall possible states of the drone. These results are later on used in the control design to define trackingerrors and the corresponding error dynamics.

3.1 Dynamic model

A schematic overview of the drone with its most important parameters is provided in Figure 3.1.

Figure 3.1: Schematic overview of the drone with its key parameters [11]

Two coordinate frames are introduced in order to describe the position and orientation of the drone inspace [9]. Now, the body fixed-frame B with basis vectors

{b1, b2, b3

}is defined in the center of mass of

the drone and the body-fixed frame moves along with the drone. The inertial frame I with basis vectors{e1, e2, e3

}is fixed with respect to the earth. The inertial frame is now used to relate the transformations

of the drone, both translations and rotations, in space to the body-fixed frame. In this way, the positionand orientation of the drone can be monitored with respect to a fixed point. Furthermore, it is assumedthat:

• The drone is axisymmetric;

• The drone is a rigid body.

Let ρ = (x, y, z) ∈ R3 denote the position of the centre of mass relative to a North-East-Down (NED)

inertial frame. Let ν = (u, v, w) ∈ R3 denote the body-fixed linear velocity. The rotation matrix R ∈

SO(3) denotes the rotation from the body-fixed frame to the inertial frame. Lastly, ω = (p, q, r) ∈ R3

denotes the body-fixed angular velocity obtained by the roll rate (p), pitch rate (q) and yaw rate (r).These notations and assumptions follow from [3], [10] and [11]. Besides this, in [10] also the equationsof motion for the drone are derived, which is explained in Sections 3.2 and 3.3 as well.

6

3. SYSTEM DYNAMICS

3.2 Translational dynamics

The dynamics are derived relative to the body-fixed frame. The rotation matrix R is used to relate theinertial velocities ρ ∈ R

3 to the body-fixed linear velocities ν by:

ρ = Rν. (3.1)

Furthermore, the translational dynamics are solved by using Newton’s second law:

ΣF =d

dt(mν)

= m(ν + ω × ν).(3.2)

This equation incorporates both the translational accelerations ν and rotational accelerations ω×ν of thebody-fixed frame. Furthermore, m denotes the mass of the drone. The forces working in the body-fixedframe are assigned by:

ΣF = −fe3 +mgRT e3, (3.3)

where f represents the sum of the thrust of all four rotors and is assumed to be maintained positive at alltime and is always pointing upwards in the e3-direction in the body-fixed frame explaining the negativesign. The gravitational force mg is always pointing downwards in the inertial frame and therefore therotation matrix R is used to describe the contribution of this force in the body-fixed frame. Consequently,the full translational equation of motion is described by:

m(ν + ω × ν) = −fe3 +mgRT e3

ν = −S(ω)ν + gRT e3 − f

me3.

(3.4)

The equation is rewritten by applying the fact that the cross product can be described by means of theskew-symmetric matrix as explained in Section 2.3.

3.3 Attitude dynamics

The attitude dynamics, or rotational dynamics, use the derivative of the rotational matrix as presentedin (2.9). Furthermore, Newton’s second law for rotation is used to set up the equation of motion for theattitude dynamics:

Στ =d

dt(Jω)

= Jω + ω × Jω,(3.5)

where J represents the mass moment of inertia matrix. This equation is also known as the Newton-Eulerequation. Now, by applying the property (2.11a) and rewriting this equation, the complete rotationalequation of motion in the body-fixed frame is given by:

Jω = −S(ω)Jω + τ

Jω = S(Jω)ω + τ.(3.6)

The required torque around a certain axis for a prescribed flight path is realized by adjusting the thrustof the individual rotors. In this way the drone can also change direction in the (xy)-plane. The nextsection uses these drone dynamics to derive the reference dynamics. These reference dynamics are usedto find expressions for the reference states (ρr,νr,Rr,ωr,fr,τr) given an arbitrary reference trajectory ρr.These reference states are used as set point in the error definitions as explained in Sections 4 and 5.

3.4 Reference dynamics

The differential equations of the reference dynamics are now given by:

ρr = Rrνr (3.7a)

7

3. SYSTEM DYNAMICS

νr = −S(ωr)νr + gRTr e3 −

frme3 (3.7b)

Rr = RrS(ωr) (3.7c)

Jωr = S(Jωr)ωr + τr. (3.7d)

The test flight path of the drone that is used here and in the remainder of the report is defined as:

ρr =

⎡⎣ cos(t)

sin(t)−1.5− sin(t)

⎤⎦ . (3.8)

The last term is defined negative to preserve that the drone ascends, because of the choice of a NEDcoordinate system meaning that the z-axis is defined positive downwards. By differentiating (3.7a) andsubstituting (3.7b) and (3.7c) in the obtained expression gives:

ρr = ge3 −Rrfrme3, (3.9)

in which fr ∈ R represents the reference thrust and for a feasible trajectory this reference thrust satisfies:0 < fmin

r ≤ fr(t), ∀t. Therefore, this expression can be rewritten to:

fr = m||ge3 − ρr||. (3.10)

Here is used that ||frRre3|| =√(frRre3)T (frRre3) =

√f2r e

T3 R

Tr Rre3 =

√f2r e

T3 e3 =

√f2r = fr. The

associated rotation is not uniquely defined since there is an arbitrary yaw possible. Subsequently, bymaking a particular choice for this yaw, the reference rotation matrix can be defined such that it satisfiesthe properties of the SO(3) group as explained in Section 2.3 and presents the rotation from e3 to Rre3in the plane spanned by e3 and Rre3 as stated in [11]:

Rr =

⎡⎢⎢⎢⎣1− r213

1+r33− r13r23

1+r33r13

− r13r231+r33

1− r2231+r33

r23

−r13 −r23 r33

⎤⎥⎥⎥⎦ . (3.11)

The advantage of defining the reference rotation matrix in this way is that the reference dynamics areonly dependent on an arbitrary user-defined flight path ρr now. The corresponding individual terms forthis particular choice of the reference rotation matrix equal:

Rre3 =

⎡⎣r13r23r33

⎤⎦ =

ge3 − ρr||ge3 − ρr|| =

ge3 − ρr√(ge3 − ρr)T (ge3 − ρr)

. (3.12)

Subsequently, the reference body-fixed linear velocity can be determined as well as the body-fixed angularvelocities, after differentiating the reference rotation matrix, by rewriting (3.7a) and (3.7c) respectively:

νr = RTr ρr (3.13a)

S(ωr) = RTr Rr. (3.13b)

After using the definition of the skew-symmetric matrix, the body-fixed angular velocity can be con-structed:

ωr =

⎡⎢⎢⎣−r23 + r23r33

1+r33

r13 − r13r331+r33

r23r13−r13r231+r33

⎤⎥⎥⎦ . (3.14)

This result requires the derivative of (3.12), which is provided in Appendix A.1. Lastly, to be able todetermine the reference torque, the body-fixed angular acceleration needs to be determined by differen-tiating the expression for the body-fixed angular velocity, hence:

ωr =

⎡⎢⎢⎢⎣

−r23 + r23r33+r23r331+r33

+−r23r

233

(1+r33)2

r13 − r13r33+r13r331+r33

− −r13r233

(1+r33)2

r23r13−r13r231+r33

+ r23r13−r13r231+r33

− (r23r13−r13r23)r33(1+r33)2

⎤⎥⎥⎥⎦ . (3.15)

8

3. SYSTEM DYNAMICS

The result requires the second derivative of (3.12), which is provided in Appendix A.1 as well. Conse-quently, by rewriting (3.7d), the reference torque τr ∈ R

3 can be determined:

τr = Jωr − S(Jωr)ωr. (3.16)

Here, J is the mass moment of inertia matrix with respect to the body-fixed frame of the drone writtenin the inertial tensor form:

J =

⎡⎣Ixx Ixy IxzIyx Iyy IyzIzx Izy Izz

⎤⎦ =

⎡⎣5.83e

−5 0 00 7.17e−5 00 0 1e−4

⎤⎦ kg ·m2. (3.17)

Due to the assumption that the body of the drone is axisymmetric, the cross products of the inertiaare all zero [9]. Furthermore, the mass of the drone equals 0.063 kg and the gravitational accelera-tion is rounded off to 9.81 m/s2. Now, all expressions are determined to solve the reference states(ρr, νr, Rr, ωr, fr, τr) such that any arbitrary trajectory can be generated by the user when defining afeasible ρr and calculating ρr, ρr,

...ρr and

....ρr . The dynamics are now solved using the test flight path

as presented in (3.8) and the results of the reference states are used as set points for the controller design.

An appropriate controller design is required to ensure a stable closed-loop system to make sure thatthe drone can follow its reference position even when the system is disturbed by external influences orthe drone starts at other initial conditions than the reference states. For this reason, also feedback isrequired of the actual states of the drone, which can be obtained from sensors. In order to model theclosed-loop of the drone, it is important to know with which sensors the Parrot Mambo is equipped andthus which states can be measured. The next section elaborates how the ideal states that follow from the6 DOF equation of motion block as explained in Section 1.1 can be used to model the sensor dynamicswith corresponding measured states.

3.5 Sensor dynamics

The Parrot Mambo drone is equipped with an inertial measurement unit (IMU) including a 3-axisaccelerometer and 3-axis gyroscope, which are used to evaluate speed, attitude and obstacle contact.Furthermore, the Parrot Mambo is equipped with an ultrasound sensor, pressure sensor and camerasensor, which are used to construct the three dimensional position ρ of the drone [16]. The three-axis inertial measurement unit of the Aerospace Blockset is used to model these states. This blockincorporates the three-axis accelerometer and three-axis gyroscope.

3.5.1 Accelerometer

The three-axis accelerometer is described by [17]:

Ameas = Aimeas · ASFCC + Abias + noise, (3.18)

where Ameas are the measured accelerations representing the measured data of the real accelerometer,ASFCC ∈ R

3x3 are the scaling factors on the diagonal to scale the accelerations along the body-axis,Abias are the biases and white noise is generated to represent the noise of the real measurement device.The values for ASFCC , Abias and the noise are obtained from constant values provided in data files bythe Simulink Support Package. The ideal measured accelerations are described by:

Aimeas = Ab + ωb × (ωb × d) + ˙ωb × d− g, (3.19)

where Ab is the ideal acceleration relative to the body-fixed frame at the center of gravity, which followsfrom the 6 DOF equation of motion block as explained in Section 1.1, ωb is the ideal body-fixed angularrate, ˙ωb the body-fixed angular acceleration and d is the lever arm. The measurement reference positionis the same as for the center of gravity input meaning that this lever arm becomes �0, so (3.19) can besimplified to:

Aimeas = Ab − g. (3.20)

Hence, the accelerometer provides the following states with respect to the inertial frame: (x, y, z).

9

3. SYSTEM DYNAMICS

3.5.2 Gyroscope

Since the g-sensitive biases are �0, the dynamics of the gyroscope can be written as [17]:

ωmeas = ωb · ωSFCC + ωbias + noise, (3.21)

where ωb follows from the 6 DOF motion block as explained in Section 1.1, ωSFCC ∈ R3x3 of scaling

factors on the diagonal to scale the angular velocities along the body-axis and ωbias the biases. Hence,the gyroscope provides the following states with respect to the body-fixed frame: (p, q, r).

3.5.3 Optical flow sensor

The optical flow sensor uses the body-fixed velocity vb ∈ R3, body-fixed angular velocity ωb ∈ R

3 andthe position with respect to the inertial frame xNED ∈ R

3. Again, these states follow from the 6 DOFmotion block. The simulated measured velocity of the center of mass of the drone with respect to theinertial frame, Vmeas ∈ R

3, is described by:

Vmeas =

⎡⎣vbx + ωbx · xDvby + ωby · xD

vbz

⎤⎦ , (3.22)

where xD is the distance from the inertial frame to the body-fixed frame in e3-direction. The velocity ofthe center of mass is subsequently multiplied with a gain with values provided by the Simulink SupportPackage to obtain the optical flow sensor data. Hence, the optical flow sensor provides the followingstates: (x, y, z). It is, however, stated in this part of the Simulink model that this is a crude simulationof the actual optical flow data due to inaccessible code of the Parrot’s firmware.

3.5.4 Ultrasound and pressure sensor

The ultrasound sensor simply uses the xD that follows again from the 6 DOF motion block. The pressuresensor also uses the xD multiplied with a gain and an added bias with values provided by the SimulinkSupport Package as well. Hence, the altitude state z follows from this.

3.6 Estimators

The estimators in the model have two main purposes. Firstly, the variables of interest can only bemeasured indirectly. Secondly, measurements are available from various sensors, but are subject tonoise.

3.6.1 Complementary filter

Integration of the gyroscope data provides the angular orientation or attitude of the drone, however,this integration over time causes the measurement to drift in the long-term. On the other hand, theaccelerator is very sensitive for external disturbances, however, has the advantage that it does notdrift. These disadvantages of both the accelerometer and gyroscope can be prevented by means ofa complementary filter inspired by [18]. This complementary filter combines the information of thegyroscope measurements and the accelerometer measurements. Thresholds are used in the model to seeif the accelerometer takes reasonable values and if so, the values of the accelerometer are used for 0.1%to calculate the roll and pitch angles and compensate for the drift of the gyroscope. The remaining partto obtain the real time Euler angles is calculated by using the gyroscope data:⎡

⎣φiθiψi

⎤⎦ = 0.999 ·

⎛⎝⎡⎣φi−1

θi−1

ψi−1

⎤⎦+

⎡⎣φiθiψi

⎤⎦ · Ts

⎞⎠ . (3.23)

The real-time data of the gyroscope is processed into φi, θi and ψi and subsequently multiplied with thesampling time Ts. After this, the Euler angles of the previous time step φi−1, θi−1 and ψi−1 are used toobtain the attitude at real-time and combines this result with the accelerometer as described above tocompensate for the long-term drift. Hence, the states (φ, θ, ψ) are obtained from this.

10

3. SYSTEM DYNAMICS

3.6.2 Kalman filter

Since the accelerometer is very sensitive to external disturbances as described above, a Kalman filter,see [19], is used to obtain the desired position states (x, y) relative to the inertial frame, instead of directintegration of these states that follow from the accelerometer. Firstly, the states that follow from theoptical flow sensor and accelerometer are combined to give a better approximation of the velocity usinga Kalman filter. This estimated velocity is then used as input of a second Kalman filter to obtain anestimation for the position states (x, y). Lastly, a third Kalman filter, which combines the informationobtained from the ultrasound sensor, pressure sensor and z from the accelerometer, provides an estima-tion for the altitude state z.

Finally, all obtained simulated measured and estimated states that follow from the sensor equipmentand estimators are: (x, y, z, x, y, z, φ, θ, ψ, p, q, r). Let (x, y, z) construct ρ ∈ R

3 and (x, y, z) constructρ ∈ R

3. Furthermore, the Euler angles (φ, θ, ψ) that follow from the complementary filter are based onmeasured data and can be used to determine the rotation matrix R ∈ SO(3) according to (2.4). This isin contrast with the rotation matrix Rr ∈ SO(3), which is designed according to the properties of theSO(3) group independent of the Euler angles. Lastly, (p, q, r) are used to determine ω ∈ R

3. Now, itis known which states can be constructed from the measurement equipment of the drone, a controllerdesign can be formulated. This is elaborated in Sections 4 and 5.

3.7 Concluding remarks

The drone dynamics are derived by using a fixed inertial frame and moving body-fixed frame. Thedynamics are expressed in the moving body-fixed frame by using the rotation matrix. Besides this,Newton’s second law is used to derive both the translational and attitude dynamics. The full dronedynamics can be described by a 12th order system:

ρ = Rν (3.24a)

ν = −S(ω)ν + gRT e3 − f

me3 (3.24b)

R = RS(ω) (3.24c)

Jω = S(Jω)ω + τ. (3.24d)

These dynamics are used to derive the reference dynamics of the drone. Now, when a user-definedarbitrary feasible four times continuously differentiable trajectory is formulated, the reference dynamicscan be solved leading to expressions for the reference states after which these reference states can beused as set point in the tracking problem of the controller design. Next to this, the sensor equipmentof the Parrot Mambo is determined, after which there is looked into the dynamics of the sensors thatmodel the measured states of the drone in the Simulink model of the Parrot Mambo using the AerospaceBlockset. Furthermore, it is discussed how estimators are used to obtain all possible states of the drone.These states, together with the reference states, are used to define the tracking error as described inSection 4 and 5.

11

4. STATE FEEDBACK CONTROL

4 State feedback control

Now the reference dynamics and drone dynamics are derived, a control law needs to be defined basedon the error dynamics. This control law should ensure that the actual position of the drone convergestowards the reference position, when the initial conditions of the actual states of the drone are not equalto the initial conditions of the reference states or an external disturbance occurs. There are two typesof feedback control considered in this report: state feedback and output feedback. In state feedbackit is assumed that all states (ρ,ν,R,ω) follow from direct measurements. However, from Section 3 itfollows that ν can not be measured directly. Therefore, for the state feedback controller this body-fixedvelocity ν is reconstructed by using (3.1), since ρ and R are expressions that can be derived from directmeasurements. This control design is elaborated in this section starting with a problem formulation,followed by respectively the altitude controller and attitude controller.


Appropriate control laws have to be found for (ρr, Rr, νr, ωr, fr, τr) being a feasible trajectory as statedin [3]

f = f(ρr, νr, Rr, ωr, ρ, ν, R, ω)

τ = τ(ρr, νr, Rr, ωr, ρ, ν, R, ω)(4.1)

such that for the resulting closed-loop system

limt→∞ ε(ρe(t), νe(t), Re(t), ωe(t)) = 0, (4.2)

where ρe is the position tracking error, νe is the body-fixed velocity tracking error, Re is the attitudetracking error and ωe is the body-fixed angular velocity tracking error, which are defined in Section 4.2and 4.3. Let ε be the corresponding error measure, defined as:

ε(ρe, νe, Re, ωe) = ||ρe||+ ||νe||+ || logRe||+ ||ωe||. (4.3)

The attitude tracking error Re should converge to the identity matrix I ∈ R3, because when taking the

matrix logarithm and norm of this result gives || logm(I)|| = 0. This explains the way (4.2) and (4.3) aredefined.

4.2 Altitude controller

The tracking error in the body-fixed frame is proposed as [3]:

ρe = RTr (ρr − ρ)

νe = νr −RTr Rν.

(4.4)

This way of defining the tracking errors in the body-fixed frame instead of defining it in the inertialframe gives the advantage that the error definition becomes invariant for both translation and rotationof the inertial frame. Subsequently, the tracking error dynamics in the body-fixed frame are derived inAppendix A.2. The resulting equations are:

ρe = −S(ωr)ρe + νe

νe = −S(ωr)νe +RTr R

f

me3 − fr

me3.

(4.5)

In order to stabilize these tracking error dynamics, RTr R

fme3 − fr

m e3 is assumed to be a virtual inputu, which should be controlled by the thrust magnitude and attitude. Consequently, (4.5) is written inclosed-loop dynamics as:

ρe = −S(ωr)ρe + νe

νe = −S(ωr)νe + u.(4.6)

12


By introducing the state space vector e = (ρe, νe) ∈ R6 these closed-loop dynamics can be written in

state space notation as [10]:e = Ae+Bu (4.7a)

y = Ce, (4.7b)

with:

A =

[−S(ωr) I0 −S(ωr)

] ∣∣∣∣x�,u�

B =

[0I

] ∣∣∣∣x�,u�

C =[I 0

] ∣∣∣∣x�,u�

,

(4.8)

where A ∈ R6x6, B ∈ R

6x1 and C ∈ R1x6. One way of selecting an appropriate control law, u, is to

linearize the system around hover meaning that: x� = (ρr, νr, Rr, ωr) = (ρ�, 0, I, 0) and u� = (fr, τr) =(−mg, 0). The resulting transfer function, that can be derived from the linear state-space notation, isdescribed by [20]:

y(s)

u(s)= C(sI −A)−1B +D. (4.9)

Subsequently, the Bode-diagram that follows is displayed in Figure 4.1.

-40

-30

-20

-10

0

10

Mag

nitu

de (

dB)

100 101-181

-180.5

-180

-179.5

-179

Pha

se (

deg)

Bode Diagram

Frequency (rad/s)

Figure 4.1: Resulting Bode-diagram of the drone linearized around hover

Now, this is the expected outcome since ωr = 0 for hovering, it follows from (4.6) that u = ρe meaningthat the resulting transfer function is a double integrator resulting in a minus two slope and a phase of-180 degrees in the Bode-diagram. Since the system is linearized now, linear control laws can be applied.Based on this Bode-diagram, a PD-controller is proposed here to add the required phase margin andcorresponding gain margin. In the time domain, this controller can be written as:

u(t) = kρρe(t) + kννe(t), (4.10)

in which kρ is the proportional gain and kν is the derivative gain. Subsequently, to have a well-definedcontroller that is UaGAS and without an ambiguity that arises in the stability proof, the crucial assump-tion has been made that the thrust should maintain positive [3]. A feasible reference trajectory satisfies:

0 < fminr ≤ fr(t) and to maintain the thrust positive it should be guaranteed that: ||u|| ≤ fmin

r −εm for any

0 < ε ≤ fminr . These conditions can be assured by saturating (4.10). The PD-action can be saturated

at once or the P-action and D-action can be saturated separately, respectively:

u = −σ(kρρe + kννe) (4.11)

u = −σ(kρρe)− σ(kννe). (4.12)

13


The saturation function can be defined as a vector-function σ(e) = s(eT e)e in which s should be a twicecontinuously differentiable function satisfying s(0) > 0 and for which

∫s(x)dx is radially unbounded. A

possible candidate is presented in [21]:

σ(e) =k0k∞e√

k2∞ + k20eT e, (4.13)

in which k∞ > 0 and k0 > 0. In case of saturating (4.11) and (4.12), k0 represents the slope of thesaturation function at very small errors. Therefore, the slope can be approximated linearly at very smallerrors meaning that the nonlinear controller behaves as a linear PD-controller. The control variable k∞makes sure that the control law is bounded for large errors and should be chosen such that the thrustremains positive as described above. If the control law is not bounded correctly, the crucial assumptionthat the thrust should remain positive does not hold. A k∞ that meets these requirements with a certainsafety margin for the saturation of the PD-action at once and for the separately saturated terms arerespectively:

k∞ =ε

m=

0.9 · fminr

m(4.14)

k∞,sep =εsepm

=0.45 · fmin

r

m, (4.15)

in which k∞,sep for the separately saturated terms is half of the value of k∞ of the saturated PD-actionat once, because when both ρe and νe are simultaneously very high, u becomes −k∞,sep − k∞,sep for theseparately saturated PD-action, which is the same as the k∞ for the saturation of the PD-action at once

at large errors. Let’s assume now that the drone starts at ρ(t0) =[0 0 0

]Tand the reference starts

at ρr(t0) =[1 0 1.5

]Tleading to the initial conditions as stated in Table 4.1.

Table 4.1: Initial conditions

State Variable Initial Condition

ρe [0.84 0 1.59]T

νe [-0.10 0 0.99]T

The gains in the linearized controller of (4.10) can be tuned around hover with linear controller tech-niques, when taking into account the disturbances of for example the sensor noise. However, for now, itsuffices to take kρ > 0 and kν > 0 for the controller gains. Subsequently, the gains of the controller aretuned in an iterative manner by looking at the lowest average tracking error over time and the fastestsettling time to the steady-state error. The obtained values are presented in Table 4.2.

Table 4.2: Controller gains

Variable Valuekρ 1.8kν 1k0 10

The differential equations of the closed-loop dynamics of (4.6) can now be solved for the differentlysaturated control laws and a comparison between the results is displayed in Figure 4.2. Since twodifferential equations are required, which are both in R

3, (4.6) can be referred to as a 6th order system.

14


0 1 2 3 4 5 6 7 8

Time [s]

0

0.2

0.4

0.6

0.8

1

1.2

1.4

1.6

1.8

2

||e||

Saturated PD-actionSaturated P-action and D-action

0 1 2 3 4 5 6 7 8

Time [s]

0

0.5

1

1.5

2

2.5

||e||

Saturated PD-actionSaturated P-action and D-action

Figure 4.2: Comparison ||ρe|| (left) and ||νe|| (right) for differently saturated control laws

From this figure can be concluded that the settling time of the complete saturated PD-action is marginallyfaster than the settling time of the separately saturated control actions to zero. Besides this, the averageposition tracking error is lower for the complete saturated PD-action as well. Therefore, it is chosen tocontinue with the virtual control law of the saturated PD-action at once, described by:

u = − k∞k0 · (kρρe + kννe)√k2∞ + k20 · (kρρe + kννe)T (kρρe + kννe)

. (4.16)

The resulting control law can now be used to let RTr Rfe3 converge to the vector fre3 +mu by [3]:

f = ||fre3 +mu||. (4.17)

This follows from:

RTr R

f

me3 − fr

me3 = u

RTr Rfe3 = mu+ fre3.

(4.18)

In this way, the thrust can be controlled, which regulates the altitude as well. By now, it is also illustratedthat the closed-loop tracking error dynamics converge to zero when time progresses as presented in Figure4.2. Finally, this means that the control law u can assure that the thrust converges towards its referencethrust, when looking at (4.17).

4.3 Attitude controller

As mentioned before, the thrust can only work in e3-direction of the body-fixed frame. Since 0 < ε ≤ f ,the desired attitude can be achieved by defining the desired thrust direction as [3]:

fd =

⎡⎣fd1fd2fd3

⎤⎦ =

fre3 +mu

||fre3 +mu|| , (4.19)

with its corresponding desired rotation matrix, satisfying fd3 > 0:

Rd =

⎡⎢⎢⎢⎣1− f2

d1

1+fd3− fd1fd2

1+fd3fd1

− fd1fd21+fd3

1− f2d2

1+fd3fd2

−fd1 −fd2 fd3

⎤⎥⎥⎥⎦ . (4.20)

The corresponding desired angular velocity follows from:

S(ωd) = RTd Rd, (4.21)

15


which gives:

ωd =

⎡⎢⎢⎢⎣−fd2 + fd2fd3

1+fd3

fd1 − fd1fd31+fd3

fd2fd1−fd1fd21+fd3

⎤⎥⎥⎥⎦ . (4.22)

The derivative of the desired thrust direction fd is required to solve this equation, which is provided inAppendix A.7. This derivative also requires both the derivative of reference thrust fr and the controllaw u presented in Appendix A.5 and A.4 respectively. Subsequently, the derivative of the tracking errordynamics is required, which is given by (4.5). By using (4.17), (4.19) and (4.20), we can write [3]:

fre3 +mu = fRde3. (4.23)

In this way, the desired rotation matrix rotates the input thrust such that its corresponding vector isassigned the desired direction and magnitude. Now, using (4.18), the goal to determine τ , which makesRT

r Rfe3 converge to fre3 +mu can be replaced by the goal to determine τ , which makes RTr R converge

towards Rd [3]. By substitution of these terms in the error definitions as stated in [3], it follows that theattitude error of the drone in the body-fixed frame can be defined as:

Re = RTd (R

Tr R). (4.24)

The associated angular velocity tracking error now follows by differentiating this equation:

Re = ReS(ω −RTRrωr −ReTωd). (4.25)

The associated angular velocity tracking error is now given by:

ωe = ω −RTRrωr −ReTωd, (4.26)

with closed-loop dynamics:Re = ReS(ωe). (4.27)

The derivative of the angular velocity tracking error is then used to derive an expression for the inputtorque [3]:

τ = Jωe − S(Jω)ω − JRTe [S(ωd)R

Td ωr − ωd]−

JS(ωe)[ω − ωe] + JRTRrJ−1[S(Jωr)ωr + τr].

(4.28)

The input torque can be determined from this equation, when using the closed-loop dynamics:

Jωe = −Kωωe +KR

3∑i=1

ki(ei ×RTe ei), (4.29)

in which ei are the respective columns of the identity matrix I ∈ R3 and Kω, KR and ki represent

the gains in the attitude controller. Furthermore, (4.28) requires the derivative of the desired angularvelocity, which is given by:

ωd =

⎡⎢⎢⎢⎣

−fd2 + fd2fd3+fd2fd31+fd3

− fd2f2d3

(1+fd3)2

fd1 − fd1fd3+fd1fd31+fd3

+fd1f

2d3

(1+fd3)2

fd2fd1−fd1fd21+fd3

+ fd2fd1−fd1fd21+fd3

− (fd2fd1−fd1fd2)fd3(1+fd3)2

⎤⎥⎥⎥⎦ . (4.30)

This equation can be solved when deriving the second derivative of the desired thrust direction fd,the second derivative of the control law u, the second derivative of the reference thrust fr, the secondderivative of the position error ρe and the second derivative of the body-fixed velocity error νe, whichare all given in Appendix A. The closed-loop dynamics as presented in (4.27) and (4.29) are now solvedby applying the following initial conditions:

Re =

⎡⎣0.999 −0.003 −0.0160.004 0.997 0.0730.016 −0.073 0.997

⎤⎦ , ωe =

[−0.0171 −0.1089 −0.003]T, (4.31)

16


with control gains:k1 = 0.9, k2 = 1, k3 = 1.1,Kω = 30J,KR = 70J. (4.32)

The result is shown in Figure 4.3. This figure (left) illustrates the distance of the attitude tracking errorwith respect to the identity matrix over time, which can be denoted by using the natural Riemannianmetric as described in Section 2.1: d(Re, I) = || log(ReI

T )|| = || log(Re)||.

0 0.5 1 1.5 2 2.5 3

Time [s]

0

0.01

0.02

0.03

0.04

0.05

0.06

0.07

0.08

||log

m(R

e)|

|

0 0.5 1 1.5 2 2.5 3

Time [s]

0

0.05

0.1

0.15

0.2

0.25

0.3

||e||

Figure 4.3: Attitude tracking error || log(Re)|| (left) and tracking error angular velocity ||ωe|| (right)obtained from the closed-loop dynamics of the state feedback controller

From this figure can be concluded that the attitude control loop converges much faster to zero thanthe altitude control loop. Furthermore, it can be noted that because of this convergence to zero, theclosed-loop system satisfies (4.2) and (4.3).


The tracking error is defined in the body-fixed frame to assure that it is invariant for both translation androtation of the inertial frame. By linearizing the system around hover, it is derived that a PD-controllersatisfies the tracking problem. Afterwards, this controller is bounded by using a saturation function suchthat the thrust is maintained positive. The saturation of the PD-action at once turns out to be mostefficient in reducing the position error to zero. The closed-loop error dynamics of the state feedbackcontroller have been illustrated to be stable. The next step is to use these closed-loop error dynamicsto model the full drone dynamics as presented in (3.24) by using the input thrust f and input torqueτ as presented in respectively (4.17) and (4.28). The results of this are discussed in Section 6. Theshortcoming of this state feedback controller is that it uses ν, however, this is not a direct measurablestate. Therefore, there is proposed an output feedback controller to overcome this shortcoming in Section5.

17

5. OUTPUT FEEDBACK CONTROL

5 Output feedback control

This section discusses the controller design based on output feedback. As stated in Section 3, the body-fixed velocity ν is not directly measured and thus the error state νe as defined in (4.4) is unavailable. Forthis reason, the proposed controller design should be based on output feedback control [10]. First theproblem formulation for this controller is defined after which a double filtered saturated PD controlleris discussed to solve the altitude tracking problem. Lastly, a combined controller and estimator for theattitude tracking problem is elaborated.


Appropriate control laws have to be found for (ρr, Rr, νr, ωr, fr, τr) being a feasible trajectory [4] [5]

f = f(ρr, ωr, ρe, νe, z)

˙ρe = fρe(ωr, ρe, νe, z)

˙νe = fνe(ωr, ρe, νe, z)

z = fz(ρr, ρ, Rr, ωr, ρe)

τ = τ(R,Rr, ωr, τr, R, ω)

˙R = fR(R,ω,Rr, ωr, τr, R, ω, y0, y1, ..., yn)

˙ω = fω(R,ω,Rr, ωr, τr, R, ω, y0, y1, ..., yn)

(5.1)

such that for the resulting closed-loop system

limt→∞ ε(ρe(t), ρe(t), νe(t), νe(t), R(t), Re(t), ω(t), ωe(t)) = 0, (5.2)

with ε being the corresponding error measure as stated in Section 4.1, ρe is the estimated position trackingerror, νe is the estimated body-fixed velocity tracking error, R is the estimated attitude tracking errorand ω is the estimated angular velocity tracking error. Also, the condition limt→∞(ρe −→ ρe, νe −→ νe)should hold. Furthermore, yi = RT vi in which vi for i = 1, 2, ..., n denote vectors in the inertial frameas stated in [4]. When looking at (5.1) it can be concluded that indeed ν is not used here, only ρ, R andω, but these states can be obtained by direct measurements as concluded in Section 3. Besides this, τonly uses the estimated and filtered ω instead of the directly measured ω.

5.2 Altitude controller

The tracking error in the body-fixed frame is proposed similar to (4.4) as well as the closed-loop dynamicspresented in (4.6). However, the virtual control law of (4.11) is now replaced by a new control law aspresented in [5] and is given by:

u = −σ(kρρe + kν νe), (5.3)

where ρe and νe are both estimators for tracking errors ρe and νe respectively. By using the saturationfunction of (4.13), this can be rewritten to:

u = − k∞k0 · (kρρe + kν νe)√k2∞ + k20 · (kρρe + kν νe)T (kρρe + kν νe)

. (5.4)

Subsequently, the estimated values are generated from the following differential equations:

˙ρe = −S(ωr)ρe + νe + L1z (5.5a)

˙νe = −S(ωr)νe + u+ L2z (5.5b)

z = −S(ωr)z − (L1 + L3)z + (L1 + L3)ρe, (5.5c)

where:ρe = ρe − ρe, (5.6)

18


furthermore:νe = νe − νe (5.7)

z = z − ρe. (5.8)

The virtual control law as presented in (5.3) is now a double filtered saturated PD controller, because firstρe is filtered to obtain (5.5c) and subsequently z is filtered to obtain (5.5a) and (5.5b). Now, when theestimation of the tracking error exactly corresponds with the actual tracking error, then the contributionof z equals zero. Otherwise, this expression behaves as a first order low pass filter. Let’s assume again

that the drone starts at ρ(t0) =[0 0 0

]Tand the reference starts at ρr(t0) =

[1 0 1.5

]Tleading to

the initial conditions as stated in Table 5.1. The estimator tracking errors are chosen such that it differsfrom the actual tracking error, so it can be illustrated that these closed-loop dynamics are stable for anycondition.

Table 5.1: Initial conditions

State Variable Initial Condition

ρe [0.84 0 1.59]T

νe [-0.10 0 0.99]T

ρe [0 0 0]T

νe [0 0 0]T

z [0 0 0]T

For the gains it again suffices to take kρ > 0 and kν > 0. Furthermore, taking L1 > 0, L2 > 0 andL3 > 2L2/L1 as presented in [5] assures stability. These values are obtained in an iterative way bylooking at the lowest average tracking position error and fastest settling time to the steady-state error.The results are presented in Table 5.2.

Table 5.2: Controller gains

Variable Valuekρ 0.5kν 1k0 10L1 1L2 1L3 3

The differential equations of the closed-loop dynamics can now be solved by using (4.4) and (5.5). Theresulting tracking errors over time for the altitude controller are presented in Figure 5.1.

0 2 4 6 8 10 12 14 16 18 20

Time [s]

0

0.5

1

1.5

2

2.5

Bod

y-fix

ed p

ositi

on tr

acki

ng e

rror

0 2 4 6 8 10 12 14 16 18 20

Time [s]

0

0.5

1

1.5

Bod

y-fix

ed v

eloc

ity tr

acki

ng e

rror

Figure 5.1: Comparison between actual ||ρe|| and estimated ||ρe|| body-fixed tracking position error (left)and actual ||νe|| and estimated ||νe|| body-fixed velocity tracking error (right)

19


The closed-loop dynamics (4.4) and (5.5) are described by five differential equations in total, all in R3,

so this can be referred to as a 15th order system. From Figure 5.1 can be concluded that the trackingerror converges to zero as desired. Besides this, the estimated tracking errors converge towards theactual tracking errors satisfying (5.2). The Lyapunov function candidate for these closed-loop dynamicsis derived in [5] in which the closed-loop dynamics are proven UaGAS. The Lyapunov function candidatefor these closed-loop dynamics look as follows:

V (ρe, νe, ρe, νe, z) = Vσ(e) +1

2kρν

Te νe +

α

2(ρe − βνe)

T (ρe − βνe) +αγ

2νTe νe +

αδ

2zT z, (5.9)

where α, β, γ and δ are constant coefficients, which are given in Appendix B.1. Furthermore, Vσ(e) =12

∫ eT e

0s(x)dx, which is positive definite and radially unbounded as presented in [21]. Consequently, since

σ(e) = s(eT e) · e as explained in Section 4.2, it follows that (4.13) equals:

s(x) =k0k∞√k2∞ + k20x

. (5.10)

Substitution into the definition of Vσ(e) now gives:

Vσ(e) =1

2

∫ eT e

0

k0k∞√k2∞ + k20x

dx

= k0k∞√k2∞ + k20 · eT e− k0k∞

√k2∞

= k0k∞√k2∞ + k20 · (kpρe + kv νe)T (kpρe + kv νe)− k0k∞

√k2∞.

(5.11)

This obtained expression can be substituted into (5.9) resulting in the following result as shown in Figure5.2.

0 5 10 15 20 25

Time [s]

10-8

10-6

10-4

10-2

100

102

104

Lyap

unov

Fun

ctio

n [-

]

Figure 5.2: Altitude Lyapunov function candidate over time

The Lyapunov function decays almost linearly at logarithmic scale meaning that it decreases at exponen-tial rate, except at the beginning. The Lyapunov function is positive definite and also decreases towardszero implying that the result is UaGAS. To complete the illustration that the closed-loop dynamics of thealtitude output feedback controller are UaGAS, also the derivative of the Lyapunov function candidateis derived in [5]. This derivative and its behaviour over time is given in Appendix B.1. The Lyapunovfunction candidate evolves into a more straight line, when increasing the gains L1 and L2, because thenthe estimation tracking errors converge faster towards the actual tracking error, although the settlingtime and average position error remains more or less equal.

20


5.3 Attitude controller

Often a controller and observer are designed separately. However, there is a fundamental problemwith this, because if there exists a stability proof for the controller and observer independently, theirinterconnection does not necessarily imply stability for the closed-loop system in the nonlinear setting.Due to this reason, the proposed attitude controller incorporates the control and estimation problemssimultaneously as presented in [4]. For the output feedback controller it is still our goal to let RT

r Rconverge towards Rd [3] as derived in Section 4.3. By substitution of these terms in the error definitionsas stated in [4], it follows that the attitude error of the drone in the body-fixed frame can be defined as:

Re = Rd(RTr R)

T = RdRTRr. (5.12)

Now note that this error definition differs from the error definition as stated in (4.24). So, in [4] R isreplaced by RT

r R and Rr is replaced by Rd. This makes sense, since in [4] the goal is to let Rr convergeto R. The associated angular velocity tracking error now follows from differentiating the attitude errorRe, which gives:

Re = S(Rd(ωd − ω +RTRrωr))Re. (5.13)

A more sophisticated derivation is given in Appendix A.3. The associated angular tracking error velocityis now given by:

ωe = ωd − ω +RTRrωr. (5.14)

This holds when the closed-loop dynamics is defined as:

Re = S(Rdωe)Re. (5.15)

Note that these closed-loop dynamics is differently defined than presented in (4.25). This means that ωe

is rotated in comparison with the closed-loop dynamics obtained in the state feedback controller. Now,when comparing (5.14) to the error definitions stated in [4], it follows that ωr should be replaced by ωd

and ω by ω−RTRrωr. This information can be used to derive an expression of the closed loop dynamicsof the angular velocity tracking error [4]:

Jωe = S(J(ω −RTRrωr))ωe + S(Jω)ωd −Kω[ωe − ω]−3∑

i=1

kiS(RTd vi)R

T vi. (5.16)

These closed-loop dynamics incorporate the explicit complementary filter without bias correction. Thisfilter is used to filter out most of the noise of the measured gyroscope data. These closed-loop dynamicsintroduce the estimated values R and ω of respectively RT

r R and ω−RTRrωr. Therefore, we want RTr R

to converge to R as well, which defines a new attitude estimation error:

R = R(RTr R)

T = RRTRr. (5.17)

Subsequently, it follows that:ω = ω − ω +RTRrωr, (5.18)

The dynamics of the estimated values follow and can be written as:

˙R = RS(ω + δR −RTRrωr) (5.19a)

J ˙ω = S(J(ω −RTRrωr))(ω −RTRrωr) + τ + δω, (5.19b)

in which the innovation terms are defined as:

δR = −cR3∑

i=1

kiS(RT vi)(R

Td vi +RTRrvi) (5.20a)

δω = −cωJS(ωd)ωe − cωKωωe − Cωω, (5.20b)

where cR, ki, cω, Kω and Cω are gains. These innovation terms follow from the derivative of Lyapunovfunction candidate of the closed-loop dynamics and assure that the the Lyapunov function candidate isUaGAS. The closed-loop dynamics now follow by:

˙R = S(RδR)R (5.21a)

21


J ˙ω = δω. (5.21b)

Finally, the input torque is presented in [4] and by executing the required substitutions as stated aboveas well as replacing τr by τd, it follows that:

τ = τd + S(J [ωd − ω])ωd +Kω[ωd − ω] +3∑

i=1

kiS(RTd vi)R

T vi, (5.22)

in which the desired torque is given by:

τd = Jωd − S(Jωd)ωd. (5.23)

This means that, to solve (5.22), ωd and ωd need to be calculated by respectively (4.22) and (4.30), whichrequires the derivatives of the desired thrust as presented in Appendix A.7, derivatives of the doublefiltered control law as presented in Appendix A.4 and lastly the derivatives of the estimated trackingerror as presented in Appendix A.3. Furthermore, the estimation values follow by solving the differentialequations as stated in (5.19). Lastly, vi for i = 1, 2, ..., n denote vectors in the inertial frame. Thedirections v1 and v2 are normalized and made orthogonal by assigning the inertial frame such that itmatches the Earth’s magnetic field and gravitational field [4]. The third direction is chosen as a virtualmeasurement. The result is shown below:

v1 =[0 0 1

]T(5.24a)

v2 =[1 0 0

]T(5.24b)

v3 = v1 × v2 =[0 1 0

]T. (5.24c)

The attitude tracking error Re (5.12), attitude estimation error R (5.17), angular velocity error ωe (5.14)and estimated angular velocity error (5.18) are solved using the initial conditions as presented below:

ρ =[0 0 0

]T, ν =

[0 0 0

]T, R =

⎡⎣1 0 00 1 00 0 1

⎤⎦ , ω =

[0 0 0

]T(5.25a)

ρe =[0 0 0

]T, νe =

[0 0 0

]T, z =

[0 0 0

]T(5.25b)

R =

⎡⎣ 0.995 0 0.101

0 1 0−0.101 0 0.995

⎤⎦ , ω =

[−0.1 0.2 0.05]T. (5.25c)

Furthermore, the gains that are used are given by:

k1 = 2, k2 = 3, k3 = 4,Kω = Cω = I, cR = 1, cω = 2. (5.26)

The results are presented in Figure 5.3.

0 1 2 3 4 5 6 7 8 9 10

Time [s]

0.02

0.04

0.06

0.08

0.1

0.12

0.14

0.16

0.18

0.2

Atti

tude

trac

king

err

or

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5

Time [s]

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

Agu

lar

velo

city

trac

king

err

or

Figure 5.3: Actual || log(Re)|| and estimated || log(R)|| attitude tracking error over time (left) and actual||ωe|| and estimated ||ω|| angular velocity tracking error over time (right) of the output feedback controller

22


From this figure can be concluded that the attitude errors indeed converge to zero quite rapidly. ForRe and R this implies that it converges to the identity matrix, because || logm(I)|| = 0 and thus (5.2)is satisfied. The Lyapunov function candidate of these closed-loop dynamics is presented in [4] and isgiven by:

V =

3∑i=1

ki2(ReR

T vi − vi)T (ReR

T vi − vi) +1

2ωTe Jωe +

3∑i=1

ki2(Rvi − vi)

T (Rvi − vi) +1

2cωωTJω (5.27)

The result of this Lyapunov function is presented in Figure 5.4.

0 1 2 3 4 5 6 7 8 9 10

Time [s]

10-8

10-6

10-4

10-2

Lyap

unov

Fun

ctio

n [-

]

Figure 5.4: Attitude Lyapunov function candidate over time

From this figure can be concluded that the function is decaying and positive definite. Subsequently,by plotting the function on a logarithmic scale, it can clearly be concluded that the Lyapunov functiondecays (almost) exponentially down towards zero, except for the beginning, implying that it is UaGASas well. The derivative and its corresponding result is presented in Appendix B.2.


In this section the state feedback controller has been extended to an output feedback controller. Theclosed-loop dynamics are illustrated to be UaGAS for both the altitude controller and the attitudecontroller using the Lyapunov stability criterion. The virtual control law is now double filtered and isnot using the body-fixed velocity ν anymore. For the attitude controller it is also achieved to not directlyuse the body-fixed angular velocity ω, meaning that the measurements that follow from the gyroscopeare filtered as well. The obtained result can now be used for modeling of the input thrust and inputtorques and use these as controller inputs in the full drone dynamics as derived in Section 3 as well asin the Simulink model of the Parrot Mambo. The results of these are discussed in Section 6.

23

6. SIMULATIONS

6 Simulations

The current controller in the Simulink model of the Parrot Mambo as discussed in Section 1.2 is replacedby the proposed controllers as presented in Sections 4 and 5 in the flight controller of the Parrot MamboSimulink model. The simulated measured states and reference states are combined to find the appropriatecontrol inputs for the thrust f and torque τ . This section compares the results of the state feedbackcontroller obtained with the ode45 solver with the results obtained from the Simulink model of theParrot Mambo. Furthermore, this section presents the results of the output feedback controller obtainedwith the ode45 solver (continuous time-integrator) and provides insights about what the effect of theestimator is. Now, because sensors consist of a finite sample time, the measured states are discrete inthe Simulink model of the Parrot Mambo and therefore the differential equations of the estimators inthe output feedback controller can only be solved with discrete time-integration. Due to this, the ode45solver has been replaced by a Forward-Euler scheme (discrete time-integrator). The main findings of thisreplacement are discussed in this section as well.

6.1 State feedback control

The complete drone dynamics as presented in (3.24) are solved for the reference trajectory as presentedin (3.8) using the expression for the input thrust f as presented in (4.17) with the control law aspresented in (4.16) and finally the input torque τ as presented in (4.28). Furthermore, the completedrone dynamics are solved by using the initial conditions as presented in (5.25a). These initial conditionsdiffer considerably from the initial conditions of the reference states to show stability of the closed-loopsystem. The controller gains are equal to the gains presented in Section 4 except for k0, which is changedto: k0 = 1 to prevent instability of the system in the Simulink model of the Parrot Mambo. The obtainedclosed-loop model has been solved using the ode45 solver in Matlab and in the Simulink model of theParrot Mambo. Figure 6.1 shows the convergence of the position over time for the reference position, thesimulated position in the ode45 solver and the simulated position in the Simulink model of the ParrotMambo.

0 5 10 15 20 25 30

Time [s]

-1

-0.5

0

0.5

1

x [m

]

0 5 10 15 20 25 30

Time [s]

-1

-0.5

0

0.5

1

y [m

]

0 5 10 15 20 25 30

Time [s]

-3

-2

-1

0

z [m

]

Figure 6.1: Reference position (red line), actual position using the ode45 solver (green line) and theactual position using the Simulink model of the Parrot Mambo (blue line)

From this figure can be concluded that both the Simulink model and the ode45 solver show almostidentical behaviour and thus both models converge almost perfectly towards the reference position inz-direction within a reasonable amount of time. For the (x, y)-direction it can be noted that the resultsobtained from the ode45 solver also converge towards its reference position quite fast. However, theSimulink model of the Parrot Mambo starts to show two main differences here. Firstly, especially

24

6. SIMULATIONS

at higher altitudes, starting at approximately 2.3 m, the result looks unstable. This is a remarkableobservation since no external environmental disturbances are taken into account as for example wind.Therefore, this observation should be validated by performing a real-time experiment on the ParrotMambo to see if a similar result can be noted. A second difference between the ode45 solver andSimulink model of the Parrot Mambo is that the actual position obtained from the Simulink modelfails to converge exactly on the reference position, although in general the actual position approachesthe reference position. A possible cause of this difference is the way the velocity ρ is generated in theSimulink model. It uses the optical flow sensor to estimate the velocity with respect to the inertialframe, however, it is stated in the sensor block of the Simulink model, that this is not a very goodrepresentation of the actual optical flow data. Besides this, ν is constructed from ρ and R, because ν isnot directly measurable, which might also slightly affect the state attitude controller in particular. Whenthe reference trajectory is set to a lower speed, the Simulink model also shows a better convergence aspresented in Appendix C. Appendix C also shows the tracking position error over time, when the dronehovers to a prescribed position in space.

6.2 Output feedback control

The complete drone dynamics as presented in (3.24) are solved again for the reference trajectory aspresented in (3.8) using the expression for the input thrust f as presented in (4.17), however, now withthe control law as presented in (5.4) and finally the input torque τ as presented in (5.22). The obtaineddifferential equations are solved by applying the initial conditions as presented in (5.25). Furthermore,the controller gains as presented in Section 5 are used. The actual position that follows from the ode45solver is compared to the reference position as illustrated in Figure 6.2.

0 5 10 15 20 25 30

Time [s]

-1

-0.5

0

0.5

1

x [m

]

0 5 10 15 20 25 30

Time [s]

-1.5

-1

-0.5

0

0.5

1

y [m

]

0 5 10 15 20 25 30

Time [s]

-2.5

-2

-1.5

-1

-0.5

0

z [m

]

Figure 6.2: Reference position (red line) and the actual position using the ode45 solver (blue line)

From this figure can be concluded that the output feedback controller assures that the actual positionconverges to its set point quite rapidly as expected. The difference with the state feedback controlleris that the output feedback controller requires a discrete integrator to solve the differential equationsof the estimation errors (ρe, νe, z, R, ω). Therefore, the continuous integrators have been replaced fordiscrete integrators using the Forward-Euler scheme. Consequently, it is concluded that the differentialequations of the estimation errors using the discrete integrator with the Forward-Euler scheme can onlybe solved properly when the sample time satisfies: Ts ≤ 1e−5 seconds. If this condition is satisfied, thenthe discrete integrator gives the same solution as the continuous integrator. However, if this conditionis not satisfied, the solution goes to infinity. This condition, however, leads to an issue in the Simulinkmodel of the Parrot Mambo since the current minimum sample time in this model equals 5e−3 secondsand by changing this to 1e−5 seconds results in a simulation error. However, it should be possible to

25

6. SIMULATIONS

change this, although further research is required to overcome this simulation error.

Furthermore, it is interesting to see what the influence of the observer is on the tracking error andto validate if the observer shows the expected outcome. Figure 6.3 shows the behaviour of the Euler an-gles over time extracted from the respective rotation matrices as well as the influence of angular velocitiesin the body-fixed frame over time.

0 2 4 6 8 10 12 14 16 18 20Time [s]

0

2

4

Rol

l [de

g.]

0 2 4 6 8 10 12 14 16 18 20Time [s]

-5

0

5

Pitc

h [d

eg.]

0 2 4 6 8 10 12 14 16 18 20Time [s]

-0.1

0

0.1

0.2

0.3

Yaw

[deg

.]

0 2 4 6 8 10 12 14 16 18 20Time [s]

-0.1

0

0.1

p [r

ad/s

]

0 2 4 6 8 10 12 14 16 18 20Time [s]

-0.1

0

0.1

0.2

q [r

ad/s

]

0 2 4 6 8 10 12 14 16 18 20Time [s]

0

0.02

0.04

r [r

ad/s

]Figure 6.3: Left figure: RT

r R (red line) and R (blue line); Right figure: ω −RTRrωr (red line), ω (blueline)

The left figure in Figure 6.3 shows the expected outcome since R is an estimator for RTr R as defined

in (5.17) meaning that those solutions should show convergence as well as a development towards zero,which implies that it converges towards the identity matrix as desired as presented in (4.3). The anglesare rather small as expected as well since the trajectory does not require a very high roll, pitch and yawat this speed. The right figure in Figure 6.3 also shows the expected convergence since ω is an estimatorof ω − RTRrωr. Therefore, also the estimated angular velocity evaluates to zero over time as desired.So, from this can be concluded that the attitude estimator contributes to the reduction of the trackingerror to zero without using the direct noisy measurements of the gyroscope. To conclude, this controllerboth filters the measurement data as well as reduces the tracking error to zero.


Both the state feedback controller and the output feedback controller show the expected convergence toits set point in the ode45 solver, when applying different initial conditions of the actual states than theinitial conditions of the reference states. Both controllers have been implemented in the Simulink modelof the Parrot Mambo as well. The state feedback controller shows similar results for the z-position andalmost similar results for the (x, y)-position. The simulation in the Simulink model of the Parrot Mambogives somewhat unstable results for altitudes higher than approximately 2.3 m. It should be validatedin a real-time experiment with the Parrot Mambo whether this is actually the case or not. The outputfeedback controller requires discrete integration to solve the tracking problem. However, it is concludedthat the model with the discrete-time integration only gives the correct output, when the sample timesatisfies: Ts ≤ 1e−5 seconds. However, since the Simulink model of the Parrot Mambo is working at:Ts = 5e−3 seconds, the required sample time leads to a simulation error.

26

7. CONCLUSIONS AND RECOMMENDATIONS

7 Conclusions and recommendations

This final section discusses the main achievements of this research as well as the recommendations forpotential further research.

7.1 Conclusions

In this report it is achieved to implement a state feedback controller. In this state feedback controllerthe tracking error is defined in the body-fixed frame such that the tracking error becomes invariant fortransformations of the inertial frame. Furthermore, by introducing the rotation matrices satisfying theSO(3) group both singularities and ambiguities due to Euler angles and quaternions respectively areavoided. By linearizing the system around hover, the required control law is derived. This controller issubsequently saturated such that a positive thrust is maintained. It follows that the saturation functionof the PD-action at once gives the most effective error reduction over time based on average trackingposition error and settling time. The state feedback controller is solved using the ode45 solver, but is alsoimplemented in the Simulink model of the Parrot Mambo. The state feedback controller is illustrated tobe stable and showed convergence to the reference position, when applying different initial conditions ofthe actual states than the reference states. Also, a similar z-position and almost similar (x, y)-positionover time between the results of the ode45 solver and Simulink model of the Parrot Mambo is shown.Furthermore, the state feedback controller is extended to an output feedback controller, that does notuse the body-fixed velocity ν since this state is not available from direct measurements. Besides this, thebody-fixed position tracking error ρe and linear body-fixed velocity tracking error νe are double filteredin the control law used in the output feedback controller. The angular body-fixed velocity ω that followsfrom the noisy gyroscopic measurements is filtered as well. The output feedback controller is illustratedto be UaGAS on the basis of the Lyapunov function candidate. Although this controller is working inthe ode45 solver, it is not yet working in the model of the Parrot Mambo, because the required sampletime to solve this model is lower than the current minimum sample time of the Simulink model of theParrot Mambo. Finally, since the state feedback controller has been successfully implemented in theSimulink model of the Parrot Mambo, this indicates that the Simulink model of the Parrot Mambo canbe used for controller tuning, although some improvements are required. These are discussed in therecommendations.

7.2 Recommendation

The output feedback controller gives simulation problems in Simulink with the discrete integrator block,because of the required sample time. Further research can investigate how the sample time of the flightcontroller can be set equal to or smaller than 1e−5 seconds without changing the sample time elsewherein the model. In this research, the rotation matrix R has been expressed using Euler angles since theSimulink model of the Parrot Mambo is also working with Euler angles as output of the complementaryfilter. This did not give any singularity problems since the test trajectory only contained small anglesand the Euler angles follow from measurement data. Working in quaternions as presented in Section 2.2will only work if the complementary filter is redesigned such that it gives quaternions as output insteadof Euler angles. Since quaternions only consist of 4 entries instead of 9 entries, calculation time can bereduced. Furthermore, it will be interesting to do some real-time experiments on the drone to comparethe results of the simulations to the real-time data. Since the Simulink model is not incorporatingdifficult dynamics this might cause some deviations at especially higher speeds. These findings can beused to help to improve the model. It will also be interesting to see if the outcomes obtained for the statefeedback controller shows similar results in a real-time experiment since in the model it shows slightlydifferent behavior than expected at especially higher altitudes. The control law as presented in thisreport is based on a PD-controller, which can be extended to a PID-controller. The integral action cancontrol the altitude of the drone better in case the the quadcopter is assumed heavier than in reality, sowhen there is an excessive reference thrust generated [3].

27

REFERENCES

References

[1] L. Brown, “Drone Applications at Present and in the Future.” https://filmora.wondershare.

com/drones/drone-applications-and-uses-in-future.html, May 2019. Accessed on 2019-05-29.

[2] E. Lefeber, “Tracking control of a drone using the Aerospace Blockset,” 2019. Accessed on 2019-02-05.

[3] E. Lefeber and S.J.A.M. van den Eijnden and H. Nijmeijer, “Almost Global Tracking Control of aQuadrotor UAV on SE(3),” in 2017 IEEE 56th Annual Conference on Decision and Control (CDC),2017.

[4] E. Lefeber and M. Greiff and A. Robertsson, “Output Feedback on TSO(3) with 9-DOF InertialMeasurements,” in 58th IEEE Conference on Decision and Control, 2019. Submitted.

[5] E. Lefeber, “Double filtered saturated PD-controller,” 2019. Personal communication.

[6] MathWorks, “Aerospace Blockset.” https://nl.mathworks.com/products/aeroblks.html. Ac-cessed on 2019-02-05.

[7] MathWorks, “6DOF (Euler Angles).” https://nl.mathworks.com/help/aeroblks/

6dofeulerangles.html. Accessed on 2019-06-10.

[8] MathWorks, “Quadcopter Project.” https://nl.mathworks.com/help/aeroblks/

quadcopter-project.html. Accessed on 2019-02-05.

[9] M.W. Spong and S. Hutchinson and M. Vidyasagar, Robot modeling and control. Wiley, 2006.

[10] S.J.A.M. van den Eijnden, “Cascade Based Tracking Control of Quadrotors,” MSc Thesis, DC2017.012, Eindhoven University of Technology, Dynamics and Control Group, Department of Me-chanical Engineering, Eindhoven, The Netherlands, January 2017.

[11] E. Lefeber, “Modeling and Control of Quadrotors,” January 2018. Accessed on 2019-02-05.

[12] R.M. Murray and Z. Li and S.S. Sastry, “A Mathematical Introduction to Robotic Manipula-tion.” http://web.tuat.ac.jp/~gvlab/ronbun/ReadingGroupControl/mls-lyap.pdf. Accessedon 2019-05-03.

[13] Wikimedia Commons, “Illustration of Asymptotic Stability.” https://commons.wikimedia.org/

wiki/File:Illustration_of_Asymptotic_Stability.png. Accessed on 2019-01-06.

[14] G.F.Franklin and J.D. Powell and A. Emami-Naeini, Feedback Control of Dynamic Systems. Pearson,2015.

[15] E. Lefeber, “Definition UGAS in Lyapunov stability criterion.” Personal communication.

[16] Parrot Drones SAS, “Parrot.” https://www.parrot.com/global/drones/parrot-mambo-fpv#

parrot-mambo-fpv-details. Accessed on 2019-05-02.

[17] MathWorks, “Three-Axis Inertial Measurement Unit.” https://nl.mathworks.com/help/

aeroblks/threeaxisinertialmeasurementunit.html. Accessed on 2019-19-05.

[18] P. Van de Maele, “Reading a IMU Without Kalman: The Complementary Filter.” http://www.

pieter-jan.com/node/11, 2013. Accessed on 2019-06-05.

[19] MathWorks, “Kalman Filter.” https://nl.mathworks.com/help/control/ref/kalmanfilter.

html. Accessed on 2019-06-21.

[20] R. Padhi, “Conversion Between State Space and Transfer Function Representations in LinearSystems.” https://nptel.ac.in/courses/101108047/module4/Lecture%2010.pdf. Accessed on2019-03-31.

[21] E. Lefeber and M.F.A. van de Westerlo and H. Nijmeijer, “Almost global decentralised formationtracking for multiple distinct UAVs,” 2019. Accepted for NOLCOS.

28

Appendices

A Derivatives and derivations

This Appendix incorporates all required derivatives in order to model the complete closed-loop system.So, both the reference dynamics as well as the expressions used in the controller design. Consequently,all obtained equations are implemented in the Matlab model in order to model the expressions of theinput force f and input torques τ . This means that these expressions can be modeled without usingdiscrete derivative blocks, which is desired, since this prevents that large numerical mistakes occur aswell as time delays. These derivations belong to Sections 3, 4 and 5.

A.1 Reference dynamics

The first derivative of the individual terms of the reference rotation matrix are:⎡⎣r13r23r33

⎤⎦ =

CrAr −BrDr

A2r

. (A.1)

The second derivative of these terms in describe the reference rotation matrix are:⎡⎣r13r23r33

⎤⎦ =

ErAr −BrGr

A2r

+(−BrHr)A

2r − (ErAr −BrGr)(C

Tr Br +BT

r Cr)

A4r

, (A.2)

using the individual terms:

Ar =√(ge3 − ρr)T (ge3 − ρr)

Br = ge3 − ρr

Cr = −...ρr

Dr =1

2(BT

r Br)− 1

2 (CTr Br +BT

r Cr)

Er = −....ρr

Fr =1

2(BT

r Br)− 1

2

Gr = Fr(ETr Br +BT

r Er)

Hr = −1

4(BT

r Br)− 3

2 (CTr Br +BT

r Cr)2 + 2Fr(C

Tr Cr).

This section contributes to Section 3.4.

A.2 Tracking error

The derivation of the derivative of the tracking position error is given by:

ρe = RTr (ρr − ρ) +RT

r (ρr − ρ)

= −S(ωr)RTr (ρr − ρ) +RT

r (Rrνr −Rν)

= −S(ωr)ρe +RTr Rrνr −RT

r Rν

= −S(ωr)ρe + νr −RTr Rν

= −S(ωr)ρe + νe.

(A.3)

29

A. DERIVATIVES AND DERIVATIONS

The derivation of the derivative of the tracking body-fixed velocity error is given by:

νe = νr − RTr Rν −RT

r Rν −RTr Rν

= −S(ωr)νr + gRTr e3 −

frme3 + S(ωr)R

Tr Rν −RT

r RS(ω)ν −RTr R

(−S(ω)ν + gRT e3 − f

me3

)

= −S(ωr)νr − frme3 + S(ωr)R

Tr Rν +RT

r Rf

me3

= −S(ωr)(νr −RTr Rν) +RT

r Rf

me3 − fr

me3

= −S(ωr)νe +RTr R

f

me3 − fr

me3.

(A.4)

The second derivative of the tracking position error for state feedback is now described by:

ρe = −S(ωr)ρe − S(ωr)ρe + νe. (A.5)

The second derivative of the tracking velocity error for state feedback equals:

νe = −S(ωr)νe − S(ωr)νe +f

mRT

r Re3 −f

mS(ωr)R

Tr Re3 +

f

mRT

r RS(ω)e3 −frme3. (A.6)

This section contributes to Section 4.3. The derivative of the thrust f is given in Appendix A.6.

A.3 Estimated tracking error

The first derivative of the estimated position tracking error for output feedback is:

˙ρe = −S(ωr)ρe + νe + L1z. (A.7)

The first derivative of the estimated body-fixed velocity tracking error is given by:

˙νe = −S(ωr)νe +RTr R

f

me3 − fr

me3 + L2z. (A.8)

The second derivative of the estimated position tracking error for output feedback is:

¨ρe = −S(ωr)ρe − S(ωr) ˙ρe + ˙νe + L1z. (A.9)

The second derivative of the estimated body-fixed velocity tracking error for output feedback is:

¨νe = −S(ωr)νe − S(ωr) ˙νe +f

mRT

r Re3 −f

mS(ωr)R

Tr Re3 +

f

mRT

r RS(ω)e3 −frme3 + L2z. (A.10)

The derivation of the derivative of the attitude tracking error is given by:

Re = RdRTRr +RdR

TRr +RdRT Rr

= RdS(ωd)RTRr −RdS(ω)R

TRr +RdRTRrS(ωr)

= S(Rdωd)RdRTRr − S(Rdω)RdR

TRr + S(RdRTRrωr)RdR

TRr

= S(Rd(ωd − ω +RTRrωr))Re.

(A.11)

The closed-loop dynamics of the attitude estimation error is derived by:

˙R =˙RRTRr + RRTRr + RRT Rr

= RS(ω + δR −RTRrωr)RTRr − RS(ω)RTRr + RRTRrS(ωr)

= S(R(ω + δR −RTRrωr − ω +RTRrωr))RRTRr

= S(RδR)R.

(A.12)

This section contributes to Section 5.3. The derivative of the thrust f is given in Appendix A.6.

30


A.4 Control law

The first derivative term of the saturated control law is described by:

u =−k∞k0DuBu + k∞k0AuCu

B2u

. (A.13)

The second derivative of the saturated control law is given by:

u =−k∞k0FuBu + k∞k0AuGu

B2u

+k∞k0AuHuB

2u + k20(k∞k0DuBu − k∞k0AuCu)(D

TuAu +AT

uDu)

B4u

.

(A.14)The derivatives of the saturated control law incorporate the following sub-equations:

Au = kρρe + kννe

Bu =√k2∞ + k20(A

TuAu)

Cu =1

2E

− 12

u (k20(DTuAu +AT

uDu))

Du = kρρe + kν νe

Eu = k2∞ + k20(ATuAu)

Fu = kρρe + kν νe

Gu =1

2E

− 12

u (k20(FTu Au +AT

uFu))

Hu = −1

4E

− 32

u (k20(DTuAu +AT

uDu))2 +

1

2E

− 12

u (k20(DTuDu +DT

uDu)).

This section contributes to Sections 4.3 and 5.3.

Note: in case of the output feedback controller ρe should be replaced by ρe and νe should be replaced byνe.

A.5 Reference thrust

The first derivative of the reference thrust is described by:

fr = BfrCfr. (A.15)

The second derivative of the reference thrust now equals:

fr = −1

4m(AT

frAfr)− 3

2C2fr + 2Bfr(−...

ρrT · −...

ρr) +Bfr(−....ρr

T · (ge3 − ρr) + (ge3 − ρr)T · −....

ρr ). (A.16)

The derivatives of the reference thrust are described by using the following substitutions:

Afr = ge3 − ρr

Bfr =1

2m(AT

frAfr)− 1

2

Cfr = −...ρr

T ·Afr +ATfr · −...

ρr.


A.6 Thrust

The derivative of the thrust is generated by:

f =1

2A

− 12

f · (CTf Bf +BT

f Cf ), (A.17)

31


using the individual terms:

Af = BTf Bf

Bf = fre3 +mu

Cf = fre3 +mu.


A.7 Desired thrust

The derivative of the desired thrust is given by:

fd =CfdAfd −BfdDfd

A2fd

, (A.18)

and its second derivative by:

fd =EfdAfd −BfdGfd

A2fd

+(−BfdHfd)A

2fd − (CfdAfd −BfdDfd)(C

TfdBfd +BT

fdCfd)

A4fd

, (A.19)

in which the following individual terms are used:

Afd =√(fre3 +mu)T (fre3 +mu)

Bfd = fre3 +mu

Cfd = fre3 +mu

Dfd =1

2(BT

fdBfd)− 1

2 (CTfdBfd +BT

fdCfd)

Efd = fre3 +mu

Ffd =1

2(BT

fdBfd)− 1

2

Gfd = Ffd(ETfdBfd +BT

fdEfd)

Hfd = −1

4(BT

fdBfd)− 3

2 (CTfdBfd +BT

fdCfd)2 + 2Ffd(C

TfdCfd).

This section contributes to Sections 4.3 and 5.3. This completes all derivations required to solve theclosed-loop dynamics. These terms can be used in case further research is desired.

32

B. LYAPUNOV STABILITY CRITERION

B Lyapunov Stability Criterion

This Appendix elaborates on the Lyapunov function candidates as presented in Section 5. The Appendixshows how some of the coefficients can be determined as well the derivatives of the Lyapunov functionsin order to illustrate that it is negative definite as well.

B.1 Lyapunov function of the altitude output feedback controller

The expressions in this section follow from [5] and this section contributes to Section 5.2. It suffices totake:

β =2L1

3L2γ =

2L21

9L22

+1

L2δ =

1

3,

and:

α =max

((kρL1 + kνL2)

2, k2ρ)

kν min

(13L1,

2L1+L2L3−√

4L21+16L2

2−4L1L2L3+L22L

23

6L2

) .

The derivative of the Lyapunov function candidate of (5.9) is given by:

V = −kνσ(e)Tσ(e) + (kρL1 + kνL2)σ(e)T ρe − kρσ(e)

T νe+

(kρL1 + kνL2)σ(e)T z + α

(−1

3L1ρ

Te ρe −

2L1

3L2νTe νe −

4

3νTe z −

1

3L3z

T z

),

(B.1)

where e = kρρe + kν νe. The complete derivation of this function can be seen in [5]. Now as long asL3 >

2L2

L1and the requirements of the coefficients as stated above are satisfied, this function is negative

definite. The proof is shown in Figure B.1.

0 5 10 15 20 25

Time [s]

-104

-102

-100

-10-2

-10-4

-10-6

-10-8

Der

ivat

ive

Lyap

unov

Fun

ctio

n [-

]

Figure B.1: Derivative of the altitude Lyapunov function candidate over time

Since the result remains smaller than zero and is converging to zero almost exponentially and togetherwith the result obtained in Section 5.2 it is illustrated that this function is UaGAS.

B.2 Lyapunov function of the attitude output feedback controller

This part follows from [4] and contributes to Section 5.3. The derivative of the Lyapunov functioncandidate of the attitude output feedback controller as presented in (5.27) is given by:

V = −cR∣∣∣∣∣∣∣∣∣∣

3∑i=1

kiS(RT vi)(R

Td vi +RTRrvi)

∣∣∣∣∣∣∣∣∣∣2

2

− ωTe Kωωe − ωT Cω

cωω. (B.2)

33

B. LYAPUNOV STABILITY CRITERION

The function as presented in [4] is adjusted by replacing Rd for Rr and RTr R for R, because the error

definition in this report is differently defined as explained in Section 5.3. The result is given in FigureB.2.

0 1 2 3 4 5 6 7 8 9 10

Time [s]

-102

-100

-10-2

-10-4

-10-6

-10-8

-10-10

Der

ivat

ive

Lyap

unov

Fun

ctio

n [-

]

Figure B.2: Derivative of the attitude Lyapunov function candidate over time

From this figure can be concluded that the result is negative definite and the function converges towardszero at almost exponential rate completing the illustration that this controller is UaGAS.

34

C. SIMULATION IN THE SIMULINK MODEL OF THE PARROT MAMBO

C Simulation in the Simulink model of the Parrot Mambo

This Appendix shows the result for a simulation at low speed and hovering in the Simulink model of theParrot Mambo for the state feedback controller discussed in Section 4 and Section 6.1. Figure C.1 showsthe reference position over time and the actual position over time obtained from the simulation with theSimulink model of the Parrot Mambo.

0 10 20 30 40 50 60

Time [s]

-2

-1

0

1

x [m

]

0 10 20 30 40 50 60

Time [s]

-1

0

1

2

y [m

]

0 10 20 30 40 50 60

Time [s]

-4

-3

-2

-1

0

z [m

]

Figure C.1: Reference position (blue line) and the actual position using the Simulink model of the ParrotMambo (red line) for a simulation at low speed (period = 2π

0.3 ≈ 20.94 seconds)

From this figure can be concluded that the convergence in the (x, y)-direction is improved, when com-paring this result with the result as presented in Figure 6.1. However, the instability at higher altitudescan still clearly be noted. The result for hovering at a prescribed point in space is given in Figure C.2.

0 2 4 6 8 10 12 14 16 18 20

Time [s]

-1.5

-1

-0.5

0

0.5

ex [m

]

0 2 4 6 8 10 12 14 16 18 20

Time [s]

0

0.2

0.4

0.6

0.8

1

ey [m

]

0 2 4 6 8 10 12 14 16 18 20

Time [s]

-1.5

-1

-0.5

0

0.5

ez [m

]

Figure C.2: Tracking position error over time for hovering

From this figure can be concluded that the drone is able to move to its set point quite fast and thesteady state error is very near to zero and also remains zero, when the drone is at its set point. Theresult is obtained using the initial conditions and gains as described in Section 6.1 and with the reference

trajectory being: ρr =[−1.5 1 −1.5

]T.

35

tracking control of a drone using the aerospace blockset

Documents