pythonのcontrolライブラリの使用方法メモ

# 概要
Pythonにデフォルトでインストールされているcontrolライブラリの使用方法の個人的メモです。

ただ、一部の関数を使用するには、slycotを追加でインストールする必要があり、そのインストールがなかなかうまくいかなかったので覚悟してください。

# include

```python
import control
```

Matlab関数も使用する場合

```python
from control import matlab
```

# 伝達関数定義

```python
tf = matlab.tf([1],[1 , 0])
print(tf)

->
1
-
s
```

# 状態方程式定義

```python
A=[[1 , 0],
   [1 , 2]]
B=[[1],
   [0]]
C=[[1 , 0]]
D=[[0]]

sys = matlab.ss(A,B,C,D)
```
# 伝達関数・状態方程式相互変換

```python
tf2 = control.ss2tf(sys)
sys2 = control.tf2ss(tf2)
```
# A,B,C,D行列へのアクセス

```python
A=sys.A
B=sys.B
C=sys.C
D=sys.D

np.linalg.eig(A)
```
最後の行は安定判別に使えます

# bode線図

```python
matlab.bode(sys)
```

ちなみに

```python
import matplotlib.pyplot as plt

fig = plt.figure()
matlab.bode(sys)

fig2 = plt.figure()
matlab.nyquist(sys)
```
とすれば、ナイキストも連続して書けます

# 離散化

```python
tf_d = control.c2d(tf,0.01,method='zoh')
```

https://qiita.com/Yuya-Shimizu/items/e03565d6b77bc9becb36

を参考にさせていただきました。

# リカッチ方程式（連続）


https://jp.mathworks.com/help/control/ref/lqr.html

https://algorithm.joho.info/seigyoriron/python-control-riccati/

を参考にさせていただきました。

```
ModuleNotFoundError: No module named 'slycot'

```
と出たら

https://algorithm.joho.info/seigyoriron/python-control-install/

うえのリンクの内容で、slycotを探すときは、ctr+Fの方が現実的です（一応）。あと、下の内容で、

```
from pip._internal.utils.compatibility_tags import get_supported
print(get_supported())
```
を実行し、cp38,cp,39等を確認し、どれをインストールするか決める必要があるかもしれません。

https://note.com/tomtom_0301/n/na8fedf6f87ee

を参考にしてください。
先人の方ありがとうございます。
助かりました。

と思ったらだめでした。

（追記）

http://mechsys.tec.u-ryukyu.ac.jp/~oshiro/SiteList/2020/06/19/65093/

で

```
conda install -c conda-forge control slycot
```
をすれば、解決！
先人の方ありがとうございました。

疲れた

# リカッチ方程式の解法

```python
import control
from control import matlab
import numpy as np
import matplotlib.pyplot as plt



A=[[1 , 0],
   [1 , 2]]
B=[[1],
   [0]]
C=[[1 , 0]]
D=[[0]]

sys = matlab.ss(A,B,C,D)

Q=np.array([[1,0],
            [0,1]]  )*10


R=np.array([[1]] )

N =[[0],
    [0]]

K,P,e = matlab.lqr(A,B,Q,R,N)

```
さらに、正しく解が得られているか確認します。

```
hoge = np.dot(sys.A.T,P) + np.dot(P,sys.A) 
hoge2 = np.dot(P,sys.B)
hoge3 = np.dot(np.dot(hoge2,np.linalg.inv(R)),hoge2.T)

hoge_final = hoge - hoge3 + Q

print(hoge_final)

->
[[9.80548975e-13 5.37170308e-12]
 [5.37170308e-12 2.91606739e-11]]

```
ほぼ０になったので、リカッチ方程式が成立していることが確認できました。

ちなみに、Kに関しては

```math
u = -Kx
```
とする前提で設計されています。

# 離散リカッチ方程式の解法
原理的な話はこちら

https://qiita.com/hiRina/items/99f2da489a6e414dad3c

関数の説明

https://jp.mathworks.com/help/control/ref/dare.html

dareが普通にPythonでも使えるのすごい

引数はnumpyでないとエラーが出るので、注意

```python
[P,L,K] = matlab.dare(sys.A,sys.B,Q,R)
```
とすれば

```math
\begin{eqnarray}
P &=& A^TPA+Q-(B^TPA)^T(R+B^TPB)^{-1}(B^TPA)\\
K &=& (R+ B^TPB)^{-1} B^T PA\\
u &=& -Kx
\end{eqnarray}
```
となる、$P,K$を見つけてきてくれます。

検算もしてみます

```python
hoge = np.dot(np.dot(sys.A.T,P),sys.A) + Q
hoge2 = np.dot(np.dot(sys.B.T,P),A)
hoge2_2 = np.linalg.inv(R+np.dot(np.dot(sys.B.T,P),B) )
hoge3 = np.dot(np.dot(hoge2.T, hoge2_2) ,hoge2)

print(hoge-hoge3-P)
->
[[-8.52651283e-14 -8.52651283e-14]
 [-1.42108547e-13 -2.55795385e-13]]
```
ほぼ、０になったので確認できました。

# 可制御・可観測

```python

Ctr = matlab.ctrb(sys.A,sys.B)

print(np.linalg.matrix_rank(Ctr))
print("Aの次元と一致すれば、可制御")

Obs = matlab.obsv(sys.A,sys.C)

print(np.linalg.matrix_rank(Obs))
print("Aの次元と一致すれば、可観測")
```

https://jp.mathworks.com/help/control/ref/ss.ctrb.html

# 極配置

```python
K = matlab.place(sys.A,sys.B,[-1,-2])

eig = np.linalg.eig(sys.A-np.dot(sys.B,K))
print(eig)
```
これで、配置と、確認ができます。[-1,-2]は配置したい場所です。

ただし、同じ場所にBの次元以上の個数、配置しようとするとエラーが出ました。数学的なものかな

# 伝達関数の分母・分子の取得

```python
tf.den      #分母
tf.num      #分子
```

# 伝達関数に　s = jw を代入する

```python
fr = matlab.evalfr(tf,1j*w)                  #s = jw　の代入結果を取得
```
便利すぎ～
全部あるやん

# bode線図を関数でない方法で書く

```python
import control
from control import matlab
import numpy as np
import matplotlib.pyplot as plt
import math
import cmath

tf = matlab.tf([1],[1 , 0])

w_max = 1000
w_min = 0.01
w = np.linspace(w_min,w_max,10000000)
fr = matlab.evalfr(tf,1j*w)                  #s = jw　の代入結果を取得

gain = abs(fr)

phase = []
for i in fr:
    phase.append(cmath.phase(i))   
phase = np.array(phase)                      #もっといい書き方あるかも


# グラフ化

fig = plt.figure()

# top
ax1 = fig.add_subplot(2, 1, 1)
ax1.plot(w, gain)
ax1.set_xlabel("w[rad/s]")
ax1.set_ylabel("gain")
ax1.set_yscale('log')
ax1.set_xscale('log')
ax1.set_xlim(w_min,w_max)

# bottom

ax2 = fig.add_subplot(2, 1, 2)
ax2.plot(w, phase)
ax2.set_xlabel("w[rad/s]")
ax2.set_ylabel("phase")
ax2.set_xlim(w_min,w_max)
ax2.set_xscale('log')
ax2.set_ylim(-np.pi-0.1,np.pi+0.1)

# show plots
fig.tight_layout()
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/73781576-13dc-0b05-f689-1f6285e05f12.png)

悪くないね。
Phaseはdeg表示に変えた方がわかりやすいかな。

# z = exp(jwT)を代入して、離散システムのボード線図を書く
離散システムのボード線図を書く場合は、代入してやれば、周波数特性がわかるのでやってみる
0.01sでサンプリングするシステムの場合、0Hzと100Hzの見分けがつかない。つまり、このシステムは0~100Hzまでの伝達関数がわかれば、周りの周波数に関しては、0~100Hzの繰り返しになるということである。

```python

import control
from control import matlab
import numpy as np
import matplotlib.pyplot as plt
import math
import cmath

T_samp = 0.01

#グラフ表示範囲
w_max = 50 * 2*np.pi
w_min = 0.01

tf = matlab.tf([1],[1 , 0])

fig = plt.figure()
matlab.bode(tf)
               
tf_d = control.c2d(tf,T_samp,method='zoh')     
print(tf_d)

fig2 = plt.figure()
matlab.bode(tf_d)


w = np.linspace(w_min,w_max,10000000)
fr = matlab.evalfr(tf_d,np.exp(1j*w*T_samp))                  #s = exp(jwT)　の代入結果を取得

gain = abs(fr)

phase = []
for i in fr:
    phase.append(cmath.phase(i))   
phase = np.array(phase)  *180 / np.pi                    #もっといい書き方あるかも


#グラフ化

fig = plt.figure()

# top
ax1 = fig.add_subplot(2, 1, 1)
ax1.plot(w, gain)
ax1.set_xlabel("w[rad/s]")
ax1.set_ylabel("gain")
ax1.set_yscale('log')
ax1.set_xscale('log')
ax1.set_xlim(w_min,w_max)

# bottom

ax2 = fig.add_subplot(2, 1, 2)
ax2.plot(w, phase)
ax2.set_xlabel("w[rad/s]")
ax2.set_ylabel("phase")
ax2.set_xlim(w_min,w_max)
ax2.set_xscale('log')
ax2.set_ylim(-190,190)

# show plots
fig.tight_layout()
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/e66e390c-4402-b109-ca3f-6c3c765580b2.png)
離散化前
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/9723ad7b-a3e5-1f22-b0e9-57bb6f19b70d.png)
離散化後(T=0.01s)
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/2d572779-598b-61b5-714f-99cad71950c4.png)
Z=exp(jwT)を代入してグラフ化

0.1rad/sで、20dB(10倍)なのが共通しており、形状もかなり近いので、離散システムに対して、bodeは正しく働いていることが分かった

# 離散ローパスフィルタの設計

https://ja.wikipedia.org/wiki/%E3%83%90%E3%82%BF%E3%83%BC%E3%83%AF%E3%83%BC%E3%82%B9%E3%83%95%E3%82%A3%E3%83%AB%E3%82%BF

を利用

### 一次

```python
import control
from control import matlab
import numpy as np
import matplotlib.pyplot as plt

cut_of_freqency = 10    #Hz   :カットオフ周波数
T_samp = 0.01           #s    :制御周期


tf = matlab.tf([1],[1/(cut_of_freqency*2*np.pi),1])

fig = plt.figure()
matlab.bode(tf)
               
tf_d = control.c2d(tf,T_samp,method='zoh')     
print(tf_d)

fig2 = plt.figure()
matlab.bode(tf_d)

print("分母")
print(tf_d.den)

print("分子")
print(tf_d.num)
```

### 二次

```python
import control
from control import matlab
import numpy as np
import matplotlib.pyplot as plt

cut_of_freqency = 1/2/np.pi    #Hz   :カットオフ周波数
T_samp = 0.01           #s    :制御周期

rad = cut_of_freqency*2*np.pi
tf = matlab.tf([1],[1/(rad**2),np.sqrt(2)/rad,1])

fig = plt.figure()
matlab.bode(tf)
               
tf_d = control.c2d(tf,T_samp,method='zoh')     
print(tf_d)

fig2 = plt.figure()
matlab.bode(tf_d)

print("分母")
print(tf_d.den)

print("分子")
print(tf_d.num)
```

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/13b2d212-6664-043d-9d74-c612b4a2f47d.png)
離散化前
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/152ae649-3503-b857-c349-2ea8a439eb1a.png)
離散化後

1rad/sでカットオフ、平坦特性、-40dB/decがきれいに表れています。

###Arduinoへの実装テンプレート
後で書きます



# 感想
クッソ便利。
Scilabを使っていたけど、やっぱPythonで書きたいという人には、ほんとにおすすめ。