def plotSpiralFlight():
t_0 = 3640.
# Start, equillibrium of elevator defl during maneuver.
defl_start, defl_equi, defl_peak = -0.7, -0.66, -0.35
# Begin/End peak.
p_0, p_1 = 9., 14.
# Set the time and input array for the simulation.
t_end, dt = 70., 0.1
t = sta(0., t_end + dt, dt)
ip = stepInput(np.ones((len(t), 2)), t, defl_start, defl_equi, defl_peak,
p_0, p_1)
timelist = list(dr.x49)
# Index of the starting condition.
index = timelist.index(t_0)
# Starting conditions
beta_0, phi_0, p_0, r_0 = 0., dr.x22[index], dr.x27[index], dr.x29[index]
p_0, r_0 = p_0 * float(cp.b) / (2. * float(cp.V0)), r_0 * float(
cp.b) / (2. * float(cp.V0))
x_0 = np.array([beta_0, phi_0, p_0, r_0])
# Simulate response.
Y, T, X = c.lsim(ss, U=ip, T=t, X0=x_0)
for i in Y:
i[2] *= (2. * float(cp.V0)) / float(cp.b)
i[3] *= (2. * float(cp.V0)) / float(cp.b)
t1, ph = df.find(t_0, t_0 + t_end, dr.x49, dr.x22)
t2, p = df.find(t_0, t_0 + t_end, dr.x49, dr.x27)
t3, r = df.find(t_0, t_0 + t_end, dr.x49, dr.x29)
t4, dangle = df.find(t_0, t_0 + t_end, dr.x49, dr.x17)
# Plotting.
plt.rc("figure", facecolor="white")
plt.figure(0)
plt.subplot(211)
plt.plot(np.array(t1) - t_0,
ph,
label="Flight data",
linestyle='--',
c='b')
plt.plot(T, -1. * Y[:, 1], label="Numerical data", linestyle='-', c='g')
plt.ylabel(r"Roll angle [deg]")
plt.xlabel("Time [s]")
plt.xlim(0, 50)
plt.legend(loc=3)
#plt.title('Roll angle, for the actual flight and numerical simulation')
plt.grid(True)
plt.figure(1)
plt.subplot(212)
plt.plot(np.array(t4) - t_0,
dangle,
label="Flight data",
linestyle='--',
c='b')
plt.plot(T, ip[:, 0], label="Numerical data", c='g')
plt.legend(loc=1)
plt.ylabel(r"Aileron deflection angle [deg]")
plt.xlabel("Time [s]")
plt.xlim(0, 50)
plt.grid(True)
#plt.title('Aileron deflection during the spiral motion, for the actual flight and numerical simulation')
plt.show()
return
def fitness_calc(self, Gp, Time, Input):
# Compute location of zeros
(self.Z1,
self.Z2) = np.roots([1, 2 * self.DP1 * self.WN1, self.WN1**2])
(self.Z3,
self.Z4) = np.roots([1, 2 * self.DP2 * self.WN2, self.WN2**2])
# Create PI controller
(self.Gc_num, self.Gc_den) = zpk2tf(
[self.Z1, self.Z2, self.Z3, self.Z4], [0],
self.K) # Controller with one pole in origin and 2 pair of zeros
self.Gc = ctrl.tf(self.Gc_num, self.Gc_den)
# Evaluate closed loop stability
self.gm, self.pm, self.Wcg, self.Wcp = ctrl.margin(self.Gc * Gp)
# Dischard solutions with no gain margin
if self.gm == None or self.gm <= 1:
self.fitness = 999
return
# Closed loop system
self.M = ctrl.feedback(self.Gc * Gp, 1)
# Closed loop step response
self.y, self.t, self.xout = ctrl.lsim(self.M, Input, Time)
# Evaluate fitness
self.fitness = evaluate(Input, self.y)
def stepResponse(t, x):
inp = np.zeros((len(t)))
for index in range(len(t)):
if t[index] < 10.0:
inp[index] = 1.
steps, T, X = c.lsim(ss, U=inp, T=t, X0=x)
return steps, T, X
def simulate_block(self,u,init,T0,len_block,dt):
T = np.arange(T0,T0+len_block+dt,dt)
MA = coeff.matrixA()
MB = coeff.matrixB()
MC = np.eye(len(MA))
MD = np.zeros(MB.shape)
ssS=control.ss(MA,MB,MC,MD)
yout,T,xout = control.lsim(ssS,u,T,init)
def my_td_model(C, unknown_params):
"""This will be the underlying model file to be used with a
similar cost function to be passed to optimize.fmin. The list of
unknown_params will be passed to optimize.fmin as the first arg in
args. This list will be used with C to build the dictionary of
parameters that are different from the defaults."""
sys = get_sys_from_params(C, unknown_params)
y, to, xo = control.lsim(sys, u_fit, t_fit)
return y[:, 0], y[:, 1]
def plotAssymetricRoll():
t_0 = 3080.
da_equi, da_peak, da_start = -0.1, -2.0, -1.3
p_0, p_1 = 1.4, 27.
t_end, dt = 30., 0.1
t = sta(0., t_end, dt)
ip = np.ones(((len(t), 2)))
for i in range(len(t)):
if t[i] <= p_0:
ip[i, 0] = da_start
elif p_0 < t[i] and t[i] <= p_1:
ip[i, 0] = da_peak
else:
ip[i, 0] = da_equi
timelist = list(dr.x49)
index = timelist.index(t_0)
beta_0, phi_0, p_0, r_0 = 0., dr.x22[index], dr.x27[index], dr.x29[index]
p_0 *= float(cp.b) / (2. * float(cp.V0))
r_0 *= float(cp.b) / (2. * float(cp.V0))
x_0 = np.array([beta_0, phi_0, p_0, r_0])
# Simulate response.
Y, T, X = c.lsim(ss, U=ip, T=t, X0=x_0)
for i in Y:
i[2] *= float(cp.V0) / float(cp.b)
i[3] *= (2. * float(cp.V0)) / float(cp.b)
t1, ph = df.find(t_0, t_0 + t_end, dr.x49, dr.x22)
t2, p = df.find(t_0, t_0 + t_end, dr.x49, dr.x27)
t3, r = df.find(t_0, t_0 + t_end, dr.x49, dr.x29)
t4, dangle = df.find(t_0, t_0 + t_end, dr.x49, dr.x17)
plt.rc("figure", facecolor="white")
plt.figure(0)
plt.subplot(211)
plt.plot(np.array(t2) - t_0, p, label="Flight data", linestyle='--', c='b')
plt.plot(T, Y[:, 2], label="Numerical data", linestyle='-', c='g')
plt.ylabel("Roll rate [deg/s]")
plt.xlabel("Time [s]")
plt.ylim(-8, 10)
plt.legend(loc=3)
#plt.title('Roll rate during the a-periodic roll, for the actual flight and numerical simulation')
plt.grid(True)
plt.figure(1)
plt.subplot(212)
plt.plot(np.array(t4) - t_0,
dangle,
label="Flight data",
linestyle='--',
c='b')
plt.plot(T, ip[:, 0], label="Numerical data", c='g')
plt.legend(loc=2)
plt.ylabel("Aileron deflection angle [deg]")
plt.ylim(-3, 1)
plt.xlabel("Time [s]")
plt.grid(True)
#plt.title('Aileron deflection during the a-periodic roll, for the actual flight and numerical simulation')
plt.show()
return
def plotPhugoid():
v_0,t_0 = 95.8,3220.
da_equi,da_peak1,da_peak2 = -0.56,-1.45,-0.9
p_0,p_1,p_2 = 4.0,8.0,16.0
t_end,dt = 150.,0.1
t = sta(0.,t_end,dt)
ip = stepInput(da_equi*np.ones((len(t))),t,p_0,p_1,p_2,da_peak1,da_peak2)
timelist = list(dr.x49)
# Index for starting condition.
index = timelist.index(t_0)
u_0,a_0 = 0.,dr.alpha[index],
th_0,q_0 = dr.x23[index],dr.x28[index]*float(cp.c)/v_0
x_0 = np.array([u_0,a_0,th_0,q_0])
# Simulate response.
Y,T,X = c.lsim(ss,U=ip,T=t,X0=x_0)
for i in Y:
i[0] += v_0
i[3] *= v_0/float(cp.c)
t1, V = df.find(t_0,t_0+t_end,dr.x49,dr.x43)
t2, a = df.find(t_0,t_0+t_end,dr.x49,dr.alpha)
t2, th = df.find(t_0,t_0+t_end,dr.x49,dr.x23)
t3, q = df.find(t_0,t_0+t_end,dr.x49,dr.x28)
t4, dangle = df.find(t_0,t_0+t_end,dr.x49,dr.x18)
# Plots.
plt.rc("figure", facecolor="white")
# First Plot.
plt.figure(0)
plt.subplot(211)
plt.plot(np.array(t1)-t_0,np.array(V)*kts, label="Flight data", linestyle='--',c='b')
plt.plot(T,Y[:,0], label="Numerical data",linestyle='-',c='g')
plt.ylabel("Velocity [m/s]")
plt.xlabel("Time [s]")
plt.ylim(0.,275.)
plt.xlim(0.,150.)
plt.legend(loc=1)
# plt.title('Velocity during the phugoid motion, for the actual flight and numerical simulation')
plt.grid(True)
# Second plot.
plt.figure(1)
plt.subplot(211)
plt.plot(np.array(t4)-t_0,dangle, label="Flight data", linestyle='--',c='b')
plt.plot(T,ip,label="Numerical data",c='g')
plt.legend(loc=4)
plt.ylabel("Elevator deflection [deg]")
plt.xlabel("Time [s]")
plt.grid(True)
#plt.title('Elevator deflection during the phugoid motion, for the actual flight and numerical simulation')
plt.show()
return
def __init__(self, Gp, Time, Input):
while (True):
# Try random parameter values
self.DP1 = np.random.uniform(0, DAMPING_MAX)
self.WN1 = np.random.uniform(0, WN_MAX)
self.DP2 = np.random.uniform(0, DAMPING_MAX)
self.WN2 = np.random.uniform(0, WN_MAX)
# Compute location of zeros
(self.Z1,
self.Z2) = np.roots([1, 2 * self.DP1 * self.WN1, self.WN1**2])
(self.Z3,
self.Z4) = np.roots([1, 2 * self.DP2 * self.WN2, self.WN2**2])
# Create test PI controller (used to calculate the maximum K for closed loop stable)
(self.Gc_num, self.Gc_den) = zpk2tf(
[self.Z1, self.Z2, self.Z3, self.Z4], [0],
1) # Controller with one pole in origin and 2 pair of zeros
self.Gc = ctrl.tf(self.Gc_num, self.Gc_den)
# Evaluate closed loop stability
self.gm, self.pm, self.Wcg, self.Wcp = ctrl.margin(self.Gc * Gp)
# Dischard solutions with no gain margin
if self.Wcg == None or (self.Wcp != None and self.Wcg >= self.Wcp):
continue
if self.gm == None: # None = inf
self.gm = K_MAX
# If K < gm => closed loop stable (gm > 0dB)
self.K = np.random.uniform(0, self.gm)
# Create PI controller for closed loop stable system
(self.Gc_num, self.Gc_den) = zpk2tf(
[self.Z1, self.Z2, self.Z3, self.Z4], [0], self.K
) # Controller with one pole in origin and 2 pair of zeros
self.Gc = ctrl.tf(self.Gc_num, self.Gc_den)
# Closed loop system
self.M = ctrl.feedback(self.Gc * Gp, 1)
# Closed loop step response
self.y, self.t, self.xout = ctrl.lsim(self.M, Input, Time)
# Evaluate fitness
self.fitness = evaluate(Input, self.y)
break
def simulate_block(self,init,T0,len_block,dt):
T = np.arange(T0-dt,T0+len_block,dt)
u = np.zeros((len(T)))
if self.diag:
print "Time segment: "+str(T[0])+" to "+str(T[-1])
self.V0,self.y0,self.R0,self.q0,self.a0 = init
self.update()
MA = self.MA(self.a0,self.M0)
MB = self.MB()
MC = np.eye(len(MA))
MD = np.zeros(MB.shape)
ssS=control.ss(MA,MB,MC,MD)
if self.diag:
print MA
print ssS
yout,T,xout = control.lsim(ssS,u,T,init)
fix1=yout[1:]
self.T_sim=T[-1]
return fix1[:int(len_block/dt)]
def runsampletime(self, sampletime, action, q_peishi, set_action):
self.n_ctrl = action[0][0]
self.p_relief = action[0][1]
#self.q_motor = q_peishi
self.q_motor = np.reshape(q_peishi, [
1,
])
self.n_set = set_action[0]
self.T_set = set_action[1]
# 不管采样时间多少，系统计算间隔为0.001
for i in range(int(sampletime / 0.001)):
self.total_T_OLD = self.total_T
self.n_real_OLD = self.n_real
self.T_elec = self.Elec_Torque(self.n_ctrl, self.n_real)
self.p_sys = self.p_relief if self.q_motor * self.eff_motor(
self.p_sys) > self.q_test / self.eff_test(self.p_sys) else 0
self.T_test = (
0.5 / np.pi) * self.p_sys * self.q_test * self.effmec_test(
self.p_sys)
self.T_motor = (
0.5 / np.pi) * self.p_sys * self.q_motor / self.effmec_motor(
self.p_sys)
# 用于加速的总扭矩
self.total_T = self.T_elec * self.eff_elec + self.T_test - self.T_motor
t = np.array([0, 0.001])
U = np.array([self.total_T_OLD, self.total_T])
y = cl.lsim(self.trans_elec, U, t, X0=self.n_real_OLD)
self.n_real = y[0][1]
self.Q_test = self.q_test * self.n_real * self.eff_test(self.p_sys)
self.reward = -(np.square(
(self.n_set - self.n_real) / self.N_MAX) + np.square(
(self.T_set - self.T_test) / self.T_MAX))
self.done = np.array([True])
self.scope = [self.n_real, self.T_test]
return [
self.observation, self.reward, self.done, self.info, self.scope
]
propInc=1*np.pi/180
propArm=4
co._steady_conditions(V0,rho0,CD,CL,alpha0)
co._aircraft_properties(b,c,A,S,e,m,Ixx,Iyy,Izz,xcg,xac,propInc,propArm)
co._tail(tail)
co._mainWing(main)
co._canard(canard)
co.deriv()
CZu=0
Cma=0
Cmu=0
co.delta_long(Cma=Cma,CZu=CZu,Cmu=Cmu)
ssS=co.stateSpace(symmetric=True)
T=np.arange(0,100,0.1)
u=np.zeros((len(T),2))
upgust = 0.#15. #m/s
alpha = np.tan(upgust/V0)
#u[1][0]=alpha
frontgust = 10. #m/s
du = frontgust/V0
#u[0][0]=du
# uhat, alpha, theta, q*c/V]
init=[du,alpha,0,0]
print init[1]*180/np.pi
yout,T,xout=control.lsim(ssS,u,T,init)
plot_impulse_symmetric_SS(co,yout,T)
Time = np.linspace(0, tfin, npts) # #INPUT# Usim = np.zeros((4, npts)) Usim_noise = np.zeros((4, npts)) Usim[0, :] = fset.PRBS_seq(npts, 0.03, [-0.33, 0.1]) Usim[1, :] = fset.PRBS_seq(npts, 0.03) Usim[2, :] = fset.PRBS_seq(npts, 0.03, [2.3, 5.7]) Usim[3, :] = fset.PRBS_seq(npts, 0.03, [8., 11.5]) #Adding noise err_inputH = np.zeros((4, npts)) err_inputH = fset.white_noise_var(npts, var_list) err_outputH = np.ones((3, npts)) err_outputH[0, :], Time, Xsim = cnt.lsim(H_sample1, err_inputH[0, :], Time) err_outputH[1, :], Time, Xsim = cnt.lsim(H_sample2, err_inputH[1, :], Time) err_outputH[2, :], Time, Xsim = cnt.lsim(H_sample3, err_inputH[2, :], Time) #OUTPUTS Yout = np.zeros((3, npts)) Yout11, Time, Xsim = cnt.lsim(g_sample11, Usim[0, :], Time) Yout12, Time, Xsim = cnt.lsim(g_sample12, Usim[1, :], Time) Yout13, Time, Xsim = cnt.lsim(g_sample13, Usim[2, :], Time) Yout14, Time, Xsim = cnt.lsim(g_sample14, Usim[3, :], Time) Yout21, Time, Xsim = cnt.lsim(g_sample21, Usim[0, :], Time) Yout22, Time, Xsim = cnt.lsim(g_sample22, Usim[1, :], Time) Yout23, Time, Xsim = cnt.lsim(g_sample23, Usim[2, :], Time) Yout24, Time, Xsim = cnt.lsim(g_sample24, Usim[3, :], Time) Yout31, Time, Xsim = cnt.lsim(g_sample31, Usim[0, :], Time)
t = sta(t_0, t_end, dt)
#Initial conditions.
b_0, vp_0, p_0, r_0 = 0., 0., 0., 0
x_0 = np.array([b_0, vp_0, p_0, r_0])
ip, flag = np.zeros((len(t), 2)), 0.
flag = 0. # 1 for aileron, else rudder
for i in range(len(t)):
if t[i] < 5.0:
if flag == 1:
ip[i, 0] = 1.
ip[i, 1] = 0.
else:
ip[i, 0] = 0.
ip[i, 1] = 1.
# Simulate step response.
Y, T, X = c.lsim(ss, U=ip, T=t, X0=x_0)
for i in Y:
i[2] *= 2. * float(cp.V0) / float(cp.b)
i[3] *= 2. * float(cp.V0) / float(cp.b)
# Plots.
plt.figure(0)
ax = []
plt.rc("figure", facecolor="white")
x_0, y_0 = 0.09, -0.025
# Plots the yaw angle
plt.figure(0)
ax.append(plt.subplot(211))
# plt.title(r"$\beta$ and $\varphi$ under a aileron step input")
# plt.title(r"$\beta$ and $\varphi$ under a rudder step input")
plt.plot(T, Y[:, 0], c='b', label="Velocity")
# Time domain check
sys = utils.get_sys_from_params(dict2.values(), dict2.keys())
sys3 = utils.get_sys_from_params(dict3.values(), dict3.keys())
td_file = txt_data_processing.Data_File('OL_pulse_test_sys_check_SLFR_RTP_OL_Test_uend=0.txt')
t = td_file.t
u = td_file.u
do_time_domain_plot = 1
if do_time_domain_plot:
td_file.Time_Plot(labels=['u','theta','a'], fignum=3)
y, to, xo = control.lsim(sys, u, t)
y3, to3, xo3 = control.lsim(sys3, u, t)
figure(3)
plot(t, y[:,0], label='model $\\theta$')
plot(t, y[:,1], label='model $\\ddot{x}_{tip}$')
plot(t, y3[:,0], label='DT-TMM $\\theta$')
plot(t, y3[:,1], label='DT-TMM $\\ddot{x}_{tip}$')
figure(3)
legend(loc=8)
show()
Time = np.linspace(0, tfin, npts) # #INPUT# Usim = np.zeros((4, npts)) Usim_noise = np.zeros((4, npts)) Usim[0, :] = fset.PRBS_seq(npts, 0.03, [-0.33, 0.1]) Usim[1, :] = fset.PRBS_seq(npts, 0.03) Usim[2, :] = fset.PRBS_seq(npts, 0.03, [2.3, 5.7]) Usim[3, :] = fset.PRBS_seq(npts, 0.03, [8., 11.5]) #Adding noise err_inputH = np.zeros((4, npts)) err_inputH = fset.white_noise_var(npts, var_list) err_outputH = np.ones((3, npts)) err_outputH[0, :], Time, Xsim = cnt.lsim(H_sample1, err_inputH[0, :], Time) err_outputH[1, :], Time, Xsim = cnt.lsim(H_sample2, err_inputH[1, :], Time) err_outputH[2, :], Time, Xsim = cnt.lsim(H_sample3, err_inputH[2, :], Time) #OUTPUTS Yout = np.zeros((3, npts)) Yout11, Time, Xsim = cnt.lsim(g_sample11, Usim[0, :], Time) Yout12, Time, Xsim = cnt.lsim(g_sample12, Usim[1, :], Time) Yout13, Time, Xsim = cnt.lsim(g_sample13, Usim[2, :], Time) Yout14, Time, Xsim = cnt.lsim(g_sample14, Usim[3, :], Time) Yout21, Time, Xsim = cnt.lsim(g_sample21, Usim[0, :], Time) Yout22, Time, Xsim = cnt.lsim(g_sample22, Usim[1, :], Time) Yout23, Time, Xsim = cnt.lsim(g_sample23, Usim[2, :], Time) Yout24, Time, Xsim = cnt.lsim(g_sample24, Usim[3, :], Time) Yout31, Time, Xsim = cnt.lsim(g_sample31, Usim[0, :], Time)
def plotShortPeriod():
t_0, shift = 3148., 0.
t_0 += shift
t_end, dt = 20., 0.1
t = sta(0., t_end, dt)
ip = np.zeros((len(t)))
for i in range(len(t)):
if t[i] <= 0.9:
ip[i] = -0.47
else:
ip[i] = -1.25
# Index starting condition.
timelist = list(dr.x49)
index = timelist.index(t_0)
u_0, a_0, th_0 = 0., dr.alpha[index], dr.x23[index]
q_0 = dr.x28[index] * float(cp.c) / float(cp.V0)
x_0 = np.array([u_0, a_0, th_0, q_0])
# Simulate response.
Y, T, X = c.lsim(ss, U=ip, T=t, X0=x_0)
for i in Y:
i[0] += float(cp.V0)
i[3] *= float(cp.V0) / float(cp.c)
t1, V = df.find(t_0, t_0 + t_end, dr.x49, dr.x43 * kts)
t2, a = df.find(t_0, t_0 + t_end, dr.x49, dr.alpha)
t3, th = df.find(t_0, t_0 + t_end, dr.x49, dr.x23)
t4, q = df.find(t_0, t_0 + t_end, dr.x49, dr.x28)
t5, dangle = df.find(t_0, t_0 + t_end, dr.x49, dr.x18)
# Plot 1.
plt.rc("figure", facecolor="white")
plt.figure(0)
plt.subplot(211)
plt.plot(np.array(t4) - t_0,
np.array(q),
label="Flight data",
linestyle='--',
c='b')
plt.plot(T - 1.5,
Y[:, 3] / (2. * pi),
label="Numerical data",
linestyle='-',
c='g')
plt.ylabel("Pitch rate [deg/s]")
plt.xlabel("Time [s]")
plt.xlim(0., 15.)
plt.legend(loc=3)
#plt.title('Pitch rate during the short-period motion, for the actual flight and numerical simulation')
plt.grid(True)
# Plot 2.
plt.figure(1)
plt.subplot(212)
plt.plot(np.array(t5) - t_0,
dangle,
label="Flight data",
linestyle='--',
c='b')
plt.plot(T, ip, label="Numerical data", c='g')
plt.legend(loc=1)
plt.ylabel("Elevator deflection angle [deg]")
plt.xlabel("Time [s]")
plt.xlim(0., 15.)
plt.grid(b=True)
#plt.title('Elevator deflection during the short-period motion, for the actual flight and numerical simulation')
plt.show()
return
sys_ig = get_sys_from_params(ig_vals, ig_keys)
# sys = get_sys_from_params(C_opt_all, ig_keys)
sys = get_sys_from_params(vals1, keys1)
myfile = txt_data_processing.Data_File("OL_pulse_test_sys_check_SLFR_RTP_OL_Test_uend=0.txt")
t = myfile.t
u = myfile.u
do_time_domain_plot = 0
if do_time_domain_plot:
myfile.Time_Plot(labels=["u", "theta", "a"], fignum=3)
y, to, xo = control.lsim(sys, u, t)
figure(3)
plot(t, y[:, 0], label="model $\\theta$")
plot(t, y[:, 1], label="model $\\ddot{x}_{tip}$")
figure(3)
legend(loc=8)
figure(1)
subplot(211)
legend(loc=3)
###################################
#
# Create Plant to be controlled
(Gp_num, Gp_den) = zpk2tf(PLANT_ZEROS, PLANT_POLES, PLANT_K)
Gp = ctrl.tf(Gp_num, Gp_den)
# Print transfer function of plant to be controlled
print('\nPLANT:')
print(Gp)
# Create time vector to be used
T = np.arange(START_TIME, STOP_TIME, STEP_TIME)
# Generate input signal
input_signal = create_input()
# Generate plant response to input signal
y1, t1, xout = ctrl.lsim(Gp, input_signal, T)
# Run evolution algorithm
Gc, K_best, DP1_best, WN1_best, DP2_best, WN2_best, M = evolution(
Gp, T, input_signal)
# Closed loop response to input signal
y2, t2, xout = ctrl.lsim(M, input_signal, T)
# Evaluate and print overshoot and rise time
print('Overshoot: %f %%' % overshoot(y2))
print('Rise time: %1.2f sec' % rise_time(y2))
print('MSE: %f' % mse(y2, input_signal))
# Fill database tables with current simulation data
fill_tables(K_best, DP1_best, WN1_best, DP2_best, WN2_best, overshoot(y2),
dt = 0.25
InputValue = 1.
#%%
# Continuous state-space matrices
A_cont = np.array([[0., 1.], [0., 0]])
B_cont = np.array([[0.], [1.]])
C_cont = np.array([[1., 0.], [0., 1.]])
D_cont = np.array([[0.], [0.]])
# Continuous system
SS_cont = ctrl.ss(A_cont, B_cont, C_cont, D_cont)
# Discrete system
SS_disc_nodelay = ctrl.sample_system(SS_cont, dt, method='tustin')
#%%
T = np.arange(0, 100, dt)
yout, T, xout = ctrl.lsim(SS_disc_nodelay,
U=np.ones(T.shape) * InputValue,
T=T)
#%%
fig = plt.figure()
plt.plot(T, yout[0, :], label='Position')
plt.plot(T, yout[1, :], label='Velocity')
plt.plot(T, yout[1, :] / yout[0, :], label='Divergence')
plt.axhline(y=dt, color='black', linestyle='--', label='dt')
plt.ylim((-100, 100))
plt.legend()
plt.grid()
plt.show()
duminput_delay = np.zeros((duminput.shape[0], NumDelayedElements[i]+1))
for k in range(NumDelayedElements[i]+1):
duminput_delay[k:,k] = np.roll(duminput, k)[k:]
idxbounds = (int((np.array(NumDelayedElements[:i])+1).sum()), int((np.array(NumDelayedElements[:i+1])+1).sum()))
Input[:,j,idxbounds[0]:idxbounds[1]] = duminput_delay
Input = Variable(torch.from_numpy(Input).float(), requires_grad=False)
Target = np.zeros((NumTimesteps, NumBatches, NumOutputs))
k = 0
while k < NumBatches :
dumX0 = (np.random.rand(2)-np.array([0.5, 0.5]))*np.array([10., 0]) + np.array([10., 0])
dumtarget,_,_ = ctrl.lsim(StateSpaceSystem, Input[:,k,0].data.numpy(), TimeVector, X0=dumX0)
Target[:,k,0] = dumtarget[0,:]
Target[:,k,1] = dumtarget[1,:]
k+= 1
Target = Target[:,:,[1]] / Target[:,:,[0]]
Target = Target[:,:,[0]]
#%%
NumEpochs = 2000
Target = Variable(torch.from_numpy(Target).float(), requires_grad=False)
OutputStart = net(Input, Target, NumEpochs, NumEpochs, 0)
def plotDutchRoll():
t_0, t_1 = 3478., 3525.
da_equi, da_start = -2., -1.9
da_peak1, da_peak2, da_peak3 = -10.0, 4.6, -6.2
p_0, p_1, p_2, p_3 = 2.5, 4, 5.5, 6.3
t_end, dt = 50., 0.1
t = sta(0., t_end, dt)
ip = stepInput(np.ones((len(t),2)),t,p_0,p_1,p_2,p_3,\
da_equi,da_start,da_peak1,da_peak2,da_peak3)
timelist = list(dr.x49)
index = timelist.index(t_0)
beta_0, phi_0, p_0, r_0 = 0., dr.x22[index], dr.x27[index], dr.x29[index]
p_0 *= float(cp.b) / (2. * float(cp.V0))
r_0 *= float(cp.b) / (2. * float(cp.V0))
x_0 = np.array([beta_0, phi_0, p_0, r_0])
# Simulate response.
Y, T, X = c.lsim(ss, U=ip, T=t, X0=x_0)
for i in Y:
i[2] *= (2. * float(cp.V0)) / float(cp.b)
i[3] *= (2. * float(cp.V0)) / float(cp.b)
t1, ph = df.find(t_0, t_0 + t_end, dr.x49, dr.x22)
t2, p = df.find(t_0, t_0 + t_end, dr.x49, dr.x27)
t3, r = df.find(t_0, t_0 + t_end, dr.x49, dr.x29)
t4, rudder = df.find(t_0, t_0 + t_end, dr.x49, dr.x19)
t5, r2 = df.find(t_1, t_1 + t_end, dr.x49, dr.x29)
t6, rudder2 = df.find(t_1, t_1 + t_end, dr.x49, dr.x19)
# Plots.
plt.rc("figure", facecolor="white")
plt.figure(0)
plt.subplot(211)
plt.plot(np.array(t3) - t_0, r, label="Flight data", linestyle='-', c='b')
plt.plot(np.array(t5) - t_1,
r2,
label="Flight data (YD)",
linestyle='-',
c='r')
plt.plot(T, -Y[:, 3], label="Numerical data", linestyle='-', c='g')
plt.ylabel(r"Roll rate [deg/s]")
plt.xlabel("Time [s]")
plt.ylim(-60., 25.)
plt.xlim(0, 30)
plt.legend(loc=4)
#plt.title('yaw rate during the dutch roll, for the actual flight and numerical simulation')
plt.grid(True)
plt.figure(1)
plt.subplot(212)
plt.plot(np.array(t4) - t_0,
rudder,
label="Flight data",
linestyle='-',
c='b')
plt.plot(np.array(t6) - t_1,
rudder2,
label="Flight data (YD)",
linestyle='-',
c='r')
plt.plot(T, ip[:, 1], label="Numerical data", c='g')
plt.legend(loc=4)
plt.ylim(-25, 6)
plt.xlim(0, 30)
plt.ylabel(r"Rudder deflectiong angle [deg]")
plt.xlabel("Time [s]")
plt.grid(b=True)
#plt.title('Rudder deflection during the dutch roll, for the actual flight and numerical simulation')
plt.show()
return
NUM_H = [1., 0.3, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.] #nc=1
DEN = [
1., -2.21, 1.7494, -0.584256, 0.0684029, 0., 0., 0., 0., 0., 0., 0., 0.,
0., 0.
] #na=4
NUM = [1., -2.07, 1.3146] #nb=3
nc = 1
na = 4
nb = 3
theta = 11
#Transfer functions
g_sample = cnt.tf(NUM, DEN, ts)
h_sample = cnt.tf(NUM_H, DEN, ts)
#Input responses
Y1, Time, Xsim = cnt.lsim(g_sample, Usim, Time)
e_t = fset.white_noise_var(Y1[0].size, [0.005])
e_t = e_t[0]
Y2, Time, Xsim = cnt.lsim(h_sample, e_t, Time)
Ytot = Y1 + Y2
#System identification from collected data
Id_sys=system_identification(Ytot,Usim,'ARMAX',IC='BIC',na_ord=[2,5],\
nb_ord=[1,5],nc_ord=[0,2],delays=[10,13],\
ARMAX_max_iterations=300)
#Output of the identified system
Y_id1, Time, Xsim = cnt.lsim(Id_sys.G, Usim, Time)
Y_hid1, Time, Xsim = cnt.lsim(Id_sys.H, e_t, Time)
Y_idTot = Y_id1 + Y_hid1