# 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^2}\Big(bU(s) + kW(s)\big),$$
where $\omega^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}$ 

The system can be written on state-space form as
\begin{align}
\dot{x} &= \underbrace{\begin{bmatrix} 0 & -\omega^2\\1 & 0 \end{bmatrix}}_{A}x + \underbrace{\begin{bmatrix}1\\0\end{bmatrix}}_{B}bu + \begin{bmatrix}1\\0\end{bmatrix}kw\\
y &= \underbrace{\begin{bmatrix} 0 & 1 \end{bmatrix}}_{C}x
\end{align}

## Discrete-time state-space model
The discrete-time state-space model using a sampling period $h$ is
\begin{align}
x(k+1) &= \Phi(h)x(k) + \Gamma(h)u + \Gamma(h)v\\
y(k) &= Cx(k)
\end{align}
where
$$ \Phi(h) = \mathrm{e}^{Ah} = \begin{bmatrix} \cos(h\omega) & -\omega\sin(h\omega)\\\frac{1}{\omega}\sin(h\omega) & \cos(h\omega) \end{bmatrix}$$
and
$$ \Gamma(h) = \int_0^h \mathrm{e}^{As}B ds = \begin{bmatrix} \frac{1}{\omega}\sin(h\omega)\\\frac{1}{\omega^2} \big(1-\cos(h\omega)\big) \end{bmatrix}.$$
### Verification by symbolic computation


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

h,omega = sy.symbols('h,omega', real=True, positive=True)
A = sy.Matrix([[0,-omega**2], [1,0]])
B = sy.Matrix([[1],[0]])
Phi = sy.simplify(sy.exp(A*h).rewrite(sy.sin))
Phi
```




$$\left[\begin{matrix}\cos{\left (h \omega \right )} & - \omega \sin{\left (h \omega \right )}\\\frac{1}{\omega} \sin{\left (h \omega \right )} & \cos{\left (h \omega \right )}\end{matrix}\right]$$




```python
s = sy.symbols('s',real=True, positive=True)
Gamma =  sy.simplify(sy.integrate(sy.exp(A*s)*B, (s, 0, h)).rewrite(sy.cos))
Gamma
```




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



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


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




    array([[ 0.8660254, -0.25     ],
           [ 1.       ,  0.8660254]])




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




    array([[1.        ],
           [0.53589838]])




```python
4-2*np.sqrt(3)
```




$$0.5358983848622456$$



## Reachability 
The controllability matrix for this second order system becomes
$$ W_c = \begin{bmatrix} \Gamma & \Phi\Gamma \end{bmatrix} = \begin{bmatrix} 1 & 0.732\\ 0.536 & 1.464 \end{bmatrix}, $$
with determinant
$$\det W_c = 1.072 \neq 0.$$


```python
Wc_n = np.hstack((Gamma_np, np.dot(Phi_np,Gamma_np)))
Wc_n
```




    array([[1.        , 0.73205081],
           [0.53589838, 1.46410162]])




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




$$1.0717967697244903$$



## 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}
where
$$ \Gamma L = \begin{bmatrix} 1\\0.536\end{bmatrix}\begin{bmatrix}l_1 & l_2\end{bmatrix} = \begin{bmatrix} l_1 & l_2\\0.536 l_1 & 0.536 l_2 \end{bmatrix} $$
and
$$ \Phi - \Gamma L = \begin{bmatrix} 0.866 & -0.25 \\ 1 & 0.866\end{bmatrix} - \begin{bmatrix} l_1 & l_2\\0.536 l_1 & 0.536 l_2 \end{bmatrix} = \begin{bmatrix} 0.866-l_1 & -0.25-l_2\\ 1 - 0.536l_1 & 0.866 - 0.536l_2\end{bmatrix}$$


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

with characteristic polynomial given by
\begin{align}
\det \left( zI - (\Phi-\Gamma L) \right) &= \det \left( \begin{bmatrix} z & 0\\0 & z \end{bmatrix} - \begin{bmatrix} 1 & h\\0 & 1 \end{bmatrix} + \begin{bmatrix} l_1\frac{h^2}{2} & l_2\frac{h^2}{2}\\ l_1h & l_2h \end{bmatrix} \right)\\
&= \det \begin{bmatrix} z-1+l_1\frac{h^2}{2} & -h+l_2\frac{h^2}{2}\\l_1h & z-1+l_2h 
\end{bmatrix}\\
&= (z-1+l_1\frac{h^2}{2})(z-1+l_2h) - l_1h(-h + l_2\frac{h^2}{2})\\
&= z^2 + (-1+l_2h-1+l_1\frac{h^2}{2}) z + (1-l_2h - l_1\frac{h^2}{2} + l_1l_2\frac{h^3}{2} +l_1h^2 -l_1l_2\frac{h^3}{2})\\
&= z^2 + (l_1\frac{h^2}{2}+l_2h-2) z + (1 +l_1\frac{h^2}{2} -l_2h)
\end{align}
### Verification by symbolic computation 


```python
l1, l2 = sy.symbols('l1, l2', real=True)
z = sy.symbols('z')
L = sy.Matrix([[l1, l2]])
ch_poly = sy.Poly((z*sy.eye(2) - (Phi - Gamma*L)).det(), z)
ch_poly.as_expr()
```




$$\frac{h^{2} l_{1}}{2} - h l_{2} + z^{2} + z \left(\frac{h^{2} l_{1}}{2} + h l_{2} - 2\right) + 1$$



### Desired closed-loop characteristic polynomial
Here we are interested in designing a deadbeat controller, so the desired closed-loop poles are
$$ p_1 = 0, \qquad p_2=0,$$
and the desired characteristic polynomial is 
$$ A_c(z) = (z-p_1)(z-p_2) = z^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.
\begin{align}
z^1: \qquad l_1\frac{h^2}{2} + l_2h -2 &= 0\\
z^0: \qquad l_1\frac{h^2}{2} - l_2h+1 &= 0
\end{align}
which can be written as the system of equations
$$ \underbrace{\begin{bmatrix} \frac{h^2}{2} & h\\\frac{h^2}{2} & -h \end{bmatrix}}_{M} \underbrace{\begin{bmatrix} l_1\\l_2\end{bmatrix}}_{L^T} = \underbrace{\begin{bmatrix}2\\-1\end{bmatrix}}_{b} $$
with solution given by 


$$L^T = M^{-1}b = \frac{1}{-h^3} \begin{bmatrix} -h & -h\\-\frac{h^2}{2} & \frac{h^2}{2} \end{bmatrix} \begin{bmatrix} 2\\-1 \end{bmatrix}$$

$$ = -\frac{1}{h^3} \begin{bmatrix} -2h+h\\-h^2-\frac{h^2}{2}\end{bmatrix} = \begin{bmatrix} \frac{1}{h^2}\\\frac{3}{2h} \end{bmatrix} $$
### Verification by symbolic calculation


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




$$\left \{ l_{1} : \frac{1}{h^{2}}, \quad l_{2} : \frac{3}{2 h}\right \}$$



In the system of equations $ML^T=b$ above, note that the matrix $M$ can be written
$$ M = \begin{bmatrix} \frac{h^2}{2} & h\\\frac{h^2}{2} & -h \end{bmatrix} = \begin{bmatrix}1 & 0\\-2 & 1\end{bmatrix}\underbrace{\begin{bmatrix} \frac{h^2}{2} & h \\ \frac{3h^2}{2} & h\end{bmatrix}}_{W_c^T}, $$
so $M$ will be invertible if and only if $\det W_c^T = \det W_c \neq 0$.

## The resulting closed-loop system
So, we have found the control law 
$$ u(k) = -Lx(k) + l_0y_{ref}(k) = -\begin{bmatrix} \frac{1}{h^2} & \frac{3}{2h} \end{bmatrix}x(k) + l_0 y_{ref}(k)$$
which gives a closed-loop system with poles in the origin, i.e. deadbeat control. The closed-loop system becomes
\begin{align*}
 x(k+1) &= \big( \Phi - \Gamma L \big) x(k) + \Gamma l_0 y_{ref}(k) + \Gamma v(k)\\
 &= \left( \begin{bmatrix} 1 & h\\0 & 1\end{bmatrix} - \begin{bmatrix} \frac{h^2}{2}\\h\end{bmatrix}\begin{bmatrix} \frac{1}{h^2} & \frac{3}{2h} \end{bmatrix}  \right) x(k) + \Gamma l_0 y_{ref}(k) + \Gamma v(k)\\
 &= \left( \begin{bmatrix} 1 & h\\0 & 1\end{bmatrix} - \begin{bmatrix} \frac{1}{2} & \frac{3h}{4}\\ \frac{1}{h} & \frac{3}{2}\end{bmatrix}\right) x(k) + \Gamma l_0 y_{ref}(k) + \Gamma v(k)\\
 &= \underbrace{\begin{bmatrix} \frac{1}{2} & \frac{h}{4} \\-\frac{1}{h} & -\frac{1}{2}\end{bmatrix}}_{\Phi_c}x(k) + \begin{bmatrix}\frac{h^2}{2}\\h\end{bmatrix} l_0 y_{ref}(k) + \begin{bmatrix}\frac{h^2}{2}\\h\end{bmatrix}  v(k)\\
 y(k) &= \begin{bmatrix} 1 & 0 \end{bmatrix} x(k)
 \end{align*}
 ### Verification using symbolic computations


```python
L = sy.Matrix([[sol[l1], sol[l2]]]) 
Phic = Phi - Gamma*L
Phic
```




$$\left[\begin{matrix}\frac{1}{2} & \frac{h}{4}\\- \frac{1}{h} & - \frac{1}{2}\end{matrix}\right]$$



## Determining the reference signal gain $l_0$
Consider the steady-state solution for a unit step in the reference signal. We set $y_{ref}=1$ and $v = 0$. This gives
$$ x(k+1) = \Phi_c x(k) + \Gamma l_0. $$
In steady-state there is no change in the state, so $x(k+1)=x(k)=x_{ss}$, which leads to
$$ x_{ss} = \Phi_c x_{ss} + \Gamma l_0$$
$$ (I - \Phi_c)x_{ss} = \Gamma l_0$$
\begin{align}
x_{ss} &= (I - \Phi_c)^{-1}\Gamma l_0\\
&= \begin{bmatrix} \frac{1}{2} &-\frac{h}{4}\\ \frac{1}{h} & \frac{3}{2} \end{bmatrix}^{-1} \begin{bmatrix} \frac{h^2}{2}\\h \end{bmatrix} l_0\\
&= \begin{bmatrix}\frac{3}{2} & \frac{h}{4}\\-\frac{1}{h} & \frac{1}{2} \end{bmatrix} \begin{bmatrix} \frac{h^2}{2}\\h\end{bmatrix} l_0\\
&= \begin{bmatrix}\frac{3h^2}{4} + \frac{h^2}{4}\\-\frac{h}{2} + \frac{h}{2} \end{bmatrix}l_0= \begin{bmatrix}h^2\\ 0 \end{bmatrix}l_0\\
\end{align}
which means that the steady-state velocity $\dot{z}(\infty) = x_2(\infty) = 0$. This makes sense. 

We can now determine $l_0$. Since  $y(k)=x_1(k)$ then $y_{ss} = h^2 l_0$ for a unit step in the reference signal. We would like the steady-state value $y_{ss}$ to be the same as the reference signal (which is equal to one, of course) so this gives
$$ h^2l_0 = 1 \quad \Rightarrow \quad l_0 = \frac{1}{h^2}. $$

##  Simulate step responses (symbolically)
### Step response from the reference


```python
l0 = 1/(h*h)
C = sy.Matrix([[1,0]])
x = sy.Matrix([[0],[0]]) # Initial state
yref = sy.Matrix([[1]])
xs = [x] # List to hold state trajectory
us = [[0]] # and control signal 
ys = [[0]] # and system output 
for k in range(6): # No need to simulate too long. It is deadbeat control after all
    us.append(-L*x + l0*yref)
    x = Phic*x + Gamma*l0*yref
    xs.append(x)
    ys.append(C*x)
xs
```




$$\left [ \left[\begin{matrix}0\\0\end{matrix}\right], \quad \left[\begin{matrix}\frac{1}{2}\\\frac{1}{h}\end{matrix}\right], \quad \left[\begin{matrix}1\\0\end{matrix}\right], \quad \left[\begin{matrix}1\\0\end{matrix}\right], \quad \left[\begin{matrix}1\\0\end{matrix}\right], \quad \left[\begin{matrix}1\\0\end{matrix}\right], \quad \left[\begin{matrix}1\\0\end{matrix}\right]\right ]$$




```python
us
```




$$\left [ \left [ 0\right ], \quad \left[\begin{matrix}\frac{1}{h^{2}}\end{matrix}\right], \quad \left[\begin{matrix}- \frac{1}{h^{2}}\end{matrix}\right], \quad \left[\begin{matrix}0\end{matrix}\right], \quad \left[\begin{matrix}0\end{matrix}\right], \quad \left[\begin{matrix}0\end{matrix}\right], \quad \left[\begin{matrix}0\end{matrix}\right]\right ]$$



### Step response from the disturbance 


```python
x = sy.Matrix([[0],[0]]) # Initial state
yref = sy.Matrix([[0]])
v = sy.Matrix([[1]])
xs = [x] # List to hold state trajectory
us = [[0]] # and control signal 
ys = [[0]] # and system output 
for k in range(6): # No need to simulate too long. It is deadbeat control after all
    us.append(-L*x + l0*yref)
    x = Phic*x + Gamma*l0*yref + Gamma*v
    xs.append(x)
    ys.append(C*x)
xs
```




$$\left [ \left[\begin{matrix}0\\0\end{matrix}\right], \quad \left[\begin{matrix}\frac{h^{2}}{2}\\h\end{matrix}\right], \quad \left[\begin{matrix}h^{2}\\0\end{matrix}\right], \quad \left[\begin{matrix}h^{2}\\0\end{matrix}\right], \quad \left[\begin{matrix}h^{2}\\0\end{matrix}\right], \quad \left[\begin{matrix}h^{2}\\0\end{matrix}\right], \quad \left[\begin{matrix}h^{2}\\0\end{matrix}\right]\right ]$$



## Simulate step-responses (numerically)


```python
import control as ctrl
import matplotlib.pyplot as plt
# Convert to from sympy matrices to numpy
hval = .1
Phi_np = np.array(Phi.subs({h:hval})).astype(np.float64)
Gamma_np = np.array(Gamma.subs({h:hval})).astype(np.float64)
L_np = np.array(L.subs({h:hval})).astype(np.float64)
l0_np = np.array(l0.subs({h:hval})).astype(np.float64)
Phic_np = Phi_np - Gamma_np*L_np
C_np = np.array(C).astype(np.float64)
D_np = np.array([[0]])
sys_c = ctrl.ss(Phic_np, Gamma_np*l0_np, C_np, D_np, hval) # From ref signal
sys_cv = ctrl.ss(Phic_np, Gamma_np, C_np, D_np, hval) # From disturbance signal

```


```python
tvec = np.asarray(np.arange(8))*hval
T, yout = ctrl.step_response(sys_c, tvec)
T, yout_v = ctrl.step_response(sys_cv, tvec)
```


```python
plt.figure(figsize=(14,3))
plt.step(tvec, yout.flatten())
```


```python
plt.figure(figsize=(14,3))
plt.step(tvec, yout_v.flatten())
```

# Exercises

## Design a less agressive controller
Consider to let the closed-loop poles be less fast. Choose something reasonable, for instance a double pole in $z=0.5$, or a pair of complex-conjugated poles in $z=0.6 \pm i0.3$. Redo the design, following the example above. Find the state feedback and simulate step-responses.

## Design a deadbeat controller for the DC-motor
From the textbook (Åström & Wittenmark) Appendix:

1. Use symbolic calculations to find the discrete-time state-space model for arbitrary sampling period $h$. 
2. Design a deadbeat controller for arbitrary sampling period.
3. Assume a disturbance is acting on the input to the system, as an unknown torque on the motor shaft. This means that the disturbance enters into the system in the same way as the disturbance on the mass on frictionless surface analyzed above. Simulate step-responses for the closed-loop system.


```python

```

b6dd48532b38571c5ca4993bcd00abecb9e01fce
ipynb
Jupyter Notebook
state-space/.ipynb_checkpoints/Under construction - Output feedback controller for the harmonic oscillator-checkpoint.ipynb
kjartan-at-tec/mr2007-computerized-control
16e35f5007f53870eaf344eea1165507505ab4aa
MIT
2020-11-07T05:20:37.000Z
2020-12-22T09:46:13.000Z
state-space/.ipynb_checkpoints/Under construction - Output feedback controller for the harmonic oscillator-checkpoint.ipynb
kjartan-at-tec/mr2007-computerized-control
16e35f5007f53870eaf344eea1165507505ab4aa
MIT
2020-06-12T20:44:41.000Z
2020-06-12T20:49:00.000Z
state-space/.ipynb_checkpoints/Under construction - Output feedback controller for the harmonic oscillator-checkpoint.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