11/21/24, 4:19 PM Project2.

ipynb

rmcsqrd / [Link]
Last active 4 years ago • Report abuse

Code Revisions 5

[Link]

[Link] 1/26
11/21/24, 4:19 PM [Link]

ASEN 5014 - Project 2

Rio McMahon, Fall 2020

For my system I selected the "Multirotor Aircraft" system. This report is organized
into several sections found in the table of contents below. Code and figures are
interspersed in-line within the report.

Given the heavy emphasis on programming in this project, I decided to forgo the
conventional report format and instead opted to use a Jupyter notebook. I am
submitting this as a PDF to conform with submittal guidelines although I encourage
graders to view this at the following link where it is rendered in its native HTML
(some odd formatting occurred during the conversion from HTML to PDF). The
content is the same.

The notebook can be viewed at: [Link]

Table of Contents
Part 1: Plant Observability
Part 2: Luenberger Observer Placement
Part 3: Closed Loop Observer/Controller Simulation - Reference Input
Part 4: Closed Loop Observer/Controller Simulation - Non-Zero IC
Part 5: LQR Controller Design

Start off by defining system dynamics etc:

In [21]:
# import some stuff
import os
import numpy as np
import control
import time
import projectutils.plot_utils2 as pltutls
from [Link] import expm
from [Link] import loadmat
[Link]()

# define matrices
A = [Link](
[[0, 1, 0, 0, 0, 0],
[0, -0.0104, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0],
[0, 0, 0, -0.0104, 0, 0],
[0, 0, 0, 0, 0, 1],
[0, 0, 0, 0, 0, -0.0208]]
)

B = [Link](
[[0, 0, 0, 0],
[-0.04167, 0, 0.04167, 0],
[0, 0, 0, 0],
[0, -0.04167, 0, 0.04167],
[0, 0, 0, 0],
[0 4 0 4 0 4 0 4]]
[Link] 2/26
11/21/24, 4:19 PM

)
[0.4, 0.4, 0.4, 0.4]]

C = [Link](
[[1, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 1, 0]]

D = [Link](

)
[[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0]]

Part 1: Plant Observability

Unobservable Subspace

Q =

Q ∈ R

⎢⎥
⎡

⎣
CA
[Link]

Begin finding the observable subspace by determining the observability matrix Q

such that:

CA
C

n−1 ⎦

nl×n
⋮

It is useful to note the following facts regarding matrix dimensions:

x ∈ R
n

C ∈ R
l
,y ∈ R ,A ∈ R

⟹
l×n
⎤

A completely observable state space is one such that any internal state x(t
be distinguished from 0 by the examination of y(t) on a finite interval [t
n×n

Note that the relationship between the plant output y(t) is directly related to
internal plant state x(t) and is agnostic of any inputs u(t) into the plant;

previous logic. Also recall that D is a zero matrix. Thus:

Although e
Theorem:
A(t−t 0 )
x(t) = e

y(t) = Cx(t)

y(t) = Ce
A(t−t 0 )

A(t−t 0 )
x(t 0 )

x(t 0 )
0,

observability is a measure of the internal state x(t) and the output y(t) - any input
u(t) are realized in x(t). Thus, when evaluating observability it is convenient to
take u(t) = 0
–
; this does not have an implication on the observed state by the
0

t]
)

is an infinite series, it can be represented using Cayley-Hamilton

[
n 1
[Link]
]
can

3/26
11/21/24, 4:19 PM

In [22]:
If rank(Q)
mapping of x(t

[ 0.000e+00 0.000e+00
[ 0.000e+00 1.000e+00
[ 0.000e+00 0.000e+00
[ 0.000e+00 0.000e+00
[ 0.000e+00 -1.040e-02
[ 0.000e+00 0.000e+00
[ 0.000e+00 0.000e+00
[ 0.000e+00 1.082e-04
[ 0.000e+00 0.000e+00
[ 0.000e+00 0.000e+00
[ 0.000e+00 -1.125e-06
[ 0.000e+00 0.000e+00
[ 0.000e+00 0.000e+00
[ 0.000e+00 1.170e-08
[ 0.000e+00 0.000e+00
[ 0.000e+00 0.000e+00

Rank(Q) = 6
= n
n−1

y(t) = C[ ∑ α k (t − t 0 )A ]x(t 0 )

k=0

y(t) = [α 0 (t − t 0 )I l×l

0
⟹
…

RN (Q) = 0

) → y(t)
k

0.000e+00 0.000e+00
0.000e+00 0.000e+00
0.000e+00 1.000e+00
0.000e+00 0.000e+00
0.000e+00 0.000e+00
0.000e+00 -1.040e-02
0.000e+00 0.000e+00
0.000e+00 0.000e+00
0.000e+00 1.082e-04
0.000e+00 0.000e+00
0.000e+00 0.000e+00
0.000e+00 -1.125e-06
0.000e+00 0.000e+00
0.000e+00 0.000e+00
0.000e+00 1.170e-08
0.000e+00 0.000e+00

= n (implies full rank)

As seen in the above code snippet, rank(Q)

[Link]

α n−1 (t − t 0 )I l×l ]

= n ⟹
⎢⎥
⎡

⎣
CA

In this expanded form, recognize the observability matrix Q defined previously. The
rank of this matrix has implications on the overall mapping from x(t
C

n−1 ⎦

definition for observability is fulfilled and the state space is fully observable.
If rank(Q)

x(t 0 )
≠ n ⟹ RN (Q) ≠ 0

there exists multiple states x(t 0)

⎤

0.000e+00 0.000e+00]
1.000e+00 0.000e+00]
0.000e+00 0.000e+00]
0.000e+00 0.000e+00]
0.000e+00 1.000e+00]
0.000e+00 0.000e+00]
0.000e+00 0.000e+00]
0)

0.000e+00 -2.080e-02]
0.000e+00 0.000e+00]
0.000e+00 0.000e+00]
0.000e+00 4.326e-04]
0.000e+00 0.000e+00]
0.000e+00 0.000e+00]
0.000e+00 -8.999e-06]
0.000e+00 0.000e+00]
0.000e+00 0.000e+00]
0.000e+00 1.872e-07]]

RN (Q) = 0

state space is fully observable by the previous logic. Further, because RN (Q) is
trivial, this means that any x(t
l
0)

[Link]
∈ R
n
is mappable from a corresponding
x(t 0 )

→ y(t)

. If RN (Q) is trivial, this means that the

is 1-1 and solutions are unique. This means that the

. If RN (Q) is non-trivial, this means that

that map into y(t) = 0
–

In the following code, the observability matrix Q is constructed and its rank is
evaluated using the rank() function.

Qobs = [Link](([Link][0]*[Link][0], [Link][1]))

for i in range(0, [Link][0]):
n = i*[Link][0]
Qobs[n:n+[Link][0], :] = [Link]([Link].matrix_power(A,i))

with [Link](precision=3):
print("Observability Matrix Q:")
print(Qobs, "\n")
print("Rank(Q) = ", [Link].matrix_rank(Qobs), " = n (implies full r

Observability Matrix Q:
[[ 1.000e+00 0.000e+00
[ 0.000e+00 0.000e+00
0.000e+00 0.000e+00
1.000e+00 0.000e+00
. This means that
cannot be distinguished from 0 and thus the state space is not fully
observable.
–

0.000e+00 0.000e+00]
:

and thus the

4/26
11/21/24, 4:19 PM [Link]
y(t) ∈ R
l
(1-1 mapping) further implying that the un-observable subspace is trivial.

Output Energy and Corresponding Implications on

Degree of Observability
The output energy of the system can be found by looking at the L2-norm of the
output y(t). This is conveniently related to the Observability gramian such that:

t 1 −t 0
T
2 T A (τ −t 0 ) T A(τ −t 0 )
||y|| = x (t 0 )[ ∫ e C Ce dτ ]x(t 0 )
T
0

Where the observability gramian G is in the brackets. G can be found by

0 0

integrating numerically on some interval [0, t 1

− t0 ] . In the following snippet of
code, G is found for several time intervals by integrating numerically using the
0

trapezoidal rule.

In [23]:
# define a range of t1-t0, then find the observability gramian for each of
delt = 1
tmax = 400
trange = [Link](delt,tmax, delt)

# load cached data if available

if [Link]('projectutils/data/[Link]'):
Gorange = [Link]([Link]()+"/projectutils/data/[Link]")

# else integrate numerically

else:
if [Link]('projectutils/data') == False:
[Link]([Link]()+'/projectutils/data')
Gorange = [Link](([Link][0], [Link][1], [Link][1]))

# integrate according to this lambda function

integrand = lambda tau: expm(A.T*tau).dot(C.T).dot(C).dot(expm(A*tau))

# loop through the overall t range

start = [Link]()
for cnt, t in enumerate(trange):

# initialize integration stuff

delt_int = 0.05
tau_range = [Link](0, t, delt_int)
integration_array = [Link]((tau_range.shape[0], [Link][1]

# integrate between 0 and t

for intcnt, tint in enumerate(tau_range):
integration_array[intcnt, :, :] = integrand(tint)

# compute integral using trapezoid rule and store result

Gorange[cnt, :, :] = [Link](integration_array, axis=0, dx=delt_i

[Link]([Link]()+"/projectutils/data/Gorange", Gorange)

Recall that in Project 1 it was shown that A is fully diagonalizable such that
A = T ΛT
−1
which implies that the matrix of eigenvectors T is invertible. This can
be used as a change of basis between the state space representation and the
modal space of A. This is convenient when perturbing the different plant modes
[Link] 5/26
11/21/24, 4:19 PM [Link]
oda space o . s s co e e t e pe tu b g t e d e e t p a t odes
and is thus used here. Recall that the change of basis takes the following form:

x(t) = T α(0)

Where x(t) is in the original coordinates of the state space and α(t) is the
equivalent representation using the eigenvectors as a basis set. This means that the
output energy can be represented in the following form:

t 1 −t 0
T T
2 A (τ −t 0 ) T A(τ −t 0 )
||y|| = (T α(t 0 )) [∫ e C Ce dτ ](T α(t 0 ))
T
0

Each of the modal spaces is given a unit perturbation and the output energy is
determined based on a range of [0, t 1
− t0 ] intervals. The unit perturbation takes
the form of the identity matrix which means that the perturbation condition for
each modal space is just the eigenvector of A. The following snippet of code takes
the range of G Observability gramians that were previously calculated for different
0

time intervals and solves for the L2-norm of the output using the above
equation/ICs.

In [24]:
eigvals, eigvects = [Link](A)

# each column is a unit perturbation in each of the plant modes

modal_perturbation = [Link]([[1, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0],
[0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1],
])

# compute output energy for each IC

output_energy = [Link]((modal_perturbation.shape[0], [Link][0]))
for IC in range(0, modal_perturbation.shape[0]):
for ti in range(0, [Link][0]):
T = eigvects
alpha = modal_perturbation[:, IC]
output_energy[IC, ti] = ([Link](alpha)).[Link](Gorange[ti]).dot([Link]

# plot stuff
pltutls.Energy_Plot(trange, delt, output_energy)

[Link] 6/26
11/21/24, 4:19 PM [Link]

The above plots show the output energy ||y|| in response to a unit perturbation in
2

each of the modal spaces for different time intervals. To evaluate them, first recall
the description of the eigenspaces from project 1:

n
R = N1 ⊕ N2 ⊕ N3

These eigenspaces are associated with the eigenvalues:

N 1 = λ 1,3,5 = 0

N 2 = λ 2,4 = −0.0104

N 3 = λ 6 = −0.0208

The plots associated with N (plots 1,3,5) are linearly increasing. N and N have
1 2 3

similar response characteristics to different time intervals but with slightly different
parameters (the output associated with N converges faster).
3

In the output energy plot, two main trends appear for the range of G observability o

gramians computed over increasing [t 0, t1 ] intervals. A plot of the eigenvalues then

plateaus for further interval lengths). This implies that for a magnitude unit IC in the
plant modes associated with

Linearly increasing: The three plots associated with the N eigenspace 1

linearly increase as the time interval linearly increases. This means that for a
unit magnitude input, a longer time interval [t 0, t1 ] corresponds to a larger
output energy. More simply put, the longer the time interval, the larger the
corresponding output y(t) is for a given internal state regardless of the relative
"magnitude" of the internal state. Plant modal spaces 1,3,5 show similar linear
trends, all of which show energies in the range ||y|| 2
.
∈ [0, 400]

Rapid increase then decaying to constant: The output energy in the

eigenspaces associated with N 2, N3 show a decaying relationship with a
linearly increasing time interval [t 0
, t1 ] (output energy increases quickly

eigenspaces N 2, N3 only a certain range of time intervals yield an increase in

output energy. Plant modes 1,3,5 show similar slopes in their rate of increase. For
[Link] 7/26
11/21/24, 4:19 PM [Link]

the decaying modes, modes 2,4 finish decaying at t 1 − t 0 ≈ 150 whereas mode 6
decays more quickly on the order of t 1 − t 0 ≈ 75 . Plant modal spaces 2,4,5 show
decaying increases with plant modes 2,4 "topping out" at ||y|| 2
= 40 whereas plant
mode 6 hits an upper limit of ||y|| 2
= 20 which is half as much as modes 2,4.

Defining "relative observability" as the ratio between ||y|| in response to non-zero2

plant modal IC's for a common time interval - the above plot implies that plant
modal spaces 1,3,5 are equally observable relative to one another. It also shows
that plant modes 1 and 3 are approximately twice as observable as mode 6
although the variable rates of decay may yield a different ratio for smaller time
intervals.

To further investigate the observability of the plant, it is convenient to look at how

the observability grammian evolves over time - looking at the eigenvalues of G is 0

convenient for this. G is a linear transformation so larger relative eigenvalues will

0

lead to larger relative scalings along the eigenvectors of G . 0

To unpack how eigenvalues correspond to observability rather than just a linear

transformation, recall that G relates the systems internal state x(t) to some
o

output y(t). It can be thought of as a measurement of "how observable" the

internal state is: the G matrix.o

If x(t) T
G o x(t) yields a small corresponding y(t), this implies that it is "harder"
to observe x(t) because it requires a large internal state shift to observe a
small shift in output. This corresponds to small eigenvalues in the G matrix. o

Conversely, if x(t) T
G o x(t) yields a large corresponding y(t), this implies that
x(t) is more "easily" observable because a relatively small internal state shift
yields a large shift in the realized y(t). This corresponds to large eigenvalues in

To evaluate the eigenvalues of G it is convenient to find them and plot them. The
0

following snippet of code computes the eigenvalues of G at the different time 0

intervals it was computed for:

In [25]:
# compute evolution of eigenvalues
eigval_observability = [Link](([Link][1], [Link][0]))
for ti in range(0, [Link][0]):
eigval_observability[:, ti] = [Link](Gorange[ti])

# plot eigenvalues
pltutls.ObsEigValsEnergy_Plot(trange, delt, eigval_observability)

[Link] 8/26
11/21/24, 4:19 PM [Link]

As can be seen in the above plot, all eigenvalues of G increase as the time interval
0

[t 0 , t 1 ] grows larger. This implies that the linear transformation G will illicit a 0

relative larger scaling along the eigenvectors of G for larger time intervals. In the
0

context of observability, because all eigenvalues are increasing with respect to time,
the system will become more observable in all modes as time increases.

The rate of growth/second order rate of growth/decay is less clear because of the
exponential/exponential decay trends seen in the plot above. Because the
eigenvectors of G are not-necessarily parallel to the eigenvectors of A it is difficult
0

to infer directly about the observability of specific plant modes based on this
eigendecomposition; that said the positive correlation between time interval and
eigenvalue magnitude generally implies an increase in observability of all plant
modes as the time interval increases.

Modal Change in Response to Observer

Recall that the observability matrix is a mathematical analog of the reachability
matrix. In Project 1 it was shown how the rank of the reachability matrix had
implications on closed loop pole placement of A − BK . For observability the
placement of observer eigenvalues is of interest in the form A − LC (assuming
that the observer dynamics model and system dynamics are the same: A
^
= A ). The
gains K, L are of interest.

To use the mathematical analogy between observability and reachability matrices, it

is first convenient to take the transpose of A − LC to put the gain L in a similar
position to the gain K in A − BK :

T T T T
(A − LC) = A − C L

Now that A − LC is in a suitable form, a reachability matrix P AC

can be
constructed using A T
,C
T
. If this reachability matrix P AC has full rank, this implies
[Link] 9/26
11/21/24, 4:19 PM [Link]
that the eigenvalues of (A − LC) can be placed arbitrarily. At this point it is
T

convenient to recognize that the reachability matrix P AC is equal to the

observability matrix Q T

Earlier, Q was shown to be full rank. Note that because the rank of a matrix does
not change when the matrix is transposed (rank(A) = rank(A
T
), this implies
)

that the reachability matrix formed by A T

,C
T
is also full rank. This means that the
eigenvalues of A T
,C
T
can be placed arbitrarily. Further, note that transposition of
a matrix does not change eigenvalues:

T
λ i (A − LC) = λ i ((A − LC) )

Ultimately, this means that the eigenvalues placed for A T

,C
T
will correspond to
the eigenvalues of A, T . This placement occurs in Part 2 through the computation
of the Luenberger observer L. By the previous argument, because the observability
matrix Q is full rank (and the various implications of this discussed above), it can be
said that poles can be arbitrarily placed (via L) such that they can change all of the
modes of the observer.

This placement is done using Matlab's place() function in Part 2. The place()
takes an input vector of desired pole locations and returns a gain matrix K such
that the eigenvalues of A − BK reflect the inputted pole locations. When placing
the observer poles, place() can also be used, however modifications must be
made because it is set up for A − BK not A − LC . Inputs can be modified such
that:

T T T T
(A − LC) = A − C L

T T T
L = place(A ,C , p)

Part 2: Luenberger Observer Placement

Modal changes can be induced through the addition of control inputs into the
observer state x
^ through the inclusion of a Luenberger observer such that:

^ = Ax
x ^ + Bu + L(y − y)
^

Recall that y
^ = Cx
^

x
^ = Ax
^ + Bu + Ly − LC x
^

= (A − LC)x
^ + Bu + Ly

Recall in Project 1 that the following response times were desired for the closed
loop eigenvalues and thus the corresponding eigenvalues were placed using
Matlab's place function.

τ 2,4 = 0.1s ⟹ λ 2,4 = −10

τ 6 = 0.05s ⟹ λ 6 = −20

τ 1,3,5 = 1s ⟹ λ 1,3,5 = −1

In all of the following observer cases, observer poles are placed using Matlab's
[Link] 10/26
11/21/24, 4:19 PM [Link]

place function. Python's pole placement was not working as well in Project 1 so
Matlab's placement was used and just copied over. Discussion of the fact that L is T

returned from the place function is provided at the end of Part 1. In all cases the
poles were placed using the following syntax:

A = [...];
C = [...];
p = [-1, -10, -1, -10, -1, -20];
Lslow = place(A',C', p*0.2);
Lequal = place(A',C', p);
Lfast = place(A',C', p*5);

The Luenberger observer gain matrices L were then saved into a matlab file and
read in the code below. Note that the returned L matrices are actually L as
T

discussed in part 1.

In [26]:
Lgains = loadmat([Link]()+"/projectutils/data/L_gains.mat")
Lslow = [Link](Lgains['Lslow'])
Lequal = [Link](Lgains['Lequal'])
Lfast = [Link](Lgains['Lfast'])

Slow Observer
Per the problem statement, a slow observer is defined as 0.2 times as fast as the
placed closed loop eigenvalues. The corresponding observer eigenvalues are:

τ 2,4 = 0.1s × 0.2 ⟹ λ 2,4 = −2

τ 6 = 0.05s × 0.2 ⟹ λ 6 = −4

τ 1,3,5 = 1s × 0.2 ⟹ λ 1,3,5 = −0.2

In [27]:
print("L slow = ")
with [Link](precision=3):
print(Lslow)

L slow =
[[ 4.071e+00 7.340e-01 -4.717e-01 -8.943e-02 1.116e-15 2.323e-16]
[-4.717e-01 -8.943e-02 2.308e+00 3.996e-01 -2.892e-16 -4.780e-17]
[ 1.311e-15 7.902e-16 -6.942e-17 8.952e-17 2.179e+00 3.547e-01]]

Equal Observer
Per the problem statement, an equal observer is defined as 1 times as fast as the
placed closed loop eigenvalues. The corresponding observer eigenvalues are:

τ 2,4 = 0.1s ⟹ λ 2,4 = −10

τ 6 = 0.05s ⟹ λ 6 = −20

τ 1,3,5 = 1s ⟹ λ 1,3,5 = −1

In [28]:
print("L equal = ")
with [Link](precision=3):
print(Lequal)

L equal =
[Link] 11/26
11/21/24, 4:19 PM [Link]
L equal =
[[ 2.096e+01 1.976e+01 -4.990e-01 -4.938e-01 6.035e-16 4.736e-16]
[-4.990e-01 -4.938e-01 1.101e+01 9.910e+00 3.190e-17 6.212e-16]
[ 7.673e-16 2.931e-15 8.598e-16 3.000e-15 1.098e+01 9.772e+00]]

Fast Observer
Per the problem statement, a fast observer is defined as 5 times as fast as the
placed closed loop eigenvalues. The corresponding observer eigenvalues are:

τ 2,4 = 0.1s × 5 ⟹ λ 2,4 = −50

τ 6 = 0.05s × 5 ⟹ λ 6 = −100

τ 1,3,5 = 1s × 5 ⟹ λ 1,3,5 = −5

In [29]:
print("L fast = ")
with [Link](precision=3):
print(Lfast)

L fast =
[[ 1.050e+02 4.989e+02 -5.000e-01 -2.495e+00 -5.674e-16 -3.440e-15]
[-5.000e-01 -2.495e+00 5.499e+01 2.495e+02 1.230e-16 5.740e-15]
[-7.071e-16 -2.162e-14 3.495e-15 -7.285e-14 5.498e+01 2.489e+02]]

Part 3: Closed Loop Observer/Controller

Simulation - Reference Input
Start by defining an overall function that is called to simulate the closed loop
response to each observer. The gain matrix K and associated state variable
feedback matrix F were found in Project 1 and are copied in here. The entire closed
loop observer/controller system is described as:

ẋ A − BK BF x BF
ẇ = [ ] = [ ][ ] + [ ]r
ė 0 A − LC e 0

x
y = [C 0] [ ]
e

More concisely, this can be described as A ′ ′

,B ,C
′
. (Recall that D ′
). The
= 0

following snippet of code implements this amalgamated state space to incorporate

both controller (K) and observer (L) gains. It does this for the slow, equal and fast
observers via their associated L matrix. It reuses the closed loop K, F matrices
from Project 1. Each of the observer/controller systems are given a unit step
response in each of the reference inputs that the system is tracking via F - these
occur in the x, y, z states. Both initial conditions x, x
^ = 0 .

In [30]:
# define function for closed loop reference input
def ClosedLoopRefInput(A, B, C, K, F, L_list, trange):

# initialize output - 3 states w/ 3 responses each

# Dim 0 = L
# Dim 1 = yout associated with ui
# Dim 2 = time
resarray = [Link]((len(L_list), [Link][0], [Link][0]))
# this xhist stuff will store last entry of L_list, fine for Part 5
xhist = [Link](([Link][1], [Link][0]*2, [Link][0])) # 3 ics,
[Link] 12/26
11/21/24, 4:19 PM [Link]
st p. e os(( .s ape[ ], .s ape[0] , t a ge.s ape[0])) 3 ics,
for Lindex, L in enumerate(L_list):
# initialize empty arrays to pack
Ap = [Link](([Link][0]*2, [Link][1]*2))
Bp = [Link](([Link][0]*2, [Link][1]))
Cp = [Link](([Link][0], [Link][1]*2))
Dp = [Link]((3,3))

# pack arrays
Ap[0:6, 0:6] = [Link](K)
Ap[0:6, 6:] = [Link](K)
Ap[6:, 6:] = [Link](C)
Bp[0:6, :] = [Link](F)
Cp[:, 0:6] = C

# create state space model

SS = [Link](Ap, Bp, Cp, Dp)

# loop through inputs and simulate step response to reference input

for ui in range(0, [Link][1]):
T, yout, xout = control.step_response(SS, T=trange, input=ui, o
resarray[Lindex, ui, :] = yout
xhist[ui, :, :] = xout

return resarray, xhist

# recall the gain matrix K computed in project 1 - also bring in the assoc
K = [Link]([
[-119.9904, -131.8647, 0.0174, 0.0174, 6.2500, 6.8620],
[0.0174, 0.0174, -239.9808, -251.8550, 6.2500, 6.8620],
[119.9904, 131.8647, -0.0174, -0.0174, 6.2500, 6.8620],
[-0.0174, -0.0174, 239.9808, 251.8550, 6.2500, 6.8620]])

F = [Link]([[-119.99040,0.01740,6.25000],
[0.01740,-239.98080,6.25000],
[119.99040,-0.01740,6.25000],
[-0.01740,239.98080,6.25000]])

tmax = 10
tstep = 0.01
tmin = 0
trange = [Link](tmin, tmax, tstep)

L_list = [Lslow.T, Lequal.T, Lfast.T] # transpose computed gains per https

# do closed loop control response to step in reference input

resarray, xhist = ClosedLoopRefInput(A, B, C, K, F, L_list, trange) # igno

# do some stuff to make plot pretty:

# - augment some zeros to beginning of response to emulate prior to unit
# - update the time history to reflect this change
prestep_size = 150
prestep_resarray = [Link](([Link][0], [Link][1], prestep_
prestep_resarray[:, :, prestep_size:] = resarray
step_ref = [Link]([Link][2]+prestep_size)
step_ref[prestep_size:] = 1
trange_plot = [Link](tmin-prestep_size*tstep, tmax, tstep)

# plot stuff
pltutls.Unit_step_plot(trange_plot, prestep_resarray, step_ref)

[Link] 13/26
11/21/24, 4:19 PM [Link]

Observer Comparisons
Referencing the Closed Loop Control Observer/Controller Response plot, note that
all three observers had identical responses to the unit step reference inputs with
zero IC in both x and x
^. The explanation for this identical response is contingent on

the fact that both x ^ = 0

= 0, x .

Expanding on this, recognize that because A is identical for the system dynamics
and the model dynamics, the error (e = x − x
^ ) can be represented in the following
form:

ẋ = Ax + Bu

y = Cx

˙
^ = Ax
x ^ + Bu + L(y − y)
^

y
^ = Cx
^

˙
^ = Ax + Bu − (Ax
^ + Bu + L(y − y))
^
ė = ẋ − x

= Ae − LCe

= (A − LC)e

(A−LC)(t−t 0 )
e(t) = e e(0)

In the final expression we recall that e = x − x

^ This simulation and associated
[Link] 14/26
11/21/24, 4:19 PM [Link]
In the final expression we recall that e = x x . This simulation and associated
response is evaluating the controller/observer closed loop system response to zero
initial conditions in x, x
^ ⟹ x(0) = x(0)
^ = 0 . In the context of e this implies
that e(0) = 0 .

Plugging this into the expression for e(t), it can be clearly seen that for an initial
condition e(0) ,
= 0 e(t) will evolve constantly over time and remain at a value of 0.

"Flipping this around", recognize that because e(t) = 0, ∀t , as the system evolves
from this initial condition, x(t) = 0, x(t)
^ = 0, ∀t . Further, recalling that y = Cx

and y^ = Cx
^ , this implies that y(t) = y(t),
^ ∀t .

Further, this means that the model input from the Luenberger gain L (in the form
L(y − y)
^ ) is also zero for the entire time interval because L(0 − 0) = 0 . This
means that regardless of the placed pole positions that generated L, the system
response/outputs are the same which is reflected in the zero IC simulation/plots.
This is also reflected in the separation principle which can be used to show that the
error states are not reachable via some reference input. Accordingly, the system
response is identical to "controller only" closed loop control that is shown in Part 5
of Project 1.

Part 4: Closed Loop Observer/Controller

Simulation - Non-Zero IC
Start by defining a function to handle the closed loop observer/controller response
to non-zero system (not model) initial conditions (x ^ = 0
≠ 0, x ). Also define the
initial condition in each of the original plant states (not modal spaces).

Note that for these simulations, each initial condition is simulated independently.
Because the model initial condition x
^ = 0 , this means that the error is just the
system state initial condition x. This is because e = x
^ − x where
x = 0 ⟹ ^
e = x .

The following snippet of code implements closed loop observer/controller systems

and simulates the response to non-zero x initial conditions. It does this for the
slow, equal, and fast observer gains L. The same K, F gain/tracking matrices
developed in Project 1 are reused here.

In [31]:
# define the initial condition
ICx = [Link]([[1, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0],
[0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1],
])

tmax = 20
tstep = 0.01
tmin = 0
trange = [Link](tmin, tmax, tstep)

# d fi f ti f l d l i iti l diti
[Link] 15/26
11/21/24, 4:19 PM [Link]
# define function for closed loop initial condition response
def ClosedLoopICResponse(A, B, C, K, F, L, ICmat, trange):

# initialize empty arrays to pack

Ap = [Link](([Link][0]*2, [Link][1]*2))
Bp = [Link](([Link][0]*2, [Link][1]))
Cp = [Link](([Link][0], [Link][1]*2))
Dp = [Link]((3,3))

# pack arrays
Ap[0:6, 0:6] = [Link](K)
Ap[0:6, 6:] = [Link](K)
Ap[6:, 6:] = [Link](C)
Bp[0:6, :] = [Link](F)
Cp[:, 0:6] = C

# create state space model

SS = [Link](Ap, Bp, Cp, Dp)

# initialize output - 3 states w/ 3 responses each

# Dim 0 = IC
# Dim 1 = yout
# Dim 2 = time
resarray = [Link](([Link][0], [Link][0], [Link][0]))
for ICindex, IC in enumerate(ICmat.T):
IC_incl_e = [Link]((IC,IC))

# loop through inputs and simulate step response to reference input

T, yout = control.initial_response(SS, T=trange, X0=IC_incl_e)
resarray[ICindex, :, :] = yout

return resarray

In [32]:
# compute response and plot - slow
resarray = ClosedLoopICResponse(A, B, C, K, F, Lslow, ICx, trange)
pltutls.Non_Zero_IC_Plot(trange, resarray, "L slow")

# compute response and plot - equal

resarray = ClosedLoopICResponse(A, B, C, K, F, Lequal, ICx, trange)
pltutls.Non_Zero_IC_Plot(trange, resarray, "L equal")

# compute response and plot - fast

resarray = ClosedLoopICResponse(A, B, C, K, F, Lfast, ICx, trange)
pltutls.Non_Zero_IC_Plot(trange, resarray, "L fast")

[Link] 16/26
11/21/24, 4:19 PM [Link]

[Link] 17/26
11/21/24, 4:19 PM [Link]

The above plots shows the system response to the slow, equal, and fast observers
developed in Part 2. Although there were 6 total initial conditions and 3 different
observer "speeds" (based on placed observer poles) two main response modalities
emerged due to the inherent similarities in the internal states and system dynamics
in general. These response similarities are seen in all three of the system output
plots for all three observer types - the different observer types have the behavior
occur in different time scales. The two response modalities can be characterized as:
then begins to decay back to equilibrium. Again each IC/observer gain shows
similar responses.

Response to Position IC: In this response modality, the system starts at a non-
zero position and must return to equilibrium. The system changes position and
reaches an inflection point with equilibrium before "overshooting" equilibrium.
This maximum overcorrection occurs similarly for all IC's/observer gain
matrices. It then decays to equilibrium.
Response to Velocity IC: In this response modality, the system starts at the
equilibrium position but departs due to the non-zero velocity (which is not part
of the output, thus not directly shown). It reaches a maximum departure from
equilibrium

The above plots graphically display the maximum velocity achieved during the
response to velocity IC and the minimum position achieved during the response to
the position IC. Because the different observers show similar response modalities
with differences in the timing of the inflection points, these inflection points
represent a convenient basis for comparison. The inflection points are labeled on
the plots above and summarized below - the pairs indicate the x,y coordinates of
the max/min position for the different IC's.
[Link] 18/26
11/21/24, 4:19 PM [Link]

L SLOW

IC V max P os min

x (2.14, 0.956) (4.21,-0.12)

y (2.3, 1.073) (4.42, -0.134)

z (2.34, 1.12) (4.51,-0.131)

Avg Time to V max = 2.26

Avg V max = 1.05

Avg Time to P os min = 4.38

Avg P os min = −0.128

L EQUAL

IC V max P os min

x (1.03, 0.478) (2.04,-0.172)

y (1.03,0.478) (2.04,-0.172)

z (1.05, 0.514) (2.08,-0.181)

Avg Time to V max = 1.03

Avg V max = 0.49

Avg Time to P os min = 2.05

Avg P os min = −0.175

L FAST

IC V max P os min

x (0.46, 0.217) (0.88, -0.143)

y (0.42,0.19) (0.83, -0.125)

z (0.47,0.225) (0.88,-0.146)

Avg Time to V max = 0.45

Avg V max = 0.211

Avg Time to P os min = 0.863

Avg P os min = −0.138

Interpreting the data above, a few comparisons can be drawn between the different
observer speeds: the average minimum position appears to be trending upward
with a maximum average position occurring for the equal observer.

Time to Inflection: As observer speed increases (speed being defined by

placed observer eigenvalue time constants; reference Part 2), the maximum
velocity and minimum position inflection points are reached more quickly. The
relative "decay" in amount of time to reach the inflection point is similar
[Link] 19/26
11/21/24, 4:19 PM [Link]
relative decay in amount of time to reach the inflection point is similar
between velocity and position. L equal
is approximately twice as fast as L slow

and L f ast is approximately 4x as fast as L slow in reaching the inflection points

for velocity and position IC's.
Relative time to Inflection: Although the linearly decreasing trend in times is
similar, the actual time to the minimum position inflection point is generally
about twice as long as the time to the velocity inflection point.
Avg Vmax: An analogous trend is compared to the time to inflection - the Avg
V max for the equal observer is approximately one-half that of the slow
observer. Similarly, the Avg V max
for the fast observer is approximately one
quarter that of the slow observer.
Avg PosMin: There is a deviation in the trend for the average minimum
position compared to the velocity/inflection behavior:

Interestingly, the equal observer has the largest average minimum position. All of
the average minimum positions are less than 20% of the initial condition (< |0.2|) .

Given the nature of a quadcopter system, response speed is important. By

inspecting the plot of the slow observer, it is clear that the system doesn't totally
return to equilibrium during the 20s simulation. The equal observer returns to
equilibrium in ≈ 7.5s while the fast observer returns to equilibrium in ≈ 4s. The
long decay times in the slow and equal observer are unacceptable for a system like
a quadcopter with tight time requirements. Based on the time to inflection rates,
the fast observer is ≈ 2 − 4x faster than the slow/equal observers which makes it a
good choice giving its relatively quick performance and the tight time demands of
a quadrotor.

The other area of interest is potential space constraints and the implications of the
minimum position response. For example, if the quadrotor was trying to fly close to
a wall without touching it, the "overshooting" effect seen in the minimum position
response could cause a crash. The equal observer had the largest "overshoot"
(smallest minimum position) and thus is not a good choice. Without more specific
design guidelines it is difficult to say how much "overshoot" is too much but the
selected design should minimize it without compromising speed.

Thus, the fast observer appears to be a good choice because it maximizes speed of
response without incurring too much positional deviation.

Part 5: LQR Controller Design

NOTE: In this section a distinction is drawn between the "placed" control output
and the "LQR" control output. This is obviously a bit of a misnomer because poles
have been placed in both cases, although it is convenient to refer to them this way.
To be clear - "placed" implies closed loop control with tracking from Project 1 (no
observer). "LQR" implies closed loop control with an observer and controller with
tracking where the controller eigenvalues were placed via LQR cost function
minimization.

Find Optimal State Feedback Gain Matrix K and Updated

[Link] 20/26
11/21/24, 4:19 PM [Link]
Find Optimal State Feedback Gain Matrix K and Updated
F
Recall that a Linear Quadratic Regulator (LQR) is a type of control where some
"regulator" is introduced that minimizes a cost function. This cost function
describes how "expensive" it is to be away from some desired equilibrium. In effect,
this minimization process drives the process to return to equilibrium.

The cost function being minimized by LQR control is:

∞
T T
J = ∫ (x (t)Qx(t) + u (t)Ru(t))dt
0

T
x (t)Qx(t) = Quadratic weighting of state

T
u (t)Ru(t) = Quadratic weighting of input

In this form, the LQR cost function J has two parameters to set:

Q : The state cost weight matrix dictating how "expensive" the state deviation
from equilibrium is.
R : The control cost weight matrix dictating how "expensive" control input into
the system is.

Note that J is an integral so we are measuring the area under the curve of the
state/input costs. If Q is large and R is small, the system will respond quickly to any
perturbation from equilibrium because control is much "cheaper" than the
deviation from equilibrium. Conversely, if Q is small and R is large, the system will
respond slowly because control input is more expensive than a long term deviation
from equilibrium.

Begin with the design process for R. In the quadcopter problem formulation, the
system inputs u(t) are:

T1
⎡ ⎤

T2
u(t) =
T3

⎣ ⎦
T4

Where T indicates the input from each of the 4 rotors. Assuming that this is a
i

conventional quadrotor, these motors are identical and it is assumed that actuating
different rotors or different combinations of rotors does not incur different costs.
This assumption is probably valid for the actuation of different rotors
independently. For the actuation of different combinations of rotors this probably is
not totally valid because actuating all four motors at once as opposed to a single
rotor will incur different current draws on the power supply - for simplicity and
given the context of the problem these implications are left for exploration in future
work.

Given the assumption that different rotors are similarly expensive, it becomes clear
that each of the expenses incurred by the actuators should be similar in the
resultant R matrix. To do this, a scalar factor is multiplied by an appropriately sized
identity matrix:

[Link] 21/26
11/21/24, 4:19 PM

In [33]:
Q and R can be compared directly:
(t)Ru(T )

Q =

⎢⎥
⎣
0

R =
1

print("Frobenius norm of Q: ", [Link](Q), "\n")

print("Frobenius norm of R: ", [Link](R), "\n")

Frobenius norm of Q:

Frobenius norm of R:

During the tuning process R

2.449489742783178

2.0
⎡

⎣
[Link]

R = R scale I p×p

To understand the basic relationship between Q, R before tuning Q, R

useful to have a basis of comparison for their relative magnitudes. To establish this
baseline, Q is temporarily selected to be a diagonal matrix of ones. Recognizing
that x T
(T )Qx(T ) and u T

Frobenius norm of Q and R as a basis for comparison to see what the resultant
impacts of different Q, R selections yield. Assuming that R

⎡
1 0 0

The following code snippet computes the Frobenius norm between Q, R:

# setup Q, R
Q = [Link]([Link][0])
Rfact = 1
R = [Link]([Link][1])

# compare norms
0

0
0

0
0

In the above snippet of code, the Frobenius norm of Q and R was determined to
⎤

serve as a back of the envelope comparison of the relative contribution to the cost
function J . The Frobenius norm of Q
This means that without any scaling, Q

approximately equal in terms of cost.

f act
= 2.45

≈ R
scale

and the Frobenius norm of R

f act

yield scalar values, it is convenient to take the

, it is

, the norms of
= 1

in terms of relative contribution to J .

and Q may change drastically, but it is useful to
establish a baseline comparison of their relative magnitudes at the outset to
understand the implications of the "tuning knobs" they represent. In their current
iteration, these Q, R values would have approximately equal weights in the cost
function meaning that the LQR is weighting state deviation and control inputs as

Now that this baseline comparison between Q, R has been established, Q, R are
selected. The problem statement gives a design requirement that the LQR selected
K has eigenvalues at least as short as the placed eigenvalues in Project 1. Recall
that the placed eigenvalues in project 1 and their associated time constants:
[Link]
= 2 .

22/26
11/21/24, 4:19 PM [Link]

τ 2,4 = 0.1s ⟹ λ 2,4 = −10

τ 6 = 0.05s ⟹ λ 6 = −20

τ 1,3,5 = 1s ⟹ λ 1,3,5 = −1

This means that the minimum design requirement for the LQR placed eigenvalues is
λ LQR ≤ −1 . Several R scale values were tested iteratively - the final selection is
shown in the code snippet below and discussed.

In [34]:
# define Q, R matrices
# Q = [Link]((25, 5, 25, 5, 20, 0.052)) # seems slightly more realistic
Q = [Link]((50, 1, 50, 1, 50, 1)) # very fast performance
Q = [Link]((2,2,2,2,1,0.05)) # gives good tracking except in Z
# Q = [Link]((25, 10, 25, 10, 25, 0.5))
Rfact = 0.0001
R = Rfact*[Link]([Link][1])

Qeig = [Link](Q)
Reig = [Link](R)
with [Link](precision=6):
print("Q eigenvalues: ")
print(Qeig, "\n")
print("R eigenvalues: ")
print(Reig, "\n")

# use LQR function to find controller gains etc

K_lqr, Slqr, Elqr = [Link](A, B, Q, R)
eigvals_lqr = [Link]([Link](K_lqr))

# confirm that eigenvalues are placed correctly

# recall the K matrix from Project 1
Kproj1 = [Link]([
[-119.9904, -131.8647, 0.0174, 0.0174, 6.2500, 6.8620],
[0.0174, 0.0174, -239.9808, -251.8550, 6.2500, 6.8620],
[119.9904, 131.8647, -0.0174, -0.0174, 6.2500, 6.8620],
[-0.0174, -0.0174, 239.9808, 251.8550, 6.2500, 6.8620]
])
eigvals_CL = [Link]([Link](Kproj1))
with [Link](precision=3):
print("LQR Selected Eigenvalues: ")
print(eigvals_lqr,"\n")
print("Project 1 Placed Eigenvalues: ")
print(eigvals_CL,"\n")

Q eigenvalues:
[2. 2. 2. 2. 1. 0.05]

R eigenvalues:
[0.0001 0.0001 0.0001 0.0001]

LQR Selected Eigenvalues:

[ -1.007 -8.273 -4.63 -17.279 -8.273 -1.007]

Project 1 Placed Eigenvalues:

[-20. -10. -10. -1. -1. -1.]

In the above output the eigenvalues of Q and R are shown to show conformance
with project/system specifications:

All eigenvalues of both matrices are greater than zero which implies that both
[Link] 23/26
11/21/24, 4:19 PM [Link]
All eigenvalues of both matrices are greater than zero which implies that both
Q and R are positive definite matrices. This satisfies the condition that R is
positive definite and Q is positive semi-definite.
All eigenvalues of A − BK LQR
≤ −1 which satisfies the condition that the
LQR placed eigenvalues have time constants at least as fast as their slowest
Project 1 placed counterpart.

In the end, the selected R needed to be ≈ 4-5 orders of magnitude smaller than Q
to have real eigenvalues that are appropriately sized. Q was chosen using an
iterative process that basically reduced to shaking the magic LQR eight-ball and
until λ i (A − BK LQR ) ≈ λ i (A − BK placed ) . The eigenvalues that were eventually
selected are shown in the output of the code snippet above. Note that both the
placed and LQR eigenvalues are non-imaginary and negative. Additionally note that
the LQR eigenvalues attempted to preserve the ratio found in the placed
eigenvalues.

Implement Design with Preferred Observer Placement

Now that an LQR has been used to determine K LQR
, the system is simulated using
the LQR placed poles. The system is exposed to unit step reference inputs and
compared to the response of the controller only response simulation from Project
1.

The following code snippet takes the selected observer from Part 4 (fast observer)
and simulates the controller/observer response to unit step reference inputs. It then
brings in the simulation data from the similar controller only simulation from
Project 1.

In [35]:
# compute updated F
F_lqr = [Link](
[Link](
[Link](
-A+[Link](K_lqr)
)
).dot(B)
)

# don't change this stuff otherwise you'll need to resave project 1 stuff
tmax = 10
tstep = 0.01
tmin = 0
trange = [Link](tmin, tmax, tstep)

L_list = [Lfast.T] # transpose computed gains per [Link]

# do closed loop control response to step in reference input

resarray, xhist_lqr = ClosedLoopRefInput(A, B, C, K_lqr, F_lqr, L_list, tr

# do some stuff to make plot pretty:

# - augment some zeros to beginning of response to emulate prior to unit
# - update the time history to reflect this change
prestep_size = 150 # don't change this stuff otherwise you'll need to resa
prestep_resarray = [Link]((2, [Link][1], prestep_size+resarray.s
xhist_prestep = [Link]((xhist_lqr.shape[0], xhist_lqr.shape[1], prestep_

[Link] 24/26
11/21/24, 4:19 PM [Link]
# add in computed results from LQR
prestep_resarray[0, :, prestep_size:] = resarray
xhist_prestep[:, :, prestep_size:] = xhist_lqr
xhist_lqr = xhist_prestep

# add in computed results from project 1

proj1response = [Link]([Link]()+"/projectutils/data/project1data/CL_pl
prestep_resarray[1, :, :] = proj1response

step_ref = [Link]([Link][2]+prestep_size)
step_ref[prestep_size:] = 1
trange_plot = [Link](tmin-prestep_size*tstep, tmax, tstep)

# plot stuff
pltutls.Unit_step_plot_LQR(trange_plot, prestep_resarray, step_ref)

The above plot shows the output responses of the controller only (Project 1 placed)
and controller/observer (LQR placed) response to a unit step input with zero initial
conditions. Visually, the response to the x, y reference inputs tracks well while there
is some deviation in the z response. This is quantified in the following section
where the energy input is compared between the two different controllers.

Compare Plant Input Energy Between LQR Design and

[Link] 25/26
11/21/24, 4:19 PM
Compare Plant Input Energy Between
[Link]
LQR Design and
Original K from Project 1
First recall the general control law that has been developed through the closed
loop feedback gain K and the reference input tracking matrix F :

Control Law

^
u(t) = −K x(t) + F r(t)

To compare the control energies in the LQR vs Project 1 approaches, it is necessary

to develop some way to quantify control energy. There are several ways to do this
and in this case the L2 norm of u(t) is sufficient; overall energy expenditures over
time is of more interest than absolute max energy "spikes". Thus, the L2 norm of
u(t) is used such that:

T
2 T
||u|| = ∫ u (t)u(t)dt
0

To compute this integral, the control law recalled above is used. As stated in the
problem specification, T = 3 closed loop time constants. Because there are several
time constants at play, the smallest is used ⟹ . In practice this is
T = 3 × 1s

sufficient for comparison because generally control energy approaches zero once
3s has passed.

Additionally, it is worth clarifying how x(t),

^ r(t) are determined - both can be

outputted by the step_response() function that was used for simulation. Also
note that the simulation started with zero initial conditions such that
x(0)
^ = x(0) = 0 . Recall from the discussion in Part 3 regarding the observer
comparisions that if the simulation starts with zero error, it will stay at zero error.
The step_response() function returns x(t); this is acceptable because
^ − x = 0
e = x ⟹ ^
x = x which means that the returned x(t) can be used
directly in the calculation of u(t):

u(t) = −K x(t)
^ + F r(t)

[Link] 26/26