Content uploaded by Umberto Saetti

Author content

All content in this area was uploaded by Umberto Saetti on May 21, 2018

Content may be subject to copyright.

Design of Dynamic Inversion and Explicit Model

Following Control Laws for Quadrotor Inner and

Outer Loops

Umberto Saetti

PhD Candidate

Joseph F. Horn

Professor

Department of Aerospace Engineering

The Pennsylvania State University

University Park, PA

Sagar Lakhmani

Graduate Student

Constantino Lagoa

Professor

Department of Electrical Engineering

The Pennsylvania State University

University Park, PA

Tom Berger

Aerospace Engineer

Aviation Development Directorate

U.S. Army AMRDEC

Moffett Field, CA

Abstract—A quadrotor was assembled with commercial off-

the-shelf (COTS) components readily available on the market as

a platform for future research at Penn State. As a ﬁrst step in this

research, a model of the quadrotor is identiﬁed from ﬂight data.

Given the largely decoupled dynamics at low speed, frequency

sweeps in different channels are performed separately on the roll,

pitch, yaw and heave axes. A frequency-domain approach is used

to perform system identiﬁcation. First, frequency responses of the

aircraft output are extracted from frequency-sweep ﬂight data.

Next, state-space models are ﬁt to the frequency response data.

Overall the identiﬁed model matched ﬂight data well in both the

frequency and time domain. Dynamic Inversion (DI) and Explicit

Model Following (EMF) with LQR disturbance rejection control

laws are developed for both an inner attitude loop and outer

velocity loop. The control laws were developed to meet similar

requirements, and have similar performance and robustness.

I. INTRODUCTION

Penn State has a long standing tradition of developing

ﬂight control systems for rotorcraft. In particular, Dynamic

Inversion (DI) and Explicit Model Following (EMF) control

laws have been major areas of research throughout the past

two decades and have been implemented in various helicopter

related simulation studies [1]–[7]. However, few studies ac-

tually have implemented DI on small scale quadrotors [8].

The U.S. Army Aviation Development Directorate (ADD) has

applied system identiﬁcation techniques, and both DI and EMF

to Micro Aerial Vehicles (MAVs) [9]–[12]. The purpose of

this paper is to merge the expertise on the rotorcraft system

identiﬁcation and ﬂight control design matured at Penn State

with the techniques developed at ADD, and to extend them

towards the application of the small scale quadrotors. For this

purpose, a quadrotor is assembled with commercial off-the-

shelf (COTS) components readily available on the market. The

quadrotor is meant to become a platform for future research

at Penn State and the base aircraft model of the Penn State’s

team for the AHS Micro-Aerial Vehicle Student Challenge.

This paper presents an overview of the hardware used on

the quadrotor, as well as the results of system identiﬁcation

0Distribution Statement A: Approved for public release; distribution is

unlimited.

performed on the quadrotor. Next, the synthesis of Dynamic

Inversion (DI) and Explicit Model Following (EMF) control

laws for both an inner attitude loop and outer velocity loop

is presented. The performance of the DI and EMF controllers

is compared in the time and frequency domain. In addition,

a robustness analysis of the controllers is performed using

an unscented transformation of the identiﬁed model parameter

uncertainties. Finally, conclusions are presented.

II. HA RDWARE OVERVI EW

The in-house quadrotor, shown in Fig. 1 is comprised of

four main components: on-board computer, ﬂight controller,

sensors, and electric motors. The Raspberry Pi is selected as an

on-board computer in this application. The on-board computer

acts as the brain of the Unmanned Aerial System (UAS) and is

responsible for processing the sensor data and communicating

with the ﬂight controller. The ﬂight controller used is the

PixHawk. It is responsible for running the built-in control

algorithm and operates on the MavLink message protocol.

The control algorithm allows to switch between manual ﬂight

using the remote controller or an automated ﬂight whenever

the pilot needs to. The ﬂight controller has a built-in inertial

measurement unit (IMU), a gyroscope, a GPS receiver and a

barometer used to determine the changes in altitude. A ground

station (GS) and transmitter are also incorporated into the

control architecture of the UAS. The GS is primarily used

for monitoring the telemetry data during ﬂight. A schematic

of the quadrotor is shown in Fig. 2.

III. SYS TE M IDE NT IFI CATI ON

System identiﬁcation is done in two steps using the

CIFER R

[13] software tool. The identiﬁcation procedure is

based on the use of frequency responses. First, frequency

responses of the aircraft output are extracted from frequency-

sweep ﬂight data. Next, state-space models are ﬁt to the fre-

quency response data. A frequency-domain modeling approach

is used, given that the dynamics of the quadrotor are unstable.

This would lead to divergence of the integration of the time-

domain equations and the consequent undermining of a time-

domain approach to system identiﬁcation. Frequency sweeps in

TABLE 1: Lateral dynamics identiﬁed parameters.

Parameter Value CR Bound [%] Insensitivity [%]

Yv-0.3022 [1/s] 6.107 1.825

Yp0 [ft/(rad s2)] - -

Lv-0.8287 [rad/ft] 5.943 1.580

Lp0 [1/s] - -

Yδlat 0.0565 [ft/(s2%)] 4.170 2.077

Lδlat 33.5146 [rad/(s2%)] 3.297 1.144

different channels are performed on each axis, separately. The

axes in consideration are roll, pitch, yaw and heave. Piloted

frequency sweeps are used rather than automated inputs due

to their excellent broadband excitation. Notice that the built-in

controller is activated during the system identiﬁcation process

to enhance the quadrotor stability and therefore to ease the

task of the pilot in performing the sweeps. A schematic of

the quadrotor control system is shown in Fig. 3. Given the

largely decoupled dynamics around each axis at low speed due

to the symmetry of the conﬁguration, system identiﬁcation in

performed separately around each axis.

A. Lateral Dynamics

The linear system describing the lateral dynamics is:

˙v

˙p

˙

φ

="YvYpg

LvLp0

0 1 0#"v

p

φ#+"Yδlat

Lδlat

0#δlat (1)

The parameters to be identiﬁed are the stability derivatives

Yv,Yp,Lv,Lp, and the control derivatives Yδlat ,Lδlat . The

outputs chosen to perform system identiﬁcation, given their

good coherence factor, are p,˙v, and ayreconstructed in the

time domain [13]. Figure 4 shows input and output time history

used for lateral dynamics system identiﬁcation. Figure 5 shows

the comparison between the approximated and real frequency

responses p

δlat ,˙v

δlat and ay

δlat . Figure 6 shows the comparison

between identiﬁed model and ﬂight test data time responses.

Table 1 shows the value of identiﬁed parameters, the Cramer-

Rao bounds, and the insensitivities.

B. Longitudinal Dynamics

The linear system describing the longitudinal dynamics is:

˙u

˙q

˙

θ

="XuXq−g

MuMq0

0 1 0 #"u

q

θ#+

Xδlong

Mδlong

0

δlong (2)

The parameters to be identiﬁed are the stability derivatives Xu,

Xq,Mu,Mq, and the control derivatives Xδlong ,Mδlong . The

outputs chosen for to perform system identiﬁcation, given their

good coherence factor, are q,˙u, and axreconstructed in the

time domain. Figure 7 shows input and output time history

used for longitudinal dynamics system identiﬁcation. Figure

8 shows the comparison between the approximated and real

frequency responses q

δlong ,˙u

δlong and ax

δlong . Figure 9 shows the

comparison between identiﬁed model and ﬂight test data time

responses. Table 2 shows the value of identiﬁed parameters,

the CR bounds, and the insensitivities.

TABLE 2: Longitudinal dynamics identiﬁed parameters.

Parameter Value CR Bound [%] Insensitivity [%]

Xu-0.2568 [1/s] 5.302 1.814

Xq0 [ft/(rad s2)] - -

Mu1.1257 [rad/ft] 5.618 1.306

Mq0 [1/s] - -

Xδlong 0.0355 [ft/(s2%)] 10.73 5.321

Mδlong 27.9188 [rad/(s2%)] 4.155 1.109

TABLE 3: Directional dynamics identiﬁed parameters.

Parameter Value CR Bound [%] Insensitivity [%]

Nr-0.5617 [1/s] 25.19 9.713

Nδped 6.0308 [rad/(s2%)] 3.877 1.848

C. Directional Dynamics

The linear system describing the directional dynamics is:

˙r

˙

ψ=Nr0

1 0r

ψ+Nδped

0δped (3)

The parameters to be identiﬁed are the stability derivative

Nrand the control derivative Nδped . The output chosen for

to perform system identiﬁcation is r. Figure 10 shows input

and output time history used for directional dynamics system

identiﬁcation. Fig. 11 shows the comparison between the

approximated and real frequency responses r

δped . Figure 12

shows the comparison between identiﬁed model and ﬂight test

data time responses. Table 3 shows the value of the identiﬁed

parameters, the CR bounds, and the insensitivities.

D. Vertical Dynamics

The linear system describing the vertical dynamics is:

˙w=Zww+Zδcoll δcoll (4)

The parameters to be identiﬁed are the stability derivative Zw

and the control derivative Zδcoll . The output chosen for to

perform system identiﬁcation is wand azreconstructed in the

time domain. Fig. 13 shows input and output time history used

for vertical dynamics system identiﬁcation. Fig. 14 shows the

comparison between the approximated and real frequency re-

sponses w

δcoll and az

δcoll . Fig. 15 shows the comparison between

identiﬁed model and ﬂight test data time responses. Table 4

shows the value of the identiﬁed parameters, the CR bounds,

and the insensitivities.

E. Complete State Space Model

The complete state-space representation of the quadrotor

rigid body dynamics can be obtained by assembling the linear

systems representing the dynamics around each axis. The

resulting 6degree-of-freedom (DoF) state space system has

the following input and output vectors, and system and control

matrices:

xT= [v p φ u q θ r ψ w](5)

TABLE 4: Vertical dynamics identiﬁed parameters.

Parameter Value CR Bound [%] Insensitivity [%]

Zw-0.1734 [1/s] 39.72 16.06

Zδcoll -49.0651 [ft/(s2%)] 2.647 1.312

uT= [δlat δlong δped δcoll](6)

A=

YvYpg0 0 0 0 0 0

LvLp0 0 0 0 0 0 0

0 1 0 0 0 0 0 0 0

0 0 0 XuXq−g0 0 0

0 0 0 MuMq0 0 0 0

0 0 0 0 1 0 0 0 0

0 0 0 0 0 0 Nr0 0

0 0 0 0 0 0 1 0 0

0 0 0 0 0 0 0 0 Zw

(7)

B=

Yδlat 0 0 0

Lδlat 0 0 0

0 0 0 0

0Xδlong 0 0

0Mδlong 0 0

0 0 0 0

0 0 Nδped 0

0 0 0 0

0 0 0 Zδcoll

(8)

The eigenvalues of the system are calculated and presented

in Fig. 16. The system appears to be stable around the yaw

and heave axes. Both the pitch and roll axes have a negative

eigenvalue on the real axis and a pair of eigenvalues in the right

half plane, representing the classical hovering cubic dynamics

present in all hovering vehicles [14]. This indicates an unstable

mode in both pitch and roll. The similarity of the eigenvalues

in pitch and roll are due to the symmetry of the conﬁguration

and is to be expected. The slight differences may be due to

the differences in the distribution of the electronics components

and sensors along the axes. Overall, these results agree with

the literature (e.g. [9], [12]).

IV. DYNA MI C INVERSION CONTRO LL ER

A. Inner Loop

A Dynamic Inversion (DI) control law is designed to

achieve stability, disturbance rejection, Attitude Command /

Attitude Hold (ACAH) response around the roll and pitch

axes, and Rate Command / Attitude Hold response around

the yaw and heave axes. A similar controller was previously

implemented on a B-430, as discussed in [7]. A general DI

scheme for a SISO system is shown in Fig. 17.

To model the DI controller, a modiﬁed state vector is

deﬁned:

ˆ

xT= [p φ q θ r w](9)

as well as modiﬁed state and control matrices:

ˆ

A=

Lp0 0 0 0 0

1 0 0 0 0 0

0 0 Mq0 0 0

0 0 1 0 0 0

0 0 0 0 Nr0

0 0 0 0 0 Zw

(10)

TABLE 5: DI and EMF inner loop command ﬁlters

properties.

Command ωn[rad/s]ζ

Roll Attitude 10 0.7

Pitch Attitude 10 0.7

Vertical Speed 1 -

Yaw Rate 2 -

ˆ

B=

Lδlat 0 0 0

0 0 0 0

0Mδlong 0 0

0 0 0 0

0 0 Nδped 0

0 0 0 Zδcoll

(11)

Given a commanded reference trajectory ycmd(t), the interest

lays in controlling the output y(t)so that it follows the

command. In this particular application the reference trajectory

and the output are given respectively by Eq. 12 and Eq. 13:

yT

cmd = [φcmd θcmd rcmd Vzcmd](12)

yT= [φ θ r Vz](13)

where φ,θ, and r,Vzare the roll attitude, pitch attitude, roll

rate, and vertical speed (positive up) respectively. The output

matrix C that identiﬁes the controlled states is given by:

C=C1

C2(14)

where

C1=010000

000100(15a)

C2=0001 0

0000−1(15b)

C1corresponds to the roll and pitch attitudes whereas C2is

related to the yaw rate and vertical speed. This partitioning is

due to the fact that the output equation has to be differentiated

two times to see the controls in the output equation while the

same procedure has to be done just once for rand Vzas given

in Eq. 16.

¨

φ

¨

θ

˙r

˙

Vz

=C1ˆ

A2x+C1ˆ

Aˆ

Bu

C2ˆ

Ax +C2ˆ

Bu (16)

Second order command ﬁlters are used for roll and pitch

attitudes, whereas ﬁrst-order ﬁlters are utilized for vertical

speed and yaw rate. Table 5 shows the values used for the

command ﬁlters.

PID/PI compensation is used to reject external disturbances

and to compensate for differences between the inversion model

described in the next section and the bare-airframe dynamics.

The dynamic inversion control law is thus given by:

u=C1ˆ

Aˆ

B

C2ˆ

B−1ν−C1ˆ

A2

C2ˆ

Ax(17)

TABLE 6: Outer loop command ﬁlter properties.

τ[s]

Vxcmd 1

Vycmd 1

where νis the pseudo-command vector and eis the error as

given respectively in Eq. 18 and Eq. 19.

νφ

νθ

νr

νVz

=

¨

φcmd

¨

θcmd

˙rcmd

˙

Vzcmd

+

eφ

eθ

er

eVz

KP+

˙eφ

˙eθ

0

0

KD+

Reφdt

Reθdt

Rerdt

ReVzdt

KI

(18)

e=ycmd −y;(19)

KP,KD, and KIare 4-by-4 diagonal matrices identifying

respectively the proportional, derivative and integral gain ma-

trices. The gain values are discussed in more detail in Section

IV-C. A DI inner loop block diagram is shown in Fig. 18.

B. Outer Loop

The outer loop controller tracks forward and lateral ground

velocities in the heading frame. The heading frame is a vehicle

carried frame where the x-axis is aligned with the current

aircraft heading, the z-axis is positive up in the inertial frame,

and the y-axis points to the right, forming a left-handed

orthogonal coordinate system. Eq. 20 shows the rotation from

body to the heading frame

Th/b ="cos θsin φsin θcos φsin θ

0 cos φ−sin φ

sin θ−sin φcos θ−cos φcos θ#(20)

so that the velocities in the heading frame are given by:

"Vx

Vy

Vz#=Th/b "u

v

w#(21)

The command ﬁlter for both lateral and forward velocities, Vx

and Vy, is ﬁrst order. The time constants are given in Table 6.

The ﬁltered velocities are subtracted from their measurements

to ﬁnd the error, which goes through a PI controller. The feed-

forward signal is subsequently added, leading to the desired

commands:

νVx

νVy=˙

Vxcmd

˙

Vycmd +eVx

eVyKP+ReVxdt

ReVydtKI(22)

Lateral velocity is controlled by acting on the roll angle.

Consider the lateral velocity equation of motion

˙

Vy=YvVy+gφcmd (23)

DI is applied, leading to the following inverse law:

φcmd =1

g(νVy−YvVy)(24)

Similarly, longitudinal velocity is controlled by acting on the

pitch angle. Consider the longitudinal velocity equation of

motion ˙

Vx=XuVx−gθcmd (25)

DI is applied, leading to the following inverse law:

θcmd =−1

g(νVx−XuVx)(26)

A DI outer loop block diagram is shown in Fig. 19.

C. Error Dynamics

Feedback compensation is needed both in the inner and

the outer loop for two main reasons. Firstly, we use approx-

imations and the inversion is not exact. Secondly, there are

external disturbances to the system. Similarly as in [15] it can

be demonstrated that for a Dynamic Inversion controller

e(n)=ν−y(n)

cmd (27)

where nis the number of times the output equation has to

be derived in order for the controls to appear explicitly in the

output equation. For the states for which this has to be done

twice, a PID control strategy applied to the pseudo-command

vector is given by:

ν= ¨ycmd(t) + KD˙e(t) + KPe(t) + KIZt

0

e(τ)dτ (28)

Substituting 28 into 27, we obtain the closed-loop error dy-

namics

¨e(t) + KD˙e(t) + KPe(t) + KIZt

0

e(τ)dτ = 0 (29)

The gains can be chosen so that the frequencies of the error

dynamics are of the same order as the command ﬁlters,

ensuring that the bandwidth of the response to disturbances is

comparable to the one of an input given by a pilot. By taking

the Laplace transform and therefore switching to frequency

domain the error dynamics becomes:

e(s)s2+KDs+sKP+1

sKI= 0 (30)

or equivalently

e(s)s3+KDs2+KP+KI= 0 (31)

In order to obtain gains that would guarantee a desired re-

sponse, the error dynamics can be set equal to the third-order

system given in Eq. 32:

(s2+ 2ζωns+ωn2)(s+p)=0 (32)

Developing the product between the polynomials leads to

s3+ (p+ 2ζωn)s2+ (2ζ ωnp+ωn2)s+ωn2p= 0 (33)

Setting the coefﬁcients of the polynomial equal to the the gains

of Eq. 31, results in:

KD= 2ζωn+p(34a)

KP= 2ζωnp+ωn2(34b)

KI=ωn2p(34c)

Speciﬁcally, this approach is used for φand θin the inner

loop.

Similarly, for those states for which PI compensation is

applied, the pseudo-command vector is given by:

ν= ˙ycmd (t) + KPe(t) + KIZt

0

e(τ)dτ (35)

TABLE 7: DI inner loop disturbance rejection frequencies,

damping ratios, and integrator poles.

ωn[rad/s]ζp

φcmd 10 0.7 2

θcmd 10 0.7 2

rcmd 1 0.7 -

Vzcmd 1 0.7 -

TABLE 8: DI outer loop disturbance rejection frequencies

and damping ratios.

ωn[rad/s]ζ

Vxcmd 1 0.7

Vycmd 1 0.7

which leads to the following closed-loop error dynamics:

˙e(t) + KPe(t) + KIZt

0

e(τ)dτ = 0 (36)

and, therefore, to:

s+KP+1

sKI= 0 (37)

The resulting gains are:

KP= 2ζωn(38a)

KI=ωn2(38b)

This type of compensation is applied to Vxand Vyin the outer

loop, and to Vzand rin the inner loop. Table 7 and Table 8

show the natural frequencies, damping ratios, time constants,

and the integrator pole values, respectively, for the inner and

the outer loop. Note that the integrator pole pis usually chosen

to be one-ﬁfth of the natural frequency, corresponding to about

one-ﬁfth of the loop crossover frequency [16]. Further, the

outer loop error dynamics needs to be at a signiﬁcantly lower

frequency than the equivalent inner loop dynamics (e.g. for

the longitudinal velocity ωn= 1 rad/s, which is one order

of magnitude less than ωn= 10 rad/s of the pitch attitude

error dynamics). The numerical value of the resulting gains

are shown in Table 9 and Table 10.

V. EXPLICIT MO DE L FOLLOWING CONTRO LL ER

A. Inner Loop

An Explicit Model Following (EMF) control law is de-

signed to achieve stability, disturbance rejection, ACAH re-

TABLE 9: DI inner loop compensation gains.

KDKPKI

φcmd 16 128 200

θcmd 16 128 200

rcmd - 1.4 1

Vzcmd - 0.3 0.5

TABLE 10: Outer loop compensation gains.

KPKI

Vxcmd 1.4 1

Vycmd 1.4 1

sponse around the roll and pitch axes, and RCAH response

around the yaw and heave axes. A general EMF scheme for

a SISO system is shown in Fig. 20. The present study does

not assume perfect inverse plant dynamics, in fact the inverse

plant is based on a set of decoupled ﬁrst- and second-order

linear models, where the stability and control derivatives are

taken from the identiﬁed model. Speciﬁcally, the inverse plant

on the roll, pitch, and yaw axes is approximated respectively

by the following inverse transfer functions:

δlat

φ=s(s−Lp)

Lδlat

(39a)

δlong

θ=s(s−Mq)

Mδlong

(39b)

δped

r=s−Nr

Nδped

(39c)

δcoll

Vz

=s−Zw

Zδcoll

(39d)

The equivalent time delay τf, used to delay the ideal response

such that it can be physically followed by the controlled

system, is estimated to be τf= 0.056 s, representative of

the brushless motors time constant plus some additional delay

given by the sensors. LQR is used for stability and disturbance

rejection. The states and dynamics of the system used to obtain

the LQR gains are:

xT=p φ Rφ q θ Rθ r Rr Vzz(40)

A=

LvLp0 0 0 0 0 0 0 0

0 1 0 0 0 0 0 0 0 0

0 0 1 0 0 0 0 0 0 0

0 0 0 MuMq0 0 0 0 0

0 0 0 0 1 0 0 0 0 0

0 0 0 0 0 1 0 0 0 0

0 0 0 0 0 0 Nr000

0 0 0 0 0 0 1 0 0 0

0 0 0 0 0 0 0 0 Zw0

0 0 0 0 0 0 0 0 1 0

(41)

B=

Lδlat 0 0 0

0 0 0 0

0 0 0 0

0Mδlong 0 0

0 0 0 0

0 0 0 0

0 0 Nδped 0

0 0 0 0

0 0 0 Zδcoll

0 0 0 0

(42)

An EMF inner loop block diagram is shown in Fig. 21.

B. Outer Loop

The EMF outer loop controller tracks forward and lateral

velocities the heading frame. Command ﬁlter are ﬁrst order and

share the same time constants as the DI command ﬁlters. The

present study does not assume perfect inverse plant dynamics,

in fact the inverse plant is based on a set of decoupled 1st order

linear models, where the stability and control derivatives are

pulled from the identiﬁed model. Speciﬁcally, the inverse plant

for forward and lateral speed is approximated respectively by

the following inverse transfer functions:

θcmd

Vx

=s−Xu

−g(43a)

φcmd

Vy

=s−Yv

g(43b)

LQR is used independently on each axis for disturbance

rejection. The system used to synthesize the gains for the

forward speed is:

˙

Vx

˙x=Xu0

1 0Vx

x+−g

0θcmd (44)

where xis the longitudinal position in the heading frame. The

system used to synthesize the gains for the lateral speed is:

˙

Vy

˙y=Yv0

1 0Vy

y+g

0φcmd (45)

where yis the lateral position in the heading frame. An EMF

outer loop block diagram is shown in Fig. 22.

C. Disturbance Rejection

Since the loop on the right hand side of Fig. 20 is

independent from the the inverse plant, the LQR gains for

both inner and outer loop can be determined from the models

discussed in Section V-A and V-B. The cost function being

minimized is:

J=Zt

0xTQx +uTRudτ (46)

The weight matrices are designed according to [17] and are of

the form:

Q=diag hα2

1

(x1)2

max ·· · α2

n

(xl)2

max i(47)

R=ρ diag hβ2

1

(u1)2

max ·· · β2

m

(um)2

max i(48)

where (xi)2

max and (uj)2

max are the largest desired response

and input for that particular component of the output/input,

Pl

i=1 α2

i= 1 and Pm

j=1 β2

i= 1 are used to add an

additional relative weighting on the various components of the

output/control input, and ρis used as the relative weighting

between the control and state penalties. Table 11 and Table 12

show the penalties on the states and controls used in the inner

loop LQR design, where ρ= 2. Table 13 and Table 14 show

the penalties on the states and controls used in the outer loop

LQR design, where ρ= 0.25 for both lateral and longitudinal

axes.

VI. RE SU LTS

A. Time Domain

Batch simulations are run to test and compare the EMF

and DI controllers. The gains are adjusted such that the two

control strategies give similar performances. The results shown

in this section refer to a lateral speed command doublet.

Figure 23 shows the heading frame velocities, whereas the

Fig. 24 shows the body frame velocities. Figure 25 and

Fig. 26 show respectively the angular rates and the attitude

TABLE 11: EMF inner loop LQR state penalties.

(xi)max α2

i

p π/180 1/10

φ π/180 1/10

Rφ0.05π/180 1/10

q π/180 1/10

θ π/180 1/10

Rθ0.05π/180 1/10

r π/180 1/10

Rr π/180 1/10

Vz1 1/10

z1 1/10

TABLE 12: EMF inner loop LQR control penalties.

(ui)max β2

i

δlat 0.01 1/4

δlong 0.01 1/4

δped 0.01 1/4

δcoll 0.01 1/4

response. Figure 27 shows the controls time history and Fig.

28 shows the actuator perturbations from trim. EMF and DI

show an outstanding agreement both in the on-axis and off-axis

response.

B. Frequency Domain

Figure 29 shows the inner attitude loop broken-loop re-

sponse (loop broken at the input to the mixer) for both the DI

and EMF controllers. Table 15 shows the inner attitude loop

crossover frequencies and gain and phase margins of both the

DI and EMF controllers. Both loops have similar crossover

frequencies ωcφby design. The inner loops were designed

with a reduced phase margin requirement of PM >35 deg. The

EMF design meets this requirement with PM = 40.3 deg, while

the DI design nearly meets the requirement with PM = 34.4

deg. Both designs meet the nominal gain margin requirement

of GM >6dB.

Figure 30 shows the attitude disturbance response for

both the DI and EMF controllers. Table 16 lists the attitude

disturbance rejection bandwidth (DRB) and peak (DRP) [18]

values for the two controllers. The DI controller has a higher

DRB and DRP than the EMF controller. This demonstrates

Bode’s Integral Theorem between the two controllers, where

a reduction in magnitude in one frequency range of the Sensi-

tivity function comes at the cost of an increase in magnitude

at another frequency range.

TABLE 13: EMF outer loop LQR state penalties.

(xi)max α2

i

Vx1 1/2

x1 1/2

Vy1 1/2

y1 1/2

TABLE 14: EMF outer loop LQR control penalties.

(ui)max β2

i

θcmd π/180 1

φcmd π/180 1

TABLE 15: Lateral axis broken loop gain and phase margins.

ωc[rad/sec] GM [dB] PM [deg]

BLφDI 17.8 9.22 34.3

BLφEM F 20.4 7.8 40.3

BLvDI 1.59 18.4 66.8

BLvEM F 1.6 21.5 67.4

TABLE 16: Lateral axis disturbance response gain and phase

margins.

DRB [rad/s] DRP [dB]

φ0/φdDI 5.12 4.45

φ0/φdEM F 3.69 2.83

v0/vdDI 1.77 1.89

v0/vdEM F 1.24 1.59

Figure 31 shows the closed-loop attitude response of the

two controllers as compared to their common command model.

Both designs have excellent model tracking performance

across a wide frequency range. In addition, the closed-loop

attitude response of both controllers is lower-order, validating

the simple inverse model designs used for the outer loop of

each design.

Figure 32 shows the outer velocity loop broken-loop re-

sponse (loop broken at the input to the inner loop) for both the

DI and EMF controllers, and Table 15 lists the outer velocity

loop crossover frequencies and gain and phase margins. Note

that by design, the outer-loop crossover frequencies are about

ωcv≈ωcφ/10, to have good frequency separation between the

inner and outer loops. Furthermore, the outer loops of both DI

and EMF designs have sufﬁcient gain and phase margins.

Figure 33 shows the velocity disturbance response for both

the DI and EMF controllers. Table 16 lists the velocity DRB

and DRP values for the two controllers. Both controllers have

similar values for velocity DRB and DRP. Finally, Fig. 34

shows the closed-loop velocity response to velocity command.

Both controllers have similar closed-loop responses and excel-

lent model following performance.

These results show that the DI and EMF controllers,

designed with different methods but to meet the same spec-

iﬁcations, produce very similar results. Both designs will be

taken to ﬂight to assess their performance.

C. Robustness Analysis

Stability and performance robustness analyses of the DI

and EMF controllers were performed by perturbing the iden-

tiﬁed stability and control derivatives of the bare-airframe and

evaluating stability margins, disturbance rejection bandwidth

(DRB), and disturbance rejection peak (DRP). Essentially, it

is desired to evaluate the statistics (mean and variance) of

the controller performance based on the known statistics of

the identiﬁed parameters. To do this robustness analysis in an

efﬁcient manner, an unscented transformation [19] was used,

as explained for the roll axis below.

To perform the unscented transformation, the covariance

matrix Pfor the vector of identiﬁed parameters:

xT= [YvLvYδlat Lδlat ](49)

is needed. The covariance matrix was taken directly from the

CIFER R

identiﬁcation results as:

P=H−1(50)

where His the parameter identiﬁcation Hessian matrix [13].

The next step in performing the unscented transformation is

determining the sigma points x(i), that will be used as the

perturbed parameters:

x(i)=¯

x+˜

x(i), i = 1, ..., 2n(51)

where:

¯

xis a vector of the nominal parameter values,

n= 4 is the number of identiﬁed parameters being

perturbed,

˜

x(i)= +(√nP)T

ifor i= 1, ..., n

˜

x(i+n)=−(√nP)T

ifor i= 1, ..., n, and

(√nP)T

icorresponds to the ith row of the matrix (√nP).

The perturbed values of the stability and control derivatives,

given by the sigma points x(i), are then used to evaluate

the stability margins and disturbance rejection characteristics

(DRB and DRP) of the controllers. This becomes the nonlinear

transformation of the sigma points x(i), with the outputs being

gain and phase margins, DRB, and DRP. Note that through

this unscented transformation, only 2nevaluations are done to

assess the robustness of the controller through the statistics of

the stability margins, DRB, and DRP. Once the 2nvalues of

gain and phase margins, DRB, and DRP are computed, they

are used to draw 1σ,2σ, and 3σconﬁdence ellipsoids on the

speciﬁcations, as shown in Figs. 35 through 38.

Figure 35 shows the inner attitude loop stability margin ro-

bustness for both the EMF and DI controllers. The conﬁdence

ellipsoids are ﬂat (appearing as lines), with gain and phase

margin varying together with perturbations to the stability and

control derivatives. This is because the inner loop crossover

frequency is around ωc= 20 rad/s, while the perturbed

stability and control derivatives affect the rigid body dynamics

of the quadrotor (namely the lateral hovering cubic) at a much

lower frequency. At high frequency (around ω= 20 rad/s),

these effects are seen as a pure gain shift. Furthermore, since

the DI controller inner attitude broken-loop response BLφ

phase curve is ﬂat around crossover frequency (Fig. 29), a gain

shift in the broken-loop response has a larger effect on the gain

margin than the phase margin, as seen by the orientation of

the DI conﬁdence ellipsoids in Fig. 35. The DI conﬁdence

ellipsoids lie along the Level 1/Level 2 boundary, staying

within around 2 deg of the boundary. For the EMF controller,

a gains in the gain of the broken-loop response affects both

the gain and the phase margin. However, as seen in Fig. 35,

the conﬁdence ellipsoids for the EMF controller are wholy

contained within the Level 1 region.

Figure 36 shows the outer velocity loop stability margin

robustness for both the EMF and DI controllers. For the

outer loop, with its lower crossover frequency, there is a

larger spread in the conﬁdence ellipsoids as compared to the

inner loop. However, the 3σconﬁdence ellipsoids for both

DI and EMF controllers are within the Level 1 region of

the speciﬁcation. In addition, the EMF controller conﬁdence

ellipsoids are more compact than the DI controller conﬁdence

ellipsoids, suggesting higher robustness to variations in the

quadrotor stability and control derivatives.

Finally, the disturbance rejection performance robustness

of the EMF and DI controllers is shown in Fig. 37 for the

attitude loop and Fig. 38 for the velocity loop. There is a

similar level of disturbance rejection performance robustness

in the inner and outer loops for the both controllers, as seen

by the similarly sized conﬁdence ellipsoids.

VII. CONCLUSION

A quadrotor was assembled with commercial off-the-

shelf (COTS) components readily available on the market.

A frequency-domain approach was used to perform system

identiﬁcation using two steps. First, frequency responses of

the aircraft output were extracted from frequency-sweep ﬂight

data. Second, state-space models were ﬁt to the frequency

response data. Next, DI and EMF controllers were designed

for both inner attitude loop and outer velocity loop for the

quadrotor. DI feedback gains were tuned based on desired error

dynamics whereas EMF feedback gains were obtained using

LQR. Based on this work, the following conclusions can be

reached:

1) Given the largely decoupled dynamics at low speed,

system identiﬁcation was performed separately on

each axis. Overall the identiﬁed model matched ﬂight

data closely in both the frequency and time domain.

This indicates that the assumptions of linearity and

decoupling of each axis are justiﬁed in hover condi-

tions for this type of conﬁguration.

2) The identiﬁed model contained a classical hovering

cubic in both pitch and roll axes. The similarity of the

eigenvalues in pitch and roll are due to the symmetry

of the conﬁguration and is to be expected.

3) DI and EMF performances were compared both in

the time and frequency domains. Both time and

frequency responses show an outstanding match be-

tween the two control laws, which were tuned to

meet the same requirements. Overall, the gain and

phase margins, disturbance rejection performance,

and model following performance for the inner and

outer loop of both DI and EMF are satisfactory and

match closely to each other.

4) An unscented transformation was used to evaluate

stability and performance robustness statistics from

the identiﬁed bare-airframe parameters uncertainty

statistics. The results showed similar outer-loop sta-

bility robustness and inner- and outer-loop perfor-

mance robustness for both the DI and EMF con-

trollers. Higher inner-loop stability robustness was

seen for the EMF controller.

REFERENCES

[1] D.B. Caudle, DAMAGE MITIGATION FOR ROTORCRAFT THROUGH

LOAD ALLEVIATING CONTROL, M.S. Thesis, The Pennsylvania State

University, December 2014.

[2] B.R. Geiger, Flight Control Optimization on a Fully Compounded Heli-

copter with Redundant Control Effectors, M.S. Thesis, The PEnnsylvania

State University, May 2005.

[3] N. Sahani, and J.F. Horn, Adaptive Model Inversion Control of a

Helicopter with Structural Load Limiting, AIAA Journal of Guidance,

Control, and Dynamics, Vol. 29,(2), March-April 2006.

[4] J.F. Horn, and N. Sahani, Detection and Avoidance of Main Rotor Hub

Moment Limits on Rotorcraft, AIAA Journal of Aircraft, Vol. 41,(2),

March-April 2004, pp. 372-379.

[5] J.M. Spires, and J.F Horn, Multi-Input Multi-Output Model-Following

Control Design Methods for Rotorcraft, AHS 71st Annual Forum, Vir-

ginia Beach, VA, May 2015.

[6] U. Saetti, and J.F. Horn, Use of Harmonic Decomposition Models in

Rotorcraft Flight Control Design with Alleviation of Vibratory Loads,

European Rotorcraft Forum, Milan, Italy, September 2017.

[7] U. Saetti, ROTORCRAFT SIMULATIONS WITH COUPLED FLIGHT

DYNAMICS, FREE WAKE, AND ACOUSTICS, M.S. Thesis, The Penn-

sylvania State University, July 2016.

[8] V.S.R. Pappu, Y. Liu, J.F. Horn, and J. Cooper Wind Gust Estimation on

a Small VTOL UAV,7th AHS Technical Meeting on VTOL Unmanned

Aircraft Systems and Autonomy, At Mesa, Arizona, USA, 2017.

[9] W. Wei, M.B. Tischler, and K.B. Cohen, System Identiﬁcation and

Controller Optimization of a Quadrotor Unmanned Aerial Vehicle in

Hover, Journal of the American Helicopter Society, Volume 62, Number

4, October 2017, pp. 1-9(9).

[10] O. Juhasz, M.J.S. Lopez, M.G. Berrios, T. Berger, and M.B. Tischler

Turbulence Modeling of a Small Quadrotor UAS Using System Identiﬁ-

cation from Flight Data, AHS Technical Meeting on VTOL Unmanned

Aircraft Systems, Mesa, AZ, January 24-26, 2017.

[11] M.G. Berrios, T. Berger, M.B. Tischler, O. Juhasz, and F.C. Sanders

Hover Flight Control Design for UAS using Performance-based Distur-

bance Rejection Requirements, AHS 73rd Annual Forum, Fort Worth,

TX, May 9-11 2017.

[12] K.K. Cheung, J.A. Wagster, M.B. Tischler, C.M. Ivler, M.G. Berrios,

T. Berger, O. Juhasz, E.L. Tobias, C.L. Goerzen, P.S. Barone,

F.C. Sanders, M.J.S. Lopez, and R.M. Lehman, An Overview of the U.S.

Army Aviation Development Directorate Quadrotor Guidance, Naviga-

tion, and Control Project, AHS 73rd Annual Forum, Fort Worth, TX,

May 9-11 2017.

[13] M.B. Tischler, and R.K. Remple, Aircraft and Rotorcraft System Identi-

ﬁcation: Engineering Methods and Flight Test Exemples, Second Edition,

Mark B. Tischler and Robert K. Remple, AIAA Education Series, AIAA,

Reston, VA, 2012.

[14] D.T. McRuer, D. Graham, and I. Ashkenas, Aircraft Dynamics and

Automatic Control, Princeton University Press Princeton, NJ, 1972.

[15] B.L. Stevens, and F.L. Lewis, Aircraft Control and Simulation: Dy-

namics, Controls Design, and Autonomous Systems, Third Edition, John

Wiley and Sons, Inc., New York, October 2015.

[16] M.B. Tischler, T. Berger, C.M. Ivler, M.H. Mansur, K.K. Cheung, and

J.Y. Soong, Practical Methods for Aircraft and Rotorcraft Flight Control

Design: An Optimization-Based Approach, AIAA Education Series, VA,

April 2017.

[17] A.E. Bryson, and Y.C. Ho, Applied Optimal Control, 43rd Hemisphere

Publishing Corporation, Washington, D.C., 1975.

[18] T. Berger, C.M. Ivler, M.G. Berrios, M.B. Tischler, and D.G. Miller,

Disturbance Rejection Handling Qualities Criteria for Rotorcraft, AHS

72nd Annual Forum, West Palm Beach, FL, May 2016.

[19] S.J. Julier, J.K. Uhlmann, and H.F. Durrant-Whyte, A new approach for

ﬁltering nonlinear systems, IEEE American Control Conference, 1995,

pp. 1628–1632.

Fig. 1: Quadrotor UAS.

Fig. 2: Quadrotor schematic.

Fig. 3: Quadrotor control system schematic.

Fig. 4: Input and output time history for lateral dynamics system identiﬁcation.

Fig. 5: Real and approximated roll axis frequency response.

Fig. 6: Comparison between nonlinear and linear roll axis time response.

Fig. 7: Input and output time history for longitudinal dynamics system identiﬁcation.

Fig. 8: Real and approximated pitch axis frequency response.

Fig. 9: Comparison between nonlinear and linear pitch axis time response.

Fig. 10: Input and output time history for directional dynamics system identiﬁcation.

Fig. 11: Real and approximated yaw axis frequency response.

Fig. 12: Comparison between nonlinear and linear yaw axis time response.

Fig. 13: Input and output time history for heave dynamics system identiﬁcation.

Fig. 14: Real and approximated heave axis frequency response.

Fig. 15: Comparison between nonlinear and linear heave axis time response.

Fig. 16: Eigenvalues of the identiﬁed model.

Fig. 17: DI block diagram.

Fig. 18: DI inner loop.

Fig. 19: DI outer loop.

Fig. 20: EMF block diagram.

Fig. 21: EMF inner loop.

Fig. 22: EMF outer loop.

Fig. 23: Heading frame velocities time response.

Fig. 24: Body frame velocities time response.

Fig. 25: Angular rates time response.

Fig. 26: Euler angles time response.

Fig. 27: Controls time response.

Fig. 28: Actuators time response.

Fig. 29: Inner-loop broken-loop response (loop broke at input

to mixer).

Fig. 30: Attitude disturbance response.

Fig. 31: Attitude command closed-loop response.

Fig. 32: Outer-loop broken-loop response (loop broke at

input to inner-loop).

Fig. 33: Velocity disturbance response.

Fig. 34: Velocity command closed-loop response.

Fig. 35: Inner attitude loop stability robustness.

Fig. 36: Outer velocity loop stability robustness.

Fig. 37: Inner attitude loop performance robustness.

Fig. 38: Outer velocity loop performance robustness.