def testInitial_mimo(self, mimo):
"""Test initial() for MIMO system"""
t = np.linspace(0, 1, 10)
x0 = np.array([[.5], [1.], [.5], [1.]])
youttrue = np.array([11., 8.1494, 5.9361, 4.2258, 2.9118, 1.9092,
1.1508, 0.5833, 0.1645, -0.1391])
sys = mimo.ss1
y_00, _t = initial(sys, T=t, X0=x0, input=0, output=0)
y_11, _t = initial(sys, T=t, X0=x0, input=1, output=1)
np.testing.assert_array_almost_equal(y_00, youttrue, decimal=4)
np.testing.assert_array_almost_equal(y_11, youttrue, decimal=4)
def testInitial(self, siso):
"""Test initial() for SISO system"""
t = np.linspace(0, 1, 10)
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])
sys = siso.ss1
yout, tout = initial(sys, T=t, X0=x0)
np.testing.assert_array_almost_equal(yout, youttrue, decimal=4)
np.testing.assert_array_almost_equal(tout, t)
# Play with arguments
yout, tout, xout = initial(sys, T=t, X0=x0, return_x=True)
np.testing.assert_array_almost_equal(yout, youttrue, decimal=4)
np.testing.assert_array_almost_equal(tout, t)
def test_initial(self, SISO_mats, MIMO_mats, mplcleanup):
A, B, C, D = SISO_mats
sys = ss(A, B, C, D)
figure(); plot_shape = (1, 3)
#X0=0 : must produce line at 0
subplot2grid(plot_shape, (0, 0))
t, y = initial(sys)
plot(t, y)
#X0=[1,1] : produces a spike
subplot2grid(plot_shape, (0, 1))
t, y = initial(sys, X0=array([[1], [1]]))
plot(t, y)
A, B, C, D = MIMO_mats
sys = ss(A, B, C, D)
#X0=[1,1] : produces same spike as above spike
subplot2grid(plot_shape, (0, 2))
t, y = initial(sys, X0=[1, 1, 0, 0])
plot(t, y)
def test_initial(self):
A, B, C, D = self.make_SISO_mats()
sys = ss(A, B, C, D)
figure(); plot_shape = (1, 3)
#X0=0 : must produce line at 0
subplot2grid(plot_shape, (0, 0))
t, y = initial(sys)
plot(t, y)
#X0=[1,1] : produces a spike
subplot2grid(plot_shape, (0, 1))
t, y = initial(sys, X0=matrix("1; 1"))
plot(t, y)
#Test MIMO system
A, B, C, D = self.make_MIMO_mats()
sys = ss(A, B, C, D)
#X0=[1,1] : produces same spike as above spike
subplot2grid(plot_shape, (0, 2))
t, y = initial(sys, X0=[1, 1, 0, 0])
plot(t, y)
def test_initial(self):
A, B, C, D = self.make_SISO_mats()
sys = ss(A, B, C, D)
figure(); plot_shape = (1, 3)
#X0=0 : must produce line at 0
subplot2grid(plot_shape, (0, 0))
t, y = initial(sys)
plot(t, y)
#X0=[1,1] : produces a spike
subplot2grid(plot_shape, (0, 1))
t, y = initial(sys, X0=matrix("1; 1"))
plot(t, y)
#Test MIMO system
A, B, C, D = self.make_MIMO_mats()
sys = ss(A, B, C, D)
#X0=[1,1] : produces same spike as above spike
subplot2grid(plot_shape, (0, 2))
t, y = initial(sys, X0=[1, 1, 0, 0])
plot(t, y)
print("K = \n", K)
t0 = 0
t_end = 5
dt = 0.01
t = np.arange(t0, t_end + dt,
dt) # deret waktu 0 - 5 dengan kenaikan dt (0,01)
tolX1 = 0.10 #toleransi x1
rt = t_end
st = t_end
rtTarget = 0.2
sistem = ctl.ss((A - B * K), np.identity(3), np.identity(3), np.identity(3))
x = matlab.initial(sistem, t, np.array([1, 0, 0]))
x1 = [1, 0, 0] @ np.transpose(x[0])
x2 = [0, 1, 0] @ np.transpose(x[0])
x3 = [0, 0, 1] @ np.transpose(x[0])
OvS = 0
peakOv = np.zeros(30)
OvKe = 0
OvBef = 0
J = 0
t0 = int(round(time() * 1000))
for i in range(0, len(t)):
print("data ke-", i, " = ", x1[i])
print("waktu ke-", i, " = ", t[i])
if (abs(x1[i]) < tolX1 and rt == t_end):
C1 = np.matrix([[-2 * muc * (c / V0**2), 0, 0, 0],
[0, (CZadot - 2 * muc) * (c / V0), 0, 0], [0, 0, -(c / V0), 0],
[0, Cmadot * (c / V0), 0, -2 * muc * KY2 * (c / V0)**2]])
C2 = np.matrix([[CXu / V0, CXa, CZ0, 0],
[CZu / V0, CZa, -CX0, (CZq + 2 * muc) * (c / V0)],
[0, 0, 0, (c / V0)], [Cmu / V0, Cma, 0, Cmq * (c / V0)]])
C3 = np.matrix([[CXde], [CZde], [0], [Cmde]])
A = -np.linalg.inv(C1) * C2
B = -np.linalg.inv(C1) * C3
eig = np.linalg.eig(A)
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)
t = np.arange(0, 100, 0.1)
X0 = np.matrix([[1], [0], [0], [0]])
y1, t = cs.initial(sys, t, X0)
plt.plot(t, y1)
plt.show()
def simulating_LQR(state, risetime_target, A, B, C, D, Q, R, dt):
K, S, E = ctl.lqr(A, B, Q, R)
print("K = \n{}\n".format(np.round(K, 4)))
print("Eigen Value ClosedLoop = \n{}\n".format(E))
# ======================> Simulation
t_start = 0
t_end = 10
t = np.arange(t_start, t_end + dt,
dt) # deret waktu 0 - 5 dengan kenaikan dt (0,01)
Acl = A - B * K
sys_cl = ctl.ss((Acl), B, C, D)
# sys_ol = ctl.ss(A, B, C, D)
# Inisialisasi Gangguan Awal
roll = math.radians(45)
gyro_roll = math.radians(0)
pitch = math.radians(45)
gyro_pitch = math.radians(0)
yaw = math.radians(45)
gyro_yaw = math.radians(0)
x = matlab.initial(
sys_cl, t,
np.array([roll, gyro_roll, pitch, gyro_pitch, yaw, gyro_yaw]))
x1 = [1, 0, 0, 0, 0, 0] @ np.transpose(x[0])
x2 = [0, 1, 0, 0, 0, 0] @ np.transpose(x[0])
x3 = [0, 0, 1, 0, 0, 0] @ np.transpose(x[0])
x4 = [0, 0, 0, 1, 0, 0] @ np.transpose(x[0])
x5 = [0, 0, 0, 0, 1, 0] @ np.transpose(x[0])
x6 = [0, 0, 0, 0, 0, 1] @ np.transpose(x[0])
# print("roll awal \t: ", x1[0])
# print("gyro roll awal \t: ", x2[0])
# print("pitch awal \t: ", x3[0])
# print("gyro pitch awal : ", x4[0])
# print("yaw awal \t: ", x5[0])
# print("gyro yaw awal \t: {}\n".format(x6[0]))
# Initialize Desired Control Paremeter
state_simulation = x1
if state == 1:
state_simulation = x1
elif state == 2:
state_simulation = x3
elif state == 3:
state_simulation = x5
toleransi_sudut = 0.1
rt = t_end
st = t_end
OvS = 0
peakOv = np.zeros(30)
OvKe = 0
OvBef = 0
J = 0
t0 = int(round(time() * 1000))
# print(t0)
# Looping Simulation
for i in range(0, len(t)):
# print("data ke-",i," = ",x1[i])
# print("waktu ke-",i," = ",t[i])
if (abs(state_simulation[i]) < toleransi_sudut and rt == t_end):
rt = t[i]
st = t[i]
if (t[i] > rt and abs(state_simulation[i]) > toleransi_sudut):
OvS = x1[i]
if (abs(OvS) < abs(OvBef)):
peakOv[OvKe] = OvBef
else:
OvBef = OvS
if (t[i] > rt and peakOv[OvKe] > 0):
OvKe = OvKe + 1
OvS = 0
OvBef = 0
# menghitung cost function
t1 = time() * 1000
dt = t1 - t0
# print("t0 = ",t0/1000)
# print("t1 = ",t1/1000)
# print("dt = ",dt/1000)
t0 = t1
xCF = np.array([[x1[i]], [x2[i]], [x3[i]], [x4[i]], [x5[i]], [x6[i]]])
u = -K @ xCF
# print("u = ", u)
J = J + ((np.transpose(xCF) @ Q @ xCF) +
(np.transpose(u) @ R @ u)) * dt / 1000
# print("J ke-",i," = ",J)
# # cost funtion
# print("Rise Time = ",rt)
# print("Overshoot =\n",peakOv)
# print("Cost Function = ",J)
return x1, x2, x3, x4, x5, x6, rt, J
Bcl = np.identity(6)
Ccl = np.identity(6)
Dcl = np.zeros((6, 6))
sys_cl = ctl.ss((Acl), B, C, D)
sys_ol = ctl.ss(A, B, C, D)
# Inisialisasi Gangguan Awal
roll = math.radians(45)
gyro_roll = math.radians(0)
pitch = math.radians(45)
gyro_pitch = math.radians(0)
yaw = math.radians(45)
gyro_yaw = math.radians(0)
x = matlab.initial(
sys_cl, t, np.array([roll, gyro_roll, pitch, gyro_pitch, yaw, gyro_yaw]))
x1 = [1, 0, 0, 0, 0, 0] @ np.transpose(x[0])
x2 = [0, 1, 0, 0, 0, 0] @ np.transpose(x[0])
x3 = [0, 0, 1, 0, 0, 0] @ np.transpose(x[0])
x4 = [0, 0, 0, 1, 0, 0] @ np.transpose(x[0])
x5 = [0, 0, 0, 0, 1, 0] @ np.transpose(x[0])
x6 = [0, 0, 0, 0, 0, 1] @ np.transpose(x[0])
print("roll awal \t: ", x1[0])
print("gyro roll awal \t: ", x2[0])
print("pitch awal \t: ", x3[0])
print("gyro pitch awal : ", x4[0])
print("yaw awal \t: ", x5[0])
print("gyro yaw awal \t: {}\n".format(x6[0]))
# Initialize Desired Control Paremeter
