-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy paththing.jl
209 lines (170 loc) · 7.45 KB
/
thing.jl
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
using LinearAlgebra
using ControlSystems
using Plots
using Printf
#pgfplots()
plotly()
# constant definitions
ρ_∞ = 1.112
θ_0 = -2
V_∞ = 28
m = 318
S = 6.11
l_t = 4.63
I_yy = 432
S_t = 1.14
l_w = 0.3
C_Lαw = 5.55
C_Lαt = 4.3
C_Lδt = 0.45
b = 15
T_0 = 0
e = 0.8
g = 9.81
function getABCD(ρ_∞, θ_0, V_∞, m, S, l_t, I_yy, S_t, l_w, C_Lαw, C_Lαt, C_Lδt, b, T_0, e, g)
C_Lα = C_Lαw + S_t * C_Lαt / S
q_∞ = 0.5 * ρ_∞ * V_∞^2
# state matrix
A = [
[
2 * ( m * g * sind(θ_0) -T_0) / (m * V_∞)
g * cosd(θ_0) * (1 - 2 * S * C_Lα / (π * b^2 * e) ) / V_∞
0
-g * cosd(θ_0)
]';
[
-2 * g * cosd(θ_0) / V_∞
(m * g * sind(θ_0) - T_0) / (m * V_∞) - ρ_∞ * V_∞ * S * C_Lα / (2 * m)
V_∞ - ρ_∞ * V_∞ * S_t * l_t * C_Lαt / (2 * m)
-g * sind(θ_0)
]';
[
0
ρ_∞ * V_∞ * (S * l_w * C_Lαw - S_t * l_t * C_Lαt) / (2 * I_yy)
-ρ_∞ * V_∞ * S_t * l_t^2 * C_Lαt / (2 * I_yy)
0
]';
[
0
0
1
0
]'
]
# input matrix
B = [
0
-q_∞ * S_t * C_Lδt / m
-q_∞ * S_t * l_t * C_Lδt / I_yy
0
]
# sensory matrix
C = Matrix(I,4,4)
# direct term
D = 0
return A,B,C,D
end
# Question 1
A,B,C,D = getABCD(ρ_∞, θ_0, V_∞, m, S, l_t, I_yy, S_t, l_w, C_Lαw, C_Lαt, C_Lδt, b, T_0, e, g)
# eigenvalue plots
λ = eigvals(A)
scatter(λ, xlabel="Re(λ)", ylabel="Im(λ)", legend = false)
# Question 2
# state space
sys = ss(A,B,C,D)
stepplot(sys, 200, legend = false)
plot!(ylabel = ["V_x (m/s)" "V_z (m/s)" "ω_y (deg/s)" "θ (deg)"], xlabel = ["" "" "" "Time (s)"], title = ["Step Response" "" "" ""], size = [600,600])
phugω = imag(λ[4]) # phugiod frequency
@printf("The phugoid frequency is %f and the time period is %f", phugω, 2 * π / phugω)
# Question 3
λ_C_Lαw = [
eigvals(getABCD(ρ_∞, θ_0, V_∞, m, S, l_t, I_yy, S_t, l_w, C_Lαw*0.8, C_Lαt, C_Lδt, b, T_0, e, g)[1])';
eigvals(getABCD(ρ_∞, θ_0, V_∞, m, S, l_t, I_yy, S_t, l_w, C_Lαw*1.2, C_Lαt, C_Lδt, b, T_0, e, g)[1])'
]'
λ_C_Lαt = [
eigvals(getABCD(ρ_∞, θ_0, V_∞, m, S, l_t, I_yy, S_t, l_w, C_Lαw, C_Lαt*0.8, C_Lδt, b, T_0, e, g)[1])';
eigvals(getABCD(ρ_∞, θ_0, V_∞, m, S, l_t, I_yy, S_t, l_w, C_Lαw, C_Lαt*1.2, C_Lδt, b, T_0, e, g)[1])'
]'
λ_I_yy = [
eigvals(getABCD(ρ_∞, θ_0, V_∞, m, S, l_t, I_yy*0.8, S_t, l_w, C_Lαw, C_Lαt, C_Lδt, b, T_0, e, g)[1])';
eigvals(getABCD(ρ_∞, θ_0, V_∞, m, S, l_t, I_yy*1.2, S_t, l_w, C_Lαw, C_Lαt, C_Lδt, b, T_0, e, g)[1])'
]'
λ_V_∞ = [
eigvals(getABCD(ρ_∞, θ_0, V_∞*0.8, m, S, l_t, I_yy, S_t, l_w, C_Lαw, C_Lαt, C_Lδt, b, T_0, e, g)[1])';
eigvals(getABCD(ρ_∞, θ_0, V_∞*1.2, m, S, l_t, I_yy, S_t, l_w, C_Lαw, C_Lαt, C_Lδt, b, T_0, e, g)[1])'
]'
# pole plots
function q3polePlot(λ,λ_ref)
scatter([λ[:,1][1:2] λ[:,1][3:4]], xlabel="Re(λ)", ylabel="Im(λ)",markercolor = :red, layout = 2, title=["SPPO" "PHUGOID"], labels = ["80%" ""])
scatter!([λ[:,2][1:2] λ[:,2][3:4]], xlabel="Re(λ)", ylabel="Im(λ)",markercolor = :green, layout = 2, title=["SPPO" "PHUGOID"], labels = ["120%" ""])
scatter!([λ_ref[1:2] λ_ref[3:4] ], xlabel="Re(λ)", ylabel="Im(λ)",markercolor = :blue, layout = 2, title=["SPPO" "PHUGOID"], labels = ["Unperturbed" ""])
scatter!(size = [800,400])
end
q3polePlot(λ_C_Lαw, λ)
q3polePlot(λ_C_Lαt, λ)
q3polePlot(λ_I_yy, λ)
q3polePlot(λ_V_∞, λ)
# freq and damp plots
function q3fdPlot(λ, λ_ref, mcolor, mname)
λ = [λ λ_ref]
sppo_freq = [sqrt(real(λ[:,i][1])^2 + imag(λ[:,i][1])^2) for i = 1:3]
phug_freq = [sqrt(real(λ[:,i][3])^2 + imag(λ[:,i][3])^2) for i = 1:3]
sppo_damp = [abs(real(λ[:,i][1])) / sppo_freq[i] for i = 1:3]
phug_damp = [abs(real(λ[:,i][3])) / phug_freq[i] for i = 1:3]
scatter!([sppo_freq[1]/sppo_freq[3] phug_freq[1]/phug_freq[3]], [sppo_damp[1]/sppo_damp[3] phug_damp[1]/phug_damp[3]], xlabel="Natural frequency (rad/s)", ylabel="Damping ratio",markercolor = mcolor, layout = 2, title=["SPPO" "PHUGOID"], markershape = :cross, labels = ["80% "*mname ""])
scatter!([sppo_freq[2]/sppo_freq[3] phug_freq[2]/phug_freq[3]], [sppo_damp[2]/sppo_damp[3] phug_damp[2]/phug_damp[3]], xlabel="Natural frequency (rad/s)", ylabel="Damping ratio",markercolor = mcolor, layout = 2, title=["SPPO" "PHUGOID"], markershape = :xcross, labels = ["120% "*mname ""])
end
scatter([1 1], [1 1], xlabel="Natural frequency (rad/s)", ylabel="Damping ratio",markercolor = :black, layout = 2, title=["SPPO" "PHUGOID"], labels = ["Unperturbed" ""], size = [800,400])
q3fdPlot(λ_C_Lαw, λ, :blue, "C_Lαw")
q3fdPlot(λ_C_Lαt, λ, :red, "C_Lαt")
q3fdPlot(λ_I_yy, λ, :green, "I_yy")
q3fdPlot(λ_V_∞, λ, :pink, "V_∞")
# Question 4
l_β = (l_w + l_t) / 2 # position of spring
σ = 5:0.1:50
k_β = 0.5 * ρ_∞ * V_∞^2 * S_t * l_t * σ
A_s = -0.5 * ρ_∞ * V_∞ ^2 * S_t * l_β * C_Lαt
A_rs = ρ_∞ * V_∞^2 * S_t * C_Lαt / (2 * m) * [0,1,l_t * m / I_yy,0]
A_sr = 0.5 * ρ_∞ * V_∞ * S_t * l_β * C_Lαt * [0 1 l_t 0]
A_k = [A + A_rs * A_sr / (k_β[i] - A_s) for i = 1:length(k_β)] # state matrix with k spring
λ_k = map((x) -> eigvals(x), A_k)
kLength = length(λ_k)
# get modes
sppo_k = [λ_k[i][2] for i = 1:kLength]
phug_k = [λ_k[i][4] for i = 1:kLength]
sppo_freq = sqrt(real(λ[1])^2 + imag(λ[1])^2)
phug_freq = sqrt(real(λ[3])^2 + imag(λ[3])^2)
sppo_damp = abs(real(λ[1])) / sppo_freq
phug_damp = abs(real(λ[3])) / phug_freq
sppo_freq_k = [sqrt(real(λ_k[i][1])^2 + imag(λ_k[i][1])^2) for i = 1:kLength]
phug_freq_k = [sqrt(real(λ_k[i][3])^2 + imag(λ_k[i][3])^2) for i = 1:kLength]
sppo_damp_k = [abs(real(λ_k[i][1])) / sppo_freq_k[i] for i = 1:kLength]
phug_damp_k = [abs(real(λ_k[i][3])) / phug_freq_k[i] for i = 1:kLength]
# plot poles
scatter([sppo_k phug_k], xlabel="Re(λ)", ylabel="Im(λ)", layout = 2, legend = false, title=["SPPO" "PHUGOID"], size = [800,400], markercolor = :red)
# frequency and damping plots
scatter([sppo_freq_k/sppo_freq phug_freq_k/phug_freq], [sppo_damp_k/sppo_damp phug_damp_k/phug_damp], xlabel="Natural frequency (rad/s)", ylabel="Damping ratio",markercolor = :red, layout = 2, title=["SPPO" "PHUGOID"], legend = false, size = [800,400])
# Question 5a
Q = C
# starting value
R=10^( -3.2 + 0.2 * 1 )
K = lqr(sys, Q, R)
P = feedback(sys,K)
λ_closed = pole(P)
scatter([λ_closed[1:2] λ_closed[3:4]], xlabel="Re(λ)", ylabel="Im(λ)", layout = 2, markercolor = :red, labels = ["closed loop poles" ""], size = [800,400], title = ["SPPO" "PHUGOID"])
for i = 2:25
R=10^( -3.2 + 0.2 * i )
K = lqr(sys, Q, R)
P = feedback(sys,K)
λ_closed = pole(P)
scatter!([λ_closed[1:2] λ_closed[3:4]], xlabel="Re(λ)", ylabel="Im(λ)", layout = 2, markercolor = :red, labels = ["" ""])
end
scatter!([λ[1:2] λ[3:4]], xlabel="Re(λ)", ylabel="Im(λ)", layout = 2, markercolor = :blue, labels = ["open loop poles" ""], markershape = :cross)
# Question 5b
R = 1
Q = Diagonal([5,0,0,5])
K = lqr(sys, Q, R)
P = feedback(sys,K)
stepplot(P,200, legend = false)
plot!(ylabel = ["V_x (m/s)" "V_z (m/s)" "ω_y (deg/s)" "θ (deg)"], xlabel = ["" "" "" "Time (s)"], title = ["Step Response" "" "" ""], size = [600,600])
#png("Files/MechanicsOfFlightCoursework/q5bstep.png")