def check_ctrb(A, B): Uc = ctrb(A, B) # 可制御性行列の計算 Nu = np.linalg.matrix_rank(Uc) # Ucのランクを計算 (N, N) = np.matrix(A).shape # 正方行列Aのサイズ(N*N) # 可制御性の判別 if Nu == N: return 0 # 可制御 else: return -1 # 可制御でない
def check_controllability(A, B): cntrb_mat = ctrb(A, B) rank = np.linalg.matrix_rank(cntrb_mat) print('Controllability matrix') print(cntrb_mat) print('') print('Rank is', rank) print('') if rank != A.shape[0]: print('This system is not controllability\n') else: print('This system is controllability\n')
def d_11(): print('\n--D--\n') msd = Sim.MassSpringDamper() max_force = 2.0 damping_ratio = 1 / (2**(1 / 2)) rise_time = 2.0 # mass = msd.dynamics.mass # spring_const = msd.dynamics.spring_const # natural_frequency = ((spring_const + max_force) / mass) ** (1 / 2) natural_frequency = np.pi / (2 * rise_time * (1 - damping_ratio**2)**(1 / 2)) # Part A coefs = [1, 2 * damping_ratio * natural_frequency, natural_frequency**2] # Small error roots = np.roots(coefs) print(f"Roots: {roots}") # Part B A = msd.dynamics.A B = msd.dynamics.B C = msd.dynamics.C # Part C control_mat = ctrl.ctrb(A, B) rank = np.linalg.matrix_rank(control_mat) order = A.shape[0] print(f"controllability matrix rank: {rank}") print(f"System order: {order}") print(f"{'Controllable' if rank == order else 'Not Controllable'}") # Part D feedback_gain = ctrl.place(A, B, roots) print(f'Feedback Gain: {feedback_gain}') reference_gain = -1 / (C @ np.linalg.inv(A - B @ feedback_gain) @ B) print(f"Reference gain: {reference_gain}") # Part E msd.add_controller(SSController.MassSpringDamper, feedback_gain, reference_gain, max_force) request = sg.generator(sg.constant, amplitude=2 / 3, t_step=msd.seconds_per_sim_step, t_final=20) handle = msd.view_animation(request) Sim.Animations.plt.waitforbuttonpress() Sim.Animations.plt.close() return handle
def is_controllable_and_observable(A, B, C): controllabilityMatrix = ctrb(A, B) observabilityMatrix = obsv(A, C) eigenvalues, eigenvectors = np.linalg.eig(A) if np.linalg.matrix_rank(controllabilityMatrix) == np.linalg.matrix_rank( A): controllable = True else: controllable = False if np.linalg.matrix_rank(observabilityMatrix) == np.linalg.matrix_rank(A): observable = True else: observable = False return controllable, observable
def testCtrb(self, siso): """Call ctrb()""" ctrb(siso.ss1.A, siso.ss1.B) ctrb(siso.ss2.A, siso.ss2.B)
h12 = mp * Lp * Lm h22 = Jm + mp * Lm * Lm DLT = h11 * h22 - h12 * h12 T = 5e-3 #サンプリング周期 A = np.array([ [0, 1, 0, 0], #システム行列設定 [h22 * mp * g * Lp / DLT, -h22 * myup / DLT, 0, h12 * myum / DLT], [0, 0, 0, 1], [-h12 * mp * g * Lp / DLT, h12 * myup / DLT, 0, -h11 * myum / DLT] ]) b = np.array([[0], [-h12 * Km / DLT], [0], [h11 * Km / DLT]]) C = np.array([[1, 0, 0, 0], [0, 0, 1, 0]]) d = np.array([[0], [0]]) Uc = matlab.ctrb(A, b) #連続系可制御性行列作成 print(Uc) #その表示 rank = np.linalg.matrix_rank(Uc) #そのランク調査 print(rank) #ランクが4なら可制御 Ev = np.linalg.eigvals(A) #固有値=特性根計算 print(Ev) #左半面なら安定 S = matlab.ss(A, b, C, d) #システム行列の設定 P = matlab.c2d(S, T, method='zoh') #離散系へ変換 Uc = matlab.ctrb(P.A, P.B) #離散系での可制御性行列 print(Uc) rank = np.linalg.matrix_rank(Uc) #ランク調査 print(rank) #ランクが4なら可制御 Ev = np.linalg.eigvals(P.A) #固有値=特性根計算 print(Ev) #単位円内なら安定
h22 = Jm + mp * Lm * Lm DLT = h11 * h22 - h12 * h12 T = 0.005 '''''' '''''' '''''' A = np.array( [[0, 1, 0, 0], [h22 * mp * g * Lp / DLT, -h22 * myup / DLT, 0, h12 * myum / DLT], [0, 0, 0, 1], [-h12 * mp * g * Lp / DLT, h12 * myup / DLT, 0, -h11 * myum / DLT]]) b = np.array([[0], [-h12 * Km / DLT], [0], [h11 * Km / DLT]]) C = np.array([[1, 0, 0, 0], [0, 0, 1, 0]]) d = np.array([[0], [0]]) #可制御性の検証 Uc = matlab.ctrb(A, b) print(Uc) rank = np.linalg.matrix_rank(Uc) print(rank) #可観測性の検証 Uo = matlab.obsv(A, C) print(Uo) #出力に三点リーダーが出てくるのはいっぱいあるのを省略している rank = np.linalg.matrix_rank(Uo) print(rank) #離散時間系でもやってみましょう! print("ここからは離散時間系での実行") S = matlab.ss(A, b, C, d) P = matlab.c2d(S, T, method='zoh') #continuous to discrete
A2 = np.matmul(A, A) # Matriz A^2 A3 = np.matmul(A2, A) # Matriz A^3 # A4 = np.matmul(A3, A) # Matriz A^4 print('A2: \n', A2) print('A3: \n', A3) # fi_A: fi_A = A3 + alfa1*A2 + alfa2*A + alfa3*np.identity(3) print('fi(A): \n', fi_A) ######################################################## # Matriz de controlabilidade (C) ######################################################## Ct = ctl.ctrb(A, B) Ct_inv = np.linalg.inv(Ct) print('Controlabilidade: \n', Ct) print('Controlabilidade^-1: \n', Ct_inv) ######################################################## # Matriz de Observabilidade (O) ######################################################## # O = ctl.obsv(A, C) # print('Observabilidade: \n', O) ######################################################## # K ########################################################
from control import matlab import matplotlib.pyplot as plt import numpy as np A = np.array([[0, 1], [-1, -1.8]]) b = np.array([[0], [1]]) Uc = matlab.ctrb(A, b) print(Uc) rank = np.linalg.matrix_rank(Uc) print(rank)
def e_11(): print('\n--E--\n') bnb = Sim.BallAndBeam() max_force = 15 damping_ratio = 1 / (2**(1 / 2)) # Tuning variables rise_time_theta = .5 bandwidth_separation = 2.4 natural_frequency_theta = np.pi / (2 * rise_time_theta * (1 - damping_ratio**2)**(1 / 2)) rise_time_z = rise_time_theta * bandwidth_separation natural_frequency_z = np.pi / (2 * rise_time_z * (1 - damping_ratio**2)**(1 / 2)) # Part A coefs_theta = [ 1, 2 * damping_ratio * natural_frequency_theta, natural_frequency_theta**2 ] coefs_z = [ 1, 2 * damping_ratio * natural_frequency_z, natural_frequency_z**2 ] roots = np.concatenate(( np.roots(coefs_theta), np.roots(coefs_z), )) # Part B A = bnb.dynamics.A B = bnb.dynamics.B C = bnb.dynamics.C # Part C control_mat = ctrl.ctrb(A, B) rank = np.linalg.matrix_rank(control_mat) order = A.shape[0] print(f"controllability matrix rank: {rank}") print(f"System order: {order}") print(f"{'Controllable' if rank == order else 'Not Controllable'}") # Part D feedback_gain = ctrl.place(A, B, roots) print(f'Feedback Gain: {feedback_gain}') reference_gain = -1 / (C[0] @ np.linalg.inv(A - B @ feedback_gain) @ B) print(f"Reference gain: {reference_gain}") # Part E bnb.add_controller(SSController.BallAndBeam, feedback_gain, reference_gain, max_force) requests = sg.generator(sg.square, amplitude=0.15, y_offset=0.25, frequency=0.05, t_step=bnb.seconds_per_sim_step, t_final=20) handle = bnb.view_animation(requests) Sim.Animations.plt.waitforbuttonpress() Sim.Animations.plt.close() return handle
def f_11(): print('\n--F--\n') pvt = Sim.PlanarVTOL() max_force = 10 damping_ratio = 1 / (2**(1 / 2)) + 0.2 # Tuning variable rise_time_h = 2.5 rise_time_theta = 0.1 bandwidth_separation = 7 natural_frequency_h = np.pi / (2 * rise_time_h * (1 - damping_ratio**2)**(1 / 2)) natural_frequency_theta = np.pi / (2 * rise_time_theta * (1 - damping_ratio**2)**(1 / 2)) rise_time_z = bandwidth_separation * rise_time_theta natural_frequency_z = np.pi / (2 * rise_time_z * (1 - damping_ratio**2)**(1 / 2)) # Part A coefs_theta = [ 1, 2 * damping_ratio * natural_frequency_theta, natural_frequency_theta**2 ] coefs_z = [ 1, 2 * damping_ratio * natural_frequency_z, natural_frequency_z**2 ] roots_lat = np.concatenate(( np.roots(coefs_theta), np.roots(coefs_z), )) print(f"Latitudinal roots: {roots_lat}") coefs_h = [ 1, 2 * damping_ratio * natural_frequency_h, natural_frequency_h * 2 ] roots_lon = np.roots(coefs_h) print(f"Longitudinal roots: {roots_lon}") # Part B A_lat = pvt.dynamics.A_lat B_lat = pvt.dynamics.B_lat C_lat = pvt.dynamics.C_lat A_lon = pvt.dynamics.A_lon B_lon = pvt.dynamics.B_lon C_lon = pvt.dynamics.C_lon # Part C control_mat_lat = ctrl.ctrb(A_lat, B_lat) rank = np.linalg.matrix_rank(control_mat_lat) order = A_lat.shape[0] print(f"Latitudinal controllability matrix rank: {rank}") print(f"Latitudinal system order: {order}") print(f"{'Controllable' if rank == order else 'Not Controllable'}") control_mat_lon = ctrl.ctrb(A_lon, B_lon) rank = np.linalg.matrix_rank(control_mat_lon) order = A_lon.shape[0] print(f"Longitudinal Controllability matrix rank: {rank}") print(f"Longitudinal system order: {order}") print(f"{'Controllable' if rank == order else 'Not Controllable'}") # Part D feedback_gain_lat = ctrl.place(A_lat, B_lat, roots_lat) print(f'Latitudinal feedback Gain: {feedback_gain_lat}') reference_gain_lat = -1 / ( C_lat[0] @ np.linalg.inv(A_lat - B_lat @ feedback_gain_lat) @ B_lat) print(f"Latitudinal reference gain: {reference_gain_lat}") feedback_gain_lon = ctrl.place(A_lon, B_lon, roots_lon) print(f'Longitudinal feedback Gain: {feedback_gain_lon}') reference_gain_lon = -1 / ( C_lon @ np.linalg.inv(A_lon - B_lon @ feedback_gain_lon) @ B_lon) print(f"Longitudinal reference gain: {reference_gain_lon}") # Part E request = zip( sg.generator(sg.constant, y_offset=1, t_step=pvt.seconds_per_sim_step, t_final=20), sg.generator(sg.sin, frequency=0.08, y_offset=0, amplitude=2.5, t_step=pvt.seconds_per_sim_step, t_final=20), ) pvt.add_controller(SSController.PlanarVTOL, feedback_gain_lat, reference_gain_lat, feedback_gain_lon, reference_gain_lon, max_force) handel = pvt.view_animation(request) Sim.Animations.plt.waitforbuttonpress() Sim.Animations.plt.close() return handel
#######State Feedback Control######## A = np.array([[0, 1, 0, 0], [ -(b1 * b2) / (m1 * m2), 0, ((b1 / m1) * ((b1 / m1) + (b1 / m2) + (b2 / m2))) - (k1 / m1), -(b1 / m1) ], [b2 / m2, 0, -((b1 / m1) + (b1 / m2) + (b2 / m2)), 1], [k2 / m2, 0, -((k1 / m1) + (k1 / m2) + (k2 / m2)), 0]]) B = np.array([[0], [1 / m1], [0], [(1 / m1) + (1 / m2)]]) # this Cr matrix will allow us to track the 3rd state Cr = np.array([[0, 0, 1, 0]]) # confirm that the system in controllable C_AB = ctl.ctrb(A, B) if np.linalg.matrix_rank(C_AB) != 4: print('not controllable') # two rise times (one set slightly higher than the other) tr1 = 0.5 # rise time 1 zeta = 0.707 # damping print('tr1', tr1) wn1 = np.pi / 2.0 / tr1 / np.sqrt(1 - zeta**2) tr2 = 0.1 # rise time 2 zeta = 0.707 # damping print('tr2', tr2) wn2 = np.pi / 2.0 / tr2 / np.sqrt(1 - zeta**2) # find desired poles based on the rise times and damping ratios
#####################STATE FEEDBACK#################### mtot = mc + ml + mr Alon = np.matrix([[0,1], [0,0]]) Blon = np.matrix([[0], [1/mtot]]) Clon = np.matrix([[1,0]]) Dlon = np.matrix([[0]]) C_ABlon = ctl.ctrb(Alon,Blon) ## altitude rise time tr_h = 1.25 zeta = .707 wn = np.pi/2/tr_h/np.sqrt(1-zeta**2) p = np.roots([1,2*zeta*wn,wn**2]) Klon = ctl.place(Alon,Blon,p) krlon = -1/(Clon*np.linalg.inv(Alon-Blon*Klon)*Blon) F = mtot*g