Application of Linear Quadratic Regulation and Tracking for Optimal

Control of Quadrotor UAV

Akila Ganlath1

Abstract In this paper I will present the development of World Frame Wf : an inertial frame of reference
a quadrotor UAV controller using techniques from Linear Body Frame Bf : a non-inertial frame of reference fixed
Quadratic Optimal Control. This controller ensure stability and to the quadrotor
provides trajectory tracking capabilities to the quadrotor. First,
the continuous-time non-linear dynamical quadrotor system is Linear and angular positions and subsequent derivatives
described and formulated as a linear system. Then this was can be defined in each of these frames, and transformed
discretized as a closed loop system and finite-horizon Linear between the two by the rotation matrix R:
Quadratic Regulation is applied in simulation. The Q and
R penalty matrices were iteratively tuned to obtain desired C C C S S S C C S C + S S
performance. Characteristics of the pair (A, B) were verified R = S C SS S + C C S S C C S
and the steady-state Kalman Gain Kss was found. This gains S C S C C
non-optimal performance was demonstrated through simulation
with initial perturbations from the operating point. An output which transforms body frame vectors into the world frame.
matrix C was designed to provide access to the translational The inverse transform is given by R1 = RT due to the
states and their velocities, reformulating the optimal control form of R.
problem as an LQ Tracking problem. This new controller In the world frame, the position, attitude, and configuration
was tuned and tested against static and dynamic trajectories.
space are defined as:
Finally, the LQ Tracking controller was successfully tested
against a stochastic process noise. x  

= y , = , q=

I. INTRODUCTION
z
Unmanned robotics systems have become ubiquitous in where , , are the Euler angles roll, pitch, and yaw. The
a growing partition of technical fields and commercial relationship between Wf angular velocity and Bf angular
interests. In particular, unmanned aerial vehicles (UAVs) velocity is given by the matrix W :
have become pervasive in telecommunications, agriculture,
surveying, and have numerous military applications. To en- 1 S T C T
sure the protection of people and property, these unmanned = 0 C S
systems must be resilient to faults and malicious intent. 0 S S/ C /C
The Cyber-Physical Systems & Distributed Computing where is the Bf angular velocities. The lift forces of the
Lab is studying the trade-offs between security and control four propellers result in the following Bf thrust and torque:
performance on autonomous robots. In order to conduct

0
these experiments a robust trajectory controller is necessary TBf = 0 , Bf = ,
to obtain experimental data using Erle-copter and Parrot T
AR.Drone 2.0 quadrotor platforms. Past work in trajectory
The model was simplified with the following assumptions:
control of the lab quadrotors involved using Proportional
The inertia tensor can be taken to be a diagonal matrix
Integral Derivative (PID) control, but these proved difficult
to tune and seldom guaranteed stability. if the body-frame aligns with its principle axes.
The rigid-body does not deform under loading, thus all
Theory and techniques pertaining to this course will be
used to design and implement this new controller, making energy is transmitted through it.
The mass and material characteristics of the rigid-body
particular use of Linear Quadratic (LQ) techniques. This
will allow formal guarantees of stability and other system are time-invariant and symmetric relative to the body
characteristics. frame.
Now the dynamics can be written using the Newton-Euler
II. APPROACH equations, with the sum of forces in the Wf given by:

A. Mathematical Model 0 Ax 0 0 x
m = 0 + RTBf 0 Ay 0 y
As per the approach of [1] the quadrotor was taken to be
g 0 0 Az z
a rigid-body, with finite-size and distinct orientation. This
necessitates the identification of two frames of reference: where the Ai elements are the first-order aerodynamic drag
in the i direction. The sum of moments in the Wf is:
1 Akila Ganlath is a student in the Bourns College, Dept. of Mechanical
Engineering, University of California Riverside aganl001@ucr.edu = J 1 (Bf C(, ))
where J and C(, ) are the Jacobian transforming the inertia
from body to inertia and the Coriolis matrices, respectively.
Full description of these can be seen in [1].
B. Linearization and Optimal Control
T

Now if we define the state vector x = q q then the
Newton-Euler equations can be written as the composite non-
linear equation.
x = f (x, u)
. where u is a vector consisting of the thrust force in the
Bfz and the torques composing Bf This system can be
linearized about a suitable equilibrium position (x, u) using
the Jacobian:
x = fx (x, u))x + fx (x, u))u gain:
.
Which is a continuous time linear system. Proceeding, the
x and u can be solved numerically using an equilibrium
point defined by the quadrotor hover state and suitable
constants for the physical characteristics. This results in the
linear system
x = Ac x + Bc u (1)
Given the form of x and u, the matrices Ac and Bc are of
size M12x12 and M12x4 , respectively.Its discretized form is
xi+1 = Axi + Bu (2)
This system is reachable and its closed loop form will
be suitable for use with discrete Linear Quadratic (LQ)
regulation and derivative techniques. This controller will
drive all the states to zero, which is not appropriate for the
quadrotor and will only be in the preliminary.
The Linear Quadratic performance index(cost function) for
this scenario is of form:
N 1
X 1 T
J= (xi Qxi + uTi T ui ) + Pi+1
T
(xi+1 + Axi + Bui )
i=0
2 The system was discretized with sampling time dT s =
(3)
with penalties on the state Q and penalties on the input
R. The minimization of the index and underlying solution
provide the pair (x, u) which are the optimum state and
inputs, provided that the relation is given by the control law:
ui = Ki xi (4)
where Ki is the Kalman Gain at step i. These are obtained
from (3) by solving the corresponding Discrete Ricatti Equa-
tion (DRE). Given that (A, B) is reachable and (A, Q)
observable, there exists Kalman gain Kss that is suboptimal
but stabilizing that can be used in lieu of the optimal but
computationally demanding Kalman Gains Matrix {Ki }.This
solution is obtained from the time-limiting case of the DRE,
the Algebraic Ricatti Equation (ARE), and is of form:
Kss = (B T SB + R)1 (B T SA) (5)
Since trajectory tracking is the intended goal, it is more
appropriate to reformulate the performance index to min-
imize the tracking error between the states and reference.
The states desired to be tracked can be extracted using the
output system
yi = Cxi (6)
with appropriate C. Then we can define the tracking error
as
ei = Cxi yr,i (7)
Taking this into account, the Performance Index for Tracking
is:
N
X 1
1 T
J= (ei Qei + uTi T ui ) + Pi+1
T
(xi+1 + Axi + Bui ).
2
i=0
(8)
This results in a new control law with an additional Kalman
ui = Ki xi + KIv vi+1 (9)
This feedback law will provide an optimal state transition in
(1) that minimizes the tracking error.
III. RESULTS & DISCUSSION
The preceding methodology was applied using model
parameters given in [1] within Matlab. The entire code can
be found at the end of this document.First, the linear system
(1) was simulated with the following initial conditions that
perturb the drone from its marginally stable operating point:
(x, y, z)0 = (10, 10, 10) : linear positions
(x, y, z)0 = (0, 0, 0) : linear velocities
(, , )0 = (0.5, 0.5, 0.05) : roll, pitch, yaw
(, , )0 = (0, 0, 0) : angular velocities
The results of the simulation can be seen in Fig. 1. The
reachability of the pair (A, B) was computed by checking
the rank of the reachability matrix [B AB A2 B...]. The
matrix was full column rank and of dimension A, meeting
the reachability condition. This allowed us to compute the
steady state Kalman gain for the LQ controller.
0.1s and initial values for the matrices Q and R were
established. They were taken to be diagonal matrices with
initial penalties for each corresponding state or input being
1 2
taken to be |r| where r is the range of each state or input, if
it exists. The Discrete Ricatti equation corresponding to (3)
was solved recursively, starting with the final kernel sequence
being 100Q. The resulting Kalman Gains provided control
inputs as per (4) which were propagated through the discrete
time simulation with the same initial conditions listed above.
After iterative tuning of Q and R, the resulting performance
can be seen in Fig.2 and applied to the continuous time
system in Fig.3. The steady state Kalman Gain was found
by recursively solving the DRE until the kernel sequence
reached a convergence criterion was reached. This occurred
at the 190th recursion. This kernel sequence was used to
solve for the Ks s which was applied to the continuous-
time simulation with same initial conditions as previous
simulations. The results, as seen in Fig.4, demonstrate the
suboptimal but stabilizing hallmarks of the steady state
Kalman Gain.
 Fig. 1. Open Loop quadrotor response to initial conditions
Fig. 2. Discrete time system under LQR Optimal Control input
To track an arbitrary reference trajectory (8) was imple-
mented, with choice of C that exposed the linear position
and linear velocity states. The first reference was taken
to be a static waypoint, yr = [15, 15, 15, 0, 0, 0]T . The
state penalties within the Q matrix was tuned iteratively to
eliminate overshoot while increasing the convergence speed.
The resulting performance can be seen in Fig.5.
The next step was to add a time-varying component to the
trajectory. A sinusoid of period 7 seconds was chosen for
the x-component and z-components. The choice of these was
arbitrary and not designed to match the constraints imparted
by the physics of the quadrotor specifically. Nonetheless
Fig. 3. Continuous time system under LQR Optimal Control input
Fig. 4. Discrete time system under nonoptimal LQR, with Kss inputs
Fig. 5. Tracking static waypoint yr = [15, 15, 15, 0, 0, 0]T
the controller produced inputs which maintained a stable
sinusoidal trajectory in these dimensions, as seen in Fig.6,
with tracking error in Fig.7.
The final aspect to be studied is the robustness of LQ
controllers to stochastic disturbances. While it is desirable
to include both the process and sensor noise in this study,
only the former was included. The disturbance enters the
system as an additional term in (2) that varies for each time
step i and effects each of the state evolutions. The result of
this noise can be seen in Fig.8 and the tracking error in Fig.9.
It should be noted that the increasing yaw angle in Fig.8 and
its error in Fig.9 are due to it not being a controlled state
due to the choices made in designing the C matrix.
IV. CONCLUSIONS & FUTURE WORK
This project sought to develop a robust controller for
quadrotor trajectory control. By applying techniques from
the course to a detailed dynamical model, a series of LQ
controllers were developed and tuned to obtain the desired
performance. Particularly, an LQ tracker was developed
that remained stable under process noise. The results of
this project will be extended by including sensor noise.
Then, a Kalman filter will be implemented to produce a
best estimates of pertinent states.Furthermore, it is desirable
for the the queing of subsequent optimal control to be
balanced between offline and online computations. This will
be addressed by developing a hierarchical receding horizon
 Fig. 6. Sinusoidal Trajectory Tracking

Fig. 7. Sinusoidal Tracking Error

trajectory controller that will allow for more adaptation to a

dynamic environment.
R EFERENCES
[1] Teppo Lukkonen, Modeling and Control of Quadcopter, Aalto Univer-
sity School of Science Publications, Espoo, Finland: 2011, pp. 3-10.
Fig. 9. Noisy Sinusoidal Tracking Error

Fig. 8. Tracking Sinusoidal Trajectory with Process Noise