## Final exam fall 2019
Inverted pendulum



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


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


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




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




```python
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)
```




$$\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}}}$$




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




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




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




$$\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 )$$




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




$$\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 )$$




```python
# 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]
```




$$\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 )$$




```python
Phi, Gamma2
```




$$\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 )$$




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




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




```python
Phi,Gamma
```




$$\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 )$$




```python
# 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
```




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




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




    array([1.22140276, 0.81873075])




```python
# 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
```




    matrix([[19.43797021, 15.42035131]])




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




    (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        ]]))




```python
# 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)
```




    array(0.37050778)




```python
l0=1/0.3705
```


```python
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)
```


```python
cm.tf(sys_c*2.7)
```




    
      0.05418 z + 0.05418
    ----------------------
    z^2 - 1.358 z + 0.4662
    
    dt = 0.2




```python
cm.pole(sys_c)
```




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




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




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




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




    array([0., 0.])



### From output feedback with observer to 2dof controller


```python
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)
```


```python
cm.tf(sys_o)
```




    
    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




```python
K
```




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




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




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




```python

```

32a00d9de205cc4b8f90b3378817b6f9d19843b3
ipynb
Jupyter Notebook
misc-notebooks/MR2007-final-exam-fall-2019.ipynb
kjartan-at-tec/mr2007-computerized-control
16e35f5007f53870eaf344eea1165507505ab4aa
MIT
2020-11-07T05:20:37.000Z
2020-12-22T09:46:13.000Z
misc-notebooks/MR2007-final-exam-fall-2019.ipynb
alfkjartan/control-computarizado
5b9a3ae67602d131adf0b306f3ffce7a4914bf8e
MIT
2020-06-12T20:44:41.000Z
2020-06-12T20:49:00.000Z
misc-notebooks/MR2007-final-exam-fall-2019.ipynb
kjartan-at-tec/mr2007-computerized-control
16e35f5007f53870eaf344eea1165507505ab4aa
MIT
2021-03-14T03:55:27.000Z
2021-03-14T03:55:27.000Z
Qwen/Qwen-72B
1. YES
2. YES