def __get_flight_conditions(t):
data = Data(r'RefData.mat')
mat_data = data.get_mat().get_data()
time = mat_data['time']
rh_fu = mat_data['rh_engine_FU']
lh_fu = mat_data['lh_engine_FU']
alt = mat_data['Dadc1_alt']
for idx, t_i in enumerate(time):
if time[idx] < t <= time[idx+1]:
break
m = get_weight_at_t(t, time, rh_fu, lh_fu)/9.80665
ts_tool = TimeSeriesTool()
specific_t_mdat_vals = ts_tool.get_t_specific_mdat_values(t)
V = specific_t_mdat_vals['Dadc1_tas'][0]
h = alt[idx]
rho = ISA(h)[2]
mub = m / (rho * S * b)
muc = m / (rho * S * c)
CL = 2 * m / (rho * V ** 2 * S) # Lift coefficient [ ]
# Cmalpha = -0.5669172330105713
Cmalpha = -0.6405
return mub, muc, CL, Cmalpha, V
def get_cg(t):
#Weight & Balance Values of Aircraft for CoG Calculations
ZFW = 4852.17
ZFW_arm = 34586.06
init_fuel = 4050 * 0.453592
xdata = list(
reversed([
4100, 4000, 3900, 3800, 3700, 3600, 3500, 3400, 3300, 3200, 3100,
3000, 2900, 2800, 2700, 2600, 2500, 2400
]))
xdata = [x * 0.453592 for x in xdata]
ydata = list(
reversed([
7.2517, 7.25055, 7.24942, 7.2482, 7.2471, 7.246112, 7.2451, 7.2442,
7.2433, 7.2428, 7.2423, 7.2424, 7.2425, 7.24293, 7.2433, 7.244207,
7.2451, 7.24636
]))
#Time Data for CoG Shift
t_start = 2573
t_end = 2659
#CoG Shift Data
x_old = 7.315
x_new = 3.3
m_shift = 87
#Aircraft Geometry
x_LEMAC = 6.643624
MAC = 2.056892
#Idx Location of CoG Shift
ts_tool = TimeSeriesTool()
idx = ts_tool.get_mdat_tstep_list_idx_for_matching_pdat_tstep(t)
idx_start = ts_tool.get_mdat_tstep_list_idx_for_matching_pdat_tstep(
t_start)
idx_end = ts_tool.get_mdat_tstep_list_idx_for_matching_pdat_tstep(t_end)
#Get Fuel Use Data
data = Data('FlightData.mat')
rhfu = data.get_mat().get_data()['rh_engine_FU'][idx]
lhfu = data.get_mat().get_data()['lh_engine_FU'][idx]
fu = rhfu + lhfu
fuel = init_fuel - fu
print(ZFW + fuel)
x_fuel = linear_spline(fuel, xdata, ydata)
# Calculate CoG for Passenger Shift
if idx >= idx_start and idx <= idx_end:
x_cg = (ZFW_arm + fuel * x_fuel - m_shift *
(x_old - x_new)) / (ZFW + fuel)
x_cg_LEMAC = (x_cg - x_LEMAC) / MAC
else:
x_cg = (ZFW_arm + fuel * x_fuel) / (ZFW + fuel)
x_cg_LEMAC = (x_cg - x_LEMAC) / MAC
return [x_cg, x_cg_LEMAC]
def __init__(self, time_start, time_length, Cma, Cmde, maneuver_name):
self.time_start = time_start
self.time_length= time_length
self.time_end = self.time_start + self.time_length
self.Cma = Cma
self.Cmde = Cmde
self.ts_tool = TimeSeriesTool()
self.maneuver_name = maneuver_name
from data.Cit_par import *
from src.data_extraction.time_series_tool import TimeSeriesTool
from src.data_extraction.time_series_tool import TimeSeriesTool
from src.data_extraction.data_main import Data
from src.data_processing.get_weight import get_weight_at_t
from src.data_processing.aerodynamics import ISA
#These reference values are missing from Cit_par:
Cma = -0.5669
#Cmde = -1.1642
Cmde = -1.2312
#Import data for given time step
ts_tool = TimeSeriesTool()
def maneuver_vals(time_start, length, name):
t = list(range(time_start, time_start + length))
da = []
dr = []
phi = []
p = []
r = []
V = []
yawRate = []
beta = [0.0]
for time in t:
specific_t_mdat_vals = ts_tool.get_t_specific_mdat_values(time)
class SymmetricalStateSpace:
def __init__(self, time_start, time_length, Cma, Cmde, maneuver_name):
self.time_start = time_start
self.time_length= time_length
self.time_end = self.time_start + self.time_length
self.Cma = Cma
self.Cmde = Cmde
self.ts_tool = TimeSeriesTool()
self.maneuver_name = maneuver_name
# self.data_tool = Data()
def maneuver_vals(self, t, dt):
t = list(range(t, t + dt))
de = []
aoa = []
pitch = []
q = []
V = []
u = []
w = []
for time in t:
specific_t_mdat_vals = self.ts_tool.get_t_specific_mdat_values(time)
de.append(specific_t_mdat_vals['delta_e'][0])
aoa.append(specific_t_mdat_vals['vane_AOA'][0])
pitch.append(specific_t_mdat_vals['Ahrs1_Pitch'][0])
V.append(specific_t_mdat_vals['Dadc1_tas'][0])
q.append(specific_t_mdat_vals['Ahrs1_bPitchRate'][0])
u.append(
specific_t_mdat_vals['Dadc1_tas'][0] * math.cos(specific_t_mdat_vals['vane_AOA'][0] * np.pi / 180.0))
w.append(
specific_t_mdat_vals['Dadc1_tas'][0] * math.sin(specific_t_mdat_vals['vane_AOA'][0] * np.pi / 180.0))
t = np.asarray(t)
aoa = np.asarray(aoa)
pitch = np.asarray(pitch)
q = np.asarray(q)
V = np.asarray(V)
u = np.asarray(u)
w = np.asarray(w)
u = u - u[0]
w = w - w[0]
# Initial conditions
x0 = np.array([[0.0], [0.0], [pitch[0]], [q[0]]])
return t, de, aoa, pitch, q, x0, u, V[0], w
def get_flight_conditions(self, t):
data = Data(r'FlightData.mat')
mat_data = data.get_mat().get_data()
time = mat_data['time']
rh_fu = mat_data['rh_engine_FU']
lh_fu = mat_data['lh_engine_FU']
alt = mat_data['Dadc1_alt']
for idx, t_i in enumerate(time):
if time[idx] < t <= time[idx + 1]:
break
m = get_weight_at_t(t, time, rh_fu, lh_fu) / 9.80665
h = alt[idx]
rho = ISA(h)[2]
mub = m / (rho * S * b)
muc = m / (rho * S * c)
return mub, muc, m, h, rho
def L2error(self, x_exact: np.array, x_numerical: np.array):
error = 0.0
if (x_exact.shape[0] != x_numerical.shape[0]):
print("Vectors must be of the same size!")
return 1
for i in range(x_exact.shape[0]):
error += (x_numerical[i] - x_exact[i]) * (x_numerical[i] - x_exact[i])
error = math.sqrt(error) / x_exact.shape[0]
return error
def create_ss_rep_of_sym_eom(self):
# Cmq = -14.32232691
forced_response_inputs = self.maneuver_vals(self.time_start, self.time_length)
g = 9.80665
V0 = forced_response_inputs[7]
# short = maneuver_vals(2760, 50)
mub, muc, m, h, rho = self.get_flight_conditions(self.time_start)
mub = float(mub[0])
muc = float(muc[0])
m = float(m[0])
h = float(h[0])
rho = float(rho[0])
CX0 = m * g * sin(forced_response_inputs[3][0] * np.pi / 180.0) / (0.5 * rho * (V0 ** 2) * S)
CZ0 = -m * g * cos(forced_response_inputs[3][0] * np.pi / 180.0) / (0.5 * rho * (V0 ** 2) * S)
c = 2.0569
C1 = np.array([[-2.0 * muc * c / V0, 0.0, 0.0, 0.0],
[0.0, (CZadot - 2.0 * muc) * c / V0, 0.0, 0.0],
[0.0, 0.0, -c / V0, 0.0],
[0.0, Cmadot * c / V0, 0.0, -2.0 * muc * KY2 * c / V0]])
C2 = np.array([[CXu, CXa, CZ0, CXq],
[CZu, CZa, -CX0, (CZq + 2.0 * muc)],
[0.0, 0.0, 0.0, 1.0],
[Cmu, Cma, 0.0, Cmq]])
C3 = np.array([[CXde],
[CZde],
[0.0],
[self.Cmde]])
# x' = A*x + B*u
# y = C*x + D*u
A = -np.matmul(np.linalg.inv(C1), C2)
B = -np.matmul(np.linalg.inv(C1), C3)
C = np.identity(4) # y = x, meaning we output the state
D = np.array([[0.0], [0.0], [0.0], [0.0]])
# Make control.ss state-space
system = control.ss(A, B, C, D)
t, y, x = control.forced_response(system, forced_response_inputs[0], forced_response_inputs[1], forced_response_inputs[5], transpose=False)
# Change dimensionless qc/V to q
y[2, :] = y[2, :] + forced_response_inputs[3][0]
y[3, :] = forced_response_inputs[7] * y[3, :] / c
# y[1, 0] = phugoid[2][0]
y[3, 0] = 0.0
fig = plt.figure(figsize=(12, 9))
# fig = plt.figure(figsize=(10, 12))
# fig.suptitle(self.maneuver_name, fontsize=16)
ax1 = fig.add_subplot(221)
ax1.plot(t, y[0, :], label="Simulation Response") # simulation response curve plot
ax1.plot(t, forced_response_inputs[6], label="Test Flight Data Response") # test flight data response curve plot
plt.legend(loc="best")
ax1.set_xlabel("Time [s]")
ax1.set_ylabel("u (x-dir. disturbance in velocity) [m/s]")
ax1.grid()
print("{0} Error in u:".format(self.maneuver_name))
eigen_motion_error_u = self.L2error(forced_response_inputs[6], y[0, :])
print(eigen_motion_error_u)
ax2 = fig.add_subplot(222)
ax2.plot(t, y[1, :], label="Simulation Response")
# alpha
ax2.plot(t, forced_response_inputs[8], label="Test Flight Data Response")
plt.legend(loc="best")
ax2.set_xlabel("Time [s]")
ax2.set_ylabel("w (z-dir. disturbance in velocity) [m/s]")
ax2.grid()
print("{0} Error in w:".format(self.maneuver_name))
eigen_motion_error_w = self.L2error(forced_response_inputs[8], y[1, :])
print(eigen_motion_error_w)
ax3 = fig.add_subplot(223)
ax3.plot(t, y[2, :], label="Simulation Response")
# theta
ax3.plot(t, forced_response_inputs[3], label="Test Flight Data Response")
plt.legend(loc="best")
ax3.set_xlabel("Time [s]")
ax3.set_ylabel("Theta (Pitch Angle) [deg]")
ax3.grid()
print("{0} Error in Theta:".format(self.maneuver_name))
eigen_motion_error_th = self.L2error(forced_response_inputs[3], y[2, :])
print(eigen_motion_error_th)
ax4 = fig.add_subplot(224)
ax4.plot(t, y[3, :], label="Simulation Response")
# q
ax4.plot(t, forced_response_inputs[4], label="Test Flight Data Response")
plt.legend(loc="best")
ax4.set_xlabel("Time [s]")
ax4.set_ylabel("q (Pitch Rate) [deg/s]")
ax4.grid()
print("{0} Error in q:".format(self.maneuver_name))
eigen_motion_error_q = self.L2error(forced_response_inputs[4], y[3, :])
print(eigen_motion_error_q)
print("Avg. {0} Error:".format(self.maneuver_name))
avg_eigen_motion_error = (eigen_motion_error_q + eigen_motion_error_th + eigen_motion_error_w + eigen_motion_error_u) / 4
print(avg_eigen_motion_error)
# fig.suptitle(self.maneuver_name, fontsize=16)
fig.suptitle(self.maneuver_name)
# plt.tight_layout()
# plt.legend(loc="best")
plt.show()
def compute_ss_rep_of_sym_eom(self, array_input):
forced_response_inputs = self.maneuver_vals(int(array_input[1]), int(array_input[2]))
g = 9.80665
V0 = forced_response_inputs[7]
# short = maneuver_vals(2760, 50)
mub, muc, m, h, rho = self.get_flight_conditions(int(array_input[1]))
mub = float(mub[0])
muc = float(muc[0])
m = float(m[0])
h = float(h[0])
rho = float(rho[0])
CX0 = m * g * sin(forced_response_inputs[3][0] * np.pi / 180.0) / (0.5 * rho * (V0 ** 2) * S)
CZ0 = -m * g * cos(forced_response_inputs[3][0] * np.pi / 180.0) / (0.5 * rho * (V0 ** 2) * S)
c = 2.0569
C1 = np.array([[-2.0 * muc * c / V0, 0.0, 0.0, 0.0],
[0.0, (CZadot - 2.0 * muc) * c / V0, 0.0, 0.0],
[0.0, 0.0, -c / V0, 0.0],
[0.0, Cmadot * c / V0, 0.0, -2.0 * muc * KY2 * c / V0]])
C2 = np.array([[CXu, CXa, CZ0, CXq],
[CZu, CZa, -CX0, (CZq + 2.0 * muc)],
[0.0, 0.0, 0.0, 1.0],
[Cmu, Cma, 0.0, array_input[0]]])
C3 = np.array([[CXde],
[CZde],
[0.0],
[self.Cmde]])
# x' = A*x + B*u
# y = C*x + D*u
A = -np.matmul(np.linalg.inv(C1), C2)
B = -np.matmul(np.linalg.inv(C1), C3)
C = np.identity(4) # y = x, meaning we output the state
D = np.array([[0.0], [0.0], [0.0], [0.0]])
# Make control.ss state-space
system = control.ss(A, B, C, D)
t, y, x = control.forced_response(system, forced_response_inputs[0], forced_response_inputs[1], forced_response_inputs[5], transpose=False)
# Change dimensionless qc/V to q
y[2, :] = y[2, :] + forced_response_inputs[3][0]
y[3, :] = forced_response_inputs[7] * y[3, :] / c
# y[1, 0] = phugoid[2][0]
y[3, 0] = 0.0
eigen_motion_error_u = self.L2error(forced_response_inputs[6], y[0, :])
eigen_motion_error_w = self.L2error(forced_response_inputs[8], y[1, :])
eigen_motion_error_th = self.L2error(forced_response_inputs[3], y[2, :])
eigen_motion_error_q = self.L2error(forced_response_inputs[4], y[3, :])
avg_eigen_motion_error = (eigen_motion_error_q + eigen_motion_error_th + eigen_motion_error_w + eigen_motion_error_u) / 4
# eigen_motion_error_u = self.L2error(forced_response_inputs[6], y[0, :])
# eigen_motion_error_u_array = np.array([eigen_motion_error_u])
# return eigen_motion_error_u
return avg_eigen_motion_error
class AsymmetricStateSpace:
def __init__(self, time_start, time_length, Cma, Cmde, maneuver_name):
self.time_start = time_start
self.time_length= time_length
self.time_end = self.time_start + self.time_length
self.Cma = Cma
self.Cmde = Cmde
self.ts_tool = TimeSeriesTool()
self.maneuver_name = maneuver_name
# self.data_tool = Data()
def maneuver_vals(self):
t = list(range(self.time_start, self.time_start + self.time_length))
da = []
dr = []
phi = []
p = []
r = []
V = []
yawRate = []
beta = [0.0]
for time in t:
specific_t_mdat_vals = self.ts_tool.get_t_specific_mdat_values(time)
da.append(specific_t_mdat_vals['delta_a'][0])
dr.append(specific_t_mdat_vals['delta_r'][0])
phi.append(specific_t_mdat_vals['Ahrs1_Roll'][0])
p.append(specific_t_mdat_vals['Ahrs1_bRollRate'][0])
r.append(specific_t_mdat_vals['Ahrs1_bYawRate'][0])
V.append(specific_t_mdat_vals['Dadc1_tas'][0])
yawRate.append(specific_t_mdat_vals['Ahrs1_bYawRate'][0])
for i in range(len(yawRate) - 1):
beta.append(beta[i] + yawRate[i])
t = np.asarray(t)
phi = np.asarray(phi)
p = np.asarray(p)
r = np.asarray(r)
da = np.asarray(da)
dr = np.asarray(dr)
V = np.asarray(V)
beta = np.asarray(beta)
x0 = np.array([[0.0], # beta
[phi[0]], # phi
[p[0]], # p
[r[0]]]) # r
return t, phi, p, r, da, dr, x0, V[0], beta
def get_flight_conditions(self):
data = Data(r'FlightData.mat')
mat_data = data.get_mat().get_data()
time = mat_data['time']
rh_fu = mat_data['rh_engine_FU']
lh_fu = mat_data['lh_engine_FU']
alt = mat_data['Dadc1_alt']
for idx, t_i in enumerate(time):
if time[idx] < self.time_start <= time[idx + 1]:
break
m = get_weight_at_t(self.time_start, time, rh_fu, lh_fu) / 9.80665
h = alt[idx]
rho = ISA(h)[2]
mub = m / (rho * S * b)
muc = m / (rho * S * c)
return mub, muc, m, h, rho
def L2error(self, x_exact: np.array, x_numerical: np.array):
error = 0.0
if (x_exact.shape[0] != x_numerical.shape[0]):
print("Vectors must be of the same size!")
return 1
for i in range(x_exact.shape[0]):
error += (x_numerical[i] - x_exact[i]) * (x_numerical[i] - x_exact[i])
error = math.sqrt(error) / x_exact.shape[0]
return error
def create_assymetric_ss_representation(self):
forced_response_inputs = self.maneuver_vals()
g = 9.80665
# DUTCH ROLL:
# State-space representation of asymmetric EOM:
# C1*x' + C2*x + C3*u = 0
V0 = forced_response_inputs[7]
mub, muc, m, h, rho = self.get_flight_conditions()
mub = float(mub[0])
muc = float(muc[0])
m = float(m[0])
h = float(h[0])
rho = float(rho[0])
CX0 = m * g * sin(forced_response_inputs[3][0] * np.pi / 180.0) / (0.5 * rho * (V0 ** 2) * S)
CZ0 = -m * g * cos(forced_response_inputs[3][0] * np.pi / 180.0) / (0.5 * rho * (V0 ** 2) * S)
c = 2.0569
C1 = np.array([[(CYbdot - 2.0 * mub) * b / V0, 0.0, 0.0, 0.0],
[0.0, -0.5 * b / V0, 0.0, 0.0],
[0.0, 0.0, -4.0 * mub * KX2 * b / V0, 4.0 * mub * KXZ * b / V0],
[Cnbdot * b / V0, 0.0, 4.0 * mub * KXZ * b / V0, -4.0 * mub * KZ2 * b / V0]])
C2 = np.array([[-CYb, -CL, -CYp, -(CYr - 4.0 * mub)],
[0.0, 0.0, -1.0, 0.0],
[-Clb, 0.0, -Clp, -Clr],
[-Cnb, 0.0, -Cnp, -Cnr]])
C3 = np.array([[-CYda, -CYdr],
[0.0, 0.0],
[-Clda, -Cldr],
[-Cnda, -Cndr]])
# x' = A*x + B*u
# y = C*x + D*u
# now u = [da]
# [dr]
A = np.matmul(np.linalg.inv(C1), C2)
B = np.matmul(np.linalg.inv(C1), C3)
C = np.identity(4) # y = x, meaning we output the state
D = np.array([[0.0, 0.0], [0.0, 0.0], [0.0, 0.0], [0.0, 0.0]])
# Make control.ss state-space
system = control.ss(A, B, C, D)
# t = np.linspace(0.0, 200.0, num = 201)
# Input:
# u = np.zeros((2, t.shape[0]))
# for i in range(t.shape[0]):
# u[0, i] = da[i] #Insert magnitude of "da" (aileron deflection) or comment out if none
# u[1, i] = dr[i] #Insert magnitude of "dr" (rudder deflection) or comment out if none
u = np.array([forced_response_inputs[4],
forced_response_inputs[5]])
# Calculate response to arbitrary input
t, y, x = control.forced_response(system, forced_response_inputs[0], u, forced_response_inputs[6], transpose=False)
# Change dimensionless pb/(2V) and rb/(2V) to p and r
y[1, :] = -y[1, :] + 2 * y[1, 0]
y[2, :] = -(2 * abs(V0) * y[2, :] / b) + 2 * y[2, 0]
y[3, :] = -(2 * abs(V0) * y[3, :] / b) + 2 * y[3, 0]
control.damp(system, doprint=True)
fig = plt.figure(figsize=(12, 9))
# fig.suptitle(self.maneuver_name, fontsize=16, )
ax1 = fig.add_subplot(221)
ax1.plot(t, y[0, :], label='Simulated response')
ax1.plot(t, forced_response_inputs[8], label='Test Flight Data Response', color='tab:orange', linestyle='--')
ax1.legend(loc='upper right')
ax1.set_xlabel("Time [s]")
ax1.set_ylabel("beta (yaw angle) [deg]")
print("{0} Error in Beta:".format(self.maneuver_name))
eigen_motion_error_beta = self.L2error(forced_response_inputs[8], y[0, :])
print(eigen_motion_error_beta)
plt.grid("True")
ax2 = fig.add_subplot(222)
ax2.plot(t, y[1, :], label='Simulated response')
ax2.plot(t, forced_response_inputs[1], label='Test Flight Data Response', color='tab:orange', linestyle='--')
ax2.legend(loc='upper right')
ax2.set_xlabel("Time [s]")
ax2.set_ylabel("phi (roll angle) [deg]")
print("{0} Error in Phi:".format(self.maneuver_name))
eigen_motion_error_phi = self.L2error(forced_response_inputs[1], y[1, :])
print(eigen_motion_error_phi)
plt.grid("True")
ax3 = fig.add_subplot(223)
ax3.plot(t, y[2, :], label='Simulated response')
ax3.plot(t, forced_response_inputs[2], label='Test Flight Data Response', color='tab:orange', linestyle='--')
ax3.legend(loc='upper right')
ax3.set_xlabel("Time [s]")
ax3.set_ylabel("p (roll rate) [deg/sec]")
print("{0} Error in p:".format(self.maneuver_name))
eigen_motion_error_p = self.L2error(forced_response_inputs[2], y[2, :])
print(eigen_motion_error_p)
plt.grid("True")
ax4 = fig.add_subplot(224)
ax4.plot(t, y[3, :], label='Simulated response')
ax4.plot(t, forced_response_inputs[3], label='Test Flight Data Response', color='tab:orange', linestyle='--')
ax4.legend(loc='upper right')
ax4.set_xlabel("Time [s]")
ax4.set_ylabel("r (yaw rate) [deg/s]")
print("{0} Error in r:".format(self.maneuver_name))
eigen_motion_error_r = self.L2error(forced_response_inputs[3], y[3, :])
print(eigen_motion_error_r)
plt.grid("True")
print("Avg. {0} Error:".format(self.maneuver_name))
avg_eigen_motion_error = (eigen_motion_error_beta + eigen_motion_error_p + eigen_motion_error_phi + eigen_motion_error_r) / 4.0
print(avg_eigen_motion_error)
# plt.grid("True")
plt.show()
def compute_assymetric_ss_representation_error(self, stability_coeffs_array):
"""
:param stability_coeffs_array: [Cnb, Cnr, CYb]
:return:
"""
forced_response_inputs = self.maneuver_vals()
# spiral = maneuver_vals(3305, 30, 'Spiral')
# aperiodic = maneuver_vals(3380, 30, 'Aperiodic')
g = 9.80665
# State-space representation of asymmetric EOM:
V0 = forced_response_inputs[7]
mub, muc, m, h, rho = self.get_flight_conditions()
mub = float(mub[0])
muc = float(muc[0])
m = float(m[0])
h = float(h[0])
rho = float(rho[0])
CX0 = m * g * sin(forced_response_inputs[3][0] * np.pi / 180.0) / (0.5 * rho * (V0 ** 2) * S)
CZ0 = -m * g * cos(forced_response_inputs[3][0] * np.pi / 180.0) / (0.5 * rho * (V0 ** 2) * S)
c = 2.0569
C1 = np.array([[(CYbdot - 2.0 * mub) * b / V0, 0.0, 0.0, 0.0],
[0.0, -0.5 * b / V0, 0.0, 0.0],
[0.0, 0.0, -4.0 * mub * KX2 * b / V0, 4.0 * mub * KXZ * b / V0],
[Cnbdot * b / V0, 0.0, 4.0 * mub * KXZ * b / V0, -4.0 * mub * KZ2 * b / V0]])
C2 = np.array([[-stability_coeffs_array[2], -CL, -CYp, -(CYr - 4.0 * mub)],
[0.0, 0.0, -1.0, 0.0],
[-Clb, 0.0, -Clp, -Clr],
[-stability_coeffs_array[0], 0.0, -Cnp, -stability_coeffs_array[1]]])
C3 = np.array([[-CYda, -CYdr],
[0.0, 0.0],
[-Clda, -Cldr],
[-Cnda, -Cndr]])
A = np.matmul(np.linalg.inv(C1), C2)
B = np.matmul(np.linalg.inv(C1), C3)
C = np.identity(4) # y = x, meaning we output the state
D = np.array([[0.0, 0.0], [0.0, 0.0], [0.0, 0.0], [0.0, 0.0]])
# Make control.ss state-space
system = control.ss(A, B, C, D)
# t = np.linspace(0.0, 200.0, num = 201)
# Input:
# u = np.zeros((2, t.shape[0]))
# for i in range(t.shape[0]):
# u[0, i] = da[i] #Insert magnitude of "da" (aileron deflection) or comment out if none
# u[1, i] = dr[i] #Insert magnitude of "dr" (rudder deflection) or comment out if none
u = np.array([forced_response_inputs[4],
forced_response_inputs[5]])
# Calculate response to arbitrary input
t, y, x = control.forced_response(system, forced_response_inputs[0], u, forced_response_inputs[6], transpose=False)
# Change dimensionless pb/(2V) and rb/(2V) to p and r
y[1, :] = -y[1, :] + 2 * y[1, 0]
y[2, :] = -(2 * abs(V0) * y[2, :] / b) + 2 * y[2, 0]
y[3, :] = -(2 * abs(V0) * y[3, :] / b) + 2 * y[3, 0]
control.damp(system, doprint=True)
eigen_motion_error_beta = self.L2error(forced_response_inputs[8], y[0, :])
eigen_motion_error_phi = self.L2error(forced_response_inputs[1], y[1, :])
eigen_motion_error_p = self.L2error(forced_response_inputs[2], y[2, :])
eigen_motion_error_r = self.L2error(forced_response_inputs[3], y[3, :])
avg_eigen_motion_error = (eigen_motion_error_beta + eigen_motion_error_p + eigen_motion_error_phi + eigen_motion_error_r) / 4.0
print("The Average Error Computed for the {0} is: {1}".format(self.maneuver_name, avg_eigen_motion_error))
return avg_eigen_motion_error