# Output feedback control for the harmonic oscillator
The harmonic oscillator is a useful model for systems that have a dominating resonance frequency with no, or very little damping. An example of such systems is the sideway movement of a container hanging from a gantry crane moving containers on and off ships.

<font size="2">by Tosaka, from Wikimedia.org</font>

Consider a container of mass $m=10^4$ kg, hanging from a wire of length $l=40$ m. We can control the system by applying an acceleration $u$ of the cart on top of the crane. The ODE describing the dynamics of the container is obtained by considering a reference frame fixed in the cart 
$$ ml^2 \ddot{\theta} = -lmg\sin\theta + lm\cos\theta u + l\cos\theta w,$$
where $\theta$ is the angle of the wires to the vertical, and $w$ is a disturbance force from wind blowing on the container. The small-angle approximation $\sin\theta \approx \theta$ works well in this case, giving the model
$$\ddot{\theta} = -\frac{g}{l}\theta + \frac{1}{l}u + \frac{1}{lm}w.$$
Write $y=\theta$ and the model in the s-domain becomes
$$ Y(s) = \frac{1}{s^2 + \omega_0^2}\Big(bU(s) + kW(s)\big),$$
where $\omega_0^2 = \frac{g}{l}= \frac{9.8}{40} \approx 0.25$ and $b=1/l= 0.025$ and $k=1/(lm) = 2.5\times 10^{-5}$ 

## Discrete-time state-space model
Zero-order-hold sampling of the continuous-time harmonic oscillator gives the pulse-transfer function
$$ H(z) = \frac{(1-\beta)(z+1)b}{z^2 - 2\beta z + 1}, \quad \beta=\cos(\omega_0 h)$$
The system can be written on state-space form (observable canonical form) as
\begin{align}
x(k+1) &= \begin{bmatrix} 2\beta & 1\\-1 & 0\end{bmatrix}x(k) = \begin{bmatrix} 1-\beta\\1-\beta\end{bmatrix}b u(k) \\
y(k) &= \begin{bmatrix}1 & 0\end{bmatrix} x(k)
\end{align}
where
### Verification by symbolic computation


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

h,omega0 = sy.symbols('h,omega0', real=True, positive=True)
z = sy.symbols('z')
beta = sy.cos(omega0*h)
Phi = sy.Matrix([[2*beta, 1],[-1, 0]])
Gamma = sy.Matrix([[1-beta],[1-beta]])
C = sy.Matrix([[1, 0]])
H = sy.simplify(C*(z*sy.eye(2)-Phi).inv()*Gamma)
H
```




$$\left[\begin{matrix}\frac{- z \cos{\left (h \omega_{0} \right )} + z - \cos{\left (h \omega_{0} \right )} + 1}{z^{2} - 2 z \cos{\left (h \omega_{0} \right )} + 1}\end{matrix}\right]$$



### Choosing the sampling ratio $h$
We may use the rule-of-thumb $\omega_0 h \approx 0.2\, \text{to} \, 0.6$ for choosing the sampling period. For our specific case we have $\omega_0^2 = 0.25$. Let's choose $\omega_0 h = \pi/6 \approx 0.53$, so that $\cos(h\omega_0) = \frac{\sqrt{3}}{2} \approx 0.866$ This gives the discrete-time system (ignoring the disturbance for now)
\begin{align}
    x(k+1) &= \begin{bmatrix} \sqrt{3} & 1 \\ -1 & 0 \end{bmatrix}x(k) + \begin{bmatrix} 1-\frac{\sqrt{3}}{2}\\1-\frac{\sqrt{3}}{2}\end{bmatrix}bu(k)\\
    y(k) &= \begin{bmatrix} 1 & 0\end{bmatrix} x
 \end{align}


```python
omegaval = 0.5
hval = np.pi/6/omegaval
Phi_np = np.array(Phi.subs({h:hval, omega0:omegaval})).astype(np.float64)
Phi_np
```




    array([[ 1.73205081,  1.        ],
           [-1.        ,  0.        ]])




```python
Gamma_np = np.array(Gamma.subs({h:hval, omega0:omegaval})).astype(np.float64)
Gamma_np
```




    array([[0.1339746],
           [0.1339746]])



## Obervability 
The observability matrix for this second order system becomes
$$ W_o = \begin{bmatrix} C\\C\Phi\end{bmatrix} = \begin{bmatrix} 1 & 0\\ \sqrt{3} & 1\end{bmatrix}, $$
with determinant
$$\det W_0 =  1-0 = 1 \neq 0.$$


```python
C_n = np.array(C).astype(np.float64)
Wo_n = np.vstack((C_n, np.dot(C_n,Phi_np)))
Wo_n
```




    array([[1.        , 0.        ],
           [1.73205081, 1.        ]])




```python
np.linalg.det(Wo_n)
```




$$1.0$$



## Observer design
The observer is given by
\begin{align}
\hat{x}(k+1) &= \Phi \hat{x}(k) +\Gamma u(k) + K\big(y(k) - C\hat{x}(k)\big)\\
&= \big(\Phi - KC\big)\hat{x}(k) + \Gamma u(k) + Ky(k)
\end{align}
where
$$ KC = \begin{bmatrix} k_1\\ k_2\end{bmatrix}\begin{bmatrix}1 & 0\end{bmatrix} = \begin{bmatrix} k_1 & 0\\ k_2 & 0\end{bmatrix} $$
and
$$ \Phi - KC = \begin{bmatrix} \sqrt{3} & 1\\-1 & 0\end{bmatrix}-\begin{bmatrix} k_1 & 0\\ k_2 & 0\end{bmatrix}=\begin{bmatrix} \sqrt{3}-k_1 & 1\\-1-k_2 & 0\end{bmatrix}$$


```python
k1,k2 = sy.symbols('k1,k2')
K = sy.Matrix([[k1], [k2]])
Phi_o=Phi.subs({h:hval, omega0:omegaval}) - K*C
Phi_o
```




$$\left[\begin{matrix}- k_{1} + 1.73205080756888 & 1\\- k_{2} - 1 & 0\end{matrix}\right]$$



The characteristic polynomial of the observer is given by
\begin{align}
\det \left( zI - (\Phi-KC) \right) &= \det \left( \begin{bmatrix} z & 0\\0 & z \end{bmatrix} - \begin{bmatrix} \sqrt{3}-k_1 & 1\\-1-k_2\end{bmatrix} \right)\\
&= \det \begin{bmatrix} z-\sqrt{3}+k_1 & -1 \\ 1+k_2 & z\end{bmatrix}\\
&= z^2 + (-\sqrt{3}+k_1) z + (1 + k_2)
\end{align}


```python
z = sy.symbols('z')
zIminPhio = z*sy.eye(2) - Phi_o
ch_poly = sy.Poly(zIminPhio.det(), z)
ch_poly.as_expr()
```




$$1.0 k_{2} + 1.0 z^{2} + z \left(1.0 k_{1} - 1.73205080756888\right) + 1.0$$



### Desired observer poles
Assume we want the observer to have a double pole in $z=p_o$, which gives the following  desired characteristic polynomial for the observer
$$ (z-p_o)^2 = z^2 - 2p_oz + p_o^2$$
#### Determining the observer gain
As usual, we determine the unknown parameters by setting the coefficients equal in the two characteristic polynomials above. This gives the system of linear equations
\begin{align}
z^1: \qquad k_1 - \sqrt{3} &= -2p_o\\
z^0: \qquad 1+k_2 &= p_o^2
\end{align}
which has the straight-forward solution 
\begin{align}
k_1 &= \sqrt{3}-2p_o\\
k_2 &= p_o^2 - 1.
\end{align}
For a deadbeat observer we get
\begin{align}
k_1 &= \sqrt{3}\\
k_2 &= - 1.
\end{align}

The solution is so simple because the state-space system was written on observable canonical form. 


```python
po = sy.symbols('p_o')
K = sy.Matrix([[Phi[0,0]-2*po],[Phi[1,0]+po**2]])
K
```




$$\left[\begin{matrix}- 2 p_{o} + 2 \cos{\left (h \omega_{0} \right )}\\p_{o}^{2} - 1\end{matrix}\right]$$




```python
Phi_o = Phi - K*C
Phi_o
```




$$\left[\begin{matrix}2 p_{o} & 1\\- p_{o}^{2} & 0\end{matrix}\right]$$




```python
Phi_o.eigenvals()
```




$$\left \{ p_{o} : 2\right \}$$



## Reachability 


```python
Wc = sy.BlockMatrix([[Gamma, Phi*Gamma]]).as_explicit()
Wc
```




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




```python
Wc.det()
```




$$- 2 \cos^{3}{\left (h \omega_{0} \right )} + 2 \cos^{2}{\left (h \omega_{0} \right )} + 2 \cos{\left (h \omega_{0} \right )} - 2$$




```python
Wc.det().subs({h:hval, omega0:omegaval})
```

## State feedback
Introducing the state-feedback control law
$$ u = -l_1x_1 - l_2 x_2 + l_0y_{ref} = -Lx + l_0y_{ref}$$
gives the closed-loop state-space system
\begin{align}
x(k+1) &= \Phi x(k) +\Gamma\big(-Lx(k) + l_0y_{ref}(k)\big) + \Gamma v(k) = \left( \Phi - \Gamma L \right) x(k) + l_0\Gamma y_{ref}(k) + \Gamma v(k)\\
y(k) &= C x(k),
\end{align}


```python
l1,l2 = sy.symbols('l1,l2')
L = sy.Matrix([[l1, l2]])
Phi_c=Phi.subs({h:hval, omega0:omegaval}) - Gamma.subs({h:hval, omega0:omegaval})*L
Phi_c
```




$$\left[\begin{matrix}- 0.133974596215561 l_{1} + 1.73205080756888 & - 0.133974596215561 l_{2} + 1\\- 0.133974596215561 l_{1} - 1 & - 0.133974596215561 l_{2}\end{matrix}\right]$$



The characteristic polynomial of the closed-loop system is given by
\begin{align}
\det \left( zI - (\Phi-\Gamma L) \right) \end{align}


```python
zIminPhic = z*sy.eye(2) - Phi_c
zIminPhic
```




$$\left[\begin{matrix}0.133974596215561 l_{1} + z - 1.73205080756888 & 0.133974596215561 l_{2} - 1\\0.133974596215561 l_{1} + 1 & 0.133974596215561 l_{2} + z\end{matrix}\right]$$




```python
ch_poly = sy.Poly(zIminPhic.det(), z)
ch_poly.as_expr()
```




$$0.133974596215561 l_{1} - 0.366025403784438 l_{2} + 1.0 z^{2} + z \left(0.133974596215561 l_{1} + 0.133974596215561 l_{2} - 1.73205080756888\right) + 1.0$$



### Desired closed-loop characteristic polynomial
Here we will aim for a pair of closed-loop poles that are critically damped, and about as fast as the plant poles. 
$$ p_1 = p_2 = \sqrt{(1-\cos(\omega_0 h))^2 + \sin(\omega_0 h)^2} = p$$
$$ A_c(z) = (z-p)^2 = z^2 - 2pz + p^2 $$
In the same spirit as when designing an RST controller using the polynomial approach, we set the calculated characteristic polynomial - obtained when introducing the linear state feedback- equal to the desired characteristic polynomial.


```python
p = sy.symbols('p')
des_ch_poly = sy.Poly((z-p)**2, z)
dioph_eqn = ch_poly - des_ch_poly
sol = sy.solve(dioph_eqn.coeffs(), (l1,l2))
sol
```




$$\left \{ l_{1} : 2.0 p^{2} - 10.9282032302755 p + 7.46410161513779, \quad l_{2} : - 2.0 p^{2} - 4.00000000000001 p + 5.46410161513777\right \}$$



### Determining $l_0$

State feedback does not change the numerator of the open-loop system, which is
$$B(z) = (1-\beta)(z+1)b$$
The characteristic polynomial of the closed-loop system is already designed to be
$$A_c(z) = (z-p)^2$$.
This means that the closed-loop system has the pulse-transfer function
$$H_c(z) = \frac{(1-\beta)(z+1)b}{(z-p)^2} l_0$$
We want this pulse-transfer function to have unit DC-gain which gives
$$ H_c(1) = \frac{(1-\beta)2b}{(1-p)^2}l_0$$
$$ l_0 = \frac{A_c(1)}{B(z)} = \frac{(1-p)^2}{2(1-\beta)b}.$$


```python
l0 = (1-p)**2/(2*(1-beta)) # We have assumed b=1 all along
```
