#!/usr/bin/env python
# coding: utf-8

# # Chapter 5

# In[1]:


from control.matlab import *
import matplotlib.pyplot as plt
import numpy as np

plt.rcParams['font.family'] ='sans-serif' #使用するフォント
plt.rcParams['xtick.direction'] = 'in' #x軸の目盛線が内向き('in')か外向き('out')か双方向か('inout')
plt.rcParams['ytick.direction'] = 'in' #y軸の目盛線が内向き('in')か外向き('out')か双方向か('inout')
plt.rcParams['xtick.major.width'] = 1.0 #x軸主目盛り線の線幅
plt.rcParams['ytick.major.width'] = 1.0 #y軸主目盛り線の線幅
plt.rcParams['font.size'] = 10 #フォントの大きさ
plt.rcParams['axes.linewidth'] = 1.0 # 軸の線幅edge linewidth。囲みの太さ
plt.rcParams['mathtext.default'] = 'regular'
plt.rcParams['axes.xmargin'] = '0' #'.05'
plt.rcParams['axes.ymargin'] = '0.05'
plt.rcParams['savefig.facecolor'] = 'None'
plt.rcParams['savefig.edgecolor'] = 'None'


# In[2]:


def linestyle_generator():
    linestyle = ['-', '--', '-.', ':']
    lineID = 0
    while True:
        yield linestyle[lineID]
        lineID = (lineID + 1) % len(linestyle)


# In[3]:


def plot_set(fig_ax, *args):
    fig_ax.set_xlabel(args[0])
    fig_ax.set_ylabel(args[1])
    fig_ax.grid(ls=':')
    if len(args)==3:
        fig_ax.legend(loc=args[2])


# In[4]:


def bodeplot_set(fig_ax, *args):
    fig_ax[0].grid(which="both", ls=':')
    fig_ax[0].set_ylabel('Gain [dB]')

    fig_ax[1].grid(which="both", ls=':')
    fig_ax[1].set_xlabel('$\omega$ [rad/s]')
    fig_ax[1].set_ylabel('Phase [deg]')
    
    if len(args) > 0:
        fig_ax[1].legend(loc=args[0])
    if len(args) > 1:
        fig_ax[0].legend(loc=args[1])


# ## 垂直駆動アームの角度追従制御

# In[5]:


g  = 9.81                # 重力加速度[m/s^2]
l  = 0.2                 # アームの長さ[m]
M  = 0.5                 # アームの質量[kg]
mu = 1.5e-2              # 粘性摩擦係数[kg*m^2/s]
J  = 1.0e-2              # 慣性モーメント[kg*m^2]

P = tf( [0,1], [J, mu, M*g*l] )

ref = 30 # 目標角度 [deg]


# ### P制御

# In[6]:


LS = linestyle_generator()
fig, ax = plt.subplots(figsize=(3, 2.3))

kp = (0.5, 1, 2)
for i in range(3):
    K = tf([0, kp[i]], [0, 1])
    Gyr = feedback(P*K, 1)
    y,t = step(Gyr, np.arange(0, 2, 0.01))
    
    pltargs = {'ls': next(LS), 'label': '$k_P$='+str(kp[i])}    
    ax.plot(t, y*ref, **pltargs)

ax.axhline(ref, color="k", linewidth=0.5)
plot_set(ax, 't', 'y', 'best')

ax.set_xlim(0, 2)
ax.set_ylim(0, 50)

# fig.savefig("pcont.pdf", transparent=True, bbox_inches="tight", pad_inches=0.0)


# In[7]:


LS = linestyle_generator()
fig, ax = plt.subplots(2, 1, figsize=(4, 3.5)) 

for i in range(len(kp)):
    K = tf([0, kp[i]], [0, 1])
    Gyr = feedback(P*K, 1)
    gain, phase, w = bode(Gyr, logspace(-1,2), plot=False)
    
    pltargs = {'ls': next(LS), 'label': '$k_P$='+str(kp[i])}
    ax[0].semilogx(w, 20*np.log10(gain), **pltargs)
    ax[1].semilogx(w, phase*180/np.pi, **pltargs)
        
bodeplot_set(ax, 'lower left')

ax[1].set_ylim(-190,10)
ax[1].set_yticks([-180,-90,0])

fig.tight_layout()
# fig.savefig("pcont_bode.pdf", transparent=True, bbox_inches="tight", pad_inches=0.0)


# ### PD制御

# In[8]:


kp = 2
kd = (0, 0.1, 0.2)

LS = linestyle_generator()
fig, ax = plt.subplots(figsize=(3, 2.3))

for i in range(3):
    K = tf([kd[i], kp], [0, 1])
    Gyr = feedback(P*K, 1)
    y,t = step(Gyr,np.arange(0, 2, 0.01))
    
    pltargs = {'ls': next(LS), 'label': '$k_D$='+str(kd[i])}    
    ax.plot(t, y*ref, **pltargs)

ax.axhline(ref, color="k", linewidth=0.5) 
plot_set(ax, 't', 'y', 'best')

ax.set_xlim(0, 2)
ax.set_ylim(0, 50)

#fig.savefig("pdcont.pdf", transparent=True, bbox_inches="tight", pad_inches=0.0)


# In[9]:


LS = linestyle_generator()
fig, ax = plt.subplots(2, 1, figsize=(4, 3.5)) 

for i in range(3):
    K = tf([kd[i], kp], [0,1])
    Gyr = feedback(P*K, 1)
    gain, phase, w = bode(Gyr, logspace(-1,2), dB=True, plot=False)
    
    pltargs = {'ls': next(LS), 'label': '$k_D$='+str(kd[i])} 
    ax[0].semilogx(w, 20*np.log10(gain), **pltargs)
    ax[1].semilogx(w, phase*180/np.pi, **pltargs)
        
bodeplot_set(ax, 'lower left')

ax[1].set_ylim(-190,10)
ax[1].set_yticks([-180,-90,0])

fig.tight_layout()
#fig.savefig("pdcont_bode.pdf", transparent=True, bbox_inches="tight", pad_inches=0.0)


# ### PID制御

# In[10]:


kp = 2
kd = 0.1
ki = (0, 5, 10)

LS = linestyle_generator()
fig, ax = plt.subplots(figsize=(3, 2.3))

for i in range(3):
    K = tf([kd, kp, ki[i]], [1, 0])
    Gyr = feedback(P*K, 1)
    y, t = step(Gyr, np.arange(0, 2, 0.01))
    
    pltargs = {'ls': next(LS), 'label': '$k_I$='+str(ki[i])}    
    ax.plot(t, y*ref, **pltargs)

ax.axhline(ref, color="k", linewidth=0.5) 
plot_set(ax, 't', 'y', 'upper left')
 
ax.set_xlim(0, 2)
ax.set_ylim(0,50)

# fig.savefig("pidcont.pdf", transparent=True, bbox_inches="tight", pad_inches=0.0)


# In[11]:


LS = linestyle_generator()
fig, ax = plt.subplots(2, 1, figsize=(4, 3.5)) 

for i in range(3):
    K = tf([kd, kp, ki[i]], [1, 0])
    Gyr = feedback(P*K,1)
    gain, phase, w = bode(Gyr, logspace(-1,2), plot=False)
    
    pltargs = {'ls': next(LS), 'label': '$k_I$='+str(ki[i])} 
    ax[0].semilogx(w, 20*np.log10(gain), **pltargs)
    ax[1].semilogx(w, phase*180/np.pi, **pltargs)
        
bodeplot_set(ax, 'best')

ax[1].set_ylim(-190,10)
ax[1].set_yticks([-180,-90,0])

fig.tight_layout()
#fig.savefig("pidcont_bode.pdf", transparent=True, bbox_inches="tight", pad_inches=0.0)


# ### 練習問題（外乱抑制）

# In[12]:


LS = linestyle_generator()
fig, ax = plt.subplots(figsize=(3, 2.3))

for i in range(3):
    K = tf([kd, kp, ki[i]], [1, 0])
    Gyd = feedback(P, K)
    y, t = step(Gyd, np.arange(0, 2, 0.01))
    
    pltargs = {'ls': next(LS), 'label': '$k_I$='+str(ki[i])}    
    ax.plot(t, y, **pltargs)

plot_set(ax, 't', 'y', 'center right')
ax.set_xlim(0, 2)
ax.set_ylim(-0.05, 0.5)

# fig.savefig("pidcont_dis.pdf", transparent=True, bbox_inches="tight", pad_inches=0.0)


# In[13]:


LS = linestyle_generator()
fig, ax = plt.subplots(2, 1, figsize=(4, 3.5))  

for i in range(3):
    K = tf([kd, kp, ki[i]], [1,0])
    Gyd = feedback(P, K)
    gain, phase, w = bode(Gyd, logspace(-1,2), plot=False)
    
    pltargs = {'ls': next(LS), 'label': '$k_I$='+str(ki[i])} 
    ax[0].semilogx(w, 20*np.log10(gain), **pltargs)
    ax[1].semilogx(w, phase*180/np.pi, **pltargs)
        
bodeplot_set(ax, 'best')

ax[1].set_ylim(-190,100)
ax[1].set_yticks([-180,-90, 0, 90])

fig.tight_layout()
# fig.savefig("pidcont_dis_bode.pdf", transparent=True, bbox_inches="tight", pad_inches=0.0)


# ## 2自由度制御

# In[14]:


kp = 2
ki = 10
kd = 0.1

K1 = tf([kd, kp, ki], [1, 0])
K2 = tf([0, ki], [kd, kp, ki])
K3 = tf([kp, ki], [kd, kp, ki])


# ### PI-D制御

# In[15]:


Gyz = feedback(P*K1, 1)

Td = np.arange(0, 2, 0.01)
r = 1*(Td>0)

z, t, _ = lsim(K3, r, Td, 0)

fig, ax = plt.subplots(1, 2, figsize=(6, 2.3))

y, _, _ = lsim(Gyz, r, Td, 0)
ax[0].plot(t, r*ref)
ax[1].plot(t, y*ref, ls='--', label='PID')

y, _, _ = lsim(Gyz, z, Td, 0)
ax[0].plot(t, z*ref)
ax[1].plot(t, y*ref, label='PI-D')

plot_set(ax[0], 't', 'r')
ax[0].set_xlim(0, 2)
ax[0].set_ylim(0,50)

ax[1].axhline(ref, color="k", linewidth=0.5)  
plot_set(ax[1], 't', 'y', 'best')

ax[1].set_xlim(0, 2)
ax[1].set_ylim(0,50)

fig.tight_layout()
# fig.savefig("2deg1.pdf", transparent=True, bbox_inches="tight", pad_inches=0.0)


# 制御入力の計算
# 
# PID制御では，$G_{ur}$ がインプロパーになるので，擬似微分を用いて計算する

# In[16]:


tau = 0.0000001 # ローパスフィルタ
Klp = tf([kd, 0], [tau, 1]) # 擬似微分器
Ktau = tf([kp, ki], [1, 0]) + Klp

Gyz = feedback(P*Ktau, 1)
Guz = Ktau/(1+P*Ktau)

Td = np.arange(0, 2, 0.01)
r = 1*(Td>0)

z, t, _ = lsim(K3, r, Td, 0)

fig, ax = plt.subplots(1, 2, figsize=(6, 2.3))

u, _, _ = lsim(Guz, r, Td, 0)
ax[0].plot(t, u, ls='--', label='PID')

u, _, _ = lsim(Guz, z, Td, 0)
ax[0].plot(t, u, label='PI-D')

y, _, _ = lsim(Gyz, r, Td, 0)
ax[1].plot(t, y*ref, ls='--', label='PID')

y, _, _ = lsim(Gyz, z, Td, 0)
ax[1].plot(t, y*ref, label='PI-D')

ax[0].set_xlim(0, 0.5)
ax[1].axhline(ref, color="k", linewidth=0.5)  
plot_set(ax[0], 't', 'u', 'best')
plot_set(ax[1], 't', 'y', 'best')
ax[1].set_xlim(0, 2)
ax[1].set_ylim(0,50)

fig.tight_layout()
# fig.savefig("2deg1_u.pdf", transparent=True, bbox_inches="tight", pad_inches=0.0)


# ### I-PD制御

# In[17]:


Gyz = feedback(P*K1, 1)

Td = np.arange(0, 2, 0.01)
r = 1*(Td>0)

z, t, _ = lsim(K2, r, Td, 0)

fig, ax = plt.subplots(1, 2, figsize=(6, 2.3))

y, _, _ = lsim(Gyz, r, Td, 0)
ax[0].plot(t, r*ref)
ax[1].plot(t, y*ref, ls='--', label='PID')

y, _, _ = lsim(Gyz, z, Td, 0)
ax[0].plot(t, z*ref)
ax[1].plot(t, y*ref, label='I-PD')

plot_set(ax[0], 't', 'r')
ax[0].set_xlim(0, 2)
ax[0].set_ylim(0,50)

ax[1].axhline(ref, linewidth=0.5)    
plot_set(ax[1], 't', 'y', 'best')

ax[1].set_xlim(0, 2)
ax[1].set_ylim(0,50)

fig.tight_layout()
# fig.savefig("2deg2.pdf", transparent=True, bbox_inches="tight", pad_inches=0.0)


# In[18]:


tau = 0.0000001 # ローパスフィルタ
Klp = tf([kd, 0], [tau, 1]) # 擬似微分器
Ktau = tf([kp, ki], [1, 0]) + Klp

Gyz = feedback(P*Ktau, 1)
Guz = Ktau/(1+P*Ktau)

Td = np.arange(0, 2, 0.01)
r = 1*(Td>0)

z, t, _ = lsim(K2, r, Td, 0)

fig, ax = plt.subplots(1, 2, figsize=(6, 2.3))

u, _, _ = lsim(Guz, r, Td, 0)
ax[0].plot(t, u, ls='--', label='PID')

u, _, _ = lsim(Guz, z, Td, 0)
ax[0].plot(t, u, label='I-PD')

y, _, _ = lsim(Gyz, r, Td, 0)
ax[1].plot(t, y*ref, ls='--', label='PID')

y, _, _ = lsim(Gyz, z, Td, 0)
ax[1].plot(t, y*ref, label='I-PD')

ax[0].set_xlim(0, 0.5)

ax[1].axhline(ref, color="k", linewidth=0.5)  
plot_set(ax[0], 't', 'u', 'best')
plot_set(ax[1], 't', 'y', 'best')
ax[1].set_xlim(0, 2)
ax[1].set_ylim(0,50)


fig.tight_layout()
# fig.savefig("2deg2_u.pdf", transparent=True, bbox_inches="tight", pad_inches=0.0)


# # 限界感度法

# ### 無駄時間要素

# In[19]:


# num_delay, den_delay = pade( 0.005, 5)
num_delay, den_delay = pade( 0.005, 1)
Pdelay = P * tf(num_delay, den_delay)
Pdelay
print(pole(Pdelay))


# In[20]:


kvect = np.arange(0, 5, 0.001)
# rlist, klist = rlocus(Pdelay)
rlist, klist = rlocus(Pdelay, kvect, plot=False)
fig, ax = plt.subplots(figsize=(3,3))

ax.plot(rlist.real, rlist.imag)
ax.set_xlim(-3, 1)
ax.grid(ls=':')


# In[21]:


rlist, klist = rlocus(P, kvect, plot=False)
fig, ax = plt.subplots(figsize=(3,3))

ax.plot(rlist.real, rlist.imag)
ax.set_xlim(-3, 1)
ax.grid(ls=':')


# ### チューニング

# In[22]:


fig, ax = plt.subplots(figsize=(3, 2.3))

kp0 = 2.9
K = tf([0, kp0], [0, 1])
Gyr = feedback(Pdelay*K, 1)
y,t = step(Gyr, np.arange(0, 2, 0.01))
    
ax.plot(t, y*ref, color='k')

ax.axhline(ref, color='k', linewidth=0.5)    
ax.set_xlim(0, 2)
ax.set_ylim(0, 50)
plot_set(ax, 't', 'y')

# fig.savefig("tune_zn.pdf", transparent=True, bbox_inches="tight", pad_inches=0.0)


# In[23]:


kp = [0, 0]
ki = [0, 0]
kd = [0, 0]
Rule = ['', '']

T0 = 0.3
# Classic ZN
Rule[0] = 'Classic'
kp[0] = 0.6 * kp0
ki[0] = kp[0] / (0.5 * T0)
kd[0] = kp[0] * (0.125 * T0)

# No overshoot
Rule[1] = 'No Overshoot'
kp[1] = 0.2 * kp0
ki[1] = kp[1] / (0.5 * T0)
kd[1] = kp[1] * (0.33 * T0)

LS = linestyle_generator()
fig, ax = plt.subplots(figsize=(3, 2.3))

for i in range(2):
    K = tf([kd[i], kp[i], ki[i]], [1, 0])
    Gyr = feedback(Pdelay*K, 1)
    y, t = step(Gyr, np.arange(0, 2, 0.01))
    
    ax.plot(t, y*ref, ls=next(LS), label=Rule[i])
    
    print(Rule[i])
    print('kP=', kp[i])
    print('kI=', ki[i])
    print('kD=', kd[i])
    print('------------------')

ax.axhline(ref, color="k", linewidth=0.5)    
ax.set_xlim(0, 2)
ax.set_ylim(0, 50)
plot_set(ax, 't', 'y', 'best')

# fig.savefig("tune_zn_result.pdf", transparent=True, bbox_inches="tight", pad_inches=0.0)


# # モデルマッチング

# In[24]:


import sympy as sp
s = sp.Symbol('s')
kp, kd, ki = sp.symbols('k_p k_d k_i')
Mgl, mu, J = sp.symbols('Mgl mu J')
sp.init_printing()

G = (kp*s+ki)/(J*s**3 +(mu+kd)*s**2 + (Mgl + kp)*s + ki)
sp.series(1/G, s, 0, 4)


# In[25]:


import sympy as sp
z, wn = sp.symbols('zeta omega_n')
kp, kd, ki = sp.symbols('k_p k_d k_i')
Mgl,mu,J = sp.symbols('Mgl mu J')
sp.init_printing()

f1 = Mgl/ki-2*z/wn
f2 = (mu+kd)/ki-Mgl*kp/(ki**2)-1/(wn**2)
f3 = J/ki-kp*(mu+kd)/(ki**2)+Mgl*kp**2/(ki**3)
sp.solve([f1, f2, f3],[kp, kd, ki])


# In[26]:


g  = 9.81                # 重力加速度[m/s^2]
l  = 0.2                 # アームの長さ[m]
M  = 0.5                 # アームの質量[kg]
mu = 1.5e-2              # 粘性摩擦係数
J  = 1.0e-2              # 慣性モーメント

P = tf( [0,1], [J, mu, M*g*l] )

ref = 30 # 目標角度 [deg]


# In[27]:


omega_n = 15
zeta = (1, 1/np.sqrt(2))
Label = ('Binomial coeff.', 'Butterworth')

LS = linestyle_generator()
fig, ax = plt.subplots(figsize=(3, 2.3))

for i in range(2):
    Msys = tf([0,omega_n**2], [1,2*zeta[i]*omega_n,omega_n**2])
    y, t = step(Msys, np.arange(0, 2, 0.01))
    
    ax.plot(t, y*ref, ls=next(LS), label=Label[i])

ax.set_xlim(0, 2)
ax.set_ylim(0, 50)
plot_set(ax, 't', 'y', 'best')

# fig.savefig("ref_model_2nd.pdf", transparent=True, bbox_inches="tight", pad_inches=0.0)  


# In[28]:


omega_n = 15
zeta = 0.707

Msys = tf([0,omega_n**2], [1,2*zeta*omega_n,omega_n**2])

kp = omega_n**2*J
ki = omega_n*M*g*l/(2*zeta)
kd = 2*zeta*omega_n*J + M*g*l/(2*zeta*omega_n) - mu

print('kP=', kp)
print('kI=', ki)
print('kD=', kd)

Gyr = tf([kp,ki], [J, mu+kd, M*g*l+kp, ki])

yM, tM = step(Msys, np.arange(0, 2, 0.01))
y, t = step(Gyr, np.arange(0, 2, 0.01))

fig, ax = plt.subplots(figsize=(3, 2.3))

ax.plot(tM, yM*ref, label='M', ls = '-.')
ax.plot(t, y*ref, label='Gyr')

ax.set_xlim(0, 2)
ax.set_ylim(0, 50)
plot_set(ax, 't', 'y', 'best')

# fig.savefig("model_match.pdf", transparent=True, bbox_inches="tight", pad_inches=0.0)


# In[60]:


alpha1 = (3, 2, 2.15)
alpha2 = (3, 2, 1.75)
omega_n = 15
Label = ('Binomial coeff.', 'Butterworth', 'ITAE')

LS = linestyle_generator()
fig, ax = plt.subplots(figsize=(3, 2.3))

for i in range(3):
    M = tf([0, omega_n**3], [1, alpha2[i]*omega_n, alpha1[i]*omega_n**2, omega_n**3])
    y,t = step(M, np.arange(0, 2, 0.01))
    
    ax.plot(t, y*ref, ls=next(LS), label=Label[i])

ax.set_xlim(0, 2)
ax.set_ylim(0, 50)
plot_set(ax, 't', 'y', 'best')

# fig.savefig("ref_model_3rd.pdf", transparent=True, bbox_inches="tight", pad_inches=0.0)   


# ## 状態フィードバック

# ### 極配置

# In[30]:


from control.matlab import *
import scipy

A = '0 1; -4 5'
B = '0; 1'
C = '1 0 ; 0 1'
D = '0; 0'
P = ss(A, B, C, D)
print(P)


# In[31]:


np.linalg.eigvals(P.A)


# In[32]:


Pole = [-1, -1]
F = -acker(P.A, P.B, Pole)
F


# In[33]:


scipy.linalg.eigvals(P.A+P.B*F)


# In[34]:


np.linalg.eigvals(P.A+P.B*F)


# In[35]:


Acl = P.A + P.B*F
Pfb = ss(Acl, P.B, P.C, P.D)

Td = np.arange(0, 5, 0.01)
X0 = [-0.3, 0.4]
x, t = initial(Pfb, Td, X0) #ゼロ入力応答

fig, ax = plt.subplots(figsize=(3, 2.3))
ax.plot(t, x[:,0], label = '$x_1$')
ax.plot(t, x[:,1], ls = '-.', label = '$x_2$')

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

# fig.savefig("sf_pole.pdf", transparent=True, bbox_inches="tight", pad_inches=0.0)


# ### 最適レギュレータ

# In[36]:


Q = [ [100, 0], [0, 1]]
R = 1

F, X, E = lqr(P.A, P.B, Q, R)
F = -F

print('--- フィードバックゲイン ---')
print(F)
print(-(1/R)*P.B.T*X)
print('--- 閉ループ極 ---')
print(E)
print(np.linalg.eigvals(P.A+P.B*F))


# In[37]:


FF = - (1/R)*(P.B.T)*X
FF


# In[38]:


X, E, F = care(P.A, P.B, Q, R)
F = -F
print('--- フィードバックゲイン ---')
print(F)
print('--- 閉ループ極 ---')
print(E)


# In[39]:


Acl = P.A + P.B*F
Pfb = ss(Acl, P.B, P.C, P.D)

tdata = np.arange(0, 5, 0.01)
xini, tini = initial(Pfb, tdata, [-0.3, 0.4]) #ゼロ入力応答

fig, ax = plt.subplots(figsize=(3, 2.3))

ax.plot(tini, xini[:,0], label = '$x_1$')
ax.plot(tini, xini[:,1], ls = '-.', label = '$x_2$')

ax.set_xlabel('t')
ax.set_ylabel('x')
ax.grid(ls=':')
ax.legend()

# fig.savefig("sf_lqr.pdf", transparent=True, bbox_inches="tight", pad_inches=0.0)


# ### 円条件（最適レギュレータのロバスト性）

# In[40]:


A = '0 1; -4 5'
B = '0; 1'
C = '1 0 ; 0 1'
D = '0; 0'
P = ss(A, B, C, D)
L = ss(P.A, P.B, -F, 0)

print(L)


# In[41]:


import matplotlib.patches as patches

fig, ax = plt.subplots(figsize=(3, 3))
x, y, w = nyquist(L, logspace(-2,3,1000), plot=False)
ax.plot(x, y)
ax.plot(x, -y, ls='--')
ax.scatter(-1, 0, color='k')

ax.grid(ls=':')
ax.set_xlim(-2.5, 2.5)
ax.set_ylim(-2.5, 2.5)

c = patches.Circle(xy=(-1, 0), radius=1, fill=False, ec='k')
ax.add_patch(c)

fig.tight_layout()


# 開ループ系のナイキスト軌跡が (-1, 0j) を中心とする単位円の中に入らない．
# 
# これにより，位相余裕が 60 [deg] 以上であることが保証される．

# ### ハミルトン行列

# In[42]:


H1 = np.c_[P.A, -P.B*(1/R)*P.B.T]
H2 = np.c_[ Q, P.A.T]
H = np.r_[H1, -H2]
eigH = np.linalg.eigvals(H)
print(eigH)

print('--- ハミルトン行列の安定固有値 ---')
eigH_stable = [ i for i in eigH if i < 0]
print(eigH_stable)

F = -acker(P.A, P.B, eigH_stable)
print('--- フィードバックゲイン ---')
print(F)


# In[43]:


H1 = np.hstack((P.A, -P.B*(1/R)*P.B.T))
H2 = np.hstack(( Q, P.A.T))
H = np.vstack((H1, -H2))
eigH = np.linalg.eigvals(H)
print(eigH)

print('--- ハミルトン行列の安定固有値 ---')
eigH_stable = [ i for i in eigH if i < 0]
print(eigH_stable)

F = -acker(P.A, P.B, eigH_stable)
print('--- フィードバックゲイン ---')
print(F)


# In[44]:


scipy.linalg.eigvals(H)


# In[45]:


E


# ## 積分サーボ系

# In[46]:


A = '0 1; -4 5'
B = '0; 1'
C = '1 0 ; 0 1'
D = '0; 0'
P = ss(A, B, C, D)
print(P)


# In[47]:


Pole = [-1, -1]
F = -acker(P.A, P.B, Pole)
F


# In[48]:


Acl = P.A + P.B*F
Pfb = ss(Acl, P.B, P.C, P.D)

Td = np.arange(0, 8, 0.01)
Ud = 0.2 * (Td>0)
x, t, _ = lsim(Pfb, Ud, Td, [-0.3, 0.4])

fig, ax = plt.subplots(figsize=(3, 2.3))

ax.plot(t, x[:,0], label = '$x_1$')
ax.plot(t, x[:,1], ls = '-.', label = '$x_2$')
plot_set(ax, 't', 'x', 'best')

# fig.savefig("sf_dis.pdf", transparent=True, bbox_inches="tight", pad_inches=0.0)


# In[49]:


A = '0 1; -4 5'
B = '0; 1'
C = '1 0'
D = '0'
P = ss(A, B, C, D)
print(P)


# In[50]:


Abar = np.r_[ np.c_[P.A, np.zeros((2,1))], -np.c_[ P.C, 0 ] ]
Bbar = np.c_[ P.B.T, 0 ].T
Cbar = np.c_[ P.C, 0 ]


# In[51]:


Pole = [-1, -1, -5]
F = -acker(Abar, Bbar, Pole)
F


# In[52]:


# Acl = P.A + P.B*F[0,0:2]
Acl = Abar + Bbar*F
Pfb = ss(Acl, Bbar, np.eye(3), np.zeros((3,1)))

Td = np.arange(0, 8, 0.01)
Ud = 0.2 * (Td>0)
x, t, _ = lsim(Pfb, Ud, Td, [-0.3, 0.4, 0])
 
fig, ax = plt.subplots(figsize=(3, 2.3))
ax.plot(t, x[:,0], label = '$x_1$')
ax.plot(t, x[:,1], ls = '-.',label = '$x_2$')
# ax.plot(t, Ud, c='k')
plot_set(ax, 't', 'x', 'best')

# fig.savefig("servo.pdf", transparent=True, bbox_inches="tight", pad_inches=0.0)


# ## 可制御性，可観測性

# In[53]:


A = '0 1; -4 5'
B = '0; 1'
C = '1 0'
D = '0'
P = ss(A, B, C, D)
print(P)


# In[54]:


Uc = ctrb(P.A, P.B)
print('Uc=\n',Uc)
print('det(Uc)=', np.linalg.det(Uc))
print('rank(Uc)=', np.linalg.matrix_rank(Uc))


# In[55]:


Uo = obsv(P.A, P.C)
print('Uo=\n', Uo)
print('det(Uo)=', np.linalg.det(Uo))
print('rank(Uo)=', np.linalg.matrix_rank(Uo))


# In[ ]: