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()
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