11/20/24, 11:33 PM Project1.

ipynb

rmcsqrd / Project1.ipynb
Last active 4 years ago • Report abuse

Code Revisions 4

Project1.ipynb

https://gist.github.com/rmcsqrd/a93e322ac92fd5089ca6aba1487ff30c 1/22
11/20/24, 11:33 PM Project1.ipynb

ASEN 5014 - Project 1

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: https://gist.github.com/rmcsqrd

Table of Contents
Part 1: Implementing the Plant
Part 2: Evaluating Reachability
Part 3: Pole Placement
Part 4: State Feedback Control
Part 5: Reference Input Tracking

Part 1: Implementing the Plant

Implement the plant and characterize plant modes
Start out by initializing the state space matrices A, B, C, D

In [1]:
# import some stuff
import numpy as np
import control
import projectutils.plot_utils as pltutls
from scipy.linalg import expm
pltutls.PlotInitParams()

# define matrices
A = np.array(
[[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 = np.array(
[[0, 0, 0, 0],
[-0.04167, 0, 0.04167, 0],
https://gist.github.com/rmcsqrd/a93e322ac92fd5089ca6aba1487ff30c 2/22
11/20/24, 11:33 PM Project1.ipynb
[0, 0, 0, 0],
[0, -0.04167, 0, 0.04167],
[0, 0, 0, 0],
[0.4, 0.4, 0.4, 0.4]]
)

C = np.array(
[[1, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 1, 0]]
)

D = np.array(
[[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0]]
)

Now let's do an eigendecomposition to see what can be learned about the system:

In [2]:
eigvals, eigvects = np.linalg.eig(A)
print("eigenvalues = \n", eigvals, "\n")
print("eigenvects = \n", eigvects, "\n")
print("Rank(eigvects) = \n", np.linalg.matrix_rank(eigvects), "\n")

eigenvalues =
[ 0. -0.0104 0. -0.0104 0. -0.0208]

eigenvects =
[[ 1. -0.99994592 0. 0. 0. 0. ]
[ 0. 0.01039944 0. 0. 0. 0. ]
[ 0. 0. 1. -0.99994592 0. 0. ]
[ 0. 0. 0. 0.01039944 0. 0. ]
[ 0. 0. 0. 0. 1. -0.99978375]
[ 0. 0. 0. 0. 0. 0.0207955 ]]

Rank(eigvects) =
6

Note that there are several repeated eigenvalues:

λ 1,3,5 = 0, λ 2,4 = −0.0104, λ 6 = −0.0208

Also note that the rank of the matrix of eigenvectors, T , is full rank, which implies
that all 6 eigenvectors are linearly independent and form a spanning set of R . 6

Further verify this by looking at the degenerecy of A.

By inspection, the eigenvalues have the following algebraic multiplicities:

m i → λ 1,3,5 = 3

m i → λ 2,4 = 2

mi → λ6 = 1

In order to evaluate their degeneracy, the eigenvalues geometric multiplicities are

determined such that:

q i = n − rank(A − λ i I )

Where n is the dimension of the square matrix A (in this case

6 6
A
https://gist.github.com/rmcsqrd/a93e322ac92fd5089ca6aba1487ff30c 3/22
11/20/24, 11:33 PM Project1.ipynb
A ∈ R
6×6
⟹ n = 6 ). Thus:

In [3]:
q135 = A.shape[0] - np.linalg.matrix_rank(A - eigvals[0] * np.identity(A.s
q24 = A.shape[0] - np.linalg.matrix_rank(A - eigvals[1] * np.identity(A.sh
q6 = A.shape[0] - np.linalg.matrix_rank(A - eigvals[5] * np.identity(A.sha

print("q_i of eigenvalues 1,3,5 = ", q135, "\n")

print("q_i of eigenvalues 2,4 = ", q24, "\n")
print("q_i of eigenvalues 6 = ", q6, "\n")

q_i of eigenvalues 1,3,5 = 3

q_i of eigenvalues 2,4 = 2

q_i of eigenvalues 6 = 1

As seen in the above code snippet, q i = mi for all eigenvalues which implies that
A is fully degenerate and thus T is invertible (which implies that A is
diagonalizable). This reflects the fact that T is full rank. Thus the eigenspaces are
characterized such that:

n
R = N1 ⊕ N2 ⊕ N3

Where N is RN (A − λ
1 1,3,5 I ) , N is RN (A − λ
2 2,4 I ) , and N is RN (A − λ
3 6I) .

Also note that because T is diagonalizable and it has no complex eigenvalues then
the Jordan form of A is a diagonal matrix with its eigenvalues on the diagonal,
Λ ⟹ Λ = T
−1
AT . This Λ is considered the real modal form of A. Because Λ
does not have any complex parts, the modal space of A is the eigenspace of A.

Simulate the plant response to non-zero IC

Start with an arbitrary non-zero initial condition. Use pythons
forced_response() function which simulates the output of a linear system. This
function is similar to Matlab's lsim() .

Our choice of non-zero IC's is based on the eigenspaces define above. Verify that
the IC's chosen are non-zero IC's in each eigenspace N 1, N2 , N3 by taking the IC
(in the basis of x) and representing it in the basis of the eigenvectors. In the x0
array defined below, x0[j,i] represents the jth state of the ith eigenspace:

In [4]:
# define model parameters

model = control.StateSpace(A, B, C, D)
delt = 0.01
Tin = np.arange(0, 1000, delt)
Tin_alpha = np.arange(0, 1000, delt)

u0 = 0 # we want zero control input right now

#N1 N2 N3
x0 = np.array([[1, 0, 0], # x
[0, 0.1, 0], # xdot
[1, 0, 0], # y
https://gist.github.com/rmcsqrd/a93e322ac92fd5089ca6aba1487ff30c 4/22
11/20/24, 11:33 PM Project1.ipynb
[0, 0.1, 0], # ydot
[1, 0, 0], # z
[0, 0, 0.1]]) # zdot

with np.printoptions(precision=3):
print("x0 N1 in basis of T:")
print(np.linalg.inv(eigvects).dot(x0[:,0]), "\n")
print("x0 N2 in basis of T:")
print(np.linalg.inv(eigvects).dot(x0[:,1]), "\n")
print("x0 N3 in basis of T:")
print(np.linalg.inv(eigvects).dot(x0[:,2]), "\n")

x0 N1 in basis of T:
[1. 0. 1. 0. 1. 0.]

x0 N2 in basis of T:
[9.615 9.616 9.615 9.616 0. 0. ]

x0 N3 in basis of T:
[0. 0. 0. 0. 4.808 4.809]

In the above output it is seen that the selected IC's represent non-zero initial
conditions in each of the eigenspaces. N is associated with the 1,3,5 eigenvectors
1

of T which is reflected above. N is associated with the 2,4 eigenvectors of T which

2

is also reflected above. Note that the N initial condition also impacts eigenvectors
2

1,3 as well which intuitively makes sense because of the positional dependence of
x, y on ẋ and ẏ. This dependence is one way in the sense that a change in position
does not immediately result in a change in velocty (as reflected in N ). The third
1

initial condition, N , is similar to N but in z/ż.

3 2

Now that it has been confirmed that appropriate IC's have been selected such that
they excite each modal space, investigate the modal responses:

In [5]:
# simulate the system using the defined model, time series, and zero contro
# then store in results array for plotting
resarray = np.zeros((x0.shape[1], C.shape[0], Tin.shape[0]))
for cnt, col in enumerate(x0.T):
T, yout, xout = control.forced_response(model, Tin, u0, x0[:, cnt])
resarray[cnt, :, :] = yout

# simulate evolution of alpha(t) - IC is x0 in terms of basis of eigenvect

alphares = np.zeros((x0.shape[0], Tin_alpha.shape[0]))
alphares[:,0] = np.linalg.inv(eigvects).dot(np.sum(x0,1)) #sum across col
for t in range(1,Tin_alpha.shape[0]):
alphares[:,t] = np.exp(eigvals * delt)*alphares[:,t-1]

# plot stuff
pltutls.IC_plot_y(Tin, resarray)

https://gist.github.com/rmcsqrd/a93e322ac92fd5089ca6aba1487ff30c 5/22
11/20/24, 11:33 PM Project1.ipynb

Let's analyze this plot by exploring how it reflects what is happening in the modal
space. First recognize that because T is diagonalizable, the eigenvectors that make
up the columns of T are linearly independent and form a basis set for R . This 6

means that x(t) can be represented as a linear combination of its eigenvectors:

∗
x(t) = ∑ α i (t)x
i i

Also recall that α i (t) = e

λi t
α i (0) ⟹

λi t ∗
x(t) = ∑ e α i (0)x
i i

This equation implies that x(t) evolves as a linear combination of its eigenvalues.
Recognize that α i (0) and x are scalar quantities and the evolution of the system
∗
i

with respect to time comes from the e λi t

term. Recalling that all of the eigenvalues
are real, describe the three ways the system can evolve as t → ∞ :

1. λ i = 0 ⟹ lim t→∞ e
λi t
= 1

2. λ i
> 0 ⟹ lim t→∞ e
λi t
= ∞

3. λ i < 0 ⟹ lim t→∞ e

λi t
= 0

Reference the above and below plot. Recognize that the evolution of the system
dynamics reflects the above characterization of system evolution. The plot above
shows how the system evolves in the basis of x and the plot below shows how the
system evolves in the modal space. Note that for the plot below some lines occlude
other lines. This is because the α terms are associated with the same eigenvalues
and thus behave in identical ways.

System response refers to the response in the basis of x (above plot) and modal
https://gist.github.com/rmcsqrd/a93e322ac92fd5089ca6aba1487ff30c 6/22
11/20/24, 11:33 PM Project1.ipynb
System response refers to the response in the basis of x (above plot) and modal
response refers to the response in each of the plant modes (below plot)

N1 : Recall that all eigenvalues that form N have a value equal to 0. Looking at
1

characterization 1 above, note that as t → ∞ ⟹ x(t) = ∑ α i (0)x

i
∗
i
. This
means that as the system evolves over time, the state associated with the
modal space of N stays at the initial condition. This is demonstrated in the
1

modal response plot where the α values associated with N lines do not 1

change as the system dynamics evolve over time. This is also reflected in the
system response plot - the position is constant with respect to time.
N2 : All eigenvalues that form N are negative and decay to equilibrium as the
2

system evolves over time (characterization 3). This is reflected in the modal
response plot where it is observed that the α terms associated with N (α 2 2,4 )
are decaying to zero as the system evolves. Similar behavior is also observed in
the system response plot where the system decays to an equilibrium point.
N3 : This eigenspace behaves similarly to N although it decays at a slightly
2

faster rate because λ 6

< λ 2,4 .
None of the plant modes diverge because no eigenvalues are positive
(characterization 2) - this is reflected in both the system and modal response
plots where all responses either stay at equilibrium or reach equilibrium after a
sufficient time interval.

In [6]:
# plot alpha stuff
pltutls.IC_alpha_plot(Tin_alpha, alphares)

https://gist.github.com/rmcsqrd/a93e322ac92fd5089ca6aba1487ff30c 7/22
11/20/24, 11:33 PM Project1.ipynb

Plant Stability
Recalling the definition of Lyapunov stability. An equilibrium x is said to be stable ∗

in the sense of Lyapunov (I.S.L.) if for any δ > 0, ϵ > 0 :

∗ ∗
||x(0) − x || < δ ⟹ ||x(t) − x || < ϵ, ∀t ≥ 0

For simplicity, select x(0) . Looking at the system

T
= [0 0 0 0 0 0]

response plot, recognize that after some amount of time t, the system converges to
an equilibrium in all states such that for some sufficiently large t,
∗ ∗
x (t) − x (t − 1) = 0 . Loosely speaking, this equilibrium point occurs at
y = [10, 10, 5]
T
. In reality, stability deals with the internal state x, but in our case y
is a 1-1 realization of x with the velocity components removed. Because position is
intrinsically related to velocity, and the position reaches an equilibrium point, by
inspection the velocity must be reaching an equilibrium as well. This justifies the
non-rigorous (but still accurate) usage of y for evaluating stability.

This equilibrium state shown in the above plot is the system's evolution from the
initial condition x(0). Because the system decays to this equilibirum point, this
implies that after some threshold time t,
∗ ∗
||x(0) − x (t)|| = ||x(0) − x (t + n)||, ∀n . Assuming that δ is selected such that
the system has evolved past this threshold time t, the condition for stability I.S.L.
will hold ∀t ≥ 0 .

Also note that the system is simple (T is diagonalizable and all eigenvectors are
real) because λ i ≤ 0, ∀i. These facts both imply that the system is stable I.S.L.

Now look to the asymptotic stability of the system. As conveyed in the above
graphs, the system evolves as t → ∞ and reaches an equilibrium point x such ∗

that it is stable I.S.L. Given the intial condition defined previously, the system
evolves to an equilibrium point y = [10, 10, 5]
T
. In order for the system to be
asymptotically stable, alternative initial conditions should also evolve into this
equilibrium point. To test this, evaluate the system evolution using the same IC as
above multiplied by a factor of 2:

In [7]:
# simulate the system using the defined model, time series, and zero contro
# then store in results array for plotting
resarrayAsymp = np.zeros((x0.shape[1], C.shape[0], Tin.shape[0]))
for cnt, col in enumerate(x0.T):
newIC = x0[:, cnt]*2
T, yout, xout = control.forced_response(model, Tin, u0, newIC)
resarrayAsymp[cnt, :, :] = yout

# plot stuff
pltutls.IC_plot_y(Tin, resarrayAsymp)

https://gist.github.com/rmcsqrd/a93e322ac92fd5089ca6aba1487ff30c 8/22
11/20/24, 11:33 PM Project1.ipynb

As seen in the above plots, for this new IC, the system evolves into an equilibrium
point y = [20, 20, 10]
T
which is a different equilibrium point than for the original
initial condition. This implies that different initial conditions will evolve into
different equilibrium points which means that the system is not asymptotically
stable I.S.L.

Part 2: Evaluating Reachability

Reachable Subspace
To find the reachable subspace, start by finding the reachability matrix P :

0 1 n−1
P = [A B A B … A B]

In [8]:
# compute P matrix:
P = np.zeros((A.shape[0], A.shape[0]*B.shape[1]))
for i in range(0,A.shape[0]):
n = i*B.shape[1]
P[:, n:n+B.shape[1]] = np.linalg.matrix_power(A,i).dot(B)

print("Rank(P) = ", np.linalg.matrix_rank(P))

Rank(P) = 6
https://gist.github.com/rmcsqrd/a93e322ac92fd5089ca6aba1487ff30c 9/22
11/20/24, 11:33 PM Project1.ipynb

As shown in the above code, rank(P ) = 6 (P is full rank). This implies that R is 6

completely reachable given the plant dynamics. Given any arbitrary initial condition
x(t 0 ), the plant can reach any arbitrary final condition x(t 1 ), ∀x(t 0 ), x(t 1 ) ∈ R
6

given a finite time interval [t 0

, t1 ] .

Find an Orthonormal Basis Set

To find an orthonormal basis for this completely reachable subspace, perform the
Graham-Schmidt procedure on P by finding the QR decomposition of P . Note that
QR decomposition gives a basis set of n orthogonal unit vectors and is thus an
orthonormal basis set.

In [9]:
Q, R = np.linalg.qr(P)
with np.printoptions(precision=3):
print("Q matrix (orthonormal basis set) = \n")
print(Q)

Q matrix (orthonormal basis set) =

[[ 0.000e+00 -2.359e-16 -2.498e-16 -1.054e-01 1.025e-01 -9.891e-01]

[ 1.036e-01 -7.014e-01 -7.052e-01 -6.765e-17 -3.469e-17 3.469e-16]
[-0.000e+00 -0.000e+00 0.000e+00 9.944e-01 1.086e-02 -1.049e-01]
[-0.000e+00 7.090e-01 -7.052e-01 -1.041e-16 8.674e-19 6.939e-18]
[-0.000e+00 -0.000e+00 -0.000e+00 0.000e+00 -9.947e-01 -1.030e-01]
[-9.946e-01 -7.307e-02 -7.346e-02 -1.388e-17 0.000e+00 0.000e+00]]

Determine Minimum Energy Control

After finding the orthonormal basis set of P and determining that P is completely
reachable, move to finding how much energy is required to transition between
states. To find this, start by finding the reachability gramian, G.
t1 T
A(t 0 −τ ) T A (t 0 −τ )
G(t 1 , t 0 ) = ∫ e BB e dτ
t0

To compute this, numerically integrate the above equation with a quick

substitution:
T
t 1 −t 0 A(σ) T A (σ)
G(t 1 , t 0 ) = ∫ e BB e dσ
0

Numerically integrate using the trapezoidal rule using pythons trapz() function.

In [10]:
# numerically integrate to find the reachability gramian G for a variety of

# initialize containers and simulation params

delt = 0.1
tmax = 5.1
trange = np.arange(delt,tmax, delt)
invGrange = np.zeros((trange.shape[0], B.shape[0], B.shape[0]))

# integrate according to this lambda function

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

# loop through the overall t range

for cnt, t in enumerate(trange):

https://gist.github.com/rmcsqrd/a93e322ac92fd5089ca6aba1487ff30c 10/22
11/20/24, 11:33 PM Project1.ipynb
# initialize integration stuff
delt_int = 0.05
tau_range = np.arange(0, t, delt_int)
integration_array = np.zeros((tau_range.shape[0], invGrange.shape[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

G = np.trapz(integration_array, axis=0, dx=delt_int)
invGrange[cnt, :, :] = np.linalg.inv(G)

The above code computed the reachability gramian for a range of [t 0, t1 ] intervals.
This is used to illustrate the energy required for a transition from some arbitrary
t0 → t1 for different intervals of time. To maintain some consistency, define
x(t 1 ) = 0 (zero vector) and x(t 0) as an orthonormal unit perturbation. Get this
–
orthonormal unit perturbation from the QR decomposition performed earlier on
the reachability matrix P .

Define ζ = e
A(t 0 −t 1
x(t 1 ) − x(t 0 ) which is an arbitrary vector in R . Using
n

x(t 1 ), x(t 0 ) previously defined ⟹ ζ = −x(t 0 ) . This ζ is the negative vector

found in the orthonormal basis set generated by the QR decomposition. To
evaluate the minimum energy, look at the L -norm of u: 2

2 T −1
||u|| = ζ G ζ
t1

The energy required using this L 2

norm in conjunction with the several
reachability gramians G calculated previously for a range of time intervals:

In [11]:
# compute the L2 norm of u for the reachability gramian at different t1-t0
# orthonormal unit perturbation

ures = np.zeros((Q.shape[0], trange.shape[0]))

for j in range(0, invGrange.shape[0]):
for i in range(0, Q.shape[0]):
uij = Q[:, i].T.dot(invGrange[j, :, :]).dot(Q[:,i])
ures[i, j] = uij

# plot these energies

pltutls.Energy_Plot(trange, delt, ures)

https://gist.github.com/rmcsqrd/a93e322ac92fd5089ca6aba1487ff30c 11/22
11/20/24, 11:33 PM Project1.ipynb

The above plot shows the required minimum-energy energy control to return to
equilibrium after a unit orthonormal perturbation. Each perturbation ζ comes from i

the QR decomposition of P ; the six subplots display the energy expenditure

required to respond to a perturbation in the span of vectors which form the basis
set for the reachable subspace of the system.

Defining δ t = t1 − t0 , recognize that ||u|| is inversely proportional to δ ; smaller

2
t

interval times δ result in much larger energy requirements to transition between

t

states.

Intuitively this makes sense: The quadcopter is given commands to transition back
to its initial condition after a unit perturbation. If given less time to complete this
transition, it must move more quickly which corresponds to a larger energy
expenditure. On the other hand, if given more time, it can take a more leisurely
approach and conserve energy.

To verify that the validity of the computed reachability gramian G, generate a

control history for the quadcopter and verify the response in the state basis. It can
be shown that the control input u(t) for a given system can be represented as:
T
T A (t 0 −t)
u(t) = B e r
–
−1
r = G ζ
– –

Develop the control history in the code below to evaluate the system response. Use
a time interval [0, 5s].

In [12]:
# set up time params
delt = 0.05
tmax = 5.1

# compute inverse G
# integrate between 0 and t
intrange = np.arange(0, tmax, delt)
integrand = lambda tau: expm(-A*tau).dot(B).dot(B.T).dot(expm(-A.T*tau))
https://gist.github.com/rmcsqrd/a93e322ac92fd5089ca6aba1487ff30c 12/22
11/20/24, 11:33 PM Project1.ipynb
integration_array = np.zeros((intrange.shape[0], A.shape[0], A.shape[1]))
for intcnt, tint in enumerate(intrange):
integration_array[intcnt, :, :] = integrand(tint)

# compute integral using trapezoid rule and store result

G = np.trapz(integration_array, axis=0, dx=delt)
invG = np.linalg.inv(G)

# get initial conditions

Q, R = np.linalg.qr(P)

# compute the control input at each max energy and simulate

simhist = np.zeros((Q.shape[0], C.shape[0], intrange.shape[0]))
MinEnergyUhist = np.zeros((Q.shape[0], B.shape[1], intrange.shape[0]))
model = control.StateSpace(A, B, C, D)

# loop through initial conditions and look at system response given reachab
for i in range(0, A.shape[0]):
zeta = Q[:, i]

for cnt, t in enumerate(intrange):

MinEnergyUhist[i, :, cnt] = B.T.dot(expm(A.T*-t)).dot(invG).dot(zet
simT, yout, xout = control.forced_response(model, intrange, MinEnergyU
simhist[i, :, :] = yout

# plot stuff
pltutls.Unit_Perturbation_Plot(intrange, simhist)

https://gist.github.com/rmcsqrd/a93e322ac92fd5089ca6aba1487ff30c 13/22
11/20/24, 11:33 PM Project1.ipynb

As seen in the plot above, the controllability gramian G causes the system to
behave as expected; after the initial unit orthonormal perturbation, the system
returns to equilibrium in the time period specified (5s). Because the control history
u(t) causes this behavior ⟹ computation of G is accurate.

Part 3: Pole Placement

Implications of Feedback Control on Plant
Posit that all plant modes can be changed by the application of feedback control.
Recalling from part 1 where the plant modes were defined as:

λi t ∗
x(t) = ∑ e α i (0)x
i i

In order for feedback control to impact all of the plant modes, the control input
needs to manifest itself in all of the α i (0) components. Verify that feedback control
does this by looking at how control is realized in the x basis then change this into
the basis of T (the eigenvectors).

Start by recognizing how control is added to the system:

ẋ = Ax + Bu

Look specifically at the Bu term:

0 0 0 0 0
⎡ ⎤ ⎡ ⎤
δ δ δ
− 0 0 T1 (T 3 − T 1 )
m m m
⎡ ⎤
0 0 0 0 T2 0
Bu ⟹ =
δ δ δ
0 0 − T3 (T 2 − T 4 )
m m m
⎣ ⎦
0 0 0 0 T4 0
γ γ γ γ γ
⎣ ⎦ ⎣ ⎦
(T 1 + T 2 + T 3 + T 4 )
m m m m m

From this, recognize that the control input capabilities within the basis of x are
limited to just the velocity states (ẋ, ẏ, ż). However, recall that in part one an
interplay was noted between the velocity terms and positional terms (x, y, z); a
change in velocity will always precipitate a change in position.

Demonstrate this by assuming some arbitrary combination of inputs T 1, T2 , T3 , T4

such that

T
Bu = [0 1 0 1 0 1]

Then, noting that Bu is currently in the basis of x, change the basis and express
Bu in the basis of T (the eigenvectors) such that:

−1
[Bu] = T Bu
T

In [13]:
Bu = np.array([[0],[1],[0],[1],[0],[1]])
BuT = np.linalg.inv(eigvects).dot(Bu)
https://gist.github.com/rmcsqrd/a93e322ac92fd5089ca6aba1487ff30c 14/22
11/20/24, 11:33 PM Project1.ipynb
print("Bu in the basis of T = \n", BuT, "\n")

Bu in the basis of T =
[[96.15384615]
[96.15904601]
[96.15384615]
[96.15904601]
[48.07692308]
[48.08732195]]

As expected, there is some degree of modal response in all plant modes. This
verifies the claim that all plant modes can be activated by control input via u.

Pole Placement
Now that the determination has been made that the state space is fully reachable
and all plant modes can be excited by control input, evaluate the different pole
placement options.

First note that in part 1 it was found that all eigenvalues of A are real:

λi t (r+i0)t rt λi t
e ⟹ e = e (cos(0) + isin(0)) ⟹ e

After verifying this using Euler's formula, it has been shown that there is no
oscillatory behavior in the systems dynamical response for a system with real
eigenvalues of A. Thus the poles can be placed anywhere in R without inducing
oscillatory motion.

In terms of the time constant for the decay rate of the naturally placed eigenvalues,
the system is slow in some modes and doesn't decay at all in others: λ 1,3,5 = 0

implies that the system does not decay at all in those modes. λ 2,4,6
have negative
eigenvalues which implies that the system decays to an equilibrium point for these
modes however it does so "slowly".

Evaluate the rate of decay using the concept of a time constant; what is the amount
of time for the system mode to decay to e −1
?

λt λτ −1
e → e = e

−1
τ =
λ

Using this, it can seen that the time constant for λ 2,4,6
is quite slow:

−1
τ 2,4 = ≈ 96s
−0.0104

−1
τ6 = ≈ 48s
−0.0208

This decay rate is very slow given the nature of a quadcopter system. The system
description does not specify performance requirements for the quadcopter,
however given the unstable nature of the system intuit that the realistic system
response should be as follows:

https://gist.github.com/rmcsqrd/a93e322ac92fd5089ca6aba1487ff30c 15/22
11/20/24, 11:33 PM Project1.ipynb
λ 2,4,6 (the eigenvalues generally associated with velocity states) should be on
the order of ~0.1s.
λ 1,3,5 (the eigenvalues generally associated with the position states) can
respond slower, but should still decay in ~1s.

Making an educated guess at the natural behavior of the system, place the poles in
such a way to preserve the ratios found in the natural, uncontrolled eigenvalues
such that:

τ 2,4 = 2τ 6

Using this concept of time constant, calculate the following eigenvalues:

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

τ 6 = 0.05s ⟹ λ 6 = −20

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

Part 4: State Feedback Control

Place Selected Poles and Find Eigen/Modal Spaces
Using the eigenvalues chosen in part 3, place the poles using Matlab's place()
function to generate the appropriate gain matrix K .

As an aside, it is worth noting that there were issues with Python's implementation
of pole placement. It is unclear if this is due to the nature of the system or
numerical/algorithmic differences between Python and Matlab. In this case, poles
were placed out of the loop via Matlab. The generated gain matrix K was then
pasted into the code.

In [14]:
# place the following poles
p = [-1, -10, -1, -10, -1, -20]

# I placed the above poles using the matlab function place

K = np.array([
[-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]
])

# confirm that the eigenvalues are placed where we want them

eigvalsCL, eigvectsCL = np.linalg.eig(A-B.dot(K))
with np.printoptions(precision=3):
print("Confirm that eigenvalues were placed correctly:\n",eigvalsCL, "

# quick sanity check that it also will properly place original eigenvalues
pog = [0, -0.0104, 0, -0.0104, 0, -0.0208]
Kog = control.place_varga(A, B, pog) # this placement works for the OG va
eigvalsog, eigvectsog = np.linalg.eig(A-B.dot(Kog))
with np.printoptions(precision=5):
print("Confirm that original eigenvalues were placed correctly:\n",eigv

Confirm that eigenvalues were placed correctly:

[ 20 10 10 1 1 1 ]
https://gist.github.com/rmcsqrd/a93e322ac92fd5089ca6aba1487ff30c 16/22
11/20/24, 11:33 PM Project1.ipynb
[-20. -10. -10. -1. -1. -1.]

Confirm that original eigenvalues were placed correctly:

[ 0. -0.0104 0. -0.0104 0. -0.0208]

In the above code a gain matrix K was found for closed loop control based on the
chosen pole placement. From this, find the closed loop eigenvectors and closed
loop modal spaces by doing an eigendecomposition of A − BK (this also done in
the above code). Define the eigenvector matrix as T and verify that it spans R by
∗ 6

showing that rank(T ∗

) = 6 to confirm. Also note that because all placed poles do
not have complex components, the eigenspace formed by each set of eigenvectors
is the same as the real modal space (part 1 explains the justification for this).

In [15]:
with np.printoptions(precision=3):
print("Closed loop eigenvectors T^* = \n", eigvectsCL, "\n")
print("Rank of T^ = *", np.linalg.matrix_rank(eigvectsCL))

Closed loop eigenvectors T^* =

[[ 7.242e-06 9.950e-02 -4.929e-11 -7.071e-01 3.668e-05 -1.001e-09]
[-1.448e-04 -9.950e-01 4.929e-10 7.071e-01 -3.668e-05 1.001e-09]
[-4.994e-02 1.443e-05 -7.166e-15 -3.659e-05 7.071e-01 -1.066e-09]
[ 9.988e-01 -1.443e-04 7.153e-14 3.659e-05 -7.071e-01 1.066e-09]
[ 1.384e-17 6.862e-11 9.950e-02 -1.011e-09 -1.111e-09 7.071e-01]
[-2.768e-16 -6.862e-10 -9.950e-01 1.011e-09 1.111e-09 -7.071e-01]]

Rank of T^ = * 6

Closed Loop System Response Simulation

Because P is full rank, this implies the system is fully reachable. Further, it can be
shown that if P is full rank, the matrix P (defined below) is also full rank and thus
∗

the system is reachable with this modification in system dynamics as well.

∗ 2 3 4
P = [B (A − BK)B (A − BK) B (A − BK) B (A − BK) B (A − BK

With this in mind, reuse the Q matrix from the QR decomposition of P as the
orthonormal unit perturbations. In order to leverage the poles that were placed
above, use the gain matrix K for control such that u = −Kx . Then:

ẋ = Ax + Bu

ẋ = Ax + B(−Kx)

= Ax − BKx

= (A − BK)x

Using this formulation of ẋ, simulate the closed loop system response to some unit
orthonormal perturbations.

In [16]:
# update simulation to include gain matrix
# set up simulation params
model = control.StateSpace(A-B.dot(K), B, C, D)
x0 = Q # perturb according to orthonormal unit perturbations from QR decom
u0 = 0 # no control right now
delt = 0.05 # this stuff has to match above because I am a lazy programmer
https://gist.github.com/rmcsqrd/a93e322ac92fd5089ca6aba1487ff30c 17/22
11/20/24, 11:33 PM Project1.ipynb
de t 0.05 t is stuff as to atc abo e because a a la y p og a e
Tin = np.arange(0, tmax, delt)

# set up result containers

resarray = np.zeros((x0.shape[0], C.shape[0], Tin.shape[0]))
ClosedLoopUHist = np.zeros((x0.shape[0], B.shape[1], Tin.shape[0]))
modalresponse = np.zeros((x0.shape[0], x0.shape[0], Tin.shape[0]))
eigvalsABK, eigvectsABK = np.linalg.eig(A-B.dot(K))
for cnt, col in enumerate(x0.T):
T, yout, xout = control.forced_response(model, Tin, u0, -x0[:, cnt])
ClosedLoopUHist[cnt] = -K.dot(xout)
resarray[cnt, :, :] = yout
modalresponse[cnt, :, :] = np.linalg.inv(eigvectsABK).dot(xout)

# plot stuff for closed loop response

pltutls.ABK_plot_y(Tin, resarray)

As seen in the above plots, the system responds quickly to the unit orthonormal
perturbation and returns to the equilibrium more quickly than the open loop
minimum energy approach from part 4. To demonstrate the differences, plot the
different control inputs for each control modality (T 1, T2 , T3 , T4 ) with respect to
the different initial conditions:

In [17]:
# plot comparison between controls
https://gist.github.com/rmcsqrd/a93e322ac92fd5089ca6aba1487ff30c 18/22
11/20/24, 11:33 PM Project1.ipynb
# plot comparison between controls
plotmax = 101
pltutls.ControlComparisonPlot(Tin[:plotmax], ClosedLoopUHist[:,:, :plotmax

As can be clearly seen in the plot above, the closed loop control signal u(t) is much
more significant than the open loop minimum energy u(t) - note that the orange
"OL" signals are not zero and do have some slope, just substantially less than the
"CL" signals.

This makes intuitive sense because the closed loop dynamics are based on a control
input u(t) CL
= −Kx where K is the gain matrix associated with the placed
eigenvalues. The eigenvalues were placed in such a way to elicit a "quick" modal
response which requires more energy than the "slow" response found in the
minimum-energy open loop control.

Further, the system's closed loop response can be evaluated by looking at the
response of the system in each one of the modal spaces of A − BK . In the
following code a quick sanity check is performed to make sure that the gain matrix
K corresponds to the proper eigenvalues that were placed. Then modal responses
to each unit orthonormal perturbations are plotted.

In [18]:
# plot comparison between controls
with np.printoptions(precision=3):
https://gist.github.com/rmcsqrd/a93e322ac92fd5089ca6aba1487ff30c 19/22
11/20/24, 11:33 PM Project1.ipynb
print("Eigvalues of A-BK = \n", eigvalsABK)
pltutls.ClosedLoopModalPlot(Tin, modalresponse)

Eigvalues of A-BK =
[-20. -10. -10. -1. -1. -1.]

In the above plots an intersection point dotted lines were superimposed on each
plot to represents the intersection of e −1
at t = 1s to verify that the modal
response reflects the placement of the eigenvalues. For the λ 2,4,6
it is clearly shown
that the plant mode decays in such a way that the time constant τ << 1s . For
λ 1,3,5 the plant mode decaus such that τ = 1s . This verifies that the plant is
behaving as expected based on the placed eigenvalues and corresponding gain
matrix K (reference part 3 for discussion of pole placement).

Part 5: Reference Input Tracking

Simulate Closed Loop System Response with Reference
Input Tracking
Consider the introduction of a reference input r which is tracked using the gain
matrix F such that:

ẋ = Ax + Bu

y = Cx

u Fr Kx
https://gist.github.com/rmcsqrd/a93e322ac92fd5089ca6aba1487ff30c 20/22
11/20/24, 11:33 PM Project1.ipynb
u = F r − Kx

ẋ = Ax + B(F r − Kx)

= Ax + BF r − BKx

= (A − BK)x + BF r

ẋ = (A − BK)x + BF r

y = Cx

The reference input r is the goal state of the system such that after some amount
of time t, the system should evolve such that y = r . To compute F , formulate it in
such a way that it resembles the DC gain. This yields F in the form

−1 −1
F = (C(−A + BK) B)

To solve this, first note the following information regarding matrix dimensions:

4
u ∈ R (1)

3
r ∈ R (2)

⟹ (3)

4×3
F ∈ R (4)

This implies that F is not invertible because F is not square. To get around this
issue, use the psuedoinverse of F .

In the following section of code, several different gain matrices K are selected:

a gain matrix for the original, unplaced eigenvalues of A

a gain matrix for the placed eigenvalues of A − BK
an arbitrarily scaled K with complex eigenvalues of K to illustrate oscillatory
motion

After selection of the gain matrices K , the reference tracking matrix F is calculated
using the psuedo-inverse described above. Then the system is simulated under a
unit step input in each of the outputs found in y (this results in perturbations in the
x, y, z portions of the state space).

In [19]:
# redefine here because jupyter runs on cell by cell basis

# compute F - multiline so easier to read

Krange = np.zeros((3, 4, 6))

Krange[0] = np.array(
[[-11.9920, -23.8662, 0.0007, 0.0007, 0.6251, 1.2371],
[0.0007, 0.0007, -12.0104, -23.8847, 0.6251, 1.2371],
[11.9920, 23.8662, -0.0007, -0.0007, 0.6251, 1.2371],
[-0.0007, -0.0007, 12.0104, 23.8847, 0.6251, 1.2371]]
)

# do not change order unless you change modal response stuff below
Krange[1] = np.array([
[-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]
https://gist.github.com/rmcsqrd/a93e322ac92fd5089ca6aba1487ff30c 21/22
11/20/24, 11:33 PM
[ , , , Project1.ipynb, , ]
])
Krange[2] = Krange[1]*0.02 # induce imaginary eigenvalues

ksamples = Krange.shape[0]
Frange = np.zeros((ksamples, 4, 3)) # magic numbers because I am lazy maso
for k in range(0, ksamples):
F = np.linalg.pinv(
C.dot(
np.linalg.inv(
-A+B.dot(Krange[k])
)
).dot(B)
)
Frange[k, :, :] = F

# compute uhist. We use conventional xdot = Ax+Bu, u=Fr-Kx form so it plays

# initialize model params

delt = 0.1
tmax = 40
trange = np.arange(0, tmax, delt)

# get initial conditions

# we have output for each possible inputs
leadup = 100
yhist = np zeros(((B dot(F)) shape[1] ksamples leadup+trange shape[0]))

https://gist.github.com/rmcsqrd/a93e322ac92fd5089ca6aba1487ff30c 22/22