Pythonで学ぶ制御工学　第23弾：オブザーバを用いた出力フィードバック制御

# #Pythonで学ぶ制御工学< オブザーバを用いた出力フィードバック制御 >

## はじめに
基本的な制御工学をPythonで実装し，復習も兼ねて制御工学への理解をより深めることが目的である．
その第23弾として「オブザーバを用いた出力フィードバック制御」を扱う．

## 概要
#### オブザーバを用いた出力フィードバック制御
以下にオブザーバを用いた出力フィードバック制御についてまとめたものを示す．
![image](https://user-images.githubusercontent.com/61382602/114270379-fb556d00-9a46-11eb-9111-f8549fa8cef1.png)
![image](https://user-images.githubusercontent.com/61382602/114270388-04ded500-9a47-11eb-8ad0-1188ce2ec4ad.png)
以下ではさらにゲインの設計と（定値）外乱がある時のゲインの設計について示す．
#### オブザーバゲインの設計
![image](https://user-images.githubusercontent.com/61382602/114270398-0dcfa680-9a47-11eb-98bd-2ebbf09d5a2a.png)

#### 外乱オブザーバ
![image](https://user-images.githubusercontent.com/61382602/114270406-188a3b80-9a47-11eb-923b-d188c3c7ab4d.png)

## 実装
以下では3つのソースコードとそのときの出力をそれぞれ示す．
##### ソースコード①：オブザーバによる状態推定
```python:observer_estimate.py
"""
2021/04/07
@Yuya Shimizu

オブザーバによる状態推定
"""
import numpy as np
import matplotlib.pyplot as plt
from control import ss, acker
from control.matlab import initial, lsim
from for_plot import plot_set

#係数行列
A = '0 1; -4 5'
B = '0; 1'
C = '1 0'
D = '0'

#状態空間モデル
P = ss(A, B, C, D)

#オブザーバ極
observer_poles = [-15+5j, -15-5j]

#オブザーバゲインの設計（状態フィードバックの双対）
L = -acker(P.A.T, P.C.T, observer_poles).T

#描画に向けて
fig, ax = plt.subplots(1, 2)
Td = np.arange(0, 3, 0.01)   #シミュレーション時間
X0 = [-1, 0.5]                      #初期状態

#制御対象Pを安定化する状態フィードバックゲインの設計
regulator_poles = [-5+5j, -5-5j]
F = -acker(P.A, P.B, regulator_poles)

#真の状態の振る舞い
Gsf = ss(P.A + P.B*F, P.B, np.eye(2), [[0], [0]])
x, t = initial(Gsf, Td, X0)     #初期条件応答
ax[0].plot(t, x[:, 0], ls='-.', label='${x}_1$')
ax[1].plot(t, x[:, 1], ls='-.', label='${x}_2$')

#オブザーバで推定した状態の振る舞い
#入力　u = Fx
u = [ [F[0, 0]*x[i, 0] + F[0, 1]*x[i, 1]] for i in range(len(x)) ]
#出力　y = Cx
y = x[:, 0]
#オブザーバによる状態推定
Obs = ss(P.A + L*P.C, np.c_[P.B, -L], np.eye(2), [[0, 0], [0, 0]] )
xhat, t, x0 = lsim(Obs, np.c_[u, y], Td, [0, 0])
ax[0].plot(t, xhat[:, 0], label='$\hat{x}_1$')
ax[1].plot(t, xhat[:, 1], label='$\hat{x}_2$')

for i in [0, 1]:
    plot_set(ax[i], 't', ' ', 'best')

ax[0].set_ylabel('$x_1, \hat{x}_1$')
ax[1].set_ylabel('$x_2, \hat{x}_2$')

fig.tight_layout()
fig.suptitle(f"estimation with observer; Obs_Pole={observer_poles}")

plt.show()
```
##### 出力
![Estimation_with_Obs](https://user-images.githubusercontent.com/61382602/114119841-5fccdb00-9926-11eb-835a-beda72c49af8.png)
はじめは少し出遅れるが，1秒も経たないうちにうまく推定ができていることがグラフから分かる．

##### ソースコード②：出力フィードバック制御器の設計
```python:FB_Control.py
"""
2021/04/08
@Yuya Shimizu

出力フィードバック制御器の設計
"""
import numpy as np
import matplotlib.pyplot as plt
from control import ss, acker, feedback, tf
from control.matlab import initial, lsim
from for_plot import plot_set

#係数行列
A = '0 1; -4 5'
B = '0; 1'
C = '1 0'
D = '0'

#状態空間モデル
P = ss(A, B, C, D)

#状態フィードバックゲインの設計
regulator_poles = [-5+5j, -5-5j]
F = -acker(P.A, P.B, regulator_poles)

#オブザーバゲインの設計
observer_poles = [-15+5j, -15-5j]
L = -acker(P.A.T, P.C.T, observer_poles).T

#出力フィードバック（オブザーバ+状態フィードバック）
K = ss(P.A+P.B*F+L*P.C, -L, F, 0)

#出力フィードバック系
Gfb = feedback(P, K, sign=1)

fig, ax = plt.subplots()
Td = np.arange(0, 3, 0.01)

#出力フィードバック制御なし
y, t = initial(P, Td, [-1, 0.5])
ax.plot(t, y, ls = '-.', label = 'w/o controller')
#出力フィードバック制御あり
y, t = initial(Gfb, Td, [-1, 0.5, 0, 0])
ax.plot(t, y, label = 'w/ controller')

plot_set(ax, 't', 'y', 'best')

ax.set_title(f"Feesback with observer; Obs_Pole={observer_poles}")

plt.show()
```

##### 出力
![FB_withOBs](https://user-images.githubusercontent.com/61382602/114119620-fa78ea00-9925-11eb-84cc-b2882981265e.png)
グラフにおいてw/o，w/それぞれ出力フィードバック制御なし，ありを表しており，出力フィードバック制御なしでは発散してしまっていることが分かる．
##### ソースコード③：外乱オブザーバ
```python:wiht_disturbance.py
"""
2021/04/09
@Yuya Shimizu

外乱オブザーバ
"""
import numpy as np
import matplotlib.pyplot as plt
from control import ss, acker
from control.matlab import initial, lsim
from for_plot import plot_set

#係数行列
A = '0 1; -4 5'
B = '0; 1'
C = '1 0'
D = '0'

#状態空間モデル
P = ss(A, B, C, D)

#オブザーバ極
observer_poles = [-15+5j, -15-5j]

#オブザーバゲインの設計（状態フィードバックの双対）
L = -acker(P.A.T, P.C.T, observer_poles).T

#制御対象Pを安定化する状態フィードバックゲインの設計
regulator_poles = [-5+5j, -5-5j]
F = -acker(P.A, P.B, regulator_poles)

#描画にむけて
fig, ax = plt.subplots(1, 2, figsize = (6, 2.3))
Td = np.arange(0, 3, 0.01)   #シミュレーション時間
d = 0.5 * (Td>0)         #ステップ状の外乱
X0 = [-1, 0.5]            #初期状態

#真の状態の振る舞い
Gsf = ss(P.A + P.B*F, P.B, np.eye(2), [[0], [0]])
x, t = initial(Gsf, Td, X0)     #初期条件応答
ax[0].plot(t, x[:, 0], ls='-.', label='${x}_1$')
ax[1].plot(t, x[:, 1], ls='-.', label='${x}_2$')


"""
#オブザーバで推定した状態の振る舞い
#入力　u = Fx
u = [ [F[0, 0]*x[i, 0] + F[0, 1]*x[i, 1]] for i in range(len(x)) ]
#出力　y = Cx
y = x[:, 0] + d     #外乱あり
#オブザーバによる状態推定
Obs = ss(P.A + L*P.C, np.c_[P.B, -L], np.eye(2), [[0, 0], [0, 0]] )
xhat, t, x0 = lsim(Obs, np.c_[u, y], Td, [0, 0])
ax[0].plot(t, xhat[:, 0], label='$\hat{x}_1$')
ax[1].plot(t, xhat[:, 1], label='$\hat{x}_2$')

for i in [0, 1]:
    plot_set(ax[i], 't', ' ', 'best')

ax[0].set_ylabel('$x_1, \hat{x}_1$')
ax[1].set_ylabel('$x_2, \hat{x}_2$')

fig.tight_layout()
fig.suptitle(f"estimation (with disturbance); Obs_Pole={observer_poles}")

plt.show()
"""

##### 外乱オブザーバゲインの設計
observer_poles = [-15+5j, -15-5j, -3]
E = [[0], [0]]
Abar = np.r_[np.c_[P.A, E], np.zeros((1, 3))] #P.Aの中身が2×2だから全体としては3×3
Bbar = np.c_[P.B.T, np.zeros((1, 1))].T
Cbar = np.c_[P.C, 1]

Lbar = -acker(Abar.T, Cbar.T, observer_poles).T

#入力　u = Fx
u = [ [F[0, 0]*x[i, 0] + F[0, 1]*x[i, 1]] for i in range(len(x)) ]
#出力　y = Cx
y = x[:, 0]

Obs = ss(Abar + Lbar*Cbar, np.c_[Bbar, -Lbar], np.eye(3), [[0, 0], [0, 0], [0, 0]] )
xhat, t, x0 = lsim(Obs, np.c_[u, y], Td, [0, 0, 0])
ax[0].plot(t, xhat[:, 0], label='$\hat{x}_1$')
ax[1].plot(t, xhat[:, 1], label='$\hat{x}_2$')

for i in [0, 1]:
    plot_set(ax[i], 't', ' ', 'best')

ax[0].set_ylabel('$x_1, \hat{x}_1$')
ax[1].set_ylabel('$x_2, \hat{x}_2$')

fig.tight_layout()
fig.suptitle(f"estimation (with disturbance) -disturbance Obs; Obs_Pole={observer_poles}")

plt.show()
```

##### 出力
![with_disturbance](https://user-images.githubusercontent.com/61382602/114119633-ffd63480-9925-11eb-9543-13f07353e85d.png)
上図は，ただのオブザーバゲイン設計を行ったもので，拡張した外乱オブザーバを用いなかったときの様子を示している．時間がいくら経過しても推定値が真値に重なることがない様子が見受けられる．
![with_disturbanceObs](https://user-images.githubusercontent.com/61382602/114119631-ff3d9e00-9925-11eb-900a-a4474c45430c.png)
上図は，拡張した外乱オブザーバを用いたときの様子を示している．時間の経過に伴いうまく状態を推定できていることが分かる．

以上のことから，外乱があるときには，その外乱まで考慮した拡張系でなければ，うまく推定ができないということが確認できた．

## 感想
オブザーバは一応学習したことはあったが，いまいちどんな時に，何のために使うのかつかめていなかった．良い復習になったと思う．

## 参考文献
Pyhtonによる制御工学入門　　南 祐樹　著　　オーム社