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)
