def Pole(W): con.pzmap(W) plt.plot() plt.grid(True) plt.show() print('Оценка по распределению корней:') Pol = con.pole(W) # print(Pol) P = [] """ Показатель колебательности характеризует склонность системы к колебаниям: чем выше М, тем менее качественна система при прочих равных условиях. Считается допустимым, если 1,1 < М < 1,5. """ degreeovershoot_M = [] for i in Pol: k = complex(i) if k.real != 0: P.append(k.real) m = k.imag / k.real degreeovershoot_M.append(m) a_min = max(P) t_reg = abs(1 / a_min) overshooting = math.exp(math.pi / max(degreeovershoot_M)) psi = 1 - math.exp((-2) * math.pi / max(degreeovershoot_M)) print("a_min= ", a_min) print("Время пп: ", t_reg, " c") print("Степень колебательности: ", max(degreeovershoot_M)) print("Перерегулирование: ", overshooting) print("Степень затухания: ", psi)
def testPoleZero(self, siso): """Call pole() and zero()""" pole(siso.ss1) pole(siso.tf1) pole(siso.tf2) zero(siso.ss1) zero(siso.tf1) zero(siso.tf2)
def rlocfind(sys, desired_zeta, kvectin=None): ''' Interactive gain selection from the root locus plot of the SISO system SYS. rlocfind lets you select a pole location in the graphics window on the root locus computed from SYS. The root locus gain associated with this point is returned as K and the system poles for this gain are returned as POLES. :param sys: [transfer function object] transfer function of open loop system :param desired_zeta: [float] desired damping coefficient value :param kvectin: [array-like] k vector of values for root locus determination, default = None Returns: :return: K: [float] gain at point clicked on root locus :return: POLES: [array-like] all (complex) pole locations for the gain chosen ''' rlist, klist, Gdict = root_locus(sys, kvect=kvectin, PrintGain=True) zetaline(desired_zeta) show() K = squeeze(Gdict["k"].real) POLES = pole(feedback(K * sys, 1)) return K, POLES
def select_order_GEN(id_method, y, u, tsample=1., na_ord=[0, 5], nb_ord=[1, 5], nc_ord=[0, 5], nd_ord=[0, 5], nf_ord=[0, 5], delays=[0, 5], method='AIC', max_iterations=200, st_m=1.0, st_c=False): # order ranges na_Min = min(na_ord) na_MAX = max(na_ord) + 1 nb_Min = min(nb_ord) nb_MAX = max(nb_ord) + 1 nc_Min = min(nc_ord) nc_MAX = max(nc_ord) + 1 nd_Min = min(nd_ord) nd_MAX = max(nd_ord) + 1 nf_Min = min(nf_ord) nf_MAX = max(nf_ord) + 1 theta_Min = min(delays) theta_Max = max(delays) + 1 # check orders sum_ords = np.sum(na_Min + na_MAX + nb_Min + nb_MAX + nc_Min + nc_MAX + nd_Min + nd_MAX + nf_Min + nf_MAX + theta_Min + theta_Max) if ((np.issubdtype(sum_ords, np.signedinteger) or np.issubdtype(sum_ords, np.unsignedinteger)) and na_Min >= 0 and nb_Min > 0 and nc_Min >= 0 and nd_Min >= 0 and nf_Min >= 0 and theta_Min >= 0) is False: sys.exit( "Error! na, nc, nd, nf, theta must be positive integers, nb must be strictly positive integer" ) # return 0.,0.,0.,0.,0.,0.,0.,np.inf elif y.size != u.size: sys.exit("Error! y and u must have tha same length") # return 0.,0.,0.,0.,0.,0.,0.,np.inf else: ystd, y = rescale(y) Ustd, u = rescale(u) IC_old = np.inf for i_a in range(na_Min, na_MAX): for i_b in range(nb_Min, nb_MAX): for i_c in range(nc_Min, nc_MAX): for i_d in range(nd_Min, nd_MAX): for i_f in range(nf_Min, nf_MAX): for i_t in range(theta_Min, theta_Max): useless1, useless2, useless3, useless4, Vn, y_id = GEN_id( id_method, y, u, i_a, i_b, i_c, i_d, i_f, i_t, max_iterations, st_m, st_c) IC = information_criterion( i_a + i_b + i_c + i_d + i_f, y.size - max(i_a, i_b + i_t, i_c, i_d, i_f), Vn * 2, method) # --> nota: non mi torna cosa scritto su ARMAX if IC < IC_old: na_min, nb_min, nc_min, nd_min, nf_min, theta_min = i_a, i_b, i_c, i_d, i_f, i_t IC_old = IC print("suggested orders are: Na=", na_min, "; Nb=", nb_min, "; Nc=", nc_min, "; Nd=", nd_min, "; Nf=", nf_min, "; Delay: ", theta_min) # rerun identification NUM, DEN, NUMH, DENH, Vn, y_id = GEN_id(id_method, y, u, na_min, nb_min, nc_min, nd_min, nf_min, theta_min, max_iterations, st_m, st_c) Y_id = np.atleast_2d(y_id) * ystd # rescale NUM coeff if id_method != 'ARMA': NUM[theta_min:nb_min + theta_min] = NUM[theta_min:nb_min + theta_min] * ystd / Ustd # FdT G = cnt.tf(NUM, DEN, tsample) H = cnt.tf(NUMH, DENH, tsample) check_st_H = np.zeros(1) if id_method == 'OE' else np.abs(cnt.pole(H)) if max(np.abs(cnt.pole(G))) > 1.0 or max(check_st_H) > 1.0: print("Warning: One of the identified system is not stable") if st_c is True: print( f"Infeasible solution: the stability constraint has been violated, since the maximum pole is {max(max(np.abs(cnt.pole(H))),max(np.abs(cnt.pole(G))))} \ ... against the imposed stability margin {st_m}") else: print( f"Consider activating the stability constraint. The maximum pole is {max(max(np.abs(cnt.pole(H))),max(np.abs(cnt.pole(G))))} " ) return na_min, nb_min, nc_min, nd_min, nf_min, theta_min, G, H, NUM, DEN, Vn, Y_id
import control.matlab as ctl import matplotlib.pyplot as plt import numpy as np num = np.array([1, 1]) den = np.array([3, 9, 0, 0]) GH = ctl.tf(num, den) print(GH) print("Zeros: ", ctl.zero(GH)) print("Plos: ", ctl.pole(GH)) rlist, klist = ctl.rlocus(GH, PrintGain=True, grid=True) plt.grid() print(plt.show())
def GEN_MIMO_id(id_method, y, u, na, nb, nc, nd, nf, theta, tsample, max_iterations, st_m, st_c): na = np.array(na) nb = np.array(nb) nc = np.array(nc) nd = np.array(nd) nf = np.array(nf) theta = np.array(theta) [ydim, ylength] = y.shape [udim, ulength] = u.shape [th1, th2] = theta.shape # check dimension sum_ords = np.sum(nb) + np.sum(na) + np.sum(nc) + np.sum(nd) + np.sum( nf) + np.sum(theta) if na.size != ydim: sys.exit( "Error! na must be a vector, whose length must be equal to y dimension" ) # return 0.,0.,0.,0.,0.,0.,np.inf elif nb[:, 0].size != ydim: sys.exit( "Error! nb must be a matrix, whose dimensions must be equal to yxu" ) # return 0.,0.,0.,0.,0.,0.,np.inf elif nc.size != ydim: sys.exit( "Error! nc must be a vector, whose length must be equal to y dimension" ) # return 0.,0.,0.,0.,0.,0.,np.inf elif nd.size != ydim: sys.exit( "Error! nd must be a vector, whose length must be equal to y dimension" ) # return 0.,0.,0.,0.,0.,0.,np.inf elif nf.size != ydim: sys.exit( "Error! nf must be a vector, whose length must be equal to y dimension" ) # return 0.,0.,0.,0.,0.,0.,np.inf elif th1 != ydim: sys.exit("Error! theta matrix must have yxu dimensions") # return 0.,0.,0.,0.,0.,0.,np.inf elif ((np.issubdtype(sum_ords, np.signedinteger) or np.issubdtype(sum_ords, np.unsignedinteger)) and np.min(nb) >= 0 and np.min(na) >= 0 and np.min(nc) >= 0 and np.min(nd) >= 0 and np.min(nf) >= 0 and np.min(theta) >= 0) == False: sys.exit( "Error! nf, nb, nc, nd, theta must contain only positive integer elements" ) # return 0.,0.,0.,0.,0.,0.,np.inf else: # preallocation Vn_tot = 0. NUMERATOR = [] DENOMINATOR = [] DENOMINATOR_H = [] NUMERATOR_H = [] Y_id = np.zeros((ydim, ylength)) # identification in MISO approach for i in range(ydim): DEN, NUM, NUMH, DENH, Vn, y_id, Reached_max = GEN_MISO_id( id_method, y[i, :], u, na[i], nb[i, :], nc[i], nd[i], nf[i], theta[i, :], max_iterations, st_m, st_c) if Reached_max == True: print("at ", (i + 1), "° output") print("-------------------------------------") # append values to vectors DENOMINATOR.append(DEN.tolist()) NUMERATOR.append(NUM.tolist()) NUMERATOR_H.append(NUMH.tolist()) DENOMINATOR_H.append(DENH.tolist()) #DENOMINATOR_H.append([DEN.tolist()[0]]) Vn_tot = Vn + Vn_tot Y_id[i, :] = y_id # FdT G = cnt.tf(NUMERATOR, DENOMINATOR, tsample) H = cnt.tf(NUMERATOR_H, DENOMINATOR_H, tsample) check_st_H = np.zeros(1) if id_method == 'OE' else np.abs(cnt.pole(H)) if max(np.abs(cnt.pole(G))) > 1.0 or max(check_st_H) > 1.0: print("Warning: One of the identified system is not stable") if st_c is True: print( f"Infeasible solution: the stability constraint has been violated, since the maximum pole is {max(max(np.abs(cnt.pole(H))),max(np.abs(cnt.pole(G))))} \ ... against the imposed stability margin {st_m}") return DENOMINATOR, NUMERATOR, DENOMINATOR_H, NUMERATOR_H, G, H, Vn_tot, Y_id
import control as con import control.matlab as ctl import numpy as np k = np.arange(1, 5.1, 0.1, dtype=float) Gc = ctl.tf([1, -2, 4], [1, 4, 2]) Hss = ctl.tf([1], [1]) for x in k: Ts = ctl.feedback(ctl.series(x, Gc), Hss, sign=-1) if ctl.pole(Ts)[0].real < 0: print("------- Sistema estável ------") print(f"Polos K = {round(x, 2)}", ctl.pole(Ts), " Sistema estável")
def get_PZ(TF): poles = mt.pole(TF) zeros = mt.zero(TF) return zeros, poles
''' A = np.array([[0, 1, 0], [0, 0, 1], [-30, -31, -10]]) B = np.array([[0], [0], [1]]) C = np.array([0, 0, 1]) D = np.array([[0]]) # Espaço de estados -> Funcao de transferencia S = ctl.ss(A, B, C, D) G = ctl.ss2tf(S) # Polos a partir do espaço de estados print('Polos = ', ctl.pole(S)) ''' ######################################################## Input (Funcao de transferencia) ######################################################## ''' # num = [0, 0, 1, 0] # den = [1, 6, 5, 1] # G = ctl.tf(num, den) # # Funcao de transferencia -> Espaço de estados # A, B, C, D = tf2ss(num, den)
plt.close("all") #% Parametros da funcao de transferencia K1 = 100 Km = 2.083 Kg = 0.1 a = 100 am = 1.71 #% #% Funcao de transferencia da posicao angular do sistema s = co.tf('s'); Gp = (K1*Km*Kg)/(s*(s+a)*(s+am)) #% #% Calculo dos polos da funcao de transferencia Gp print(co.pole(Gp)) #% #% Expansao em fracoes parciais #% caso nao haja multiplos polos: #% B(s) R(1) R(2) R(n) #% ---- = -------- + -------- + ... + -------- + K(s) #% A(s) s - P(1) s - P(2) s - P(n) #% B=K1*Km*Kg; A=[1,a+am,a*am,0] print(sympy.apart(Gp)) #% #% Plot dos polos e zeros de Gm #% co.pzmap(Gp)
# [0]] ## For example, the value [m, c, k] = [2, 0.5, 3]. m, c, k = (2, 0.5, 3) print("For example, \n m = %d, c = %d, k = %d. \n" % (m, c, k)) ## Write down the A matrix and B matrix. A = np.array( [[(-c / m), (-k / m)], [1, 0]] ) B = np.array( [[(1 / m), 0], [0, 1]] ) ## Take the outputs = states, so C is identity matrix. Set D to 0. C = np.identity(np.ndim(A)) D = np.zeros((np.shape(C)[0], np.shape(B)[1])) my_sys = cm.ss(A, B, C, D) print("""Assume the outputs are all states. My Spring-Mass system continuous time state-space representation: \n""", my_sys) ## The stability of my Spring-Mass system can be represented by the poles location poles = cm.pole(my_sys) print("The poles locations are: \n", poles) pole, zero = control.pzmap(my_sys) plt.show() print("The poles real parts are negative. Thus, my Spring-Mass system is stable.")
def main(t0, deltat, t, input_type, input_u, CZu, CXu, CZa, Cmq): """Input type: elevator rudder airleron""" # Find time idx = np.where(timelst == t0)[0] # Flight condition # m_fuel = 1197.484 # CHANGE Total fuel mass [kg] hp = altlst[idx] # CHANGE pressure altitude in the stationary flight condition [m] V = taslst[idx] # CHANGE true airspeed in the stationary flight condition [m/sec] alpha = np.radians(aoalst[idx]) # angle of attack in the stationary flight condition [rad] theta = np.radians(pitchlst[idx]) # pitch angle in the stationary flight condition [rad] gamma = theta - alpha # CHANGE flight path angle - # Aircraft mass m = mass(t0) # mass [kg] # Longitudinal stability Cma = -0.4435 # CHANGE longitudinal stabilty [ ] Cmde = -1.001 # CHANGE elevator effectiveness [ ] # air density [kg/m^3] rho = rho0 * pow(((1 + (lam * hp / Temp0))), (-((g / (lam * R)) + 1))) W = m * g # [N] (aircraft weight) # Aircraft inertia (depend on t0): muc = m / (rho * S * c) mub = m / (rho * S * b) KX2 = 0.019 KZ2 = 0.042 KXZ = 0.002 KY2 = 1.25 * 1.114 # Aerodynamic constants: Cmac = 0 # Moment coefficient about the aerodynamic centre [ ] CNwa = CLa # Wing normal force slope [1/rad] CNha = 2 * np.pi * Ah / (Ah + 2) # Stabiliser normal force slope [ ] depsda = 4 / (A + 2) # Downwash gradient [ ] # Lift and drag coefficient (depend on t0): CL = 2 * W / (rho * V ** 2 * S) # Lift coefficient [ ] CD = CD0 + (CLa * alpha) ** 2 / (np.pi * A * e) # Drag coefficient [ ] # Stabiblity derivatives CX0 = W * np.sin(theta) / (0.5 * rho * V ** 2 * S) # CXu = -0.095 #corrected CXa = +0.47966 # Positive! (has been erroneously negative since 1993) CXadot = +0.08330 CXq = -0.28170 CXde = -0.03728 CZ0 = -W * np.cos(theta) / (0.5 * rho * V ** 2 * S) # CZu = -0.37616 CZa = -5.74340 CZadot = -0.00350 CZq = -5.66290 CZde = -0.69612 Cmu = +0.06990 # positive! Cmadot = +0.17800 # positive! Cmq = -8.79415 #CYb = -0.7500 #CYbdot = 0 #CYp = -0.0304 #CYr = +0.8495 #CYda = -0.0400 #CYdr = +0.2300 #Clb = -0.10260 #Clp = -0.71085 #Clr = +0.23760 #Clda = -0.23088 #Cldr = +0.03440 #Cnb = +0.1348 #Cnbdot = 0 #Cnp = -0.0602 #Cnr = -0.2061 #Cnda = -0.0120 #Cndr = -0.0939 # c-matrix dimensions s1 = (4, 4) s2 = (4, 1) s3 = (4, 2) # Creating the different c-matrices (c1, c2 &c3) for symmetrical flight # c1 matrix c1 = np.zeros(s1) c1[0, 0] = -2 * muc * (c / V) c1[1, 1] = (CZadot - 2 * muc) * (c / V) c1[2, 2] = -(c / V) c1[3, 1] = Cmadot * (c / V) c1[3, 3] = -2 * muc * KY2 * ((c / V) ** 2) # c2 matrix c2 = np.zeros(s1) c2[0, 0] = -CXu c2[0, 1] = -CXa c2[0, 2] = -CZ0 c2[0, 3] = -CXq * (c / V) c2[1, 0] = -CZu c2[1, 1] = -CZa c2[1, 2] = -CX0 c2[1, 3] = -(CZq + 2 * muc) * (c / V) c2[2, 3] = -(c / V) c2[3, 0] = -Cmu c2[3, 1] = -Cma c2[3, 3] = -Cmq * (c / V) # c3 matrix c3 = np.zeros(s2) c3[0, 0] = -CXde c3[1, 0] = -CZde c3[3, 0] = -Cmde # Creating the different c-matrices (c4, c5 &c6) for asymmetrical flight # Time responses for unit steps: # t = np.linspace(t0,t0+ deltat, nsteps) -t0 u = input_u # print("u and t:",u,t,sep='\n') # print("u shape:",u.shape) # print("t shape:",t.shape) if t.shape != u.shape: print("Wrong slicing for input and time!\n") return -1 # Now, we distinct between inputs: if input_type == "elevator": #print("Calculating for elevator input...") # Symmetric system is triggered: # Creating the state matrix(A) and the input matrix(B) for symmetrical flight - xdot = c1^-1*c2*x c1^-1*c3*u = Ax + Bu A_s = np.dot(np.linalg.inv(c1), c2) B_s = np.dot(np.linalg.inv(c1), c3) C_s = np.identity(4) D_s = np.zeros((4, 1)) # System in state-space sys_s = cm.StateSpace(A_s, B_s, C_s, D_s) poles_s = cm.pole(sys_s) # print("Eigenvalues of the symmetric system: ", poles_s,sep='\n') #verified # Time responses for unit steps: yout, tout, uout = cm.lsim(sys_s, u, t) # general time response u_out_s = yout[:, 0] alpha_out_s = yout[:, 1] + alpha theta_out_s = yout[:, 2] + theta q_out_s = yout[:, 3] # Plotting.... # plotting(t,u_out_s,str("u Response for " +input_type+ " input, t0= "+ str(t0)),"u","m/s") # plotting(t,alpha_out_s,str("Alpha Response for " +input_type+ " input, t0= "+ str(t0)),r"$\alpha$","deg") # plotting(t,theta_out_s,str("Theta Response for " +input_type+ " input, t0= "+ str(t0)),r"$\theta$","deg") # plotting(t,q_out_s,str("q Response for " +input_type+ " input, t0= "+ str(t0)),"$q$",r"deg/s") # print("\tPlotted!") return theta_out_s, q_out_s, poles_s return 1
#Controlador PID else: H3 = Kp3 * (1 + (1 / (Ti3 * s)) + (Td3 * s / ((Td3 / N3) * (s) + 1))) Hw3 = Ktac * H3 #% #% Definicao da malha aberta #% Sao definidos 3 sistemas distintos #% GHw1 = Hw1 * Gw GHw2 = Hw2 * Gw GHw3 = Hw3 * Gw #% #% Polos e zeros de malha aberta #% print('Polos e zeros - GHw1') print(co.pole(GHw1)) print(co.zero(GHw1)) print('\nPolos e zeros - GHw2') print(co.pole(GHw2)) print(co.zero(GHw2)) print('\nPolos e zeros - GHw3') print(co.pole(GHw3)) print(co.zero(GHw3)) #% #% Plot com os polos e zeros de malha aberta #% co.pzmap(GHw1, title="GHw1") co.pzmap(GHw2, title="GHw2") co.pzmap(GHw3, title="GHw3")
else: H3 = Kp3*(1+(1/(Ti3*s))+(Td3*s/((Td3/N3)*(s)+1))) Hp3 = Kpot*H3 #% #% Definicao da malha aberta #% Sao definidos 3 sistemas distintos #% GHp1 = Hp1*Gp GHp2 = Hp2*Gp GHp3 = Hp3*Gp #% #% Polos e zeros de maplha aberta #% print('Polos e zeros - GHp1') print(co.pole(GHp1)) print(co.zero(GHp1)) print('\nPolos e zeros - GHp2') print(co.pole(GHp2)) print(co.zero(GHp2)) print('\nPolos e zeros - GHp3') print(co.pole(GHp3)) print(co.zero(GHp3)) #% #% Plot com os polos e zeros de malha aberta #% co.pzmap(GHp1, title = "GHp1") co.pzmap(GHp2,title = "GHp2") co.pzmap(GHp3, title = "GHp3")
plt.figure(figsize=(10, 8)) for K in KPKD: KP, KD = K KI = 4 # To construct the closed-loop transfer function Gcl from theta^d to theta , # we have a number of options: # (1) we can use the command "feedback" as introduced earlier. ###### Gcl = cm.feedback((KP + KD * s) * G, 1) # PD controller # Gcl = cm.feedback((KP + KD * s + KI/s) * G, 1) # PID controller print(Gcl) print(cm.pole(Gcl)) cm.damp(Gcl, True) ###### # (2) If you are interested to know how Eq. (6.18) is derived, # you may want to use the following general rule: # +++++++++++++++++++ # Gcl = <Transfer function of the open loop between the input and output>/(1 # + <Transfer function of the closed loop>) # +++++++++++++++++++ # Accordingly: ###### # Gcl = (KP*G + KD*s*G)/(1 + KP*G + KD*s*G) ###### # Gd = (G) / (1 + (KP + KD * s) * (G))
plt.close('all') # Parametros da funcao de transferencia K1 = 100 Km = 2.083 Kg = 0.1 a = 100 am = 1.71 # # Funcao de transferencia da posicao angular do sistema s = co.tf('s') Gtheta = (K1 * Km * Kg) / (s * (s + a) * (s + am)) # # Calculo dos polos da funcao de transferencia Gm print('-------------') print('POLOS DA FUNCAO DE TRANSFERENCIA') print(co.pole(Gtheta)) print('-------------') print('FT DA PLANTA Gtheta(s) = ') print(Gtheta) # # Expansao em fracoes parciais # caso nao haja multiplos polos: # B(s) R(1) R(2) R(n) # ---- = -------- + -------- + ... + -------- + K(s) # A(s) s - P(1) s - P(2) s - P(n) # print('-------------') print('EXPANSAO EM FRACOES PARCIAIS') B = K1 * Km * Kg A = [1, a + am, a * am, 0]
plt.close('all') # Parametros da funcao de transferencia K1 = 100; Km = 2.083; Kg = 0.1; a = 100; am = 1.71; # # Funcao de transferencia da velocidade angular do sistema s = co.tf('s'); Gomega = (K1*Km*Kg)/((s+a)*(s+am)) # # Calculo dos polos da funcao de transferencia Gm print('-------------') print('POLOS DA FUNCAO DE TRANSFERENCIA') print(co.pole(Gomega)) print('-------------') print('FT DA PLANTA Gomega(s) = ') print(Gomega) # # Expansao em fracoes parciais # caso nao haja multiplos polos: # B(s) R(1) R(2) R(n) # ---- = -------- + -------- + ... + -------- + K(s) # A(s) s - P(1) s - P(2) s - P(n) # print('-------------') print('EXPANSAO EM FRACOES PARCIAIS') B = K1*Km*Kg; A = [1, a+am, a*am];
def do_sys_id(self): num_poles = 2 num_zeros = 1 if not self._use_subspace: method = 'ARMAX' #sys_id = system_identification(self._y.T, self._u[0], method, IC='BIC', na_ord=[0, 5], nb_ord=[1, 5], nc_ord=[0, 5], delays=[0, 5], ARMAX_max_iterations=300, tsample=self._Ts, centering='MeanVal') sys_id = system_identification(self._y.T, self._u[0], method, ARMAX_orders=[num_poles, 1, 1, 0], ARMAX_max_iterations=300, tsample=self._Ts, centering='MeanVal') print(sys_id.G) if self._verbose: print(sys_id.G) print("System poles of discrete G: ", cnt.pole(sys_id.G)) # Convert to continuous tf G = harold.Transfer(sys_id.NUMERATOR, sys_id.DENOMINATOR, dt=self._Ts) G_cont = harold.undiscretize(G, method='zoh') self._sys_tf = G_cont self._A, self._B, self._C, self._D = harold.transfer_to_state( G_cont, output='matrices') if self._verbose: print("Continuous tf:", G_cont) # Convert to state space, because ARMAX gives transfer function ss_roll = cnt.tf2ss(sys_id.G) A = np.asarray(ss_roll.A) B = np.asarray(ss_roll.B) C = np.asarray(ss_roll.C) D = np.asarray(ss_roll.D) if self._verbose: print(ss_roll) # simulate identified system using input from data xid, yid = fsetSIM.SS_lsim_process_form(A, B, C, D, self._u) y_error = self._y - yid self._fitness = 1 - (y_error.var() / self._y.var())**2 if self._verbose: print("Fittness %", self._fitness * 100) if self._plot: plt.figure(1) plt.plot(self._t[0], self._y[0]) plt.plot(self._t[0], yid[0]) plt.xlabel("Time") plt.title("Time response Y(t)=U*G(t)") plt.legend([ self._y_name, self._y_name + '_identified: ' + '{:.3f} fitness'.format(self._fitness) ]) plt.grid() plt.show() else: sys_id = system_identification(self._y, self._u, self._subspace_method, SS_fixed_order=num_poles, SS_p=self._subspace_p, SS_f=50, tsample=self._Ts, SS_A_stability=True, centering='MeanVal') #sys_id = system_identification(self._y, self._u, self._subspace_method, SS_orders=[1,10], SS_p=self._subspace_p, SS_f=50, tsample=self._Ts, SS_A_stability=True, centering='MeanVal') if self._verbose: print("x0", sys_id.x0) print("A", sys_id.A) print("B", sys_id.B) print("C", sys_id.C) print("D", sys_id.D) A = sys_id.A B = sys_id.B C = sys_id.C D = sys_id.D # Get discrete transfer function from state space sys_tf = cnt.ss2tf(A, B, C, D) if self._verbose: print("TF ***in z domain***", sys_tf) # Get numerator and denominator (num, den) = cnt.tfdata(sys_tf) # Convert to continuous tf G = harold.Transfer(num, den, dt=self._Ts) if self._verbose: print(G) G_cont = harold.undiscretize(G, method='zoh') self._sys_tf = G_cont self._A, self._B, self._C, self._D = harold.transfer_to_state( G_cont, output='matrices') if self._verbose: print("Continuous tf:", G_cont) # get zeros tmp_tf = cnt.ss2tf(self._A, self._B, self._C, self._D) self._zeros = cnt.zero(tmp_tf) # simulate identified system using discrete system xid, yid = fsetSIM.SS_lsim_process_form(A, B, C, D, self._u, sys_id.x0) y_error = self._y - yid self._fitness = 1 - (y_error.var() / self._y.var())**2 if self._verbose: print("Fittness %", self._fitness * 100) if self._plot: plt.figure(1) plt.plot(self._t[0], self._y[0]) plt.plot(self._t[0], yid[0]) plt.xlabel("Time") plt.title("Time response Y(t)=U*G(t)") plt.legend([ self._y_name, self._y_name + '_identified: ' + '{:.3f} fitness'.format(self._fitness) ]) plt.grid() plt.show()
import numpy as np import control.matlab as ml import matplotlib.pyplot as plt # If using termux import subprocess import shlex #end if # num is the numerator of the trasfer function which is (2s) # dem is the denominator of the transfer function which is (0.25s + 1)(0.25s + 1)(0.5s + 1) num = np.array([2, 0]) den = np.polymul(np.array([0.5, 1]), np.array([0.25, 1])) den = np.polymul(den, np.array([0.25, 1])) # Generating the transfer function g = ml.tf(num, den) print("The transfer function is: ", g) print("The poles of the above function are ", ml.pole(g)) print("The zeros of the above function are ", ml.zero(g)) # Generating the bode plot as well as plotting it mag, phase, w = ml.bode(g) # If using termux plt.savefig("./figs/ep18btech11016_plot.pdf") plt.savefig("./figs/ep18btech11016_plot.eps") subprocess.run(shlex.split("termux-open ./figs/ep18btech11016_plot.pdf")) # else plt.show()
def main(t0, deltat, t, input_type, input_u, CZa=-5.74340, CZadot=-0.00350, CZq=-5.66290, Cmadot=0.17800, Cmq=-8.79415, CXu=-0.095, CZu=-0.37616): """Input type: elevator rudder airleron""" #Find time idx = np.where(timelst == t0)[0] #Flight condition # m_fuel = 1197.484 # CHANGE Total fuel mass [kg] hp = altlst[ idx] # CHANGE pressure altitude in the stationary flight condition [m] V = taslst[ idx] # CHANGE true airspeed in the stationary flight condition [m/sec] alpha = np.radians( aoalst[idx] ) # angle of attack in the stationary flight condition [rad] theta = np.radians( pitchlst[idx]) # pitch angle in the stationary flight condition [rad] gamma = theta - alpha # CHANGE flight path angle - # Aircraft mass m = mass(t0) # mass [kg] # Longitudinal stability Cma = -0.4435 # CHANGE longitudinal stabilty [ ] Cmde = -1.001 # CHANGE elevator effectiveness [ ] # air density [kg/m^3] rho = rho0 * pow(((1 + (lam * hp / Temp0))), (-((g / (lam * R)) + 1))) W = m * g # [N] (aircraft weight) # Aircraft inertia (depend on t0): muc = m / (rho * S * c) mub = m / (rho * S * b) KX2 = 0.019 KZ2 = 0.042 KXZ = 0.002 KY2 = 1.25 * 1.114 # Aerodynamic constants: Cmac = 0 # Moment coefficient about the aerodynamic centre [ ] CNwa = CLa # Wing normal force slope [1/rad] CNha = 2 * np.pi * Ah / (Ah + 2) # Stabiliser normal force slope [ ] depsda = 4 / (A + 2) # Downwash gradient [ ] # Lift and drag coefficient (depend on t0): CL = 2 * W / (rho * V**2 * S) # Lift coefficient [ ] CD = CD0 + (CLa * alpha)**2 / (np.pi * A * e) # Drag coefficient [ ] # Stabiblity derivatives CX0 = W * np.sin(theta) / (0.5 * rho * V**2 * S) #CXu = -0.095 #corrected original CXa = +0.47966 # Positive! (has been erroneously negative since 1993) CXadot = +0.08330 CXq = -0.28170 CXde = -0.03728 print("CX0 = ", CX0) CZ0 = -W * np.cos(theta) / (0.5 * rho * V**2 * S) #CZu = -0.37616 #original #CZa = -5.74340 #CZadot = -0.00350 #CZq = -5.66290 CZde = -0.69612 Cmu = +0.06990 #positive! #Cmadot = +0.17800 #positive! #Cmq = -8.79415 CYb = -0.7500 CYbdot = 0 CYp = -0.0304 CYr = +0.8495 CYda = -0.0400 CYdr = +0.2300 Clb = -0.10260 Clp = -0.71085 Clr = +0.23760 Clda = -0.23088 Cldr = +0.03440 Cnb = +0.1348 Cnbdot = 0 Cnp = -0.0602 Cnr = -0.2061 Cnda = -0.0120 Cndr = -0.0939 #c-matrix dimensions s1 = (4, 4) s2 = (4, 1) s3 = (4, 2) #Creating the different c-matrices (c1, c2 &c3) for symmetrical flight #c1 matrix c1 = np.zeros(s1) c1[0, 0] = -2 * muc * (c / V) c1[1, 1] = (CZadot - 2 * muc) * (c / V) c1[2, 2] = -(c / V) c1[3, 1] = Cmadot * (c / V) c1[3, 3] = -2 * muc * KY2 * ((c / V)**2) #c2 matrix c2 = np.zeros(s1) c2[0, 0] = -CXu c2[0, 1] = -CXa c2[0, 2] = -CZ0 c2[0, 3] = -CXq * (c / V) c2[1, 0] = -CZu c2[1, 1] = -CZa c2[1, 2] = -CX0 c2[1, 3] = -(CZq + 2 * muc) * (c / V) c2[2, 3] = -(c / V) c2[3, 0] = -Cmu c2[3, 1] = -Cma c2[3, 3] = -Cmq * (c / V) #c3 matrix c3 = np.zeros(s2) c3[0, 0] = -CXde c3[1, 0] = -CZde c3[3, 0] = -Cmde #Creating the different c-matrices (c4, c5 &c6) for asymmetrical flight #c4 matrix c4 = np.zeros(s1) c4[0, 0] = (CYbdot - 2 * mub) * (b / V) c4[1, 1] = (-0.5) * (b / V) c4[2, 2] = -4 * mub * KX2 * (b / V) * (b / (2 * V)) c4[2, 3] = 4 * mub * KXZ * (b / V) * (b / (2 * V)) c4[3, 0] = Cnb * (b / V) c4[3, 2] = 4 * mub * KXZ * (b / V) * (b / (2 * V)) c4[3, 3] = -4 * mub * KZ2 * (b / V) * (b / (2 * V)) #c5 matrix c5 = np.zeros(s1) c5[0, 0] = CYb c5[0, 1] = CL c5[0, 2] = CYp * (b / (2 * V)) c5[0, 3] = (CYr - 4 * mub) * (b / (2 * V)) c5[1, 2] = (b / (2 * V)) c5[2, 0] = Clb c5[2, 2] = Clp * (b / (2 * V)) c5[2, 3] = Clr * (b / (2 * V)) c5[3, 0] = Cnb c5[3, 2] = Cnp * (b / (2 * V)) c5[3, 3] = Cnr * (b / (2 * V)) #c6 matrix c6 = np.zeros(s3) c6[0, 0] = -CYda c6[0, 1] = -CYdr c6[2, 0] = -Clda c6[2, 1] = -Cldr c6[3, 0] = -Cnda c6[3, 1] = -Cndr # Time responses for unit steps: # t = np.linspace(t0,t0+ deltat, nsteps) -t0 u = input_u # print("u and t:",u,t,sep='\n') # print("u shape:",u.shape) # print("t shape:",t.shape) if t.shape != u.shape: print("Wrong slicing for input and time!\n") return -1 #Now, we distinct between inputs: if input_type == "elevator": print("Calculating for elevator input...") #Symmetric system is triggered: #Creating the state matrix(A) and the input matrix(B) for symmetrical flight - xdot = c1^-1*c2*x c1^-1*c3*u = Ax + Bu A_s = np.dot(np.linalg.inv(c1), c2) B_s = np.dot(np.linalg.inv(c1), c3) C_s = np.identity(4) D_s = np.zeros((4, 1)) #System in state-space sys_s = cm.StateSpace(A_s, B_s, C_s, D_s) poles_s = cm.pole(sys_s) # print("Eigenvalues of the symmetric system: ", poles_s,sep='\n') #verified # Time responses for unit steps: yout, tout, uout = cm.lsim(sys_s, u, t) #general time response u_out_s = yout[:, 0] alpha_out_s = yout[:, 1] + alpha * 180 / math.pi theta_out_s = yout[:, 2] + theta * 180 / math.pi + 2 q_out_s = yout[:, 3] if CXu != -0.095 or CZu != -0.37616 or CZa != -5.74340 or CZadot != -0.00350 or CZq != -5.66290 or Cmadot != 0.17800 or Cmq != -8.79415: #Plotting.... plotting( t, u_out_s, str("u Response for " + input_type + " input, t0= " + str(t0)), "u", "m/s", "Modified Model") plotting( t, alpha_out_s, str("Alpha Response for " + input_type + " input, t0= " + str(t0)), r"$\alpha$", "deg", "Modified Model") plotting( t, theta_out_s, str("Theta Response for " + input_type + " input, t0= " + str(t0)), r"$\theta$", "deg", "Modified Model") plotting( t, q_out_s, str("q Response for " + input_type + " input, t0= " + str(t0)), "$q$", r"deg/s", "Modified Model") print("\tPlotted!") else: plotting( t, u_out_s, str("u Response for " + input_type + " input, t0= " + str(t0)), "u", "m/s") plotting( t, alpha_out_s, str("Alpha Response for " + input_type + " input, t0= " + str(t0)), r"$\alpha$", "deg") plotting( t, theta_out_s, str("Theta Response for " + input_type + " input, t0= " + str(t0)), r"$\theta$", "deg") plotting( t, q_out_s, str("q Response for " + input_type + " input, t0= " + str(t0)), "$q$", r"deg/s") print("\tPlotted!") return q_out_s, theta_out_s, poles_s else: #Creating the state matrix(A) and the input matrix(B) for asymmetrical flight - y = c4^-1*c5*x c4^-1*c5*u = Ax + Bu A_a = -np.dot(np.linalg.inv(c4), c5) B_a = np.dot(np.linalg.inv(c4), c6) C_a = np.identity(4) #D_a depends on the input if input_type == "rudder": print("Calculating for rudder input...") D_a = np.zeros((4, 2)) D_a[:, 0] = 0 #we should check this... uarray = np.ones((len(t), 2)) #step input uarray[:, 1] = -u #ADDED MINUS!!!!! uarray[:, 0] = 0 elif input_type == "aileron": print("Calculating for aileron input...") D_a = np.zeros((4, 2)) D_a[:, 1] = 1 uarray = np.ones((len(t), 2)) #step input uarray[:, 0] = -u #ADDED MINUS!!!!! uarray[:, 1] = 0 #System in state-space sys_a = cm.StateSpace(A_a, B_a, C_a, D_a) poles_a = cm.pole(sys_a) # print("Eigenvalues of the asymmetric system: ", poles_a) #verified yout, tout, uout = cm.lsim( sys_a, uarray, t) #general time response for the input uarray beta_out_a = yout[:, 0] phi_out_a = yout[:, 1] p_out_a = yout[:, 2] r_out_a = yout[:, 3] if CXu != -0.095 or CZu != -0.37616 or CZa != -5.74340 or CZadot != -0.00350 or CZq != -5.66290 or Cmadot != 0.17800 or Cmq != -8.79415: # #Plotting... plotting( t, beta_out_a, str("Beta Response for " + input_type + " input, t0= " + str(t0)), r"$\beta$", "deg", "Modified Model") plotting( t, phi_out_a, str("Phi Response for " + input_type + " input, t0= " + str(t0)), r"$\phi$", "deg", "Modified Model") plotting( t, p_out_a, str("p Response for " + input_type + " input, t0= " + str(t0)), r"$p$", "deg/s", "Modified Model") plotting( t, r_out_a, str("r Response for " + input_type + " input, t0= " + str(t0)), "$r$", r"deg/s", "Modified Model") print("\tPlotted!") else: plotting( t, beta_out_a, str("Beta Response for " + input_type + " input, t0= " + str(t0)), r"$\beta$", "deg") plotting( t, phi_out_a, str("Phi Response for " + input_type + " input, t0= " + str(t0)), r"$\phi$", "deg") plotting( t, p_out_a, str("p Response for " + input_type + " input, t0= " + str(t0)), r"$p$", "deg/s") plotting( t, r_out_a, str("r Response for " + input_type + " input, t0= " + str(t0)), "$r$", r"deg/s") print("\tPlotted!") return poles_a return 1
plt.axhline(1, color="b", linestyle="--") plt.xlim(0, 3) plt.show() # impulse response yout, T = matlab.impulse(sys, t) plt.plot(T, yout) plt.axhline(0, color="b", linestyle="--") plt.xlim(0, 3) #plt.show() # nyquist diagram matlab.nyquist(sys) #plt.show() # bode diagram matlab.bode(sys) #plt.show() # root locus matlab.rlocus(sys) #plt.show() # pole matlab.pole(sys) #plt.show() # margin matlab.margin(sys) #plt.show()
# Sao definidos 3 sistemas distintos # GHp1 = Hp1 * Gp GHp2 = Hp2 * Gp GHp3 = Hp3 * Gp print('\nFUNCAO DE TRANSFERENCIA DE MALHA ABERTA') print('GHp1 = ', GHp1) print('GHp2 = ', GHp2) print('GHp3 = ', GHp3) # # Polos e zeros de malha aberta # print('\nPOLOS E ZEROS DE MALHA ABERTA') print('-------------') print('POLOS E ZEROS - GHp1') print('Polos = ', co.pole(GHp1)) print('Zeros = ', co.zero(GHp1)) print('-------------') print('POLOS E ZEROS - GHp2') print('Polos = ', co.pole(GHp2)) print('Zeros = ', co.zero(GHp2)) print('-------------') print('POLOS E ZEROS - GHp3') print('Polos = ', co.pole(GHp3)) print('Zeros = ', co.zero(GHp3)) # # definicao da malha fechada # realimentacao unitaria # # Funcao de transferencia em malha fechada # pode ser definida em Matlab utilizando-se
def polos(G): return co.pole(G)