In [1]:

    

import numpy as np
import sympy as sy
import control.matlab as cm
sy.init_printing(use_latex='mathjax', order='lex')


    

In [2]:

    

h,w0 = sy.symbols('h,omega0', real=True, positive=True)
s,z = sy.symbols('s,z')
t = sy.symbols('t', real=True)


    

In [3]:

    

# Zero-order hold sampling
G = w0**2/(s**2 - w0**2)
ystep = sy.inverse_laplace_transform(G/s, s, t)
sy.expand(ystep)


    

    
Out[3]:





$$\frac{\theta\left(t\right)}{2} e^{\omega_{0} t} - \theta\left(t\right) + \frac{\theta\left(t\right)}{2} e^{- \omega_{0} t}$$

In [4]:

    

Yz = w0**2/(2*w0**2) * ( z/(z-sy.exp(w0*h)) - 2*z/(z-1) + z/(z-sy.exp(-w0*h)))
Uz = z/(z-1)
H = sy.simplify(Yz/Uz)
sy.cancel(H)


    

    
Out[4]:





$$\frac{z e^{2 h \omega_{0}} - 2 z e^{h \omega_{0}} + z + e^{2 h \omega_{0}} - 2 e^{h \omega_{0}} + 1}{2 z^{2} e^{h \omega_{0}} - 2 z e^{2 h \omega_{0}} - 2 z + 2 e^{h \omega_{0}}}$$

In [5]:

    

num, den = sy.fraction(sy.cancel(H))
den


    

    
Out[5]:





$$2 z^{2} e^{h \omega_{0}} - 2 z e^{2 h \omega_{0}} - 2 z + 2 e^{h \omega_{0}}$$

In [6]:

    

num = sy.simplify(num/(2*sy.exp(h*w0)))
den = sy.simplify(den/(2*sy.exp(h*w0)))
den, num


    

    
Out[6]:





$$\left ( z^{2} - z e^{h \omega_{0}} - \frac{z}{e^{h \omega_{0}}} + 1, \quad \frac{z}{2} e^{h \omega_{0}} - z + \frac{z}{2 e^{h \omega_{0}}} + \frac{e^{h \omega_{0}}}{2} - 1 + \frac{1}{2 e^{h \omega_{0}}}\right )$$

In [7]:

    

# Coefficients
dencoeffs = sy.Poly(den, z).all_coeffs()
numcoeffs = sy.Poly(num, z).all_coeffs()
dencoeffs, numcoeffs


    

    
Out[7]:





$$\left ( \left [ 1, \quad \frac{1}{e^{h \omega_{0}}} \left(- e^{2 h \omega_{0}} - 1\right), \quad 1\right ], \quad \left [ \frac{1}{2 e^{h \omega_{0}}} \left(e^{2 h \omega_{0}} - 2 e^{h \omega_{0}} + 1\right), \quad \frac{1}{2 e^{h \omega_{0}}} \left(e^{2 h \omega_{0}} - 2 e^{h \omega_{0}} + 1\right)\right ]\right )$$

In [8]:

    

# Observable canonical form
Phi = sy.simplify(sy.Matrix([[-dencoeffs[1], 1],[-dencoeffs[2], 0]]))
Gamma2 = sy.trigsimp(sy.cancel(sy.Matrix([[numcoeffs[0]],[numcoeffs[1]]])))
C = sy.Matrix([[1, 0]])
Htest = sy.simplify(C*(z*sy.eye(2)-Phi).inv()*Gamma2)
sy.cancel(H), Htest[0]


    

    
Out[8]:





$$\left ( \frac{z e^{2 h \omega_{0}} - 2 z e^{h \omega_{0}} + z + e^{2 h \omega_{0}} - 2 e^{h \omega_{0}} + 1}{2 z^{2} e^{h \omega_{0}} - 2 z e^{2 h \omega_{0}} - 2 z + 2 e^{h \omega_{0}}}, \quad \frac{z e^{2 h \omega_{0}} - 2 z e^{h \omega_{0}} + z + e^{2 h \omega_{0}} - 2 e^{h \omega_{0}} + 1}{2 \left(z^{2} - 2 z \cosh{\left (h \omega_{0} \right )} + 1\right) e^{h \omega_{0}}}\right )$$

In [9]:

    

Phi, Gamma2


    

    
Out[9]:





$$\left ( \left[\begin{matrix}2 \cosh{\left (h \omega_{0} \right )} & 1\\-1 & 0\end{matrix}\right], \quad \left[\begin{matrix}\frac{1}{2 e^{h \omega_{0}}} \left(e^{2 h \omega_{0}} - 2 e^{h \omega_{0}} + 1\right)\\\frac{1}{2 e^{h \omega_{0}}} \left(e^{2 h \omega_{0}} - 2 e^{h \omega_{0}} + 1\right)\end{matrix}\right]\right )$$

In [10]:

    

Gamma = sy.Matrix([[sy.cosh(h*w0)-1],[sy.cosh(h*w0)-1]])
sy.simplify(Gamma2-Gamma)


    

    
Out[10]:





$$\left[\begin{matrix}0\\0\end{matrix}\right]$$

In [11]:

    

Phi,Gamma


    

    
Out[11]:





$$\left ( \left[\begin{matrix}2 \cosh{\left (h \omega_{0} \right )} & 1\\-1 & 0\end{matrix}\right], \quad \left[\begin{matrix}\cosh{\left (h \omega_{0} \right )} - 1\\\cosh{\left (h \omega_{0} \right )} - 1\end{matrix}\right]\right )$$

In [12]:

    

# Some numerical values
w0n = 1.0
hn = 0.2/w0n
Phi_n = np.asarray(Phi.subs({w0:w0n, h:hn})).astype(np.float64)
Gamma_n = np.asarray(Gamma.subs({w0:w0n, h:hn})).astype(np.float64)
Phi_n, Gamma_n


    

    
Out[12]:





(array([[ 2.04013351,  1.        ],
        [-1.        ,  0.        ]]), array([[0.02006676],
        [0.02006676]]))

In [13]:

    

C = np.array([1., 0])
D = np.array([[0]])
sys = cm.ss(Phi_n, Gamma_n, C, D, hn)
cm.pole(sys)


    

    
Out[13]:





array([1.22140276, 0.81873075])

In [14]:

    

# Desired poles: double pole in -2w0 (continuous time)
pd = np.exp(-2*w0n*hn)
L = cm.acker(A=Phi_n, B=Gamma_n,poles = [pd,pd])
L


    

    
Out[14]:





matrix([[19.43797021, 15.42035131]])

In [15]:

    

L2 = np.array([[19,15]])
np.linalg.eig(Phi_n - np.dot(Gamma_n, L2))


    

    
Out[15]:





(array([0.67893191+0.07235727j, 0.67893191-0.07235727j]),
 array([[-0.57809328-0.04268582j, -0.57809328+0.04268582j],
        [ 0.81485341+0.j        ,  0.81485341-0.j        ]]))

In [16]:

    

# Closed-loop system
Phic = Phi_n - np.dot(Gamma_n, L2)
sys_c = cm.ss(Phic, Gamma_n, C, D, hn)
# Steady-state solution
cm.dcgain(sys_c)


    

    
Out[16]:





array(0.37050778)

In [17]:

    

l0=1/0.3705


    

In [18]:

    

import matplotlib.pyplot as plt
tvec = np.arange(0,40)*hn
yout, tout =  cm.step(sys_c*2.7, tvec)
plt.figure(figsize=(13,4))
plt.plot(tout, yout)


    

    
Out[18]:





[<matplotlib.lines.Line2D at 0x7feee2787ac8>]






    

In [19]:

    

cm.tf(sys_c*2.7)


    

    
Out[19]:





  0.05418 z + 0.05418
----------------------
z^2 - 1.358 z + 0.4662

dt = 0.2

In [20]:

    

cm.pole(sys_c)


    

    
Out[20]:





array([0.67893191+0.07235727j, 0.67893191-0.07235727j])

In [21]:

    

# Finding the deadbeat observer
CT = C.copy()
CT.shape = (2,1)
KT = cm.acker(Phi_n.T, CT, [0,0])
K = KT.T
K


    

    
Out[21]:





matrix([[ 2.04013351],
        [-1.        ]])

In [22]:

    

# Verify
sys_o = cm.ss(Phi_n - np.dot(K, CT.T), Gamma_n, C, D, hn)
cm.pole(sys_o)


    

    
Out[22]:





array([0., 0.])

In [23]:

    

D_o = np.array([[l0, 0]])
sys_o = cm.ss(Phi_n - np.dot(K, CT.T)-np.dot(L2, Gamma_n), 
              np.hstack((l0*Gamma_n,K)), -L, D_o, hn)


    

In [24]:

    

cm.tf(sys_o)


    

    
Out[24]:





Input 1 to output 1:
2.699 z^2 + 1.795 z + 0.7887
----------------------------
   z^2 + 1.365 z + 0.6823

Input 2 to output 1:
    -24.24 z + 11.1
----------------------
z^2 + 1.365 z + 0.6823

dt = 0.2

In [25]:

    

K


    

    
Out[25]:





matrix([[ 2.04013351],
        [-1.        ]])

In [26]:

    

np.hstack((l0*Gamma_n, K))


    

    
Out[26]:





matrix([[ 0.05416128,  2.04013351],
        [ 0.05416128, -1.        ]])

In [ ]: