def testLsim(self, siso):
"""Test lsim() for SISO system"""
t = np.linspace(0, 1, 10)
# compute step response - test with state space, and transfer function
# objects
u = np.array([1., 1, 1, 1, 1, 1, 1, 1, 1, 1])
youttrue = np.array([
9., 17.6457, 24.7072, 30.4855, 35.2234, 39.1165, 42.3227, 44.9694,
47.1599, 48.9776
])
yout, tout, _xout = lsim(siso.ss1, u, t)
np.testing.assert_array_almost_equal(yout, youttrue, decimal=4)
np.testing.assert_array_almost_equal(tout, t)
with pytest.warns(UserWarning, match="Internal conversion"):
yout, _t, _xout = lsim(siso.tf3, u, t)
np.testing.assert_array_almost_equal(yout, youttrue, decimal=4)
# test with initial value and special algorithm for ``U=0``
u = 0
x0 = np.array([[.5], [1.]])
youttrue = np.array([
11., 8.1494, 5.9361, 4.2258, 2.9118, 1.9092, 1.1508, 0.5833,
0.1645, -0.1391
])
yout, _t, _xout = lsim(siso.ss1, u, t, x0)
np.testing.assert_array_almost_equal(yout, youttrue, decimal=4)
def main():
# 制御対象を定義(今回はモータを想定)
J = 0.6 # 慣性
D = 0.2 # 粘性
Kt = 0.15 # トルク定数
P = crl.tf([Kt], [J, D, 0]) # モータ角度の電流制御
# 参照モデルの定義
M = crl.tf([50], [1, 50])
# シミュレーション条件を定義
Ts = 0.001 # サンプリング時間
# 制御器構造の定義
beta = crl.ss("0", "1", "0; 1", "1; 0") # PI制御器
print(crl.tf(beta))
beta = crl.c2d(beta, Ts)
# 制御対象と参照モデルのゲイン線図
plt.figure(1)
crl.bode([P, M])
plt.legend(["Plant", "Ref. model"])
# prbsを生成する
n = 12 # m系列信号の次数
Tp = 5 # 何周期印加するか？
Ap = 0.5 # 振幅 [A]
u0 = 2 * prbs(n)
u0 = Ap * (u0 - np.average(u0))
u0 = np.tile(u0, Tp)
t = np.linspace(0, Ts * u0.shape[0], u0.shape[0])
plt.figure(2)
plt.plot(t, u0)
plt.xlabel("Time [s]")
plt.ylabel("Current [A]")
# 実験データの取得
print("=== Start Experiment ===")
Pd = crl.c2d(P, Ts, "tustin") # 双一次変換で離散時間モデルを作成
t = np.arange(start=0, stop=Tp * Ts * (2**n - 1), step=Ts)
(y0, t0, xout) = crl.lsim(Pd, u0[:], t[:]) # 応答を取得
plt.figure(3)
plt.plot(y0)
# VRFTを実行する
print("=== Start VRFT ===")
W = crl.tf([50], [1, 50]) # 周波数重み
L = crl.minreal((1 - M) * M * W) # プレフィルタ
Ld = crl.c2d(L, Ts, "tustin")
(ul, tl, xlout) = crl.lsim(Ld, u0, t)
(phi_l, tl, xlout) = crl.lsim(beta, y0[:])
print(phi_l)
rho = np.linalg.solve(phi_l, ul)
print(rho)
plt.show()
def test_lsim(self):
A, B, C, D = self.make_SISO_mats()
sys = ss(A, B, C, D)
figure()
plot_shape = (2, 2)
#Test with arrays
subplot2grid(plot_shape, (0, 0))
t = linspace(0, 1, 100)
u = r_[1:1:50j, 0:0:50j]
y, _t, _x = lsim(sys, u, t)
plot(t, y, label='y')
plot(t, u / 10, label='u/10')
legend(loc='best')
#Test with U=None - uses 2nd algorithm which is much faster.
subplot2grid(plot_shape, (0, 1))
t = linspace(0, 1, 100)
x0 = [-1, -1]
y, _t, _x = lsim(sys, U=None, T=t, X0=x0)
plot(t, y, label='y')
legend(loc='best')
#Test with U=0, X0=0
#Correct reaction to zero dimensional special values
subplot2grid(plot_shape, (0, 1))
t = linspace(0, 1, 100)
y, _t, _x = lsim(sys, U=0, T=t, X0=0)
plot(t, y, label='y')
legend(loc='best')
#Test with MIMO system
subplot2grid(plot_shape, (1, 1))
A, B, C, D = self.make_MIMO_mats()
sys = ss(A, B, C, D)
t = array(linspace(0, 1, 100))
u = array([r_[1:1:50j, 0:0:50j], r_[0:1:50j, 0:0:50j]])
x0 = [0, 0, 0, 0]
y, t_out, _x = lsim(sys, u, t, x0)
plot(t_out, y[0], label='y[0]')
plot(t_out, y[1], label='y[1]')
plot(t_out, u[0] / 10, label='u[0]/10')
plot(t_out, u[1] / 10, label='u[1]/10')
legend(loc='best')
#Test with wrong values for t
#T is None; - special handling: Value error
self.assertRaises(ValueError, lsim(sys, U=0, T=None, x0=0))
#T="hello" : Wrong type
#TODO: better wording of error messages of ``lsim`` and
# ``_check_convert_array``, when wrong type is given.
# Current error message is too cryptic.
self.assertRaises(TypeError, lsim(sys, U=0, T="hello", x0=0))
#T=0; - T can not be zero dimensional, it determines the size of the
# input vector ``U``
self.assertRaises(ValueError, lsim(sys, U=0, T=0, x0=0))
#T is not monotonically increasing
self.assertRaises(ValueError,
lsim(sys, U=0, T=[0., 1., 2., 2., 3.], x0=0))
def step_response(self):
if self._verbose:
print("[PID Design] Calculating PID gains")
pid_tf, closedLoop_tf = self.pid_design()
end_time = 5 # [s]
npts = int(old_div(end_time, self._dt)) + 1
T = np.linspace(0, end_time, npts)
yout, T = cnt.step(closedLoop_tf, T)
u, _, _ = cnt.lsim(pid_tf, 1 - yout, T)
return T, u, yout
def validation(SYS, u, y, Time):
# check dimensions
y = 1. * np.atleast_2d(y)
u = 1. * np.atleast_2d(u)
[n1, n2] = y.shape
ydim = min(n1, n2)
ylength = max(n1, n2)
if ylength == n1:
y = y.T
[n1, n2] = u.shape
ulength = max(n1, n2)
if ulength == n1:
u = u.T
# one-step ahead predictor (MISO approach)
Yval = np.zeros((ydim, ylength))
# centering inputs and outputs on 0
for i in range(u.shape[0]):
u[i, :] = u[i, :] - u[i, 0]
for i in range(ydim):
Y_u, T, Xv = cnt.lsim((1 / SYS.H[i, 0]) * SYS.G[i, :], u, Time)
Y_y, T, Xv = cnt.lsim(1 - (1 / SYS.H[i, 0]), y[i, :] - y[i, 0], Time)
Yval[i, :] = np.atleast_2d(Y_u + Y_y + y[i, 0])
return Yval
def testLsim_mimo(self, mimo):
"""Test lsim() for MIMO system.
first system: initial value, second system: step response
"""
t = np.linspace(0, 1, 10)
u = np.array([[0., 1.], [0, 1], [0, 1], [0, 1], [0, 1],
[0, 1], [0, 1], [0, 1], [0, 1], [0, 1]])
x0 = np.array([[.5], [1], [0], [0]])
youttrue = np.array([[11., 9.], [8.1494, 17.6457],
[5.9361, 24.7072], [4.2258, 30.4855],
[2.9118, 35.2234], [1.9092, 39.1165],
[1.1508, 42.3227], [0.5833, 44.9694],
[0.1645, 47.1599], [-0.1391, 48.9776]])
yout, _t, _xout = lsim(mimo.ss1, u, t, x0)
np.testing.assert_array_almost_equal(yout, youttrue, decimal=4)
def action_zpk(sys, final_time, setpoint, z, p, k):
# open-loop system transfer function
try:
num, den = model(sys)
except:
# for error detection
print("Err: system in not defined")
return
Gs = control.tf(num, den)
z = np.array([z])
p = np.array([p])
num, den = matlab.zpk2tf(z, p, k)
Ds = matlab.tf(num, den)
# closed-loop unity-feedback transfer function
Ts = control.feedback(Ds*Gs, 1)
# simulation time parameters
initial_time = 0
nsteps = 40 * int(final_time) # number of time steps
t = np.linspace(initial_time, final_time, round(nsteps))
output, t = matlab.step(Ts, t)
output = setpoint*output
# calculate list of error
setpoint_arr = setpoint * np.ones(nsteps)
err = setpoint_arr - output
# calculate control action
action = matlab.lsim(Ds, err, t)
# covert numpy arrays to lists
t = list(t)
action = list(action[0])
# round lists to 6 decimal digits
ndigits = 6
t = [round(num, ndigits) for num in t]
action = [round(num, ndigits) for num in action]
# calculate maximum control action
max_action = max(action)
min_action = min(action)
return t, action, min_action, max_action
def Asymetric(name):
# Assigning coefficients to matrices
C1 = np.matrix([[(CYbdot - 2 * name.mub), 0, 0, 0], [0, -0.5, 0, 0],
[0, 0, -4 * name.mub * KX2, 4 * name.mub * KXZ],
[Cnbdot, 0, 4 * name.mub * KXZ, -4 * name.mub * KZ2]])
C1[:] *= b / name.V0
C1[:, 2] *= b / (2 * name.V0)
C1[:, 3] *= b / (2 * name.V0)
print np.linalg.eig(C1)
C2 = np.matrix([[CYb, name.CL, CYp, CYr - 4 * name.mub], [0, 0, 1, 0],
[Clb, 0, Clp, Clr], [Cnb, 0, Cnp, Cnr]])
C2[:, 2] *= b / (2 * name.V0)
C2[:, 3] *= b / (2 * name.V0)
print np.linalg.eig(C2)
C3 = np.matrix([[CYda, CYdr], [0, 0], [Clda, Cldr], [Cnda, Cndr]])
#combining the matrices to generate the state space system
A = -np.linalg.inv(C1) * C2
B = -np.linalg.inv(C1) * C3
C = np.matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
D = np.matrix([[0, 0], [0, 0], [0, 0], [0, 0]])
sys = cs.ss(A, B, C, D)
print sys
#input of control system
delev = np.column_stack((name.delta_a_stab, name.delta_r_stab))
t = np.linspace(0,
len(name.delta_r) * 0.1,
num=len(name.delta_r),
endpoint=True,
retstep=False) #time step and range
Xinit = np.matrix([[0], [0], [0],
[0]]) # initial values for control system
y, t, x = cs.lsim(sys, U=delev, T=t,
X0=Xinit) # computing dynamic stability
# print sys, C1, C2, C3, np.linalg.eig(A)
#plotting
y1 = []
y2 = []
y3 = []
y4 = []
y1.append(y[:, 0])
y2.append(y[:, 1])
y3.append(y[:, 2])
y4.append(y[:, 3])
y1 = np.transpose(y1)
y2 = np.transpose(y2)
y3 = np.transpose(y3)
y4 = np.transpose(y4)
y2 *= (180 / np.pi)
y3 *= (180 / np.pi)
y4 *= (180 / np.pi)
y2 += name.RollAngle0
y3 += name.RollRate0
y4 += name.YawRate0
return y1, y2, y3, y4, t
## plotting the total grpahs
# plt.figure(0)
## plt.gca().set_ylim([-1,0])
## plt.gca().set_xlim([0,140])
# plt.tick_params(axis="x", labelsize=15)
# plt.tick_params(axis="y", labelsize=15)
# plt.title('Angle of Sideslip ')
# plt.plot(t, y1, label="Numerical Optimized", color="blue")
# plt.ylabel(r"$\beta$ [deg]")
# plt.xlabel("t [s]")
# plt.legend(loc=2)
# plt.savefig((namem + "Angle of Sideslip"))
# plt.show()
#
# plt.figure(1)
## plt.gca().set_ylim([-1,0])
## plt.gca().set_xlim([0,140])
# plt.tick_params(axis="x", labelsize=15)
# plt.tick_params(axis="y", labelsize=15)
# plt.title('Roll Angle ')
# plt.plot(t, y2, label="Numerical Optimized", color="blue")
# plt.ylabel("$\phi$ [deg]")
# plt.xlabel("t [s]")
# plt.plot(t, name.RollAngle, label="Experimental", color="green")
# plt.legend(loc=2)
# plt.savefig((namem + "RollAngle"))
# plt.show()
#
# plt.figure(2)
## plt.gca().set_ylim([-1,0])
## plt.gca().set_xlim([0,140])
# plt.tick_params(axis="x", labelsize=15)
# plt.tick_params(axis="y", labelsize=15)
# plt.title('Roll Rate')
# plt.plot(t, y3, label="Numerical Optimized", color="blue")
# plt.ylabel("p [deg/s]")
# plt.xlabel("t [s]")
# plt.plot(t, name.RollRate, label="Experimental", color="green")
# plt.legend(loc=2)
# plt.savefig((namem + "RollRate"))
# plt.show()
#
# plt.figure(3)
## plt.gca().set_ylim([-1,0])
## plt.gca().set_xlim([0,140])
# plt.tick_params(axis="x", labelsize=15)
# plt.tick_params(axis="y", labelsize=15)
# plt.title('Yaw Rate')
# plt.plot(t, y4, label="Numerical Optimized", color="blue")
# plt.ylabel("r [deg/s]")
# plt.xlabel("t [s]")
# plt.plot(t, name.YawRate, label="Experimental", color="green")
# plt.legend(loc=2)
# plt.savefig((namem + "YawRate"))
# plt.show()
#
# plt.figure(4)
# plt.gca().set_ylim([-6,6])
## plt.gca().set_xlim([0,140])
# plt.tick_params(axis="x", labelsize=15)
# plt.tick_params(axis="y", labelsize=15)
# plt.title("Aileron Deflection")
# plt.plot(t, (name.delta_a_stab*180/np.pi), color="green")
# plt.ylabel("$\delta_a$ [deg]")
# plt.xlabel("t [s]")
# plt.savefig((namem + "Ailerondeflection"))
#
#
# plt.figure(5)
# plt.gca().set_ylim([-6,6])
## plt.gca().set_xlim([0,140])
# plt.tick_params(axis="x", labelsize=15)
# plt.tick_params(axis="y", labelsize=15)
# plt.title("Rudder Deflection")
# plt.plot(t, (name.delta_r_stab*180/np.pi), color="green")
# plt.ylabel("$\delta_r$ [deg]")
# plt.xlabel("t [s]")
# plt.savefig((namem + "Rudder Deflection"))
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
import control.matlab as ctl
import numpy as np
import matplotlib.pyplot as plt
Gca = ctl.tf([2, 1], [1, 0])
Gp = ctl.tf([-10], [1, 10]) # Servo do Profundor
Gma = ctl.tf([-1, -5], [1, 3.5, 6, 0]) # sistema da aero nave
t = np.arange(0, 17, 1)
y = 0.5 * t
Ls = ctl.series(Gca, Gp, Gma)
print("L(s) = ", Ls)
Hss = ctl.tf([1], [1])
print("H(s) = ", Hss)
Ts = ctl.feedback(Ls, Hss, sign=-1)
print("T(s) = ", Ts)
yout, T, Xo = ctl.lsim(Ts, y, t, 0)
plt.plot(T, yout, '-k')
plt.plot(t, y, '-r')
plt.grid()
plt.show()
def lsim(sys, U=0.0, T=None, X0=0.0):
U_ = U
if isinstance(U_, (np.ndarray, list)):
U_ = U_.T
return cnt.lsim(sys, U_, T, X0)
# Teste dos casos:
for i in range (10):
(b, c) = casos [i];
print "b = ", b, ", c = ", c
# Controlador feedforward:
Cff = ctrl.tf ([c*Td*Ti, Kc*b*Ti, 1], [Ti, 0]);
# Funcoes de Transferencia:
YR = Cff*G / (1 + C*G); # Saida em relacao a referencia
YL = G / (1 + C*G); # Saida em relacao ao disturbio
YN = 1 / (1 + C*G); # Saida em relacao ao ruido
# Simulacao com YR:
ur = list (np.ones(10000)) # step em t0 = 0
yr, tr, _ = ctrl.lsim (YR, ur, t);
# Simulacao com YL:
ul = list (np.zeros (1000)) + list (np.multiply (-1.0, np.ones (9000))); # adotando ts = 5*tau = 20s, step em t1 = ts
yl, tl, _ = ctrl.lsim (YL, ul, t);
# Simulacao com YN:
un = list (np.zeros (3000)) + list (np.multiply(0.3, np.ones (1000)));
un += list (np.zeros (1000)) + list(np.multiply(-0.3, np.ones (1000))) + list (np.zeros (4000));
# sinal de ruido: un = 0.3 * (step (t - 3ts) - step (t - 4ts) - step (t - 5ts) + step (t - 6ts))
yn, tn, _ = ctrl.lsim (YN, un, t);
# Resultados:
plt.subplot (2,2,1);
plt.grid()
plt.plot (tr, yr);
target.append(10.0)
fig, ax = plt.subplots()
ims = []
kp = 500
ki = 0
kd = 0
for ki in np.arange(0, 500, 100):
num = [kd, kp, ki]
den = [1, 0]
K = matlab.tf(num, den)
sys = matlab.feedback(K * G, 1)
(Y, t, X) = matlab.lsim(sys, target, t)
ax.legend()
ax.set_xlim(0, 20)
ax.set_xlabel('time [sec]')
ax.set_ylabel('velocity [m/s]')
im = ax.plot(t, Y, label='Ki=' + str(ki))
im += ax.plot(t, target, color='r')
ims.append(im)
# ani = animation.ArtistAnimation(fig, ims, interval=100)
# ani.save('kp.gif', writer='pillow')
# (Y, T) = matlab.step(sys, t)
# (Y, t, X) = matlab.lsim(sys, target, t)
# plt.plot(t, Y)
# plt.plot(t, target)
# spiral sys_spir_r, sys, e1_spir_r, e2_spir_r = ABCD(1, 5) sys_spir_f, sys, e1_spir_f, e2_spir_f = ABCD(2, 5) ########### SIMULATION OF EIGENMOTIONS######## dt = 0.1 # Short Period t1 = np.arange(0, 15, dt) t_ini = 3634 t_fin = 3636 u1, cell = inputcr(reference_delta_e, time, t1, 3633, 3648) y1_r, T1_r, x1_r = ml.lsim(sys_sp_r, u1, t1) u1_f, celli_1 = inputcr(flight_delta_e, time, t1, 3156, 3171) y1_f, T1_f, x1_f = ml.lsim(sys_sp_f, u1_f, t1) # Phugoid t2 = np.arange(0, 150, dt) u2, cell = inputcr(reference_delta_e, time, t2, 3237, 3247) y2_r, T2_r, x2_r = ml.lsim(sys_ph_r, u2, t2) u2_f, celli_2 = inputcr(flight_delta_e, time, t2, 3235, 3249) y2_f, T2_f, x2_f = ml.lsim(sys_ph_f, u2_f, t2)
Gca = ctl.tf([2], [1])
Gp = ctl.tf([-10], [1, 10]) # Servo do Profundor
Gma = ctl.tf([-1, -5], [1, 3.5, 6, 0]) # sistema da aero nave
print("------------------------------------------")
print("G(s) = ", Gca)
print("servo profunor = ", Gp)
print("Modelo Aeronave = ", Gma)
print("------------------------------------------")
t = np.arange(0, 15, 1)
y = 0.5 * t
# print(t)
# print("y(t) = ", y)
Ls = mat.series(Gca, Gp, Gma)
print("L(s) = ", Ls)
Hss = ctl.tf([1], [1])
print("H(s) = ", Hss)
Ts = ctl.feedback(Ls, Hss)
print("T(s) = ", Ts)
yout, T, Xo = mat.lsim(Ts, y, t, 0)
# print(T)
plt.plot(T, yout, '-b')
plt.plot(t, y, '-r')
plt.grid()
plt.show()
def step_zpk_servo(final_time, setpoint, z, p, k):
# Define Servomotor by Differential Equation
# 36/(s^2 + 3.6s)
def servo(y, t, u, extra):
extra = 0.0
dy_dt = y[1]
dy2_dt = 36*u - 3.6* y[1]
return [dy_dt, dy2_dt]
# simulation time parameters
nsteps = int(final_time*40) # number of time steps
t = np.linspace(0,final_time,nsteps) # linearly spaced time vector
delta_t = t[1] # how long each step is
# simulate step input operation
y0 = [0.0, 0.0] # servo initial degree
step = np.zeros(nsteps)
sp = setpoint # set point
step[0] = 0.0
step[1:] = sp
yt = np.zeros(nsteps) # for storing degrees results - degrees vector
yt[0] = y0[1]
et = np.zeros(1) # error value
#ut = np.zeros(nsteps) # actuation vector
# Controller TF
z = np.array([z])
p = np.array([p])
num, den = matlab.zpk2tf(z, p, k)
Ds = matlab.tf(num, den)
# Actuation limits
max_ac = 5
min_ac = -5
# simulate with ODEINT
for i in range(nsteps):
error = step[i] - y0[0]
et[0] = error
q = matlab.lsim(Ds, [0, et[0]], [0, t[i]])
q = list(q[0])
u = q[1]
# clip input to -100% to 100%
if u > max_ac:
u = max_ac
elif u < min_ac:
u = min_ac
# ut[i+1] = u
y = odeint(servo, y0, [0,delta_t],args=(u,0))
y0 = y[-1]
yt[i] = y0[0]
t = list(t)
output = list(yt)
# round lists to 4 decimal digits
ndigits = 4
t = [round(num, ndigits) for num in t]
output = [round(num, ndigits) for num in output]
return t, output
def step_zpk_cruise(final_time, setpoint, z, p, k):
# Define engine lag
def lag(p, t, u, extra):
# inputs
# u = power demand
# p = output power
# t = time
extra = 0.0
dp_dt = 2*(u - p)
return dp_dt
# Define car plant
def vehicle(v, t, p, load):
# inputs
# v = behicle speed
# t = time
# p = power
# load = cargo + passenger load
# Car specifications and initialization data
rho = 1.225 # kg/m3
CdA = 0.55 # m2
m = 1800 # kg
# Calculate derivative of vehicle velocity
dv_dt = (1.0/m) * (p*hp2watt/v - 0.5*CdA*rho*v**2)
return dv_dt
# Conversion factors
hp2watt = 746
mps2kmph = 3.6
kmph2mps = 1/mps2kmph
# simulation time parameters
tf = final_time # final time for simulation
nsteps = int(40* final_time) # number of time steps
t = np.linspace(0,tf,nsteps) # linearly spaced time vector
delta_t = t[1] # how long each step is
v0_kmph = 1.0 # car initial speed in km/h
sp_kmph = setpoint # car final speed in km/h
# simulate step input operation
load = 0.0 # passenger(s) and cargo load - in Kg
v0 = v0_kmph*kmph2mps # car initial speed
step = np.zeros(nsteps)
sp = sp_kmph*kmph2mps
step[0] = v0
step[1:] = sp
vt = np.zeros(nsteps) # for storing speed results - speed vector
vt[0] = v0
et = np.zeros(1) # error value
#ut = np.zeros(nsteps) # actuation vector
#pt = np.zeros(nsteps) # power vector
p0 = 0.0 # initial car power
# Actuation limits - Car Physical limits
max_power = 120
min_power = -120
# Controller TF
z = np.array([z])
p = np.array([p])
num, den = matlab.zpk2tf(z, p, k)
Ds = matlab.tf(num, den)
# simulate with ODEINT
for i in range(nsteps):
error = step[i] - v0
et[0] = error
q = matlab.lsim(Ds, [0, et[0]], [0, t[i]])
q = list(q[0])
u = q[1]
# clip input to -100% to 100%
if u > max_power:
u = max_power
elif u < min_power:
u = min_power
# ut[i+1] = u
p = odeint(lag, p0, [0,delta_t],args=(u,0))
v = odeint(vehicle,v0,[0,delta_t],args=(p[-1], load))
p0 = p[-1]
v0 = v[-1]
vt[i] = v0
# pt[i+1] = p0
# convert output to km/h
vt_kmph = vt*mps2kmph
t = list(t)
output = list(vt_kmph)
# round lists to 4 decimal digits
ndigits = 4
t = [round(num, ndigits) for num in t]
output = [round(num, ndigits) for num in output]
return t, output
Time = np.linspace(0, tfin, npts) #INPUT# Usim = np.zeros((4, npts)) Usim_noise = np.zeros((4, npts)) [Usim[0, :], _, _] = fset.GBN_seq(npts, 0.03, Range=[-0.33, 0.1]) [Usim[1, :], _, _] = fset.GBN_seq(npts, 0.03) [Usim[2, :], _, _] = fset.GBN_seq(npts, 0.03, Range=[2.3, 5.7]) [Usim[3, :], _, _] = fset.GBN_seq(npts, 0.03, Range=[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_outputH1, Time, Xsim = cnt.lsim(H_sample1, err_inputH[0, :], Time) err_outputH2, Time, Xsim = cnt.lsim(H_sample2, err_inputH[1, :], Time) err_outputH3, Time, Xsim = cnt.lsim(H_sample3, err_inputH[2, :], Time) # Noise-free output 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) Yout32, Time, Xsim = cnt.lsim(g_sample32, Usim[1, :], Time) Yout33, Time, Xsim = cnt.lsim(g_sample33, Usim[2, :], Time)
DEN = [1., -2.21, 1.7494, -0.584256, 0.0684029, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.]
# ### Numerator of input transfer function has 3 roots: nb = 3
NUM = [1., -2.07, 1.3146]
# ### Define transfer functions
g_sample = cnt.tf(NUM, DEN, sampling_time)
h_sample = cnt.tf(NUM_H, DEN, sampling_time)
# ## Time responses
# ### Input reponse
Y1, Time, Xsim = cnt.lsim(g_sample, Usim, Time)
plt.figure(0)
plt.plot(Time, Usim)
plt.plot(Time, Y1)
plt.xlabel("Time")
plt.title("Time response y(t)=g*u(t)")
plt.legend(['u(t)', 'y(t)'])
plt.grid()
plt.show()
# ### Noise response
Y2, Time, Xsim = cnt.lsim(h_sample, e_t, Time)
plt.figure(1)
plt.plot(Time, e_t)
plt.plot(Time, Y2)
Created on Mon Jun 8 16:05:06 2020 @author: Nadine Snijders """ import numpy as np import matplotlib.pylab as plt import control import control.matlab as cm # 5/s+5 teller = np.array([5.0]) noemer = np.array([1.0,5.0]) H = control.tf(teller, noemer) w = 10 #controle van magnitude t = np.arange(0,5,0.005) u = np.sin(2* np.pi * w * t) plt.plot(t,u) y,t,x = cm.lsim(H,u,t) y = cm.lsim(H,u,t)[0] plt.plot(t,y) #plt.plot(t,x)
def test_lsim(self):
A, B, C, D = self.make_SISO_mats()
sys = ss(A, B, C, D)
figure(); plot_shape = (2, 2)
#Test with arrays
subplot2grid(plot_shape, (0, 0))
t = linspace(0, 1, 100)
u = r_[1:1:50j, 0:0:50j]
y, _t, _x = lsim(sys, u, t)
plot(t, y, label='y')
plot(t, u/10, label='u/10')
legend(loc='best')
#Test with U=None - uses 2nd algorithm which is much faster.
subplot2grid(plot_shape, (0, 1))
t = linspace(0, 1, 100)
x0 = [-1, -1]
y, _t, _x = lsim(sys, U=None, T=t, X0=x0)
plot(t, y, label='y')
legend(loc='best')
#Test with U=0, X0=0
#Correct reaction to zero dimensional special values
subplot2grid(plot_shape, (0, 1))
t = linspace(0, 1, 100)
y, _t, _x = lsim(sys, U=0, T=t, X0=0)
plot(t, y, label='y')
legend(loc='best')
#Test with matrices
subplot2grid(plot_shape, (1, 0))
t = matrix(linspace(0, 1, 100))
u = matrix(r_[1:1:50j, 0:0:50j])
x0 = matrix("0.; 0")
y, t_out, _x = lsim(sys, u, t, x0)
plot(t_out, y, label='y')
plot(t_out, asarray(u/10)[0], label='u/10')
legend(loc='best')
#Test with MIMO system
subplot2grid(plot_shape, (1, 1))
A, B, C, D = self.make_MIMO_mats()
sys = ss(A, B, C, D)
t = matrix(linspace(0, 1, 100))
u = array([r_[1:1:50j, 0:0:50j],
r_[0:1:50j, 0:0:50j]])
x0 = [0, 0, 0, 0]
y, t_out, _x = lsim(sys, u, t, x0)
plot(t_out, y[0], label='y[0]')
plot(t_out, y[1], label='y[1]')
plot(t_out, u[0]/10, label='u[0]/10')
plot(t_out, u[1]/10, label='u[1]/10')
legend(loc='best')
#Test with wrong values for t
#T is None; - special handling: Value error
self.assertRaises(ValueError, lsim(sys, U=0, T=None, x0=0))
#T="hello" : Wrong type
#TODO: better wording of error messages of ``lsim`` and
# ``_check_convert_array``, when wrong type is given.
# Current error message is too cryptic.
self.assertRaises(TypeError, lsim(sys, U=0, T="hello", x0=0))
#T=0; - T can not be zero dimensional, it determines the size of the
# input vector ``U``
self.assertRaises(ValueError, lsim(sys, U=0, T=0, x0=0))
#T is not monotonically increasing
self.assertRaises(ValueError, lsim(sys, U=0, T=[0., 1., 2., 2., 3.], x0=0))
x = np.array([0, 0, 1, 0, 0])
y = np.array([0, 0, 0, 1, 0, 0, 0])
z = np.convolve(x, y)
z1 = faltung(x, y)
np.set_printoptions(linewidth=80)
print(z)
print(z1)
s1 = c.tf(1, [1, 1])
N = 1000
t = np.linspace(0, 10, N)
# x = np.sin(2*math.pi*1.5*t)
x = np.ones(N)
y, t1, x1 = cm.lsim(s1, x, t)
# pdb.set_trace()
f = plt.figure(0)
#
plt.subplot(211)
plt.plot(t, x)
plt.grid()
plt.title("input")
#
plt.subplot(212)
plt.plot(t, y)
plt.grid()
plt.title("output")
#
plt.show()
npts = int(old_div(tfin, ts)) + 1 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_outputH1, Time, Xsim = cnt.lsim(H_sample1, err_inputH[0, :], Time) err_outputH2, Time, Xsim = cnt.lsim(H_sample2, err_inputH[1, :], Time) err_outputH3, 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)
A = [[0, 1], [0, 0]] B = [[0], [1]] C = [1, 0] D = [0] Ts = 1 / 4 sys = cntmat.ss(A, B, C, D) # Plot with reference (control) signal t = np.arange(0, 20, 0.01) u = np.zeros(t.shape) y = cntmat.lsim(sys, u, t) print(y[0]) plt.plot(t, u, 'k', label='u=0') plt.plot(t, y[0], 'k--', label='y(u0)') u = np.sin(t) y = cntmat.lsim(sys, u, t) plt.plot(t, u, 'r', label='u=sin') plt.plot(t, y[0], 'r--', label='y(usin)') plt.show() # Next plot with PID controller Kp = 10 Ki = 10 Kd = 10 # PID feedback controller s = control.TransferFunction.s
t = np.linspace(0, 200, 1000)
t1, imp = co.impulse_response(g1, t)
t2, stp = co.step_response(g1, t)
tsq = np.linspace(0, 10, 1000)
l1 = len(np.linspace(0, 4, 400, endpoint=False))
l2 = len(np.linspace(4, 8, 400, endpoint=False))
l3 = len(np.linspace(8, 10, 200))
sq = np.append(np.append(np.zeros(l1), np.ones(l2)), np.zeros(l3))
plt.plot(tsq, sq)
plt.grid()
plt.show()
yout, T, xout = matlab.lsim(g1, sq, tsq)
print(
f"g1 impulse : time= {max_amplitude(imp, t1)[0]}, amplitude max= {max_amplitude(imp, t1)[1]}"
)
print(
f"g1 impulse : time= {min_amplitude(imp, t1)[0]}, amplitude min= {min_amplitude(imp, t1)[1]}"
)
print(
f"g1 step : time= {max_amplitude(stp, t2)[0]}, amplitude max= {max_amplitude(stp, t2)[1]}"
)
print(
f"g1 step : time= {min_amplitude(stp, t2)[0]}, amplitude min= {min_amplitude(stp, t2)[1]}"
)
print(
f"g1 square : time= {max_amplitude(yout, T)[0]}, amplitude max= {max_amplitude(yout, T)[1]}"
# Input force
u = 0 * t
u[100:120] = 100
u[1500:1520] = -100
# Input disturbance
u_dist = np.random.randn(4, np.size(t))
# Input noise
u_noise = np.random.randn(np.size(t))
# The input taking all the parts
aug_u = np.row_stack([u, Vd @ Vd @ u_dist, u_noise])
# Calculating the outputs
y, t, ph = matlab.lsim(single_sys, aug_u.transpose(), t)
fs_x, t, ph = matlab.lsim(full_sys, aug_u.transpose(), t)
x_hat, t, ph = matlab.lsim(kf_sys, np.row_stack([u, y]).transpose(), t)
# Plotting the results
fig, axs = plt.subplots(2, figsize=(14, 8))
axs[0].plot(t, y, label='System output', linewidth=0.8)
axs[0].plot(t,
x_hat[:, 0],
'r',
label='Estimative of this variable',
linewidth=1.5)
axs[0].legend()
axs[0].set_xlabel('t (s)')
axs[0].set_ylabel('Amplitude')
axs[0].set_title('Noise and disturbance rejection')
# -*- coding: utf-8 -*- """ Created on Fri Apr 24 14:14:38 2020 @author: Nadine Snijders """ import numpy as np import matplotlib.pylab as plt import control import control.matlab as cm # 5/s+5 teller = np.array([5.0]) noemer = np.array([1.0, 5.0]) H = control.tf(teller, noemer) #controle van magnitude t = np.arange(0, 1000, 0.1) u = np.sin(t * 5) #plt.plot(t,u) y, t, x = cm.lsim(H, u, t) #y = cm.lsim(H,u,t)[0] print(y, x, t)
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
if (t < t0):
u.append (0);
else:
u.append (1);
return u;
casos = [(0, 0), (0.5, 0), (1, 0), (0, 0.5), (0, 1), (0.5, 0.5), (0.5, 1), (1, 0.5), (1, 1), (1.5, 1.5)]
G = ctrl.tf (1, [1, 3, 3, 1]);
t = np.arange (0, 100, 0.01);
for i in range (10):
(b, c) = casos [i];
spec = {'Kc' : 0.9259 / (0.2705*0.8045), 'Ti' : 4 * 0.8045, 'b' : b, 'c' : c} # minimo IAE p77, l4
C = Controller(spec).Gc;
Cff = Controller(spec).Gff;
YR = Cff*G / (1 + C*G);
YL = G / (1 + C*G);
YN = 1 / (1 + C*G);
u = step (t, tx[);
tr, yr = ctrl.lsim (YR, u, t);
]
# ### Numerator of input transfer function has 3 roots: nb = 3
NUM = [1.5, -2.07, 1.3146]
# ### Define transfer functions
g_sample = cnt.tf(NUM, DEN, sampling_time)
h_sample = cnt.tf(NUM_H, DEN, sampling_time)
# ## Time responses
# ### Input reponse
Y1, Time, Xsim = cnt.lsim(g_sample, Usim, Time)
plt.figure(0)
plt.plot(Time, Usim)
plt.plot(Time, Y1)
plt.xlabel("Time")
plt.title("Time response y$_k$(u) = g$\cdot$u$_k$")
plt.legend(['u(t)', 'y(t)'])
plt.grid()
plt.show()
# ### Noise response
Y2, Time, Xsim = cnt.lsim(h_sample, e_t, Time)
plt.figure(1)
plt.plot(Time, e_t)
plt.plot(Time, Y2)
def validation(SYS,u,y,Time, k = 1, centering = 'None'):
# check dimensions
y = 1. * np.atleast_2d(y)
u = 1. * np.atleast_2d(u)
[n1, n2] = y.shape
ydim = min(n1, n2)
ylength = max(n1, n2)
if ylength == n1:
y = y.T
[n1, n2] = u.shape
udim = min(n1, n2)
ulength = max(n1, n2)
if ulength == n1:
u = u.T
Yval = np.zeros((ydim,ylength))
# Data centering
#y_rif = np.zeros(ydim)
#u_rif = np.zeros(udim)
if centering == 'InitVal':
y_rif = 1. * y[:, 0]
u_rif = 1. * u[:, 0]
elif centering == 'MeanVal':
for i in range(ydim):
y_rif = np.mean(y,1)
#y_rif[i] = np.mean(y[i, :])
for i in range(udim):
u_rif = np.mean(u,1)
#u_rif[i] = np.mean(u[i, :])
elif centering == 'None':
y_rif = np.zeros(ydim)
u_rif = np.zeros(udim)
else:
# elif centering != 'None':
sys.stdout.write("\033[0;35m")
print("Warning! \'Centering\' argument is not valid, its value has been reset to \'None\'")
sys.stdout.write(" ")
# MISO approach
# centering inputs and outputs
for i in range(u.shape[0]):
u[i,:] = u[i,:] - u_rif[i]
for i in range(ydim):
# one-step ahead predictor
if k == 1:
Y_u, T, Xv = cnt.lsim((1/SYS.H[i,0])*SYS.G[i,:], u, Time)
Y_y, T, Xv = cnt.lsim(1 - (1/SYS.H[i,0]), y[i,:] - y_rif[i], Time)
Yval[i,:] = np.atleast_2d(Y_u + Y_y + y_rif[i])
else:
# k-step ahead predictor
# impulse response of disturbance model H
hout, T = cnt.impulse(SYS.H[i,0], Time)
# extract first k-1 coefficients
h_k_num = hout[0:k]
# set denumerator
h_k_den = np.hstack((np.ones((1,1)), np.zeros((1,k-1))))
# FdT of impulse response
Hk = cnt.tf(h_k_num,h_k_den[0],SYS.ts)
# plot impulse response
# plt.subplot(ydim,1,i+1)
# plt.plot(Time,hout)
# plt.grid()
# if i == 0:
# plt.title('Impulse Response')
# plt.ylabel("h_j ")
# plt.xlabel("Time [min]")
# k-step ahead prediction
Y_u, T, Xv = cnt.lsim(Hk*(1/SYS.H[i,0])*SYS.G[i,:], u, Time)
#Y_y, T, Xv = cnt.lsim(1 - Hk*(1/SYS.H[i,0]), y[i,:] - y[i,0], Time)
Y_y, T, Xv = cnt.lsim(1 - Hk*(1/SYS.H[i,0]), y[i,:] - y_rif[i], Time)
#Yval[i,:] = np.atleast_2d(Y_u + Y_y + y[i,0])
Yval[i,:] = np.atleast_2d(Y_u + Y_y + y_rif[i])
return Yval
def plot_response(self):
""" Plots the response of the CH-53 to a step input of 1 [deg] longitudinal cyclic. Note that the state-space
representation models the change in inputs and state-variables, thus to use the same input vectors the initial
condition is set to zero control deflection. This is then compensated by adding the control deflection
necessary for trim during the Forward Euler integration for the non-linear equations. """
pbar = ProgressBar('Performing Linear Simulation')
time = np.linspace(0, 40, 1000)
# Initial Input Conditions
cyclic_input = [0]
collective_input = [0]
# Defining Step Input into the Longitudinal Cyclic
for i in range(1, len(time)):
if 0.5 < time[i] < 1.0:
cyclic_input.append(radians(1.0))
else:
cyclic_input.append(cyclic_input[0])
collective_input.append(collective_input[0])
# Input Matrix U for the State-Space Simulation
input_matrix = np.array([collective_input, cyclic_input]).T
# Simulating the Linear System
yout, time, xout = lsim(self.system, U=input_matrix, T=time)
pbar.update(100)
delta_t = time[1] - time[0]
u = [self.stability_derivatives.u]
w = [self.stability_derivatives.w]
q = [self.stability_derivatives.q]
theta_f = [self.stability_derivatives.theta_f]
current_case = self.stability_derivatives
# Forward Euler Integration
pbar = ProgressBar('Performing Forward Euler Integration')
for i in range(1, len(time)):
u.append(current_case.u + current_case.u_dot * delta_t)
w.append(current_case.w + current_case.w_dot * delta_t)
q.append(current_case.q + current_case.q_dot * delta_t)
theta_f.append(current_case.theta_f + current_case.theta_f_dot * delta_t)
current_case = StabilityDerivatives(u=u[i], w=w[i], q=q[i], theta_f=theta_f[i],
longitudinal_cyclic=cyclic_input[i] +
self.initial_trim_case.longitudinal_cyclic,
collective_pitch=self.initial_trim_case.collective_pitch)
pbar.update_loop(i, len(time)-1)
# Creating First Figure
plt.style.use('ggplot')
fig, (cyc_plot, vel_plot) = plt.subplots(2, 1, num='EulervsStateVelocities', sharex='all')
# Plotting Input
cyc_plot.plot(time, [degrees(rad) for rad in cyclic_input])
cyc_plot.set_ylabel(r'Lon. Cyclic [deg]')
cyc_plot.set_xlabel('')
cyc_plot.yaxis.set_major_formatter(FormatStrFormatter('%.3f'))
cyc_plot.set_title(r'Velocity Response as a Function of Time')
# Plotting Velocity Response
vel_plot.plot(time, u, label='Horizontal')
vel_plot.plot(time, w, label='Vertical')
vel_plot.plot(time, [num+self.stability_derivatives.u for num in yout[:, 0]], linestyle=':', color='black')
vel_plot.plot(time, [num+self.stability_derivatives.w for num in yout[:, 1]], linestyle=':', color='black',
label='Lin. Simulation')
vel_plot.set_ylabel(r'Velocity [m/s]')
vel_plot.set_xlabel('')
vel_plot.yaxis.set_major_formatter(FormatStrFormatter('%.2f'))
vel_plot.legend(loc='best')
# Creating Labels & Saving Figure
plt.xlabel(r'Time [s]')
plt.show()
fig.savefig(fname=os.path.join(working_dir, 'Figures', '%s.pdf' % fig.get_label()), format='pdf')
# Creating Second Figure
fig, (cyc_plot, q_plot, theta_plot) = plt.subplots(3, 1, num='EulervsStateAngles', sharex='all')
cyc_plot.plot(time, [degrees(rad) for rad in cyclic_input])
# Plotting Input
cyc_plot.set_ylabel(r'$\theta_{ls}$ [deg]')
cyc_plot.yaxis.set_major_formatter(FormatStrFormatter('%.3f'))
cyc_plot.set_title(r'Angular Response as a Function of Time')
# Plotting Fuselage Pitch-Rate
q_plot.plot(time, [degrees(rad) for rad in q])
q_plot.plot(time, [degrees(num+self.stability_derivatives.q) for num in yout[:, 2]],
linestyle=':',
color='black',
label='Lin. Simulation')
q_plot.set_ylabel(r'$q$ [deg/s]')
q_plot.set_xlabel('')
q_plot.yaxis.set_major_formatter(FormatStrFormatter('%.2f'))
q_plot.legend(loc='best')
# Plotting Fuselage Pitch
theta_plot.plot(time, [degrees(rad) for rad in theta_f])
theta_plot.plot(time, [degrees(num+self.stability_derivatives.theta_f) for num in yout[:, 3]],
linestyle=':',
color='black',
label='Lin. Simulation')
theta_plot.set_ylabel(r'$\theta_f$ [deg]')
theta_plot.set_xlabel('')
theta_plot.yaxis.set_major_formatter(FormatStrFormatter('%.2f'))
theta_plot.legend(loc='best')
# Creating x-label & Saving Figure
plt.xlabel(r'Time [s]')
plt.show()
fig.savefig(fname=os.path.join(working_dir, 'Figures', '%s.pdf' % fig.get_label()), format='pdf')
return 'Figures Plotted and Saved'
B = -np.linalg.inv(C1) * C3 C = np.matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) D = np.matrix([[0], [0], [0], [0]]) sys = cs.ss(A, B, C, D) H = cs.tf(sys) mt = 200 step = 0.1 t = np.arange(0, mt, step) Xinit = np.matrix([[0], [0], [0], [0]]) delev = deltae * (np.pi / 180) y, t, x = cs.lsim(sys, delev, t, Xinit) #plotting y1 = [] y2 = [] y3 = [] y4 = [] y1.append(y[:, 0]) y2.append(y[:, 1]) y3.append(y[:, 2]) y4.append(y[:, 3]) y1 = np.transpose(y1) y2 = np.transpose(y2) y3 = np.transpose(y3) y4 = np.transpose(y4)
