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