H∞制御理論_理論的解説5:ガンマイタレーション

#概要
ついに，終着駅に向かってまいりました．
長かったですが，まだまださらにかかります・・・

#H∞制御とは
いろいろなところで，語られているので軽く

```math
min \|T_{zw}\|_{\infty} 
```
を達成する，制御器$K$を設計し，制御に用いるということである．

#計算方法：ガンマイタレーション
ガンマイタレーションという方法で計算する．LMIとかはまだ勉強していないが，有界実補題でLMIに変換すれば，可能だと思われる

次の定理を基本とした方法である．

---------------------------------------

```math
 \|T_{zw}\|_{\infty} <\gamma
```
を満たす制御器が存在すること
⇔次の3つが成立
(1) $H_{\infty} \in dom(Ric)$かつ，解$X_{\infty}$が正定

```math
H_{\infty}=\left(
    \begin{array}{ccc}
     A &\gamma^{-2}B_1B_1^*-B_2B_2^*\\
-C_1^*C_1&-A^*\\
    \end{array}
  \right)
```
$\gamma = \infty$ならば，(1,2)成分が負定になり，可制御・可観測ならば，$dom(Ric)$になる(証明は余裕があるときに）．


(2) $J_{\infty} \in dom(Ric)$かつ，解$Y_{\infty}$が正定

```math
J_{\infty}=\left(
    \begin{array}{ccc}
     A^* & \gamma^{-2}C_1^*C_1-C_2^*C_2\\
-B_1B_1^*&-A\\
    \end{array}
  \right)
```

(3)$\rho(A)$をAの最大ユークリッドノルムの固有値を示すとする．

```math
\rho(A) = max_{1<i<n}\|\lambda_i\|
```
と定義する．この時，次を満たす．

```math
\rho(X_{\infty}Y_{\infty} ) < \gamma^2
```


さらに，これらの条件が成立するとき，制御器の一つは

```math
K_{sub}=\left(
    \begin{array}{c|c}
      \hat{A}_{\infty}&-Z_{\infty} L_{\infty}\\ \hline
      F_{\infty}&0\\
    \end{array}
  \right)\\
\hat{A}_{\infty} = A+ \gamma^{-2}B_1 B_1^* X_{\infty} +B_2F_{\infty} + Z_{\infty}L_{\infty}C_2\\
F_{\infty} = -B_2^*X_{\infty}\\
L_{\infty} = -Y_{\infty}C_2^*\\
Z_{\infty} = (I-\gamma^{-2}Y_{\infty}X_{\infty})^{-1}\\
```
となる．

------------------------------------

この定理を用いて，

```math
min \|T_{zw}\|_{\infty} 
```
を達成するには，

(1) 初期値$\gamma_1$を与え，3条件を満たすか確認する
(2) 満たす場合$\gamma_1$より小さい値で，3条件を満たすか確認する
(3) 満たさない場合，$\gamma_1$より大きい値で3条件を満たすか確認する

二分探索等の探索アルゴリズムを使用し，3条件を満たす，できるだけ小さい$\gamma$を計算する．
最後に，その時の$K_{sub}$を計算すれば，計算終了となる．ガンマ値を色々変えて繰り返すので，ガンマイタレーションと呼ばれる．

以降では，定理の証明をする．補題3つまず，証明し，定理を示す

ちなみに，すでに

```math
\gamma > \| G \|_{\infty}
```
であると事と


```math
R=\gamma ^2 I -D^*D\\
F=R^{-1}(B^*X+D^*C)\\
S=-C^*D\\
P=-C^*C\\
```
としたときのハミルトン行列H

```math
H=
\left(
    \begin{array}{ccc}
      A+BR^{-1}S^*&-BR^{-1}B^*\\
-(P-SR^{-1}S^*)&-( A+BR^{-1}S^*)^*\\
    \end{array}
  \right)

```
が$H \in dom(Ric)$であることが

同値であることは示している．

#Pythonで実装
証明は後にして，Scilabで設計したコントローラと，Pythonで自作した，ガンマイタレーションの結果の比較をしようと思います．

```python

"""
二分探索で　ガンマイタレーションを行い，　準最適制御器　を設計する．
"""


import numpy as np		#	Numpyライブラリ
import copy


def check(gamma,A,B1,B2,C1,C2):
    """
    ハミルトン行列の生成
    """
    
    H=copy.deepcopy(A)
    hoge = np.dot(B1,B1.conj().T) / (gamma**2) - np.dot(B2,B2.conj().T) 
    H = np.append(H,hoge,axis = 1)
    hoge2 =  -1 * np.dot(C1.conj().T,C1) 
    hoge2 = np.append(hoge2,-1*A.conj().T ,axis =1 )
    H = np.append(H,hoge2,axis = 0)
    
    J=copy.deepcopy(A.conj().T)
    hoge = np.dot(C1.conj().T,C1) / (gamma**2) - np.dot(C2.conj().T,C2) 
    J = np.append(J,hoge,axis = 1)
    hoge2 =  -1 * np.dot(B1,B1.conj().T) 
    hoge2 = np.append(hoge2,-1*A ,axis =1 )
    J = np.append(J,hoge2,axis = 0)
    
    
    """
    H
    
    ハミルトン行列に固有値0を持たない，ハミルトン行列の解を求める，
    """
    H=H.astype(np.float64)
    Hw,Hv = np.linalg.eig(H) 
    
    for i in range(len(Hw)):        
        if Hw[i].real == 0:
            print("H have pole 0")            
            return 0,0,0
            
    v_num = 0 
    for i in range(len(Hw)): 
        if Hw[i].real < 0:
            if v_num == 0:
                V = Hv[:,i].reshape(H.shape[0],1)
                v_num = v_num+1
            else:
                V = np.append(V,Hv[:,i].reshape(H.shape[0],1),axis = 1)
                v_num = v_num+1
    
    n=A.shape[0]
    X1_H = V[0:n,0:n]
    X2_H = V[2:n+2,0:n]
    
    if np.linalg.det(X1_H) == 0:
        print("H doesn't have Hojyuu seisitu")
        return 1,0,0
    else:
        X_H = np.dot(X2_H,np.linalg.inv(X1_H))
        
    """
    さらに，解の正定性を確認
    """
    if np.all(np.linalg.eigvals(X_H) > 0):
        print("X of H is positive")
    else:
            return 2,0,0
    
    """
    リカッチ方程式の確認
    """
    hoge = np.dot(A.conj().T,X_H)
    hoge = hoge + np.dot(X_H,A)
    R=np.dot(B1,B1.conj().T) / (gamma**2) - np.dot(B2,B2.conj().T) 
    hoge = hoge + np.dot(X_H,np.dot(R,X_H))
    hoge = hoge + np.dot(C1.conj().T,C1)
    #print(hoge)
    #print(" is 0? Is riccati correct ?")
    
    
    
    """
    J
    
    ハミルトン行列に固有値0を持たない，ハミルトン行列の解を求める，
    """
    J=J.astype(np.float64)
    Jw,Jv = np.linalg.eig(J) 
    
    for i in range(len(Jw)):        
        if Jw[i].real == 0:
            print("J have pole 0")
            return 7,0,0
            
    v_num = 0 
    for i in range(len(Jw)): 
        if Jw[i].real < 0:
            if v_num == 0:
                V = Jv[:,i].reshape(J.shape[0],1)
                v_num = v_num+1
            else:
                V = np.append(V,Jv[:,i].reshape(J.shape[0],1),axis = 1)
                v_num = v_num+1
    
    n=A.shape[0]
    X1_J = V[0:n,0:n]
    X2_J = V[2:n+2,0:n]
    
    if np.linalg.det(X1_J) == 0:
        print("J doesn't have Hojyuu seisitu")
        return 3,0,0
    else:
        X_J = np.dot(X2_J,np.linalg.inv(X1_J))
        
    """
    さらに，解の正定性を確認
    """
    if np.all(np.linalg.eigvals(X_J) > 0):
        print("X of J is positive")
    else:
        return 4,0,0
    
    """
    リカッチ方程式の確認
    """
    hoge = np.dot(A,X_J)
    hoge = hoge + np.dot(X_J,A.conj().T)
    R=np.dot(C1.conj().T,C1) / (gamma**2) - np.dot(C2.conj().T,C2)
    hoge = hoge + np.dot(X_J,np.dot(R,X_J))
    hoge = hoge + np.dot(B1,B1.conj().T) 
    #print(hoge)
    #print(" is 0? Is riccati correct ?")
    
    
    """
    スペクトル半径が gamma^2 以下である事の確認
    """
    hoge = np.dot(X_H,X_J)
    spectro_radius = max(np.abs(np.linalg.eigvals(hoge)))
    
    if spectro_radius < gamma**2:
        print("spectro radius < gamma^2")
        return 6,X_H,X_J
    else:
        print("spectro radius  error")
        return 5,0,0

A=np.array([[-1,10],[2,-0.1]])

w,v = np.linalg.eig(A) 

for i in range(len(w)):
    if w[i].real < 0:
        print("A is unstable")
        
    if w[i].real == 0:
        print("A have pole 0")
        

B1=np.array([[1,0],[2,0]])
B2=np.array([[1],[2]])

C1=np.array([[1,3],[0,0]])
C2=np.array([[1,3]])

D12 = np.array([[0],[1]])

D21= np.array([[0,1]])

"""
仮定の確認
"""

hoge = np.dot(D12.conj().T,D12)
I=np.diag(np.ones( np.shape(D12)[1] ))

if ( hoge == I).all:
    print("D12* D12 = I OK")
else:
    print("D12* D12 = I error")

hoge = np.dot(D21,D21.conj().T)
I=np.diag(np.ones( np.shape(D21)[0] ))

if (hoge == I).all():
    print("D21 D21* = I OK")
else:
    print("D21 D21* = I error")
    
hoge = np.dot(D12.conj().T,C1)
zeros = np.zeros(hoge.shape)

if (hoge == zeros).all():
    print("D12* C1 = 0 OK")
  

hoge = np.dot(B1,D21.conj().T)
zeros = np.zeros(hoge.shape)

if (hoge == zeros).all():
    print("B1 D21* = 0 OK")
    
    
"""
可制御・可観測の確認
"""

VC = copy.deepcopy(B1)
hoge = copy.deepcopy(A)
for i in range(A.shape[0]-1):
    hoge = np.dot(hoge,B1)
    VC=np.append(VC,hoge,axis = 1)  #横につなげる
    
if A.shape[0] == np.linalg.matrix_rank(VC):
    print("A B1 is controllable")
    
VC = copy.deepcopy(B2)
hoge = copy.deepcopy(A)
for i in range(A.shape[0]-1):
    hoge = np.dot(hoge,B2)
    VC=np.append(VC,hoge,axis = 1)  #横につなげる
    
if A.shape[0] == np.linalg.matrix_rank(VC):
    print("A B2 is controllable")
    

VO = copy.deepcopy(C1)
hoge = copy.deepcopy(A)
for i in range(A.shape[0]-1):
    hoge = np.dot(C1,hoge)
    VO=np.append(VO,hoge,axis = 0)  #横につなげる
    
if A.shape[0] == np.linalg.matrix_rank(VO):
    print("C1 A is observable")
    
VO = copy.deepcopy(C2)
hoge = copy.deepcopy(A)
for i in range(A.shape[0]-1):
    hoge = np.dot(C2,hoge)
    VO=np.append(VO,hoge,axis = 0)  #横につなげる
    
if A.shape[0] == np.linalg.matrix_rank(VO):
    print("C2 A is observable")

"""
ガンマイタレーションの開始
"""
gamma_up = 10**10
gamma_down = 0

while True:
    if gamma_up - gamma_down <0.001:
        break
    
    gamma = (gamma_up - gamma_down)/2 + gamma_down
    
    flag,X_H,X_J = check(gamma,A,B1,B2,C1,C2)
    
    if flag == 6:        #条件を満たす
        gamma_up = gamma
    else:                #条件を満たさない
        gamma_down = gamma
    
    print(gamma,flag )

print("gamma_up",gamma_up)

print("gamma_down",gamma_down)

gamma = (gamma_up - gamma_down)/2 + gamma_down


F = -1*np.dot(B2.conj().T,X_H)
n=A.shape[0]
eye = np.diag(np.ones(n))
Z = np.linalg.inv(eye - np.dot(X_J,X_H)/(gamma**2))
L = -1*np.dot(X_J,C2.conj().T)

A_K = A + np.dot(B1,np.dot(B1.conj().T,X_H))/(gamma**2)+np.dot(B2,F)+np.dot(Z,np.dot(L,C2))
B_K = -1*np.dot(Z,L)
C_K = F
D_K = 0

print("A_K",A_K)
print("B_K",B_K)
print("C_K",C_K)
print("D_K",D_K)
```

###scilab

```c
A=[-1 10
   2 -0.1]

B=[1 0 1
   2 0 2]

C=[1 3
   0 0
   1 3]
   
D=[0 0 0
   0 0 1
   0 1 0]

x0=[0
0.05]

sl = syslin('c',A,B,C,D,x0);

gopt = gamitg(sl,[1,1])

//[K,ro] = h_inf(sl,[1,1],0,100,100)

K=ccontrg(sl,[1,1],100);

```

ガンマの最適値はほぼ一致しますが，設計されるコントローラが一致しません・・・

#やめます
途中ですが，こんなことを頑張る意味がない気がしてきました．
理論がわからなくても何とかなりますし，時間をかける必要がない気がしてきました．

もう，やめます．