<SYSTEM_TASK:>
Given the following text description, write Python code to implement the functionality described below step by step
<END_TASK>
<USER_TASK:>
Description:
 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
Step1:  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}
Step2:  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.$$
Step3:  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}$$
Step4:  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}
Step5:  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
Step6:  Reachability
Step7:  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}
Step8:  The characteristic polynomial of the closed-loop system is given by
\begin{align}
\det \left( zI - (\Phi-\Gamma L) \right) \end{align}
Step9:  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.
Step10:  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}.$$
<ASSISTANT_TASK:>
Python Code:
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
Explanation: 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
End of explanation
omegaval = 0.5
hval = np.pi/6/omegaval
Phi_np = np.array(Phi.subs({h:hval, omega0:omegaval})).astype(np.float64)
Phi_np
Gamma_np = np.array(Gamma.subs({h:hval, omega0:omegaval})).astype(np.float64)
Gamma_np
Explanation: 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}
End of explanation
C_n = np.array(C).astype(np.float64)
Wo_n = np.vstack((C_n, np.dot(C_n,Phi_np)))
Wo_n
np.linalg.det(Wo_n)
Explanation: 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.$$
End of explanation
k1,k2 = sy.symbols('k1,k2')
K = sy.Matrix([[k1], [k2]])
Phi_o=Phi.subs({h:hval, omega0:omegaval}) - K*C
Phi_o
Explanation: 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}$$
End of explanation
z = sy.symbols('z')
zIminPhio = z*sy.eye(2) - Phi_o
ch_poly = sy.Poly(zIminPhio.det(), z)
ch_poly.as_expr()
Explanation: 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}
End of explanation
po = sy.symbols('p_o')
K = sy.Matrix([[Phi[0,0]-2*po],[Phi[1,0]+po**2]])
K
Phi_o = Phi - K*C
Phi_o
Phi_o.eigenvals()
Explanation: 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.
End of explanation
Wc = sy.BlockMatrix([[Gamma, Phi*Gamma]]).as_explicit()
Wc
Wc.det()
Wc.det().subs({h:hval, omega0:omegaval})
Explanation: Reachability
End of explanation
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
Explanation: 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}
End of explanation
zIminPhic = z*sy.eye(2) - Phi_c
zIminPhic
ch_poly = sy.Poly(zIminPhic.det(), z)
ch_poly.as_expr()
Explanation: The characteristic polynomial of the closed-loop system is given by
\begin{align}
\det \left( zI - (\Phi-\Gamma L) \right) \end{align}
End of explanation
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
Explanation: 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.
End of explanation
l0 = (1-p)**2/(2*(1-beta)) # We have assumed b=1 all along
Explanation: 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}.$$
End of explanation
<END_TASK>