def check_obsv(A, C):
Uo = obsv(A, C) # 可制御性行列の計算
No = np.linalg.matrix_rank(Uo) # Ucのランクを計算
(N, N) = np.matrix(A).shape # 正方行列Aのサイズ(N*N)
# 可制御性の判別
if No == N: return 0 # 可観測
else: return -1 # 可観測でない
def check_observability(A, C):
obsv_mat = obsv(A, C)
rank = np.linalg.matrix_rank(obsv_mat)
print('Observability matrix')
print(obsv_mat)
print('')
print('Rank is', rank)
print('')
if rank != A.shape[0]:
print('This system is not observability\n')
else:
print('This system is observability\n')
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 testObsv(self, siso):
"""Call obsv()"""
obsv(siso.ss1.A, siso.ss1.C)
obsv(siso.ss2.A, siso.ss2.C)
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) #単位円内なら安定
Uo = matlab.obsv(P.A, P.C) #可観測性行列作成
print(Uo)
rank = np.linalg.matrix_rank(Uo) #ランク調査
print(rank) #ランクが4なら可観測
x0 = np.array([[0.1], [0.1], [0.1], [.1]])
N = 200
t = np.arange(0, N * T + T, T)
x = x0
y = x0.T
for i in range(N):
x = P.A @ x
y = np.r_[y, x.T]
#plt.plot(t,y)
[[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
#科制御性の検証
Uc = matlab.ctrb(P.A, P.B)
rank = np.linalg.matrix_rank(Uc)
print(rank)
print('A2: \n', A2)
# fi_A:
fi_A = A2 + alfa1 * A + alfa2 * np.identity(2)
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)
print('det controlab =', np.linalg.det(Ct))
print('Posto controlab =', matrix_rank(Ct))
########################################################
# Matriz de Observabilidade (O)
########################################################
O = ctl.obsv(A, C)
print('Observabilidade: \n', O)
print('det observ =', np.linalg.det(O))
print('Posto observ =', matrix_rank(O))
########################################################
# K
########################################################
pos = np.array([0, 1])
K = pos.dot(Ct_inv).dot(fi_A)
print('K=', K)
