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
