これは、古典制御理論と現代制御理論の関係を調べた計算ノートのようなものです。
古典制御理論(伝達関数）で記述された制御対称を現代制御理論（状態空間）で表現してみて、それぞれの制御方式で制御した際の
振る舞いを比較してみました。

現代制御理論の状態フィードバックのパラメータは古典制御理論のPID制御のようなわかりやすさにかけるのも、現代制御理論が受け入れられない
原因の一つかもしれません、状態フィードバックのパラメータの取り扱いについて、一つの提案をしています。この方法は取り扱いやすく、
かつ安定なフィードバックを実現できるので、使いやすいのではと思っています。ご意見をお聞かせいただければ、嬉しく思います。

このノートはまた、Python/controlモジュールの使い方の一例を示すものにもなっていると思います。ここで取り上げているのは、古典/現代　制御理論
の入門で取り上げられるような機能しか使っておりません。python/controlでは非線形のシステムの解析などにも役立つ関数が用意されているようです。
python/controlへのお誘いとして、役に立てばいいのですが。

最初はPythonプログラムは無視して、グラフなどを眺めるだけでもいいかと思います。興味がわいたところで、いろいろパラメータを換えてみるなど
試してみると面白いでしょう。

では、始めましょう。

# 伝達関数から状態方程式へ
時間によって変化しない線形なシステムの制御については伝達関数表示を用いる古典制御理論と状態変数表示を使う
現代制御理論の二つがよく知られています。単一入力単一出力(SISO)のシステムでは、これら二つの表示を行き来することは
簡単です。このメモでは、一つの制御対象を例題に二つの制御理論の関係をみていきたいと思います。

## 制御対象システム
このメモで取り上げる制御対象となるシステムとして、「Python による制御工学入門」第5章の垂直駆動アームをの例題を使います。
制御対象の運動方程式は、
$$
J\ddot{\theta}= -\mu \dot{\theta} - M g l \sin{\theta} + \tau(t)
$$
となります。$\tau(t)$ はモータのトルクとします。

線形化することで、

$$
J\ddot{\theta}= -\mu \dot{\theta} - M g l \theta + \tau(t)
$$

と簡単化されます。この方程式を

$$
\frac{d}{dt} \begin{bmatrix}\theta \\ \dot{\theta}\end{bmatrix} =  \begin{bmatrix} 0, & 1 \\  -\frac{M g l}{J},& -\frac{\mu}{J}\\ \end{bmatrix} \begin{bmatrix}\theta \\ \dot{\theta} \end{bmatrix} +\begin{bmatrix}0 \\ \tau(t)\end{bmatrix}
$$

と書き直せることに注意しましょう。

制御対象は、伝達関数で与えられています。
Python による制御工学入門」に従って、制御の無駄時間(遅れ）を導入します。無駄時間(delay)の導入によって、フィードバックシステムの発振が起こります。


```python
import control, numpy
# import sympy, sympy.physics.control,sympy.abc
from numpy import diag,matrix,eye,diag, matmul
from numpy.linalg import eig, matrix_rank
from control import tf2ss, ss2tf,tf,ss,impulse_response, step_response, initial_response
from control.matlab import linspace, logspace
import  matplotlib.pyplot as pyplot

# グラフの既定の設定を変更する。
control.reset_defaults()
# control.use_fbs_defaults()
# control.use_matlab_defaults()
# control.use_numpy_matrix(flag=True, warn=True)
# control.use_legacy_defaults("0.8.4")
# ## モデルの設定
# model:「Python による制御工学入門」　リスト 5.1 144ページ
g=9.81
l=0.2
M=0.5
Mgl=M*g*l
mu=1.5e-2
J=1.0e-2
ref=30 

# 測定の時間送れをパデ近似を使って設定します。
delay=0.005
num_delay,den_delay=control.pade(delay,1) #ここでパデ近似の次数を指定します。
delay_tf=tf(num_delay,den_delay)
#
Plant0=tf([0, 1],[J, mu, Mgl],name="制御モデル 時間遅れ無",inputs="r",outputs="y")
# add delay with pade approximation
Plant=delay_tf*Plant0
Plant.name="制御モデル 時間遅れ有"
print("最小実現(時間遅れ無):",Plant0.minreal())
print("最小実現(時間遅れ有):",Plant.minreal())
print("極(時間遅れ無)：",Plant0.poles(),"零点:(時間遅れ無)",Plant0.zeros())
print("極(時間遅れ有)：", Plant.poles(),"零点(時間遅れ有):",Plant.zeros())
print("DCgain:(時間遅れ無)",Plant0.dcgain(),"(時間遅れ有)",Plant.dcgain())
```

    最小実現(時間遅れ無): 
           100
    ------------------
    s^2 + 1.5 s + 98.1
    
    最小実現(時間遅れ有): 
               -100 s + 4e+04
    -------------------------------------
    s^3 + 401.5 s^2 + 698.1 s + 3.924e+04
    
    極(時間遅れ無)： [-0.75+9.87610753j -0.75-9.87610753j] 零点:(時間遅れ無) []
    極(時間遅れ有)： [-400.  +0.j           -0.75+9.87610753j   -0.75-9.87610753j] 零点(時間遅れ有): [400.+0.j]
    DCgain:(時間遅れ無) 1.019367991845056 (時間遅れ有) 1.019367991845056


### モデルの時間応答グラフ
モデルの制御無の状態での時間応答のグラフを作成します。controlには、forced_response, impulse_response, initial_response,step_response,
input_output_responseといったシステムの時間応答を求める関数が用意されています。ここでは、ステップ応答とインパルス応答を時間遅れ有／無の二つで
比較してみます。 生成した時間応答データのグラフ化には、matplotlibを使います。



```python
pyplot.clf()
fig,axes=pyplot.subplots(2,1)
fig.set_size_inches(10,5)
axes[0].set_title("Step Resonse");axes[1].set_title("impulse Resonse");
axes[0].axhline(1);axes[1].axhline(0);
fig.tight_layout();

T=linspace(0,2,1000)

sres=step_response(Plant0,T)
ires=impulse_response(Plant0,T)
axes[0].plot(sres.t, sres.y[0,0,:], label="Plant0")
axes[1].plot(ires.t, ires.y[0,0,:])

sres=step_response(Plant,T)
ires=impulse_response(Plant,T)
axes[0].plot(sres.t, sres.y[0,0,:], label="Plant")
axes[1].plot(ires.t, ires.y[0,0,:])

fig.legend();
```


    <Figure size 432x288 with 0 Axes>



    
![png](output_3_1.png)
    


## P制御付きシステムの時間応答
モデルにP制御のフィードバックを
### control.feedback
python/control module にはフィードバックループを作成するための関数　`control.feedback` 関数が用意されています。
関数のプロトタイプは、
```
contro.feedback(SYS1, SYS2=1, sign=-1)
```
で、フィードバックループを表現する伝達関数あるいは、状態変数表示を値として返します。
作成された State オブジェクトは プライマリの　State：SYS1と同じ、入力と出力を持ちます。
次のセルのGraphvizで作成した図を参考にしてください。

また、伝達関数(`tf`)や状態変数表示(`ss`)のオブジェクトにも、feedbackメソッドが存在します。`control.feedback(SYS1, SYS2, sign)`と
`SYS1.feedback(SYS2,sign)` は等価です。


```python
"""
## control.feedback(Sys1,Sys2,sign)
"""
#pip install -U graphviz
import graphviz
g=graphviz.Digraph("G", graph_attr={"rankdir":"TB","rank":"same"},engine="dot") # or circo
g.attr(fontname="Helvetica,Arial,sans-serif")
g.attr(label="control.feedback(Sys1,Sys2,sign) or Sys1.feedback(Sys2,sign)")
#g.attr(orientation="L")

g.attr("node",fontname="Helvetica,Arial,sans-serif")
g.attr("edge",fontname="Helvetica,Arial,sans-serif")

g.node("input", shape="plaintext")
g.node("mixer", label="", shape="ellipse", height="0.1", width="0.1",)
g.node("output", shape="plaintext")
g.node("splitter", label="", shape="point", height="0.05", width="0.05")
g.node("corner1", label="", shape="point", height="0.02", width="0.02")
g.node("corner2", label="", shape="point", height="0.02", width="0.02")

g.node("Sys1",shape="box")
g.node("Sys2",shape="box")

with g.subgraph(name="upper") as sg:
  sg.graph_attr['rankdir']='LR'
  sg.graph_attr['rank']='same'
  sg.edge("input","mixer", label="r")
  sg.edge("mixer","Sys1", label="u")
  sg.edge("Sys1","splitter")
  sg.edge("splitter","output",label="y")
  
g.edge("splitter","corner2",label="y")
g.edge("corner1","mixer",label="sign*fb")

with g.subgraph(name="lower") as sg:
  sg.attr("graph",rankdir="LR",rank="same")
  sg.edge("Sys2","corner2",dir="back")
  sg.edge("corner1","Sys2",dir="back")
g
```




    
![svg](output_5_0.svg)
    



時間遅れ有のモデル(`Plant`)に異なるゲインのP制御を追加したシステム $\frac{ P K}{1 + P K}$の時間応答を調べてみます。
`kp=3`の場合には、振動がほぼ一定値で続いていることがわかります。  このフィードバックされたシステムは、`control.feedback(P*K, 1, -1)`あるいは `(P*K).feedback()`と書くことができます。


```python
"""
## control.feedback(Plant*K, 1, -1) or (Plant*K).feedback()
"""
#pip install -U graphviz
import graphviz
g=graphviz.Digraph("G", graph_attr={"rankdir":"TB","rank":"same"},engine="dot") 
g.attr(fontname="Helvetica,Arial,sans-serif")
g.attr(label="control.feedback(P*K, 1, -1) or (P*K).feedback())")
#g.attr(orientation="L")

g.attr("node",fontname="Helvetica,Arial,sans-serif")
g.attr("edge",fontname="Helvetica,Arial,sans-serif")

g.node("input", shape="plaintext")
g.node("mixer", label="", shape="ellipse", height="0.1", width="0.1",)
g.node("output", shape="plaintext")
g.node("corner1", label="", shape="point", height="0.02", width="0.02")
g.node("splitter", label="", shape="point", height="0.05", width="0.05")
g.node("corner2", label="", shape="point", height="0.02", width="0.02")

g.node("K",shape="box")
g.node("P",shape="box")
g.node("U",shape="box", label="1")

with g.subgraph(name="upper") as sg:
  sg.graph_attr['rankdir']='LR'
  sg.graph_attr['rank']='same'
  sg.edge("input","mixer", label="r")
  sg.edge("mixer","K", label="u")
  sg.edge("K","P")
  sg.edge("P","splitter")
  sg.edge("splitter","output",label="y")
  
with g.subgraph(name="lower") as sg:
  sg.attr("graph",rankdir="LR",rank="same")
  sg.edge("corner1","U",dir="back")
  sg.edge("U","corner2",dir="back")

g.edge("corner1","mixer",label="-1")
g.edge("splitter","corner2",label="y")

g
```




    
![svg](output_7_0.svg)
    




```python
pyplot.clf()
fig,axes=pyplot.subplots(2,1)
fig.set_size_inches(10,5)
axes[0].axhline(1, color="k", linewidth=0.5)
axes[1].axhline(0, color="k", linewidth=0.5)
axes[0].set_title("Step Resonse");axes[1].set_title("impulse Resonse");

T=linspace(0,3,1000)
kps=(1, 1.76, 3)

for kp in (kps):
  K=tf([kp],[1])  # feedback gain kp
  fbsys=(Plant*K).feedback()  #signの既定値はsign=-1です。
  fbsys=control.feedback(Plant*K, 1, -1)
  sres=step_response(fbsys,T)
  ires=impulse_response(fbsys,T)
  axes[0].plot(sres.t, sres.y[0,0,:], label=f"kp={kp}")
  axes[1].plot(ires.t, ires.y[0,0,:])

fig.tight_layout(); fig.legend();
```


    <Figure size 432x288 with 0 Axes>



    
![png](output_8_1.png)
    


## P制御されたモデルの周波数応答を表示します。
P制御されたモデルの周波数応答をみるために、bode図を作成してみましょう。controlモジュールの`bode()`関数はSIOの伝達関数あるいは状態変数表示
を引数に取ります。


```python
#omega=linspace(0.1,1e3,1024)
omega=logspace(1,1e2,1024)
print("margin        : (gm:gain margin, pm:phase margin, pc:phase cross over, gc:gain cossover)")
print("margin(plant) :", control.margin(Plant))

bp=control.bode_plot(Plant,omega_limits=(1,1e2),omega_num=1024, plot=True,label=f"Plant")

for kp in kps:
  K=tf([kp],[1])
  fbsys=control.feedback(Plant*K,1)
  bp=control.bode_plot(fbsys,omega_limits=(1,1e2),omega_num=1024, plot=True,label=f"kp={kp}")
  print("margin(kp={:02.1f}):".format(kp), control.margin(fbsys))

fig=pyplot.gcf()
axes=fig.axes
axes[1].axhline(-180,color="red", linewidth=1.0)
axes[0].legend()
fig.set_size_inches(10,5)
fig.tight_layout()
```

    margin        : (gm:gain margin, pm:phase margin, pc:phase cross over, gc:gain cossover)
    margin(plant) : (3.007450405489704, 8.110565187521445, 19.933764307100382, 13.995414100411582)
    margin(kp=1.0): (2.0074504054897044, 4.992367243147015, 19.933764307100382, 17.21148740405995)
    margin(kp=1.8): (0.7087786394827867, -1.7661127433755155, 19.933764307100382, 21.180681929127427)
    margin(kp=3.0): (0.0024834684965693034, -7.511516128136748, 19.93376430710039, 26.372392813967004)



    
![png](output_10_1.png)
    


### ナイキスト線図
ナイキスト線図も`nyquist_plot()`を使うことで、簡単に作成できます。
原点付近での振る舞いを調べるために、表示範囲を明示的に指定しています。


```python
#ナイキスト線図
np=control.nyquist_plot(Plant, omega_limits=(0.01,1e3),omega_num=4096, plot=True)
for kp in kps:
  fbsys=control.feedback(Plant*tf([kp],[1]),1)
  np=control.nyquist_plot(fbsys, omega_limits=(0.01,1e3),omega_num=4096*2, plot=True)
pyplot.tight_layout()
axes=pyplot.gca()
axes.set_xlim(-2,2)
axes.set_ylim(-1,1)
lines=axes.lines
[l.set_label(leg) for l,leg in zip(lines[::8],("Plant",)+tuple((f"kp={kp}" for kp in kps)))]
pyplot.gcf().set_size_inches(10,5)
pyplot.legend()
```




    <matplotlib.legend.Legend at 0x14da3ad60>




    
![png](output_12_1.png)
    


# 限界感度法によるフィードバック
P制御されたモデルの時間応答／周波数応答をしらべることで、`kp=3.0`付近が限界感度であることが確認できました。
「Pythonによる制御工学入門」に従って限界感度法を使ったリスト5.12のゲインチューニングの結果を調べてみましょう。
PID制御のコントローラーの伝達関数を定義します。



```python
T0=0.3
kp0=2.9
kp=0.2*kp0
ki=kp/(0.5 * T0)
kd=kp*(0.33 * T0)
print(kp,ki,kd)

PID_Cnt=tf([kd,kp,ki],[1,0])
PID_Cnt
```

    0.58 3.8666666666666667 0.05742





$$\frac{0.05742 s^2 + 0.58 s + 3.867}{s}$$



このコントローラーを使ったフィードバックは、次のようになります。


```python
fbsys=control.feedback(Plant*PID_Cnt)
print(fbsys.dcgain(), Plant.dcgain())
fbsys
```

    1.0 1.019367991845056





$$\frac{-0.05742 s^3 + 22.39 s^2 + 228.1 s + 1547}{0.01 s^4 + 3.958 s^3 + 29.37 s^2 + 620.5 s + 1547}$$



### 限界感度法フィードバックの応答
それでは、限界感度法フィードバックを使ったシステムの時間応答、周波数応答をみてみましょう。


```python
LimSenFB=fbsys 
print(fbsys)
pyplot.clf()
fig,axes=pyplot.subplots(2,1)
fig.set_size_inches(10,5)
axes[0].set_title("Step Resonse");axes[1].set_title("impulse Resonse");
axes[0].axhline(1, color="k", linewidth=0.5)
axes[1].axhline(0, color="k", linewidth=0.5)

T=linspace(0,3,1000)
sres=step_response(Plant,T,output=0)
ires=impulse_response(Plant,T,input=0)
axes[0].plot(sres.t,sres.y[0,0,:],label="Plant")
axes[1].plot(ires.t,ires.y[0,0,:])

#sres=step_response(fbsys,T,input=0,output=0)
sres=step_response(fbsys,T,output=0)
ires=impulse_response(fbsys,T,input=0)
axes[0].plot(sres.t,sres.y[0,0,:],label="FB")
axes[1].plot(ires.t,ires.y[0,0,:])
axes[0].legend()
fig.tight_layout()
```

    
        -0.05742 s^3 + 22.39 s^2 + 228.1 s + 1547
    -------------------------------------------------
    0.01 s^4 + 3.958 s^3 + 29.37 s^2 + 620.5 s + 1547
    



    <Figure size 432x288 with 0 Axes>



    
![png](output_18_2.png)
    



```python
omega=linspace(0.1,1e2,1000)
bp=control.bode_plot(fbsys,omega,label="FB")
bp=control.bode_plot(Plant,omega,label="Plant")

pyplot.legend()
fig=pyplot.gcf()
axes=fig.axes
fig.set_size_inches(10,5)
axes[0].axhline(1, color="r", linewidth=0.5)
axes[1].axhline(-180, color="r", linewidth=0.5)
```




    <matplotlib.lines.Line2D at 0x14dad1e80>




    
![png](output_19_1.png)
    



```python
np=control.nyquist_plot(fbsys, omega_limits=(0.1,1e3),omega_num=10000, plot=True)
xlim=pyplot.gcf().axes[0].get_xlim()
ylim=pyplot.gcf().axes[0].get_ylim()
np=control.nyquist_plot(Plant, omega_limits=(0.1,1e3),omega_num=10000, plot=True)
pyplot.gcf().axes[0].set_xlim(xlim)
pyplot.gcf().axes[0].set_ylim(ylim)
lines=pyplot.gca().lines
[l.set_label(leg) for l,leg in zip(lines[::8],("FB","Plant"))]
pyplot.gcf().set_size_inches(10,5)
pyplot.legend();pyplot.tight_layout()
```


    
![png](output_20_0.png)
    



```python
control.margin(LimSenFB), control.dcgain(LimSenFB)
```




    ((67.11139373729841, 100.7707762678616, 391.3017858416141, 13.993743426305345),
     1.0)



# 状態空間へ
伝達関数を状態空間表示へ変換してみましょう。python/controlモジュールの`tf2ss`関数を使います。
伝達関数の分母の次数に応じて、状態空間の次数が決まります。極や零点の位置が変わらないこと、`ss2tf`を使って状態空間表示を伝達関数に変換すると元の伝達関数と等価な伝達関数に戻っていることなどが、確認できます。



```python
Plant.zeros
```




    <bound method TransferFunction.zeros of TransferFunction(array([ -1., 400.]), array([1.000e-02, 4.015e+00, 6.981e+00, 3.924e+02]))>




```python
SS=tf2ss(Plant,name="Plant")
dim_state=SS.nstates
# 極と零点の確認
print(SS, SS.poles(),SS.zeros(), SS.nstates)
print(eig(SS.A)[0])
#伝達関数がもとに戻ることを確認
print(ss2tf(SS).minreal(),Plant.minreal())
```

    A = [[-4.01500000e+02  6.98100000e+00  3.92400000e+00]
         [-1.00000000e+02 -4.14602353e-15  3.17619603e-15]
         [ 0.00000000e+00  1.00000000e+02  2.06822897e-14]]
    
    B = [[-0.1]
         [ 0. ]
         [ 0. ]]
    
    C = [[  0. -10.  40.]]
    
    D = [[0.]]
     [-400.  +0.j           -0.75+9.87610753j   -0.75-9.87610753j] [400.+0.j] 3
    [-400.  +0.j           -0.75+9.87610753j   -0.75-9.87610753j]
    
               -100 s + 4e+04
    -------------------------------------
    s^3 + 401.5 s^2 + 698.1 s + 3.924e+04
     
               -100 s + 4e+04
    -------------------------------------
    s^3 + 401.5 s^2 + 698.1 s + 3.924e+04
    


## 可制御性／可観測性
このシステムの可制御性、可観測性を調べましょう。


```python
#可制御？
numpy.linalg.matrix_rank(control.ctrb(SS.A,SS.B)) == SS.nstates
```




    True




```python
#可観測？
numpy.linalg.matrix_rank(control.obsv(SS.A,SS.C))==  SS.nstates
```




    True



状態空間表示を直接物理的な運動方程式から導くこともできます。

運動方程式から、状態変数表示の状態方程式を導きます。入力のトルクは振り子の角度と単位が一致するように、スケールしておきます。


$$
\frac{d}{dt} \begin{bmatrix}\theta \\ \dot{\theta}\end{bmatrix} =  \begin{bmatrix} 0, & 1 \\  -\frac{M g l}{J},& -\frac{\mu}{J}\\ \end{bmatrix} \begin{bmatrix}\theta \\ \dot{\theta} \end{bmatrix} +\begin{bmatrix}0 \\ \frac{M g l}{J} \end{bmatrix}\theta_{ref}(t)
$$


```python
phym=ss([[0,1],[-Mgl/J, -mu/J]],[[0],[1/J]],
        [[1,0]],[[0]],
        name="physical model")
phym
```




$$
\left(\begin{array}{rllrll|rll}
0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&1\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
-98.&\hspace{-1em}1&\hspace{-1em}\phantom{\cdot}&-1.&\hspace{-1em}5&\hspace{-1em}\phantom{\cdot}&100\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
\hline
1\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
\end{array}\right)
$$




```python
Delay=tf2ss(tf(num_delay,den_delay))
(Delay*phym).minreal()
```




$$
\left(\begin{array}{rllrllrll|rll}
21.&\hspace{-1em}9&\hspace{-1em}\phantom{\cdot}&-6.&\hspace{-1em}44&\hspace{-1em}\phantom{\cdot}&1.&\hspace{-1em}2&\hspace{-1em}\phantom{\cdot}&9.&\hspace{-1em}7&\hspace{-1em}\phantom{\cdot}\\
94.&\hspace{-1em}5&\hspace{-1em}\phantom{\cdot}&-23.&\hspace{-1em}3&\hspace{-1em}\phantom{\cdot}&4.&\hspace{-1em}85&\hspace{-1em}\phantom{\cdot}&-2.&\hspace{-1em}43&\hspace{-1em}\phantom{\cdot}\\
0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&-5.&\hspace{-1em}15&\hspace{-1em}\phantom{\cdot}&-400\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
\hline
0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&-8\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
\end{array}\right)
$$




```python
control.canonical_form(Delay*phym,"observable")
```




    (StateSpace(array([[-4.015e+02,  1.000e+00,  0.000e+00],
            [-6.981e+02,  0.000e+00,  1.000e+00],
            [-3.924e+04,  0.000e+00,  0.000e+00]]), array([[ 4.6040208e-16],
            [-1.0000000e+02],
            [ 4.0000000e+04]]), array([[1., 0., 0.]]), array([[0.]])),
     array([[-1.0000000e+00,  4.6040208e-18,  8.0000000e+01],
            [ 3.9850000e+02, -1.0000000e+00,  1.2000000e+02],
            [ 6.0000000e+02,  4.0000000e+02,  7.8480000e+03]]))




```python
ss2tf((Delay*phym)).minreal(),Plant.minreal()
```




    (TransferFunction(array([ -100., 40000.]), array([1.000e+00, 4.015e+02, 6.981e+02, 3.924e+04])),
     TransferFunction(array([ -100., 40000.]), array([1.000e+00, 4.015e+02, 6.981e+02, 3.924e+04])))



この状態空間表示は、伝達関数から変換した状態空間表示とは異なっています。これらが等価であることを、それぞれのシステムを
`canonical_form`に変換して確認してみましょう。`canonical_form`は状態空間表示を等価な `reachable`, `observable`, `modal` のいずれかの
正準型に変換できます。ここでは"reachable"な正準型に変換してみましょう。


```python
SS.minreal()
```




$$
\left(\begin{array}{rllrllrll|rll}
-353\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&-89.&\hspace{-1em}4&\hspace{-1em}\phantom{\cdot}&3.&\hspace{-1em}51&\hspace{-1em}\phantom{\cdot}&0.&\hspace{-1em}0968&\hspace{-1em}\phantom{\cdot}\\
-194\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&-24.&\hspace{-1em}5&\hspace{-1em}\phantom{\cdot}&-5.&\hspace{-1em}17&\hspace{-1em}\phantom{\cdot}&0.&\hspace{-1em}025&\hspace{-1em}\phantom{\cdot}\\
0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&97.&\hspace{-1em}2&\hspace{-1em}\phantom{\cdot}&-23.&\hspace{-1em}5&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
\hline
0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&-41.&\hspace{-1em}2&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
\end{array}\right)
$$




```python
SS_R, U_SS_R=control.canonical_form(SS,"observable")
SS_R
```




$$
\left(\begin{array}{rllrllrll|rll}
-402\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&1\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&7.&\hspace{-1em}43&\hspace{-1em}\cdot10^{-15}\\
-698\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&1\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&-100\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
-3.&\hspace{-1em}92&\hspace{-1em}\cdot10^{4}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&4\phantom{.}&\hspace{-1em}&\hspace{-1em}\cdot10^{4}\\
\hline
1\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
\end{array}\right)
$$




```python
PY_R, U_PY_R=control.canonical_form(Delay*phym,"observable")
PY_R
```




$$
\left(\begin{array}{rllrllrll|rll}
-402\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&1\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&4.&\hspace{-1em}6&\hspace{-1em}\cdot10^{-16}\\
-698\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&1\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&-100\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
-3.&\hspace{-1em}92&\hspace{-1em}\cdot10^{4}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&4\phantom{.}&\hspace{-1em}&\hspace{-1em}\cdot10^{4}\\
\hline
1\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
\end{array}\right)
$$



と計算誤差を除いて、等価なシステムであることが確認できました。

#　状態フィードバック


```python
"""
## control.feedback(Sys1,Sys2,sign)
"""
#pip install -U graphviz
import graphviz
g=graphviz.Digraph("G", graph_attr={"rankdir":"TB","rank":"same"},engine="dot") # or circo
g.attr(fontname="Helvetica,Arial,sans-serif")
g.attr(label="状態フィードバック")
#g.attr(orientation="L")

g.attr("node",fontname="Helvetica,Arial,sans-serif")
g.attr("edge",fontname="Helvetica,Arial,sans-serif")

g.node("input", shape="plaintext")
g.node("mixer", label="", shape="ellipse", height="0.1", width="0.1",)
g.node("output", shape="plaintext")
g.node("splitter", label="", shape="point", height="0.05", width="0.05")
g.node("corner1", label="", shape="point", height="0.02", width="0.02")
g.node("corner2", label="", shape="point", height="0.02", width="0.02")

g.node("Plant",shape="box")
g.node("Obsv",shape="box")

with g.subgraph(name="upper") as sg:
  sg.graph_attr['rankdir']='LR'
  sg.graph_attr['rank']='same'
  sg.edge("input","mixer", label="r")
  sg.edge("mixer","Plant", label="u")
  sg.edge("Plant","splitter")
  sg.edge("splitter","output",label="y")
  
g.edge("splitter","corner2",label="y")
g.edge("corner1","mixer",label=" -K X")

with g.subgraph(name="lower") as sg:
  sg.attr("graph",rankdir="LR",rank="same")
  sg.edge("Obsv","corner2",dir="back")
  sg.edge("corner1","Obsv",dir="back")
g
```




    
![svg](output_40_0.svg)
    




```python
matmul(SS.C.transpose(),SS.C)
```




    array([[   0.,    0.,    0.],
           [   0.,  100., -400.],
           [   0., -400., 1600.]])



[#] ここでの重み関数の選び方は、結局最適制御の目標汎函数として$\int dt X^{T}QX$ ではなく、$\int dt Y^{T} Q Y$ を採用していることに相当する。システムに対する入力$u$と出力$y$を最適化する方が、物理的にはもっともらしい。状態推定器についても同様でしょう。 $D$がゼロでない時にどうなるかは、検討が必要。LQEでは$X$と$U$のクロスタームがもともとあるので、問題はないはず。


```python
# 状態フィードバックゲイン
#Qw=2e3
# Qとして、matmul(SS.C.transpose(),SS.C)を使ってみる、これは $\int dt y^{T} y $を最小化するのと等価。
Qw=10
Q=matmul(SS.C.transpose(),SS.C)
#K,P,e_K=control.lqr(SS.A, SS.B, eye(SS.nstates),diag([1/Qw]))
K,P,e_K=control.lqr(SS.A, SS.B, Q ,diag([1/Qw]))

K=matrix(K) # あるいは、行列としての積がが必要なところで、numpy.matmulを使う。
K, e_K, eig(SS.A-numpy.matmul(SS.B,K))[0], numpy.linalg.det(SS.A-numpy.matmul(SS.B,K))
```




    (matrix([[-201.38833773,  828.85280728,   93.19782541]]),
     array([-400.       +0.j      ,  -10.819417+14.629928j,
             -10.819417-14.629928j], dtype=complex64),
     array([-400.         +0.j        ,  -10.81941689+14.62992761j,
             -10.81941689-14.62992761j]),
     -132437.82541261593)




```python
#推定器ゲイン
Gw=10
H,P,e_H=control.lqe(SS.A, 
                  #SS.nstates*[[1]],
                  Gw*SS.B,
                  SS.C, 
                  diag([1]),
                  diag([1]))
H=matrix(H) # あとで、行列としての演算が必要なのでarrayではなく、matrixにしておく。
print("H^{T} =",H.transpose(),"\ne_H:", e_H)
```

    H^{T} = [[0.01370951 0.20783127 1.0797191 ]] 
    e_H: [-400.       +0.j        -21.305225+23.470995j  -21.305225-23.470995j]



```python
# ObsvはPlantからの出力　:math:`y` を入力として、フィードバック用の出力　:math:`u`を出力する。
# この推定器だと、元のシステムで入力が0の場合の動作しか推定していない。
Obsv=control.ss(
  SS.A - numpy.matmul(H,SS.C) - numpy.matmul(SS.B, K), H,
  -K , numpy.matrix([0]))
Obsv.name="Observer"
Obsv
```




$$
\left(\begin{array}{rllrllrll|rll}
-422\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&90\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&12.&\hspace{-1em}7&\hspace{-1em}\phantom{\cdot}&0.&\hspace{-1em}0137&\hspace{-1em}\phantom{\cdot}\\
-100\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&2.&\hspace{-1em}08&\hspace{-1em}\phantom{\cdot}&-8.&\hspace{-1em}31&\hspace{-1em}\phantom{\cdot}&0.&\hspace{-1em}208&\hspace{-1em}\phantom{\cdot}\\
0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&111\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&-43.&\hspace{-1em}2&\hspace{-1em}\phantom{\cdot}&1.&\hspace{-1em}08&\hspace{-1em}\phantom{\cdot}\\
\hline
201\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&-829\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&-93.&\hspace{-1em}2&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
\end{array}\right)
$$




```python
-SS.C*matrix(SS.A - matmul(SS.B,K))**(-1)*SS.B
```




    matrix([[0.30202852]])




```python
-matrix(SS.C)*(SS.A-matmul(SS.B, K))**(-1)*matrix(SS.B)
```




    matrix([[0.30202852]])




```python
-SS.C*matrix(SS.A - numpy.matmul(SS.B,K))**(-1)*SS.B*K*matrix(SS.A-numpy.matmul(H,SS.C))**(-1)*H
```




    matrix([[-0.35135434]])




```python
SS.C*matrix(SS.A - numpy.matmul(SS.B,K))**(-1)*SS.B*K*matrix(SS.A-numpy.matmul(H,SS.C))**(-1)*SS.B
```




    matrix([[0.35918011]])




```python
numpy.matmul(SS.C,SS.B)
```




    array([[0.]])




```python
-SS.C*(matrix(SS.B) - matrix(SS.A - numpy.matmul(SS.B,K))**(-1)*numpy.matmul(H,SS.C)*matrix(SS.A-numpy.matmul(H,SS.C))**(-1)*matrix(SS.B))
```




    matrix([[0.56168636]])




```python
numpy.linalg.eig(SS.A-numpy.matmul(H,SS.C)- numpy.matmul(SS.B,K))
```




    (array([-400.39058452 +0.j        ,  -31.17935032+35.00624191j,
             -31.17935032-35.00624191j]),
     matrix([[ 0.96817791+0.j        ,  0.06001544+0.06343206j,
               0.06001544-0.06343206j],
             [ 0.23902823+0.j        ,  0.10241463+0.29852843j,
               0.10241463-0.29852843j],
             [-0.074142  +0.j        ,  0.94486324+0.j        ,
               0.94486324-0.j        ]]))




```python
print(SS,Obsv)
print(Obsv*SS)
```

    A = [[-4.01500000e+02  6.98100000e+00  3.92400000e+00]
         [-1.00000000e+02 -4.14602353e-15  3.17619603e-15]
         [ 0.00000000e+00  1.00000000e+02  2.06822897e-14]]
    
    B = [[-0.1]
         [ 0. ]
         [ 0. ]]
    
    C = [[  0. -10.  40.]]
    
    D = [[0.]]
     <LinearIOSystem>: Observer
    Inputs (1): ['u[0]']
    Outputs (1): ['y[0]']
    States (3): ['x[0]', 'x[1]', 'x[2]']
    
    A = [[-421.63883377   90.00337586   12.695402  ]
         [-100.            2.07831273   -8.31325093]
         [   0.          110.79719103  -43.18876412]]
    
    B = [[0.01370951]
         [0.20783127]
         [1.0797191 ]]
    
    C = [[ 201.38833773 -828.85280728  -93.19782541]]
    
    D = [[0.]]
    
    <LinearICSystem>: sys[135]
    Inputs (1): ['u[0]']
    Outputs (1): ['y[0]']
    States (6): ['Plant_x[0]', 'Plant_x[1]', 'Plant_x[2]', 'Observer_x[0]', 'Observer_x[1]', 'Observer_x[2]']
    
    A = [[-4.01500000e+02  6.98100000e+00  3.92400000e+00  0.00000000e+00
           0.00000000e+00  0.00000000e+00]
         [-1.00000000e+02 -4.14602353e-15  3.17619603e-15  0.00000000e+00
           0.00000000e+00  0.00000000e+00]
         [ 0.00000000e+00  1.00000000e+02  2.06822897e-14  0.00000000e+00
           0.00000000e+00  0.00000000e+00]
         [ 0.00000000e+00 -1.37095135e-01  5.48380540e-01 -4.21638834e+02
           9.00033759e+01  1.26954020e+01]
         [ 0.00000000e+00 -2.07831273e+00  8.31325093e+00 -1.00000000e+02
           2.07831273e+00 -8.31325093e+00]
         [ 0.00000000e+00 -1.07971910e+01  4.31887641e+01  0.00000000e+00
           1.10797191e+02 -4.31887641e+01]]
    
    B = [[-0.1]
         [ 0. ]
         [ 0. ]
         [ 0. ]
         [ 0. ]
         [ 0. ]]
    
    C = [[   0.            0.            0.          201.38833773 -828.85280728
           -93.19782541]]
    
    D = [[0.]]
    



```python
(SS.A-matmul(H,SS.C))**(-1)
```




    matrix([[-0.00206838, -0.00169544,  0.00016468],
            [ 0.01074561, -0.04314362,  0.00914443],
            [ 0.02756697, -0.11068138,  0.00030512]])




```python
fbsys=SS.feedback(Obsv,1) 
print("dcgain:",fbsys.dcgain())
# 出力はSSの出力。入力も　SSと同じ (u)
#fbsys=control.feedback(SS,Obsv,1) # same as above
#以下は似ているが違うシステム
#fbsys=control.feedback(Obsv*SS,1,1)　# 出力は　u
#fbsys=control.feedback(SS*Obsv,1,1) # 出力は　y 、入力は　y
#fbsys=(SS*Obsv).feedback(1,1) # 　入力は 観測値yで出力もy
#fbsys=(Obsv*SS).feedback(1,1) # 入力はuで出力は u
# 分離定理が成り立っていることの確認
#print(e_K,"\n", e_H,"\n",
#      numpy.linalg.eig(fbsys.A)[0]
#     )
print(numpy.concatenate([
  eig(SS.A-numpy.matmul(SS.B,K))[0],
  eig(SS.A-numpy.matmul(H,SS.C))[0]],0),"\n",
  numpy.linalg.eig(fbsys.A)[0]
)

T=linspace(0,3,1000)
pyplot.clf()
fig,axes=pyplot.subplots(2,1)
fig.set_size_inches(10,5)

#r0=-0.0001/4
r0=0.1
sres=step_response(SS,T)
ires=impulse_response(SS,T)
axes[0].plot(sres.t,sres.y[0,0,:],label="Plant")
axes[1].plot(ires.t,ires.y[0,0,:])

sres=step_response(fbsys,T)
ires=impulse_response(fbsys,T)
axes[0].plot(sres.t,sres.y[0,0,:], label="State FB")
axes[1].plot(ires.t,ires.y[0,0,:])
axes[0].axhline(1,color="black",linewidth=0.5)
axes[1].axhline(0,color="black",linewidth=0.5)
fig.legend();fig.tight_layout()
```

    dcgain: 0.6612086261047866
    [-400.         +0.j          -10.81941689+14.62992761j
      -10.81941689-14.62992761j -400.         +0.j
      -21.30522569+23.47099576j  -21.30522569-23.47099576j] 
     [-399.99999942 +0.j         -400.00000058 +0.j
      -10.81941689+14.62992761j  -10.81941689-14.62992761j
      -21.30522569+23.47099576j  -21.30522569-23.47099576j]



    <Figure size 432x288 with 0 Axes>



    
![png](output_55_2.png)
    



```python
pyplot.clf()
fig,axes=pyplot.subplots(2,1)
fig.set_size_inches(10,5)

r0=-0.001
sres=step_response(SS,T,r0)
ires=impulse_response(SS,T)
axes[0].plot(sres.t,sres.y[0,0,:],label="Plant")
axes[1].plot(ires.t,ires.y[0,0,:])

sres=step_response(fbsys,T,r0)
ires=impulse_response(fbsys,T)
axes[0].plot(sres.t,sres.y[0,0,:],label="State FB")
axes[1].plot(ires.t,ires.y[0,0,:])
axes[0].set_title("Step Response")
axes[1].set_title("Initial Response")

axes[0].axhline(1,color="black",linewidth=0.5)
axes[1].axhline(0,color="black",linewidth=0.5)
fig.tight_layout()
fig.legend()
print("DC gain",fbsys.dcgain())
```

    DC gain 0.6612086261047866



    <Figure size 432x288 with 0 Axes>



    
![png](output_56_2.png)
    


状態フィードバックのDCゲインが1ではないので、さらに外側にフィードバックをかけてDCゲインが1になるようにしてみる。


```python
FFB=fbsys.feedback(-(1-fbsys.dcgain())/fbsys.dcgain())
#kp=(1-fbsys.dcgain())/fbsys.dcgain()
#ki=0
#PIFB=control.tf([kp,ki],[1,0])
#FFB=(fbsys*PIFB).feedback()

print(FFB.dcgain())
#FFB=fbsys.feedback(-0.001)

pyplot.clf()
fig,axes=pyplot.subplots(2,1)
pyplot.gcf().set_size_inches(10,5)

sres=step_response(FFB,T)
ires=impulse_response(FFB,T,)
axes[0].plot(sres.t,sres.y[0,0,:],label="FB")
axes[1].plot(ires.t,ires.y[0,0,:])

sres=step_response(fbsys,T)
ires=impulse_response(fbsys,T,)
axes[0].plot(sres.t,sres.y[0,0,:],label="SFB")
axes[1].plot(ires.t,ires.y[0,0,:])

sres=step_response(LimSenFB,T)
ires=impulse_response(LimSenFB,T,)
axes[0].plot(sres.t,sres.y[0,0,:],label="SensLimFB")
axes[1].plot(ires.t,ires.y[0,0,:])

sres=step_response(SS,T)
ires=impulse_response(SS,T,)
axes[0].plot(sres.t,sres.y[0,0,:],label="Plant")
axes[1].plot(ires.t,ires.y[0,0,:])

axes[0].axhline(1,color="black",linewidth=0.5)
axes[1].axhline(0,color="black",linewidth=0.5)
fig.legend()
fig.tight_layout()
```

    1.0000000000000029



    <Figure size 432x288 with 0 Axes>



    
![png](output_58_2.png)
    



```python
omega=linspace(1,1e2,4000)
bp=control.bode_plot(FFB,omega,label="FB")
bp=control.bode_plot(fbsys,bp[2],label="SFB")
bp=control.bode_plot(LimSenFB,bp[2],label="SensLimFB")
bp=control.bode_plot(SS,omega,label="Plant")
pyplot.gcf().axes[0].axhline(1,color="black",linewidth=0.7)
pyplot.gcf().axes[1].axhline(-180,color="red",linewidth=0.7)

pyplot.gcf().set_size_inches(10,5)
pyplot.gcf().axes[1].legend();pyplot.tight_layout()
```


    
![png](output_59_0.png)
    



```python
omega=linspace(0.1,1e2,4000)
np=control.nyquist_plot(FFB,omega, plot=True)
np=control.nyquist_plot(fbsys, omega, plot=True)
np=control.nyquist_plot(LimSenFB, omega, plot=True)
xlim=pyplot.gcf().axes[0].get_xlim()
ylim=pyplot.gcf().axes[0].get_ylim()
np=control.nyquist_plot(SS, omega_limits=(0.1,1e3),omega_num=4000, plot=True)
pyplot.gcf().axes[0].set_xlim(xlim)
pyplot.gcf().axes[0].set_ylim(ylim)
pyplot.tight_layout()
pyplot.gcf().set_size_inches(10,5)
```


    
![png](output_60_0.png)
    



```python
SS.nstates
```




    3



## 制御対象への入力に追従する状態推定器を構成する。
状態推定器に入力　$u$ を伝えるため、出力に $u$を追加する。
プラントを状態空間へ変換するだけでなく、制御値 $u$ と出力の目標値　$r$　を出力として持つ、状態空間に拡張することが必要です。


```python
"""
## control.feedback(Sys1,Sys2,sign)
"""
#pip install -U graphviz
import graphviz
g=graphviz.Digraph("G", graph_attr={"rankdir":"TB","rank":"same"},engine="dot") # or circo
g.attr(fontname="Helvetica,Arial,sans-serif")
g.attr(label="状態フィードバック")

#g.attr(orientation="L")

g.attr("node",fontname="Helvetica,Arial,sans-serif")
g.attr("edge",fontname="Helvetica,Arial,sans-serif")

g.node("input", shape="plaintext")
g.node("mixer", label="", shape="ellipse", height="0.1", width="0.1",)
g.node("output", shape="plaintext")
g.node("splitter", label="", shape="point", height="0.05", width="0.05")
g.node("corner1", label="", shape="point", height="0.02", width="0.02")
g.node("corner2", label="", shape="point", height="0.02", width="0.02")

g.node("Plant",shape="box")
g.node("Obsv",shape="box")

with g.subgraph(name="upper") as sg:
  sg.graph_attr['rankdir']='LR'
  sg.graph_attr['rank']='same'
  sg.edge("input","mixer", label="r")
  sg.edge("mixer","Plant", label="u")
  sg.edge("Plant","splitter")
  sg.edge("splitter","output",label="y")
  
g.edge("splitter","corner2",label="y")
g.edge("corner1","mixer",label=" -K X")
g.edge("Plant","Obsv",label="u")

with g.subgraph(name="lower") as sg:
  sg.attr("graph",rankdir="LR",rank="same")
  sg.edge("Obsv","corner2",dir="back")
  sg.edge("corner1","Obsv",dir="back")
g
```




    
![svg](output_63_0.svg)
    




```python
#拡張された 状態空間
SSe=ss(SS.A, SS.B,
       numpy.concatenate([SS.C,[SS.nstates*[0]]],0),
       numpy.concatenate([SS.D,[[1]]],0)) # 出力に u を加える。
```


```python
SSe
```




$$
\left(\begin{array}{rllrllrll|rll}
-401\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&6.&\hspace{-1em}98&\hspace{-1em}\phantom{\cdot}&3.&\hspace{-1em}92&\hspace{-1em}\phantom{\cdot}&-0.&\hspace{-1em}1&\hspace{-1em}\phantom{\cdot}\\
-100\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&-4.&\hspace{-1em}15&\hspace{-1em}\cdot10^{-15}&3.&\hspace{-1em}18&\hspace{-1em}\cdot10^{-15}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&100\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&2.&\hspace{-1em}07&\hspace{-1em}\cdot10^{-14}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
\hline
0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&-10\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&40\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&1\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
\end{array}\right)
$$




```python
#SSeからのuを使う オブザーバ を定義します。状態推定器のゲインは同じものを使います。
Obsv_e=control.ss(
  SSe.A - H*SS.C, 
  numpy.concatenate([H, SS.B - numpy.matmul(H,SS.D)], 1),
  -K , 
  numpy.matrix([[0,0]]))
Obsv_e
```




$$
\left(\begin{array}{rllrllrll|rllrll}
-401\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&7.&\hspace{-1em}12&\hspace{-1em}\phantom{\cdot}&3.&\hspace{-1em}38&\hspace{-1em}\phantom{\cdot}&0.&\hspace{-1em}0137&\hspace{-1em}\phantom{\cdot}&-0.&\hspace{-1em}1&\hspace{-1em}\phantom{\cdot}\\
-100\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&2.&\hspace{-1em}08&\hspace{-1em}\phantom{\cdot}&-8.&\hspace{-1em}31&\hspace{-1em}\phantom{\cdot}&0.&\hspace{-1em}208&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&111\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&-43.&\hspace{-1em}2&\hspace{-1em}\phantom{\cdot}&1.&\hspace{-1em}08&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
\hline
201\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&-829\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&-93.&\hspace{-1em}2&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
\end{array}\right)
$$




```python
fbsys_e=SSe.feedback(Obsv_e, 1) # 出力はSSの出力+u。入力も　SS_eと同じ (u)
#固有値の確認
print(eig(SS.A - numpy.matmul(SS.B, K))[0],"\n",
      eig(SS.A - numpy.matmul(H, SS.C))[0],"\n",
      numpy.linalg.eig(fbsys_e.A)[0]
     )
fbsys_e
```

    [-400.         +0.j          -10.81941689+14.62992761j
      -10.81941689-14.62992761j] 
     [-400.         +0.j          -21.30522569+23.47099576j
      -21.30522569-23.47099576j] 
     [-399.99999942 +0.j         -400.00000058 +0.j
      -10.81941689+14.62992761j  -10.81941689-14.62992761j
      -21.30522569+23.47099576j  -21.30522569-23.47099576j]





$$
\left(\begin{array}{rllrllrllrllrllrll|rll}
-401\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&6.&\hspace{-1em}98&\hspace{-1em}\phantom{\cdot}&3.&\hspace{-1em}92&\hspace{-1em}\phantom{\cdot}&-20.&\hspace{-1em}1&\hspace{-1em}\phantom{\cdot}&82.&\hspace{-1em}9&\hspace{-1em}\phantom{\cdot}&9.&\hspace{-1em}32&\hspace{-1em}\phantom{\cdot}&-0.&\hspace{-1em}1&\hspace{-1em}\phantom{\cdot}\\
-100\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&-4.&\hspace{-1em}15&\hspace{-1em}\cdot10^{-15}&3.&\hspace{-1em}18&\hspace{-1em}\cdot10^{-15}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&100\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&2.&\hspace{-1em}07&\hspace{-1em}\cdot10^{-14}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&-0.&\hspace{-1em}137&\hspace{-1em}\phantom{\cdot}&0.&\hspace{-1em}548&\hspace{-1em}\phantom{\cdot}&-422\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&90\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&12.&\hspace{-1em}7&\hspace{-1em}\phantom{\cdot}&-0.&\hspace{-1em}1&\hspace{-1em}\phantom{\cdot}\\
0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&-2.&\hspace{-1em}08&\hspace{-1em}\phantom{\cdot}&8.&\hspace{-1em}31&\hspace{-1em}\phantom{\cdot}&-100\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&2.&\hspace{-1em}08&\hspace{-1em}\phantom{\cdot}&-8.&\hspace{-1em}31&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&-10.&\hspace{-1em}8&\hspace{-1em}\phantom{\cdot}&43.&\hspace{-1em}2&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&111\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&-43.&\hspace{-1em}2&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
\hline
0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&-10\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&40\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&201\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&-829\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&-93.&\hspace{-1em}2&\hspace{-1em}\phantom{\cdot}&1\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
\end{array}\right)
$$




```python
T=linspace(0,3,1000)
pyplot.clf()
fig,axes=pyplot.subplots(2,1)
pyplot.gcf().set_size_inches(10,5)

#r0=-0.0001/4
r0=0.1
sres=step_response(SS,T)
ires=impulse_response(SS,T)
axes[0].plot(sres.t,sres.y[0,0,:],label="Plant")
axes[1].plot(ires.t,ires.y[0,0,:])

sres=step_response(fbsys_e,T,output=0,input=0)
ires=impulse_response(fbsys_e,T,output=0,input=0)
axes[0].plot(sres.t,sres.y[0,0,:], label="State FB")
axes[1].plot(ires.t,ires.y[0,0,:])
axes[0].axhline(1.0,color='black',linewidth=0.5)
axes[1].axhline(0.0,color='black',linewidth=0.5)

fig.legend();fig.tight_layout()
```

    /Library/Frameworks/Python.framework/Versions/3.9/lib/python3.9/site-packages/control/timeresp.py:1812: UserWarning: System has direct feedthrough: ``D != 0``. The infinite impulse at ``t=0`` does not appear in the output.
    Results may be meaningless!
      warnings.warn("System has direct feedthrough: ``D != 0``. The "



    <Figure size 432x288 with 0 Axes>



    
![png](output_68_2.png)
    



```python
#DC gain: -> .dcgain(): .dcgain()の結果と、平衡値の理論式との比較
print("DC gain:",fbsys_e.dcgain().transpose()[0])
# r -> y, r->u
( (-matrix(SS.C-matmul(SS.D,K))*matrix(SS.A-matmul(SS.B,K))**(-1)*SS.B + SS.D)[0,0], #r->y
 (1+K*matrix(SS.A-matmul(SS.B,K))**(-1)*SS.B )[0,0])# r-> u
```

    DC gain: [0.30202852 0.29628998]





    (0.3020285169692138, 0.2962899751468011)




```python
#出力の応答も見てみましょう
pyplot.clf()
fig,axes=pyplot.subplots(2, 1)
pyplot.gcf().set_size_inches(10,5)

sres=step_response(fbsys_e, T, output=1, input=0)
ires=impulse_response(fbsys_e, T, output=1, input=0)
axes[0].plot(sres.t, sres.y[0,0,:], label="State FB")
axes[1].plot(ires.t, ires.y[0,0,:])
fig.legend();fig.tight_layout()
```


    <Figure size 432x288 with 0 Axes>



    
![png](output_70_1.png)
    


状態フィードバックだけでは、状態を0とするようにフィードバックが働くので、dcgainが1より小さくなっている。
この外側にP制御を追加してDCgainが1になるよう調整してみます。


```python
# 1 loop correction
# DC gain for u
#dcgain=-SS.C*matrix(SS.A-numpy.matmul(SS.B,K))**(-1)*SS.B
dcgain=fbsys_e.dcgain()[0,0]
FFB=fbsys_e.feedback(ss([],[],[], [1,0]), (1-dcgain)/dcgain) # positive feedback 
print(f"{dcgain=}")
print(f"{FFB.dcgain()[0][0]=}")
```

    dcgain=0.3020285169692138
    FFB.dcgain()[0][0]=0.9999999999999885



```python
pyplot.clf()
fig,axes=pyplot.subplots(2,1)
pyplot.gcf().set_size_inches(10,5)

sres=step_response(FFB,T)
ires=impulse_response(FFB,T,)
axes[0].plot(sres.t,sres.y[0, 0,:],label="FB")
axes[1].plot(ires.t,ires.y[0, 0,:])

sres=step_response(fbsys_e,T)
ires=impulse_response(fbsys_e,T,)
axes[0].plot(sres.t,sres.y[0,0,:],label="SFB")
axes[1].plot(ires.t,ires.y[0,0,:])

sres=step_response(LimSenFB,T)
ires=impulse_response(LimSenFB,T,)
axes[0].plot(sres.t,sres.y[0,0,:],label="SensLimFB")
axes[1].plot(ires.t,ires.y[0,0,:])

sres=step_response(SS,T)
ires=impulse_response(SS,T,)
axes[0].plot(sres.t,sres.y[0,0,:],label="Plant")
axes[1].plot(ires.t,ires.y[0,0,:])
fig.legend()
fig.tight_layout()
```


    <Figure size 432x288 with 0 Axes>



    
![png](output_73_1.png)
    



```python
#出力の応答も見てみましょう
pyplot.clf()
fig,axes=pyplot.subplots(2, 1)
pyplot.gcf().set_size_inches(10,5)

sres=step_response(FFB, T, output=1, input=0)
ires=impulse_response(FFB, T, output=1, input=0)
axes[0].plot(sres.t, sres.y[0,0,:], label="State FB") # step
axes[0].plot(sres.t, sres.outputs, label="output") # step
axes[1].plot(ires.t, ires.y[0,0,:])
axes[1].plot(ires.t, ires.outputs[:])
fig.legend();fig.tight_layout()
```


    <Figure size 432x288 with 0 Axes>



    
![png](output_74_1.png)
    



```python
omega=linspace(1,1e2,4000)
bp=control.bode_plot(ss([],[],[], [1,0])*FFB,omega,label="FB")
bp=control.bode_plot(ss([],[],[], [1,0])*fbsys_e,bp[2],label="SFB")
bp=control.bode_plot(LimSenFB,bp[2],label="SensLimFB")
bp=control.bode_plot(SS,omega,label="Plant")
pyplot.legend();pyplot.tight_layout()
pyplot.gcf().set_size_inches(10,5)
```


    
![png](output_75_0.png)
    



```python
np=control.nyquist_plot(ss([],[],[], [1,0])*FFB, omega_limits=(0.1,1e3),omega_num=4000, plot=True)
np=control.nyquist_plot(ss([],[],[], [1,0])*fbsys_e, omega_limits=(0.1,1e3),omega_num=4000, plot=True)
np=control.nyquist_plot(LimSenFB, omega_limits=(0.1,1e3),omega_num=4000, plot=True)
pyplot.gcf().set_size_inches(10,5)
```


    
![png](output_76_0.png)
    


# 外乱オブザーバを使ったフィードバックを構成する。
外乱オブザーバ(「Pythonによる制御工学入門」Page. 233 ) を使った制御システムを作ってみます。
外乱オブザーバを使ったシステムは、入力 $u$ に対するDCgainは元のプラントと同じになることも確認してみます。


```python
"""
## control.feedback(Sys1,Sys2,sign)
"""
#pip install -U graphviz
import graphviz
g=graphviz.Digraph("G", graph_attr={"rankdir":"TB","rank":"same"},engine="dot") # or circo
g.attr(fontname="Helvetica,Arial,sans-serif")
g.attr(label="外乱フィードバック")

#g.attr(orientation="L")

g.attr("node",fontname="Helvetica,Arial,sans-serif")
g.attr("edge",fontname="Helvetica,Arial,sans-serif")

g.node("input", shape="plaintext")
g.node("mixer", label="", shape="ellipse", height="0.1", width="0.1",)
g.node("output", shape="plaintext")
g.node("splitter", label="", shape="point", height="0.05", width="0.05")
g.node("corner1", label="", shape="point", height="0.02", width="0.02")
g.node("corner2", label="", shape="point", height="0.02", width="0.02")

g.node("Plant",shape="box")
g.node("Obsv",shape="box")

with g.subgraph(name="upper") as sg:
  sg.graph_attr['rankdir']='LR'
  sg.graph_attr['rank']='same'
  sg.node("disturbance", shape="plaintext")

with g.subgraph(name="middle") as sg:
  sg.graph_attr['rankdir']='LR'
  sg.graph_attr['rank']='same'
  sg.edge("input","mixer", label="r")
  sg.edge("mixer","Plant", label="u")
  sg.edge("Plant","splitter")
  sg.edge("splitter","output",label="y")
  
g.edge("splitter","corner2",label="y")
g.edge("corner1","mixer",label=" -K X")
g.edge("disturbance","mixer",label="d")

with g.subgraph(name="lower") as sg:
  sg.attr("graph",rankdir="LR",rank="same")
  sg.edge("Obsv","corner2",dir="back")
  sg.edge("corner1","Obsv",dir="back")
g
```




    
![svg](output_78_0.svg)
    




```python
# 外乱オブザーバ のあるシステムでのObserver gainを求めてみる。
Target=SS
# 状態空間の切り替えにうまく対応できていない。
PhyM=ss(delay_tf*phym)
#PhyM=control.canonical_form(delay_tf*phym,"observable")[0].minreal()
#Target=PhyM 
Q=matmul(Target.C.transpose(),Target.C)
K_T,*result=control.lqr(Target.A, Target.B, Q ,diag([1/Qw]))

Ad=numpy.concatenate( 
  ( numpy.concatenate( (Target.A - matmul(Target.B, K_T), numpy.zeros((Target.nstates,1))),1),
    numpy.zeros((1,Target.nstates+1))
  ),
  0
)
Bd=numpy.concatenate(
  (
   (Target.B, 
    [[0]])
  )
  ,0
)
Cd=numpy.concatenate((Target.C, [[1]]),1)
Dd=numpy.zeros((1,1))
print("Observable?", matrix_rank(control.obsv(Ad,Cd))== Target.nstates+1)# observable!
print("Controllable?",matrix_rank(control.ctrb(Ad,Bd))== Target.nstates+1)# not cntrolable 

gamma=0.0
G=(Target.nstates+1)*[[1]]
G=Bd
G[-1]=[1]
for beta in ( 0.1,1,10,100):# weight parameter for disturbance in $G$
  print("Hd for beta={}\n".format(beta), 
        control.lqe(Ad, beta*G, Cd, diag([1]) ,diag([1]))[0])

# 状態変数と外乱の推定器ゲインを求める。 外乱推定を行わない推定器のゲイン＄H$を使っても大きな違いは出ない。
beta=20
g=30
G=Bd
G=beta*G
G[-1]=[g]
print("Controllable?",matrix_rank(control.ctrb(Ad,G))== Target.nstates+1)# this case is contrallable.
Hd,*rest=control.lqe(Ad, G, Cd, diag([1]) ,diag([1]))
Hd, matmul(Hd,Cd), matmul(H,Target.C)
```

    Observable? True
    Controllable? False
    Hd for beta=0.1
     [[ 5.46878986e-06]
     [-1.10671081e-06]
     [ 2.47892393e-04]
     [ 1.00000000e-01]]
    Hd for beta=1
     [[5.28218075e-04]
     [1.88341753e-04]
     [2.22379888e-02]
     [1.00000000e+00]]
    Hd for beta=10
     [[ 0.01253813]
     [ 0.08939591]
     [ 0.49702284]
     [10.        ]]
    Hd for beta=100
     [[ -1.36229129]
     [  1.69987341]
     [  1.53696267]
     [100.        ]]
    Controllable? True





    (array([[-0.04663812],
            [ 0.2819605 ],
            [ 0.70396577],
            [30.        ]]),
     array([[ 0.00000000e+00,  4.66381206e-01, -1.86552482e+00,
             -4.66381206e-02],
            [ 0.00000000e+00, -2.81960501e+00,  1.12784200e+01,
              2.81960501e-01],
            [ 0.00000000e+00, -7.03965768e+00,  2.81586307e+01,
              7.03965768e-01],
            [ 0.00000000e+00, -3.00000000e+02,  1.20000000e+03,
              3.00000000e+01]]),
     matrix([[      0.        ,  -36267.48727403,  145069.94909613],
             [      0.        , -173506.66492087,  694026.65968349],
             [      0.        ,  -41109.94787284,  164439.79149138]]))




```python
## 外乱オブザーバを定義します。
Obsv_d=control.ss(
  Ad - matmul(Hd, Cd),
  Hd,
  numpy.concatenate((K_T, [[gamma]]),1) , numpy.matrix([[0]]),
  name="Obsv_d"
)
print(Obsv_d)
```

    <LinearIOSystem>: Obsv_d
    Inputs (1): ['u[0]']
    Outputs (1): ['y[0]']
    States (4): ['x[0]', 'x[1]', 'x[2]', 'x[3]']
    
    A = [[-4.21638834e+02  8.93998995e+01  1.51093074e+01  4.66381206e-02]
         [-1.00000000e+02  2.81960501e+00 -1.12784200e+01 -2.81960501e-01]
         [ 0.00000000e+00  1.07039658e+02 -2.81586307e+01 -7.03965768e-01]
         [ 0.00000000e+00  3.00000000e+02 -1.20000000e+03 -3.00000000e+01]]
    
    B = [[-0.04663812]
         [ 0.2819605 ]
         [ 0.70396577]
         [30.        ]]
    
    C = [[-201.38833773  828.85280728   93.19782541    0.        ]]
    
    D = [[0.]]
    



```python
fbsys_d=Target.feedback(Obsv_d) # 出力はSSの出力+u。入力も　SS_eと同じ (u)
fbsys_d.name="fbsys_d"
fbsys_d
```




$$
\left(\begin{array}{rllrllrllrllrllrllrll|rll}
-401\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&6.&\hspace{-1em}98&\hspace{-1em}\phantom{\cdot}&3.&\hspace{-1em}92&\hspace{-1em}\phantom{\cdot}&-20.&\hspace{-1em}1&\hspace{-1em}\phantom{\cdot}&82.&\hspace{-1em}9&\hspace{-1em}\phantom{\cdot}&9.&\hspace{-1em}32&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&-0.&\hspace{-1em}1&\hspace{-1em}\phantom{\cdot}\\
-100\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&-4.&\hspace{-1em}15&\hspace{-1em}\cdot10^{-15}&3.&\hspace{-1em}18&\hspace{-1em}\cdot10^{-15}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&100\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&2.&\hspace{-1em}07&\hspace{-1em}\cdot10^{-14}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0.&\hspace{-1em}466&\hspace{-1em}\phantom{\cdot}&-1.&\hspace{-1em}87&\hspace{-1em}\phantom{\cdot}&-422\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&89.&\hspace{-1em}4&\hspace{-1em}\phantom{\cdot}&15.&\hspace{-1em}1&\hspace{-1em}\phantom{\cdot}&0.&\hspace{-1em}0466&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&-2.&\hspace{-1em}82&\hspace{-1em}\phantom{\cdot}&11.&\hspace{-1em}3&\hspace{-1em}\phantom{\cdot}&-100\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&2.&\hspace{-1em}82&\hspace{-1em}\phantom{\cdot}&-11.&\hspace{-1em}3&\hspace{-1em}\phantom{\cdot}&-0.&\hspace{-1em}282&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&-7.&\hspace{-1em}04&\hspace{-1em}\phantom{\cdot}&28.&\hspace{-1em}2&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&107\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&-28.&\hspace{-1em}2&\hspace{-1em}\phantom{\cdot}&-0.&\hspace{-1em}704&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&-300\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&1.&\hspace{-1em}2&\hspace{-1em}\cdot10^{3}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&300\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&-1.&\hspace{-1em}2&\hspace{-1em}\cdot10^{3}&-30\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
\hline
0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&-10\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&40\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
\end{array}\right)
$$




```python
print(tf(fbsys_d.minreal()))
fbsys_d.dcgain()
```

    
                  -100 s^5 - 7698 s^4 + 1.574e+07 s^3 + 1.22e+09 s^2 + 4.595e+10 s + 1.589e+11
    --------------------------------------------------------------------------------------------------------
    s^7 + 878.5 s^6 + 2.256e+05 s^5 + 1.491e+07 s^4 + 5.11e+08 s^3 + 8.654e+09 s^2 + 8.396e+10 s + 1.559e+11
    





    1.0193679918450373




```python
print(fbsys_d.poles(),"\n",fbsys_d.zeros())
```

    [-400.         +0.j         -400.33306731 +0.j
      -27.0760555 +22.7187251j   -27.0760555 -22.7187251j
      -10.81941689+14.62992761j  -10.81941689-14.62992761j
       -2.35384738 +0.j        ] 
     [-400.79149195 +0.j         -36.17870477+35.7786829j
      -36.17870477-35.7786829j   -3.82895798 +0.j
      400.         +0.j       ]



```python
print(fbsys_e.poles(),"\n",fbsys_e.zeros())
```

    [-399.99999942 +0.j         -400.00000058 +0.j
      -10.81941689+14.62992761j  -10.81941689-14.62992761j
      -21.30522569+23.47099576j  -21.30522569-23.47099576j] 
     [ -21.30522569+23.47099576j  -21.30522569-23.47099576j
     -400.         +0.j        ]



```python
# 外乱オブザーバを入れた時に期待される DCgain(次の節参照）
-Target.C*matrix(Target.A)**(-1)*Target.B
```




    matrix([[1.01936799]])




```python
for s in (fbsys_d, fbsys_e, FFB, LimSenFB, Target):
  print(s.name, s.dcgain(),"\n\t",s.poles())
```

    fbsys_d 1.0193679918450373 
    	 [-400.         +0.j         -400.33306731 +0.j
      -27.0760555 +22.7187251j   -27.0760555 -22.7187251j
      -10.81941689+14.62992761j  -10.81941689-14.62992761j
       -2.35384738 +0.j        ]
    sys[166] [[0.30202852]
     [0.29628998]] 
    	 [-399.99999942 +0.j         -400.00000058 +0.j
      -10.81941689+14.62992761j  -10.81941689-14.62992761j
      -21.30522569+23.47099576j  -21.30522569-23.47099576j]
    sys[173] [[1.   ]
     [0.981]] 
    	 [-398.77529207 +0.j         -400.         +0.j
      -21.30522569+23.47099576j  -21.30522569-23.47099576j
       -5.92012251 +0.j          -16.94341919 +0.j        ]
    sys[94] 1.0 
    	 [-388.6087963  +0.j           -2.21733464+11.90386149j
       -2.21733464-11.90386149j   -2.71453442 +0.j        ]
    Plant 1.0193679918450478 
    	 [-400.  +0.j           -0.75+9.87610753j   -0.75-9.87610753j]



```python
# 時間応答の比較
pyplot.clf()
fig,axes=pyplot.subplots(2,1)
pyplot.gcf().set_size_inches(10,5)

T=linspace(0,3,10000)

sres=step_response(fbsys_d,T)
ires=impulse_response(fbsys_d,T,)
axes[0].plot(sres.t,sres.y[0, 0,:],label="FB_D")
axes[1].plot(ires.t,ires.y[0, 0,:])

sres=step_response(FFB,T)
ires=impulse_response(FFB,T,)
axes[0].plot(sres.t,sres.y[0,0,:],label="FFB")
axes[1].plot(ires.t,ires.y[0,0,:])

sres=step_response(fbsys_e,T)
ires=impulse_response(fbsys_e,T,)
axes[0].plot(sres.t,sres.y[0,0,:],label="SFB")
axes[1].plot(ires.t,ires.y[0,0,:])
             
sres=step_response(LimSenFB,T)
ires=impulse_response(LimSenFB,T,)
axes[0].plot(sres.t,sres.y[0,0,:],label="SensLimFB")
axes[1].plot(ires.t,ires.y[0,0,:])

sres=step_response(SS,T)
ires=impulse_response(SS,T,)
axes[0].plot(sres.t,sres.y[0,0,:],label="Plant")
axes[1].plot(ires.t,ires.y[0,0,:])
fig.legend()
fig.tight_layout()
```

    /Library/Frameworks/Python.framework/Versions/3.9/lib/python3.9/site-packages/control/timeresp.py:1812: UserWarning: System has direct feedthrough: ``D != 0``. The infinite impulse at ``t=0`` does not appear in the output.
    Results may be meaningless!
      warnings.warn("System has direct feedthrough: ``D != 0``. The "



    <Figure size 432x288 with 0 Axes>



    
![png](output_87_2.png)
    



```python
omega=linspace(1.0,1e2,4000)
bp=control.bode_plot(fbsys_d,omega,label="FB_D")
bp=control.bode_plot(ss([],[],[], [1,0])*FFB, bp[2],label="FFB")
bp=control.bode_plot(ss([],[],[], [1,0])*fbsys_e,bp[2],label="SFB")
bp=control.bode_plot(LimSenFB,bp[2],label="SensLimFB")
bp=control.bode_plot(SS,omega,label="Plant")
pyplot.gcf().set_size_inches(10,5)
pyplot.legend();pyplot.tight_layout()
```


    
![png](output_88_0.png)
    



```python
omega=linspace(1.0,1e2,40000)
pyplot.clf()
np=control.bode_plot([
  fbsys_d,
  ss([],[],[], [1,0])*FFB,
  ss([],[],[], [1,0])*fbsys_e,
  LimSenFB,
  ],
  omega,
)
pyplot.gca().axhline(-180,color="red", linewidth=0.5)
pyplot.gcf().set_size_inches(10,5)
pyplot.tight_layout()
```


    
![png](output_89_0.png)
    


### Python-controlで利用可能なその他のプロット


```python
np=control.nyquist_plot([fbsys_d,
                         ss([],[],[], [1,0])*FFB,
                         ss([],[],[], [1,0])*fbsys_e,
                         LimSenFB,
                        ], omega_limits=(0.001,1e2), omega_num=4000, plot=True)
fig=pyplot.gcf()
fig.set_size_inches(10,5)
fig.tight_layout()
```


    
![png](output_91_0.png)
    



```python
#    gm : float
#        Gain margin
#    pm : float
#        Phase margin (in degrees)
#    wcg : float or array_like
#        Crossover frequency associated with gain margin (phase crossover
#        frequency), where phase crosses below -180 degrees.
#    wcp : float or array_like
#        Crossover frequency associated with phase margin (gain crossover
#        frequency), where gain crosses below 1.

for s in [fbsys_d,
  ss([],[],[], [1,0])*FFB,
  ss([],[],[], [1,0])*fbsys_e,
  LimSenFB, Plant
  ]:
  print(f"name: {s.name},\n\t dcgain:{s.dcgain()}, \n\t poles:{s.poles()}\n\t margin:{control.margin(s)}")
```

    name: fbsys_d,
    	 dcgain:1.0193679918450373, 
    	 poles:[-400.         +0.j         -400.33306731 +0.j
      -27.0760555 +22.7187251j   -27.0760555 -22.7187251j
      -10.81941689+14.62992761j  -10.81941689-14.62992761j
       -2.35384738 +0.j        ]
    	 margin:(7.776876025634035, 171.63322451057388, 35.16135765234983, 0.6017375201797025)
    name: sys[378],
    	 dcgain:0.9999999999999885, 
    	 poles:[-398.77529207 +0.j         -400.         +0.j
      -21.30522569+23.47099576j  -21.30522569-23.47099576j
       -5.92012251 +0.j          -16.94341919 +0.j        ]
    	 margin:(46.81557959923929, inf, 67.351055495972, nan)
    name: sys[381],
    	 dcgain:0.3020285169692138, 
    	 poles:[-399.99999942 +0.j         -400.00000058 +0.j
      -10.81941689+14.62992761j  -10.81941689-14.62992761j
      -21.30522569+23.47099576j  -21.30522569-23.47099576j]
    	 margin:(44.504633963923915, inf, 67.35105549597202, nan)
    name: sys[94],
    	 dcgain:1.0, 
    	 poles:[-388.6087963  +0.j           -2.21733464+11.90386149j
       -2.21733464-11.90386149j   -2.71453442 +0.j        ]
    	 margin:(67.11139373729841, 100.7707762678616, 391.3017858416141, 13.993743426305345)
    name: 制御モデル 時間遅れ有,
    	 dcgain:1.019367991845056, 
    	 poles:[-400.  +0.j           -0.75+9.87610753j   -0.75-9.87610753j]
    	 margin:(3.007450405489704, 8.110565187521445, 19.933764307100382, 13.995414100411582)



```python
control.nichols_plot(
  (fbsys_d,
   ss([],[],[], [1,0])*FFB,
   ss([],[],[], [1,0])*fbsys_e,
   LimSenFB, 
   Plant
  ) ,linspace(0.1,1e2,40000))
pyplot.gca().legend(["fbsys_d","FFB","fbsys_e","LimSensFB", "Target Plant"])
pyplot.gcf().set_size_inches(8,5)
pyplot.tight_layout()
```


    
![png](output_93_0.png)
    


### Gang of 4 plot
入力の二つのシステム $P$ , $C$　に対して、
```
 L = P * C
 S = feedback(1, L)
 T = L * S
```
で定義される$T,S$を使い、　$[T, PS; CS, S]$の四つの周波数応答をプロットしたもの


```python
control.gangof4_plot(fbsys_d,Plant, omega)
fig=pyplot.gcf()
pyplot.gcf().set_size_inches(10,5)
#fig.legend()
fig.tight_layout()
```


    
![png](output_95_0.png)
    


### singular values plot


```python
control.singular_values_plot([fbsys_d,
                                ss([],[],[], [1,0])*FFB,
                                ss([],[],[], [1,0])*fbsys_e,
                                LimSenFB, Plant
                               ]
                               ,omega)
fig=pyplot.gcf()
fig.set_size_inches(10,5); fig.tight_layout()
pyplot.gca().legend(["fbsys_d","FFB","fbsys_e","LimSensFB", "Target Plant"])
```




    <matplotlib.legend.Legend at 0x14fb4cd60>




    
![png](output_97_1.png)
    


### タイムドメインでの応答
python/controlモジュールの機能を使って、タイムドメインでのシステムの振る舞いを見てみます。


```python
# control.forced_response 関数を使って、矩形入力に対する応答を見てみる。
# control.matlab.lsimも同様の機能をもつ。
Nd=4096;Tw=10; Tsw=5
T=numpy.linspace(0,Tw,Nd)
U=numpy.zeros(Nd)
U[0:Nd//(Tw//Tsw)]=1

pyplot.clf()
fig,axes=pyplot.subplots(2,2, figsize=(8,5))
fig.set_size_inches(16,5)

# control.matlab.lsim 関数
result=control.forced_response(fbsys_d,T=T,U=U)
axes[0,0].plot(result.time,result.outputs, label="fbsys_d")
axes[0,0].plot(result.time,result.states[-1,:], label="displacement estimate")
ufb=numpy.array(matmul(-K, result.states[0:3,:]))[0,:]
axes[1,0].plot(result.time, ufb, label="ufb")
axes[1,0].plot(result.time,result.inputs, label="input")

# control.input_output_response 関数
# 第一引数は　SSとは別のクラスオブジェクトが必要
result=control.input_output_response(control.ss2io(fbsys_d), T=T, U=U)

axes[0,1].plot(result.time,result.outputs, label="fbsys_d")
axes[0,1].plot(result.time,result.states[-1,:], label="displacement estimate")
ufb=numpy.array(matmul(-K, result.states[0:3,:]))[0,:]
axes[1,1].plot(result.time, ufb, label="ufb")
axes[1,1].plot(result.time,result.inputs, label="input")
pyplot.tight_layout()
pyplot.gcf().axes[0].legend()
#axes[0,0].legend();axes[0,1].legend()
```




    <matplotlib.legend.Legend at 0x14dfa7160>




    <Figure size 432x288 with 0 Axes>



    
![png](output_99_2.png)
    



```python
# margin関数は、システムの以下の情報を表示します。
# gm : float
#        Gain margin
#    pm : float
#        Phase margin (in degrees)
#    wcg : float or array_like
#        Crossover frequency associated with gain margin (phase crossover
#        frequency), where phase crosses below -180 degrees.
#    wcp : float or array_like
#        Crossover frequency associated with phase margin (gain crossover
#        frequency), where gain crosses below 1.
print(control.margin(fbsys_d),control.margin(Plant))
```

    (6.9837149160099345, 173.680387035502, 34.01352620030383, 0.2524178632090365) (3.007450405489704, 8.110565187521445, 19.933764307100382, 13.995414100411582)


## 等価な状態空間を用いた解析

二つの状態空間の状態変数が $X' = U X$ の関係を持つ時、状態方程式の係数が $A = U A' U^{-1}, B= U B', C= C U^{-1}, D= D'$ と変換されれば、等価な状態方程式を与える。　この時、最適状態制御の制御則は、$ u = -K' X' = - K X$を与える　$K$ および　$K$は　$K' = K U$で与えられる。この時、最適化されている量は、

$\int dt \left[ X^{T} Q X + u^{T} R u \right]= \int dt \left[ {X'}^{T} U^{T} Q U X' + u^{T} R u \right]
となり、$X'$空間で見た時の、状態変数に対する重み関数 $Q'$は　$Q$とは異なり対称ではあるが、対角な行列とは限らない。このため、単純に対角な重み関数を選んでも、元の状態空間での結果を再現することはできない。

最適化の条件を$\int dt \left[ X^{T} Q X + u^{T} R u \right]$ではなく、物理的な入出力に限って
$\int dt \left[ Y^{T} Q Y + u^{T} R u \right]$を極小とすることで、状態空間の選択によらないフィードバックが得られるであろう。


```python
# 状態フィードバックゲイン
PhyM=control.canonical_form(Delay*phym,"observable")[0].minreal()
#Qw=2e3
# Qとして、matmul(SS.C.transpose(),SS.C)を使ってみる、これは $\int dt y^{T} y $を最小化するのと等価。
Qw=10
Q=matmul(PhyM.C.transpose(), PhyM.C)
#K,P,e_K=control.lqr(SS.A, SS.B, eye(SS.nstates),diag([1/Qw]))
K,P,e_K=control.lqr(PhyM.A, PhyM.B, Q ,diag([1/Qw]))

K=matrix(K) # あるいは、行列としての積がが必要なところで、numpy.matmulを使う。
print(K, e_K, eig(PhyM.A-numpy.matmul(PhyM.B,K))[0],
      numpy.linalg.det(PhyM.A-numpy.matmul(PhyM.B,K))
     )
#推定器ゲイン
Gw=10
H,P,e_H=control.lqe(PhyM.A, 
                  Gw*PhyM.B,
                  PhyM.C, 
                  diag([1]),
                  diag([1]))
H=matrix(H) # あとで、行列としての演算が必要なのでarrayではなく、matrixにしておく。
print("H^{T} =",H.transpose(),"\ne_H:", e_H)

# ObsvはPlantからの出力　:math:`y` を入力として、フィードバック用の出力　:math:`u`を出力する。
# この推定器だと、元のシステムで入力が0の場合の動作しか推定していない。
Obsv=control.ss(
  PhyM.A - numpy.matmul(H,PhyM.C) - numpy.matmul(PhyM.B, K), H,
  -K , numpy.matrix([0]))
Obsv.name="Observer"
Obsv
```

    [[ 0.05157826  0.00492469 -0.00057985]] [-400.       +0.j        -10.819417+14.629928j  -10.819417-14.629928j] [ -10.81941689+14.62992761j  -10.81941689-14.62992761j
     -400.         +0.j        ] -132437.82541129898
    H^{T} = [[ 3626.7487274  17350.66649209  4110.99478728]] 
    e_H: [-400.       +0.j        -21.305202+23.470829j  -21.305202-23.470829j]





$$
\left(\begin{array}{rllrllrll|rll}
-20.&\hspace{-1em}6&\hspace{-1em}\phantom{\cdot}&-1.&\hspace{-1em}97&\hspace{-1em}\phantom{\cdot}&-40\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&3.&\hspace{-1em}63&\hspace{-1em}\cdot10^{3}\\
105\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&0.&\hspace{-1em}492&\hspace{-1em}\phantom{\cdot}&-181\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&1.&\hspace{-1em}74&\hspace{-1em}\cdot10^{4}\\
-5.&\hspace{-1em}86&\hspace{-1em}\cdot10^{-15}&100\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&-443\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}&4.&\hspace{-1em}11&\hspace{-1em}\cdot10^{3}\\
\hline
-0.&\hspace{-1em}0516&\hspace{-1em}\phantom{\cdot}&-0.&\hspace{-1em}00492&\hspace{-1em}\phantom{\cdot}&0.&\hspace{-1em}00058&\hspace{-1em}\phantom{\cdot}&0\phantom{.}&\hspace{-1em}&\hspace{-1em}\phantom{\cdot}\\
\end{array}\right)
$$




```python
fbsys=PhyM.feedback(Obsv,1) 
print("dcgain:",fbsys.dcgain())

T=linspace(0,3,1000)
pyplot.clf()
fig,axes=pyplot.subplots(2,1)
fig.set_size_inches(10,5)

#r0=-0.0001/4
r0=0.1
sres=step_response(SS,T)
ires=impulse_response(SS,T)
axes[0].plot(sres.t,sres.y[0,0,:],label="Plant")
axes[1].plot(ires.t,ires.y[0,0,:])

sres=step_response(fbsys,T)
ires=impulse_response(fbsys,T)
axes[0].plot(sres.t,sres.y[0,0,:], label="State FB")
axes[1].plot(ires.t,ires.y[0,0,:])
axes[0].axhline(1,color="black",linewidth=0.5)
axes[1].axhline(0,color="black",linewidth=0.5)
fig.legend();fig.tight_layout()
```

    dcgain: 0.661209708732448



    <Figure size 432x288 with 0 Axes>



    
![png](output_103_2.png)
    


## 外乱オブザーバを入れた時に期待されるDCgainの説明

$\frac{d \delta}{dt} = 0$ から、$C \left(\hat{x} - x \right) = \delta$ となる。
これを $\frac{d \hat{x}}{d t} = 0$に代入すると、$0= \left(A - B K\right)\hat{x}$ となるが、$K$の選択によって、$A- B K$は正則。
従って、$\hat{X} =0 $。$\frac{d X} {d t}=0$　の条件は、結局、　$A X + B r = 0$ に帰着される。
$A$が正則の場合には、
$$
y= -C A^{-1} B r
$$
となります。

一般には、$A$は正則とは限りませんが、その場合は$\det{A} = 0$となります。これは、システムは$s=0$に極をもち、入力の積分を含むことを意味しています。
この場合、入力が $r=0$ でなければ状態変数が発散してしまいます。ということで、DC Gainが意味を持つためには $A$ が正則である場合のみととも言えますね。

#### 以下は間違った議論なので無視

$A$は正則とは限らないので、
$ X = - A^{-1} B r$とすることはできない。

$A,C$が可観測の条件から、dcgainを確定できることを示す。まず、$ A X = -B r$ の両辺に　$ C A^{k},\quad(k=0\dots N-1)$  を掛けた式を並べてみる。


$$
\begin{bmatrix} C A \\ C A^2\\ C A^{3}\\ \vdots\\ C A^n\end{bmatrix} X = - \begin{bmatrix} C B r \\ C A B r\\ C A^{2} B r \\ \vdots\\ C A^{n-1} B r\end{bmatrix} = - \begin{bmatrix} C  \\ C A \\ C A^{2} \\ \vdots\\ C A^{n-1}\end{bmatrix} B r
$$ 可観測性が成り立てば、(外乱オブザーバーの条件）--左辺の係数行列は正則だから--、$A$が正則でなければ、$\begin{bmatrix} C A \\ C A^2\\ C A^{3}\\ \vdots\\ C A^n\end{bmatrix}$も正則ではないはず。

$$
 X = - \begin{bmatrix} C A \\ C A^2\\ C A^{3}\\ \vdots\\ C A^n\end{bmatrix}^{-1}\begin{bmatrix} C \\ C A \\ C A^{2}\\ \vdots\\ C A^{n-1} \end{bmatrix} B r
$$

これから出力$y$は、
$$
 y = - C \begin{bmatrix} C A \\ C A^2\\ C A^{3}\\ \vdots\\ C A^n\end{bmatrix}^{-1}\begin{bmatrix} C \\ C A \\ C A^{2}\\ \vdots \\ C A^{n-1}\end{bmatrix} B r
$$と与えられることがわかります。

これを例題のシステムについて計算すると、


```python
- matmul(SS.C, matrix(control.obsv(SS.A, matmul(SS.C,SS.A)))**(-1))*matmul(control.obsv(SS.A,SS.C),SS.B)
```




    matrix([[1.01936799]])




```python
- (SS.C*matrix(control.obsv(SS.A,matmul(SS.C,SS.A)))**(-1))*matmul(control.obsv(SS.A,SS.C),SS.B)
```




    matrix([[1.01936799]])



とDCgainは 被制御対象システムのDCgainと同じであることが確認できました。

# 伝達関数から状態方程式への変換について

伝達関数の考え方は、入力 $u$ と出力 $y$ の関係は、微分方程式::

$$
\begin{cases}
y^{(N)} &= - \Sigma_{k=0}^{N-1} a_k y^{(k)} + \Sigma_{k=0}^{M} b_k u^{(k)} \\
y^{(k)} &\equiv \frac{d^k y}{d t^k}\quad\text{for}\; k \ge 1,\\ 
y^{(0)} &\equiv y\\
\end{cases}
$$

によって記述されるということです。ラプラス変換を行うと、

$$
Y(s) = \frac{\Sigma_{k=0}^{M} b_k s^k}{\Sigma_{k=0}^{N} a_k s^k} U(s) 
$$

となります ( $a_N \equiv 1$ とします)。

この微分方程式を状態変数$ X=\begin{pmatrix} x_0\\x_1\\ \vdots \\ x_n \end{pmatrix}$ の状態方程式に書き換えることができれば、システムを伝達関数の記述から、状態方程式に書き換えることができたということになる。
よく知られたように、システムがプロパー、すなわち 条件：　$ N \ge M$が成り立つ、であればこれは可能です。
実際、この場合には、等価な状態方程式は、

$$
\frac{d \vec{X}}{d t} =  A \vec{X} +  B {u} \\
{y} = {C} \vec{X} + {D} {u}
$$

となります。(入力 $u$ および　出力$y$はスカラーであることに注意）
状態　$X$は、
$$
\vec{X} =\begin{pmatrix} x \\ x^{(1)}\\ x^{(2)}\\\vdots\\x^{(N-1)}\end{pmatrix}
$$
となっています。

ここで、$ A,B,C,D $を　
$$
{A} = \begin{pmatrix} 0,& 1,& 0,& 0,& \cdots&, 0\\
   0,& 0,& 1,& 0,& \cdots &, 0\\
   \vdots& \vdots& \vdots& \vdots& \ddots& \vdots\\
   0,& 0,& 0,& 0,& \cdots&, 1\\
   -a_0,& -a_1,& -a_2,& -a_3,& \cdots&, -a_{N-1} \\
   \end{pmatrix}\\
   \\
{B} = \begin{pmatrix} 0\\ 0\\  \vdots \\ 0\\ 1\end{pmatrix} \\
{C} = \begin{pmatrix} b_0 - b_N a_0,& b_1 - b_N a_1,& \cdots, b_{N-1}-b_N a_{N-1}\end{pmatrix}\\
   \\
{D} = b_N \\
$$
で定義します。　ここで、$b_k= 0 \text{ for } N \le k \gt M$ としています。

状態方程式の第一式は、

$$
 x^{(n)} = -\Sigma_{k=0}^{N-1} a_k x^{(k)} + u
$$

に帰着されます。　$a_N=1$とすれば、

$$
 \Sigma_{k=0}^{N} a_k x^{(k)} = u
$$

となります。

$$
   y = \Sigma_{k=0}^{N-1}\left(b_k-b_N a_k\right)\frac{d^k x}{d t^k} + b_N u
$$

となる。両辺から $\Sigma_{k=0}^{N} a_k \frac{d^k}{d t^k}$ を掛けて

$$
   \Sigma_{k=0}^{N} a_k \frac{d^k y}{d t^k} = \Sigma_{k=0}^{N-1}\left(b_k-b_N a_k\right)\frac{d^k u}{d t^k} + b_N \Sigma_{k=0}^{N} a_k \frac{d^k u}{d t^k}\\
   = \Sigma_{k=0}^{N-1}b_k\frac{d^k u}{d t^k} + b_N \frac{d^N u}{d t^N}\\
   = \Sigma_{k=0}^{N}b_k\frac{d^k u}{d t^k}\\
$$

となります( $a_N=1$ に注意)。 このように、$ N \ge M$であれば、伝達関数表示と状態方程式表示が等価であることが明示的に示されます。

### 可制御標準型
この伝達関数表示から作成した、状態変数表示は可制御標準型と呼ばれます。可制御性を調べるために、$B, A B, A^2B, \dots A^{N-1}B$を考えると、

$$
\begin{pmatrix} 0\\ 0\\  \vdots \\ 0\\ 1\end{pmatrix}, \begin{pmatrix} 0\\ 0\\  \vdots \\ 0 \\ 1\\ -a_{N-1}\end{pmatrix},
\begin{pmatrix} 0\\ 0\\  \vdots  \\ 1\\ -a_{N-1}\\- a_{N-2}-a_{N-1}^2\end{pmatrix},\dots 
\begin{pmatrix} 1\\ a_{N-1}\\- a_{N-2}-a_{N-1}^2 \\ \vdots \\\vdots \\ \end{pmatrix}
$$

となって、この状態変数表示のシステムは可観測であることが自明となる。

## 伝達関数が既約でない時？
$$
\Sigma_{k=0}^{N+1} a'_k s^k = (s-\lambda) \left(\Sigma_{k=0}^{N} a_k s^k\right) = \Sigma_{k=0}^{N}\left(a_k s^{k+1} - \lambda a_k s^k\right)
$$
より、
$$\begin{cases}
a'_0= -\lambda a_0, \\
a'_k=a_{k-1}-\lambda a_{k} \quad\left( 1\le k \le N\right), \\
a'_{N+1} = a_{N}\\
\end{cases}
$$
$$
{C'} = \begin{pmatrix} b'_0 - b'_{N+1} a'_0,&
b'_1 - b'_{N+1} a'_1,&
\cdots, &
b'_{N}-b'_{N+1} a'_{N}\end{pmatrix}\\
$$
$$
{C'} = \begin{pmatrix} \lambda b_0 + b_N \lambda a_0 ,&
  b_0 -\lambda b_1 - b_{N} (a_0 - \lambda a_1),& \cdots, b_{N-1}-\lambda b_{N} - b_{N}(a_{N-1} - \lambda a_{N})\end{pmatrix}\\
$$
$$
{A'} = \begin{pmatrix} 0,& 1,& 0,& 0,& \cdots&, 0\\
   0,& 0,& 1,& 0,& \cdots &, 0\\
   \vdots& \vdots& \vdots& \vdots& \ddots& \vdots\\
   0,& 0,& 0,& 0,& \cdots&, 1\\
   \lambda a_0,& -a_0 + \lambda a_1,& -a_1 +\lambda a_2,& , -a_2+\lambda a_3,& \cdots &, -a_{N-1}+\lambda a_N\\
   \end{pmatrix}\\
$$
$$
{B'} = \begin{pmatrix} 0\\ 0\\  \vdots \\ 0\\ 1\end{pmatrix} \\
$$
$$
B',A'*B', ...
$$
を考えると
$$
\begin{bmatrix}0\\0\\0\\\vdots\\1\end{bmatrix},
\begin{bmatrix}0\\0\\\vdots\\1\\-a_{N-1}+\lambda a_N\end{bmatrix}
\begin{bmatrix}0\\\vdots\\1\\-a_{N-1}+\lambda a_N\\ -a_{N-2}+\lambda a_{N-1}+(-a_{N-1}+\lambda a_N)\end{bmatrix}
$$


## なぜプロパーでない伝達関数は状態方程式にかきかえられないのか？

今見たように、プロパーなシステムでは、一次の微分方程式である状態方程式にかきかえられますので、ある時点での状態と入力値から次の状態を微分方程式から決定できます。インプロパーなシステムでは、階数がNより大きい入力の微分$u^{(m)}$が必要となります。
これを、"将来の入力値”が必要となると表現されますが、これについて少し考察してみます。

伝達関数のラプラス変換を出力$Y(s)$について解いた式：
$$
Y(s) = \frac{\Sigma_{k=0}^{M} b_k s^k}{\Sigma_{k=0}^{N} a_k s^k} U(s)
$$
の分母を因数分解をおこない、部分分数展開すれば、

$$
Y(s) = \left( \Sigma_{k=0}^{M-N} \alpha_k s^k + \Sigma_{k=0}^{N}\frac{\beta_k}{s-\lambda_k} \right)U(s)
$$
となる。これのラプラス逆変換を行えば、

$$
y(t) =  \Sigma_{k=0}^{M-N} \alpha_k \frac{d^k u(t)}{dt^k} + \Sigma_{k=0}^{N} \beta_k \int_{-\infty}^{0}d\tau e^{-\lambda_k \tau}u(t+\tau)
$$

$ M \gt N $の場合には、この式の右辺は、入力の微分項を含むことになる。
このことから、プロパーでない伝達関数は微分を含むということになる。

プロパーでない系では、上記の形式解の右辺に現れる微分$s$を不完全微分$\frac{s}{1+T_D s}$などで置き換えるのが普通のやり方だそうです。

$$
\frac{d y}{d t} = a_0 y + b_0 u +b_1 u' + b_2 u''
$$


### 不完全微分 $\frac{s}{1+T_D s}$ と単純な微分回路

抵抗　$R$ と　コンデンサ $C$ で構成される微分回路のインピーダンスは $\frac{R}{R+ \frac{1}{j C \omega}}$ です。これをラプラス変換に書き直せば $\frac{R C s}{1+R C s}$ と不完全微分の形になります。

# 伝達関数と状態表示の関係。

上で見たように、プロパーな伝達関数:

$$
Y(s) = \frac{b_M s^M +b_{M-1}s^{M-1} \dots b_1 s + b_0}{s^N+a_{N-1} s^{N-1} + \dots + a_1 s +a_0} U(s)
$$

は、状態方程式表示:

$$
\left\lbrace\begin{aligned}
\frac{d X}{d t} = A x + B u \\
Y = C X + D u\\
\end{aligned}\right.
$$

に変換することが可能です。逆に、状態変数表示からは、

$$
Y(s) = \left[C\left(s I - A\right)^{-1} B +D\right] U(s)
$$

によって、伝達関数表示に移ることが可能です。この時、複数の状態変数が同じ伝達関数が実現されます。
特に、 1入力1出力のシステムであれば、　$Y^T=Y, U^T=U, D^T = D$ ですから、
$$
\begin{aligned}
Y(s) &= \left[C\left(s I - A\right)^{-1} B +D\right] U(s) \\
&=  \left[B^T\left(s I - A^T\right)^{-1} C^T +D\right] U(s)
\end{aligned}
$$

が成り立ちます。これは、もとの伝達関数のシステムは、

$$
\left\lbrace\begin{aligned}
\frac{d \hat{X}}{d t} = A^T \hat{X} + C^T u \\
Y = B^T \hat{X} + D u\\
\end{aligned}\right.
$$
と等価です。プロパーな伝達関数から構成される状態変数表示では、$A, B$ は可制御でした。これから転置行列を使った状態変数表示のシステムは、
$A^T, B^T$ から構成される可観測行列のランクが状態変数の次元に一致することから、可観測ということになります。
結局、プロパーな伝達関数を持つシステムは、状態変数表示に移ると、可制御かつ可観測なシステムで表現されることがわかります。

.. note::

   双対な制御システムは通常、信号の向きを逆転したシステムとして認識される。可制御性と可観測性が入れ替わるシステムとも言える。
   
   

$$
{A_o} = \begin{pmatrix} 0,& 0,& 0,& \cdots&,0,&  -a_0\\
   1,& 0,& 0,& \cdots &, 0,& -a_1\\
   0,& 1,& 0,& \cdots &, 0,&-a_2\\
   \vdots& \vdots& \vdots& \ddots& \vdots& \vdots\\
   0,& 0,& 0,& \cdots&,0,&  a_{N-2}\\
   0,& 0,& 0,& \cdots&, 1,& -a_{N-1} \\
   \end{pmatrix}\\
   \\
{B_o} = \begin{pmatrix} b_0 - b_N a_0,\\ b_1 - b_N a_1,\\ \vdots \\ b_{N-1}-b_N a_{N-1}\end{pmatrix}\\
{C_o} = \begin{pmatrix} 0, 0,  \cdots , 0, 1\end{pmatrix} \\
   \\
{D_o} = b_N \\
$$
$$
s*x_0= -a_0 x _{N-1}+ (b_0-b_N a_0) u\\
s*x_1 = x_0 - a_1 x_{N-1} + (b_1 -b_N a_1) u\\
\vdots\\
s*x_{N-1}= x_{N-2} - a_{N-1} x_{N-1} + (b_{N-1} - b_N a_{N-1}) u\\
$$

$$
\begin{aligned}
s*x_0 &= -a_0 x_{N-1}+ (b_0-b_N a_0) u\\
s^2*x_1 &= s*x_0 - s*a_1 x_{N-1} + s*(b_1 -b_N a_1) u \\
&= -a_0 x_{N-1} - a_1 s x_{N-1} + (b_0-b_N a_0) u  + (b_1 -b_N a_1)s u\\
&\vdots\\
s^{N-1} x_{N-2} &= s^{N-2} x_{N-3} - a_{N-2} s^{N-2} x_{N-1} + (b_{N-2} - b_N a_{N-2})s^{N-2} u\\
&= s^{N-3} x_{N-4} - a_{N-3} s^{N-3} x_{N-1} +(b_{N-3} - b_N a_{N-3}) s^{N-3} u  - a_{N-2} s^{N-2} x_{N-1}
+ (b_{N-2} - b_N a_{N-2})s^{N-2} u\\
s^{N}*x_{N-1}&= s^{N-1}x_{N-2} - a_{N-1} s^{N-1} x_{N-1} + (b_{N-1} - b_N a_{N-1}) s^{N-1} u\\
&= \Sigma_{k=0}^{N-1} \left( - a_k s^k x_{N-1}  + (b_k-b_N a_k) s^k u\right)
\end{aligned}
$$

これより、
$$
\Sigma_{k=0}^{N}a_k s^k x_{N-1} =  \Sigma_{k=0}^{N-1} (b_k-b_N a_k) s^k u
$$

$y= x_{N-1}  + b_N u$にこれを代入すると、

$$
\begin{aligned}
\Sigma_{k=0}^{N}a_k s^k y &= \Sigma_{k=0}^{N-1} (b_k-b_N a_k) s^k u + \Sigma_{k=0}^{N}a_k b_N s^k  u\\
&= \Sigma_{k=0}^{N-1} b_k s^k u+ a_N b_N s^N u =  \Sigma_{k=0}^{N} b_k s^k u\\
\end{aligned}
$$ 
ここで $a_N \equiv 1$ を使ったことに注意しておきます。

# 外乱オブザーバー
測定値 $Y$ に定値の外乱 $d$ がある時、状態を拡大して、

$$
X_e=\begin{bmatrix} X \\ d \\\end{bmatrix}
$$
を考えると、状態方程式は、

$$
\left\lbrace\begin{aligned}
\frac{d X_e}{d t} &=\begin{bmatrix} A,& 0\\ 0,& 0 \\\end{bmatrix} X_e 
+ \begin{bmatrix} B \\ 0 \end{bmatrix} \begin{bmatrix} u\\ 0 \end{bmatrix}\\
Y &= \begin{bmatrix} C,& 1 \end{bmatrix} X_e + D u \\
\end{aligned}\right.
$$

このとき、拡張された系 $A_e,B_e, C_e D_e$ は可観測だが、不可制御になってしまいます。それは単に外乱 $d$は推定可能だが、制御不能であることを言っているに過ぎなません。

可安定性:
   可制御ではなくても、制御不能なモードが安定であれば、システム全体を安定化させることはできる。このようなシステムを**可安定**なシステムとよびます。(「制御理論の基礎」page. 187)
   



```python
#pip install -U graphviz
#https://h1ros.github.io/posts/introduction-to-graphviz-in-jupyter-notebook/
import graphviz
```


```python
n = graphviz.Digraph(name='splines', engine='neato',
                     graph_attr={'splines': 'true'},
                     node_attr={'shape': 'point'})
n.node('a', pos='0,0!', color='blue')
n.node('b', pos='100,0!', color='green')
n.node('c', pos='50,50!', color='red')
n.edge('a', 'b', pos='0,0 30,66 70,60 100,0')
n.render(neato_no_op=2, directory='doctest-output').replace('\\', '/')
```




    'doctest-output/splines.gv.pdf'



# モデルマッチングによるゲインチューニング(Pythonによる制御工学入門)


```python
# model: Python による制御工学入門」　リスト 5.1 144ページ
g=9.81
l=0.2
M=0.5
Mgl=M*g*l
mu=1.5e-2
J=1.0e-2
ref=30 

num_delay,den_delay=control.pade(0.005,1)
Plant5=tf([1],[J, mu, Mgl])*tf(num_delay,den_delay)
print(Plant5)
print(Plant5.poles(),Plant5.zeros(),Plant5.dcgain())
```

    
                   -s + 400
    --------------------------------------
    0.01 s^3 + 4.015 s^2 + 6.981 s + 392.4
    
    [-400.  +0.j           -0.75+9.87610753j   -0.75-9.87610753j] [400.+0.j] 1.019367991845056



```python

```