# Realization of Recursive Filters

*This jupyter notebook is part of a [collection of notebooks](../index.ipynb) on various topics of Digital Signal Processing. Please direct questions and suggestions to [Sascha.Spors@uni-rostock.de](mailto:Sascha.Spors@uni-rostock.de).*

## Quantization of Variables and Operations

As for [non-recursive filters](../nonrecursive_filters/quantization_effects.ipynb#Quantization-Effects), the practical realization of recursive filters may suffer from the quantization of variables and algebraic operations. The effects of [coefficient quantization](quantization_of_coefficients.ipynb) were already discussed. This section takes a look at the quantization of variables.  We limit the investigations to the recursive part of a second-order section (SOS), since any recursive filter of order $N \geq 2$ can be [decomposed into SOSs](cascaded_structures.ipynb).

The computation of the output signal $y[k] = \mathcal{H}\{ x[k] \}$ by a difference equation involves a number of multiplications and additions. As discussed already for [non-recursive filters](../nonrecursive_filters/quantization_effects.ipynb#Quantization-of-Signals-and-Operations), multiplying two numbers in a binary representation (e.g. [two's complement](https://en.wikipedia.org/wiki/Two's_complement) or [floating point](https://en.wikipedia.org/wiki/Floating_point)) requires requantization of the result to keep the word length constant. The addition of two numbers may fall outside the maximum/minimum values of the representation and may suffer from clipping.

The resulting round-off and clipping errors depend on the number and sequence of algebraic operations. These depend on the structure used for implementation of the SOSs. For ease of illustration we limit our discussion to the [direct form I and II](direct_forms.ipynb). Similar insights can be achieved in a similar manner for other structures.

### Analysis of Round-Off Errors

Round-off errors are a consequence of reducing the word length after a multiplication. In order to investigate the influence of these errors on a recursive filter, the statistical model for [round-off errors in multipliers](../nonrecursive_filters/quantization_effects.ipynb#Model-for-round-off-errors-in-multipliers) as introduced for non-recursive filters is used. We furthermore neglect clipping.

The difference equation for the recursive part of a SOS realized in direct form I or II is given as

\begin{equation}
y[k] =  x[k] - a_1 \, y[k-1] - a_2 \, y[k-2]
\end{equation}

where $a_0 = 1$, $a_1$ and $a_2$ denote the coefficients of the recursive part. Introducing the requantization after the multipliers into the difference equation yields the output signal $y_Q[k]$

\begin{equation}
y_Q[k] = x[k] - \mathcal{Q} \{ a_{1} \, y[k-1] \} - \mathcal{Q} \{ a_{2} \, y[k-2] \}
\end{equation}

where $\mathcal{Q} \{ \cdot \}$ denotes the requantizer. Requantization is a non-linear process which results in a requantization error. If the value to be requantized is much larger that the quantization step $Q$, the average statistical properties of this error can be modeled as additive uncorrelated white noise. Introducing the error into above difference equation gives

\begin{equation}
y_Q[k] = x[k] - a_1 \, y[k-1] - e_1[k] - a_2 \, y[k-2] - e_2[k]
\end{equation}

where the two white noise sources $e_1[k]$ and $e_2[k]$ are assumed to be uncorrelated to each other. This difference equation can be split into a set of two difference equations

\begin{align}
y_Q[k] &= y[k] + e[k] \\
y[k] &=  x[k] - a_1 \, y[k-1] - a_2 \, y[k-2] \\
e[k] &= - e_1[k] - e_2[k] - a_1 \, e[k-1] - a_2 \, e[k-2]
\end{align}

The first difference equation computes the desired output signal $y[k]$ as a result of the input signal $x[k]$. The second one the additive error $e[k]$ due to requantization as a result of the requantization error $- (e_1[k] + e_2[k])$ injected into the recursive filter.
The power spectral density (PSD) $\Phi_{ee}(\mathrm{e}^{\,\mathrm{j}\,\Omega})$ of the error $e[k]$ is then given as

\begin{equation}
\Phi_{ee}(\mathrm{e}^{\,\mathrm{j}\,\Omega}) = | H(\mathrm{e}^{\,\mathrm{j}\,\Omega})|^2 \cdot (\Phi_{e_1 e_1}(\mathrm{e}^{\,\mathrm{j}\,\Omega}) + \Phi_{e_2 e_2}(\mathrm{e}^{\,\mathrm{j}\,\Omega}))
\end{equation}

According to the model for the requantization errors, their PSDs are given as $\Phi_{e_1 e_1}(\mathrm{e}^{\,\mathrm{j}\,\Omega}) = \Phi_{e_2 e_2}(\mathrm{e}^{\,\mathrm{j}\,\Omega}) = \frac{Q^2}{12}$. Introducing this together with the transfer function of the SOS yields

\begin{equation}
\Phi_{ee}(\mathrm{e}^{\,\mathrm{j}\,\Omega}) = \left| \frac{1}{1 + a_1 \, \mathrm{e}^{\,-\mathrm{j}\,\Omega} + a_2 \, \mathrm{e}^{\,-\mathrm{j}\,2\,\Omega}} \right|^2 \cdot \frac{Q^2}{6}
\end{equation}

#### Example - Round-off error of a SOS

The following example evaluates the error $e[k] = y_Q[k] - y[k]$ for a SOS which only consists of a recursive part. The desired system response $y[k]$ is computed numerically by floating point operations with double precision, $y_Q[k]$ is computed by applying a uniform midtread quantizer after the multiplications. The system is excited by uniformly distributed white noise. Besides the PSD $\Phi_{ee}(\mathrm{e}^{\,\mathrm{j}\,\Omega})$, the signal-to-noise ratio (SNR) $10 \cdot \log_{10} \left( \frac{\sigma_y^2}{\sigma_e^2} \right)$ in dB of the filter is evaluated.

```python
%matplotlib inline
import numpy as np
import matplotlib.pyplot as plt
import scipy.signal as sig

N = 8192  # length of signals
w = 8  # wordlength for requantization of multiplications

def uniform_midtread_quantizer(x):
    # linear uniform quantization
    xQ = Q * np.floor(x/Q + 1/2)

    return xQ

def no_quantizer(x):

    return x

def sos_df1(x, a, requantize=None):
    y = np.zeros(len(x)+2)  # initial value appended
    for k in range(len(x)):
        y[k] = x[k] - requantize(a[1]*y[k-1])  - requantize(a[2]*y[k-2])

    return y[0:-2]

# cofficients of the SOS
p = 0.90*np.array([np.exp(1j*np.pi/3), np.exp(-1j*np.pi/3)])
a = np.poly(p)
# quantization step
Q = 1/(2**(w-1))

# compute input signal
x = np.random.uniform(low=-1, high=1, size=N)
# compute output signals w and w/o requantization
yQ = sos_df1(x, a, requantize=uniform_midtread_quantizer)
y = sos_df1(x, a, requantize=no_quantizer)
# compute requantization error
e = yQ-y
# Signal-to-noise ratio
SNR = 10*np.log10(np.var(y)/np.var(e))
print('SNR due to requantization: %f dB'%SNR)

# estimate PSD of requantization error
nf, Pxx = sig.welch(e, window='hamming', nperseg=256, noverlap=128)
Pxx = .5*Pxx  # due to normalization in scipy.signal
Om = 2*np.pi*nf
# compute frequency response of system
w, H = sig.freqz([1,0,0], a)

# plot results
plt.figure(figsize=(10,4))
plt.plot(Om, Pxx/Q**2 * 12, 'b', label=r'$|\hat{\Phi}_{ee}(e^{j \Omega})|$')
plt.plot(w, np.abs(H)**2 * 2, 'g', label=r'$|H(e^{j \Omega})|^2$')
plt.title('Estimated PSD and transfer function of requantization noise')
plt.xlabel(r'$\Omega$')
plt.ylabel(r'$Q^2/12$')
plt.axis([0, np.pi, 0, 100])
plt.legend()
plt.grid();
```

### Small Limit Cycles

Besides the requantization noise, recursive filters may be subject to periodic oscillations present at the output. These undesired oscillations are termed *limit cycles*. Small limit cycles emerge from the additive round-off noise due to requantization after a multiplication. The feedback in a recursive filter leads to a feedback of the requantization noise. This may lead to a periodic output signal with an amplitude range of some quantization steps $Q$, even after the input signal is zero. The presence, amplitude and frequency of small limit cycles depends on the location of poles and the structure of the filter. A detailed treatment of this phenomenon is beyond the scope of this notebook and can be found in the literature.

#### Example - Small limit cycles of a SOS

The following example illustrates small limit cycles for the system investigated in the previous example. The input signal is uniformly distributed white noise till time-index $k=256$ and zero for the remainder.

```python
# compute input signal
x = np.random.uniform(low=-1, high=1, size=256)
x = np.concatenate((x, np.zeros(1024)))
# compute output signal
yQ = sos_df1(x, a, requantize=uniform_midtread_quantizer)

# plot results
np.seterr(divide='ignore')
plt.figure(figsize=(10, 3))
plt.plot(20*np.log10(np.abs(yQ)))
plt.title('Level of output signal')
plt.xlabel(r'$k$')
plt.ylabel(r'$|y_Q[k]|$ in dB')
plt.grid()

plt.figure(figsize=(10, 3))
k = np.arange(1000, 1050)
plt.stem(k, yQ[k]/Q)
plt.title('Output signal for zero input')
plt.xlabel(r'$k$')
plt.ylabel(r'$y_Q[k] / Q$ ')
plt.axis([k[0], k[-1], -3, 3])
plt.grid();
```

**Exercise**

* Estimate the period of the small limit cycles. How is it related to the poles of the system?
* What amplitude range is spanned?

Solution: The period of the small limit cycles can be estimated from the second illustration as $P = 6$. The normalized frequency of a harmonic exponential signal with the same periodicity is given as $\Omega_0 = \frac{2 \pi}{P} = \frac{\pi}{3}$. The poles of the system can be extracted from the code of the first example as $z_{\infty 0,1} = 0.9 \cdot e^{\pm j \frac{\pi}{3}}$. The periodicity of the small limit cycles is hence linked to the normalized frequency of the poles. The amplitude range spanned by the small limit cycles is $\pm 2 Q$.

### Large Limit Cycles

Large limit cycles are periodic oscillations of a recursive filter due to overflows in the multiplications/additions. As for small limit cycles, large limit cycles may be present even after the input signal is zero. Their level is typically in the range of the minimum/maximum value of the requantizer. Large limit cycles should therefore be avoided in a practical implementation. The presence of large limit cycles depends on the scaling of input signal and coefficients, as well as the strategy used to cope for clipping. Amongst others, they can be avoided by proper scaling of the coefficients to prevent overflow. Again, a detailed treatment of this phenomenon is beyond the scope of this notebook and can be found in the literature.

#### Example - Large limit cycles of a SOS

The following example illustrates large limit cycles for the system investigated in the first example. In order to trigger large limit cycles, the coefficients of the filter have been doubled. The input signal is uniformly distributed white noise till time-index $k=256$ and zero for the remainder.

```python
def uniform_midtread_quantizer(x, xmin=1):
    # limiter
    x = np.copy(x)
    if x <= -xmin:
        x = -1
    if x > xmin - Q:
        x = 1 - Q
    # linear uniform quantization
    xQ = Q * np.floor(x/Q + 1/2)

    return xQ

# compute input signal
x = np.random.uniform(low=-1, high=1, size=256)
x = np.concatenate((x, np.zeros(1024)))
# compute output signal
yQ = sos_df1(x, 2*a, requantize=uniform_midtread_quantizer)

# plot results
plt.figure(figsize=(10, 3))
plt.plot(20*np.log10(np.abs(yQ)))
plt.title('Level of output signal')
plt.xlabel(r'$k$')
plt.ylabel(r'$|y_Q[k]|$ in dB')
plt.grid()

plt.figure(figsize=(10, 3))
k = np.arange(1000, 1050)
plt.stem(k, yQ[k])
plt.title('Output signal for zero input')
plt.xlabel(r'$k$')
plt.ylabel(r'$y_Q[k]$ ')
plt.grid();
```

**Exercise**

* Determine the period of the large limit cycles. How is it related to the poles of the system?

Solution: The period of the large limit cycles can be estimated from the second illustration as $P = 6$. The normalized frequency of a harmonic exponential signal with the same periodicity is given as $\Omega_0 = \frac{2 \pi}{P} = \frac{\pi}{3}$. The poles of the system can be extracted from the code of the first example as $z_{\infty 0,1} = 0.9 \cdot e^{\pm j \frac{\pi}{3}}$. The periodicity of the large limit cycles is hence linked to the normalized frequency of the poles.

**Copyright**

This notebook is provided as [Open Educational Resource](https://en.wikipedia.org/wiki/Open_educational_resources). Feel free to use the notebook for your own purposes. The text is licensed under [Creative Commons Attribution 4.0](https://creativecommons.org/licenses/by/4.0/), the code of the IPython examples under the [MIT license](https://opensource.org/licenses/MIT). Please attribute the work as follows: *Sascha Spors, Digital Signal Processing - Lecture notes featuring computational examples, 2016-2018*.


```python
%matplotlib inline

import numpy as np
import pylab as pl
import math
from sympy import *
import matplotlib.pyplot as plt
import matplotlib.animation as animation
from mpl_toolkits.mplot3d import Axes3D
```

```python
import numpy as np
import pylab as pl
import math
from sympy import *

class RNN:
    def __init__(self):
        pass

    def set_input_layer(self, x):
        self.input_layer = np.zeros(x)
        return self.input_layer

    def set_output_layer(self, x):
        self.output_layer = np.zeros(x)
        self.before_output_layer = np.zeros(x)
        self.supervised_data = np.zeros(x)
        return self.output_layer, self.before_output_layer, self.supervised_data

    def set_hidden_layer(self, x):
        self.hidden_layer = np.zeros(x)
        self.before_hidden_layer = np.zeros(x)
        return self.hidden_layer, self.before_hidden_layer

    def setup(self):
        w_k = np.zeros(len(self.output_layer))
        self.w_kj = np.array([w_k for i in range(len(self.hidden_layer))])
        w_j = np.zeros(len(self.hidden_layer))
        self.w_ji = np.array([w_j for i in range(len(self.input_layer))])
        return self.w_kj, self.w_ji

    def initialize(self, hidden=None):
        for i in range(len(self.hidden_layer)):
            for j in range(len(self.output_layer)):
                self.w_kj[i][j] = np.random.uniform(-1.0/math.sqrt(1.0/len(self.hidden_layer)), 1.0/math.sqrt(1.0/len(self.hidden_layer)))

        for i in range(len(self.input_layer)):
            for j in range(len(self.hidden_layer)):
                self.w_ji[i][j] = np.random.uniform(-1.0/math.sqrt(1.0/len(self.input_layer)), 1.0/math.sqrt(1.0/len(self.input_layer)))

        if hidden is None:
            u = Symbol('u')
            self.hfunction = 1/(1+exp(-u))
            self.diff_hf = diff(self.hfunction)
        else:
            self.hfunction = hidden
            self.diff_hf = diff(self.hfunction)

    def supervised_function(self, f, sdata):
        for i in range(len(self.supervised_data)):
            self.supervised_data[i] = sdata[i]

    def set_hidden_error(self, j):
        u = Symbol("u")
        diff_hf = self.diff_hf
        hidden_error = 0
        for k in range(len(self.output_layer)):
            delta_z = diff_hf.subs([(u, self.before_output_layer[k])])
            hidden_error += self.w_kj[j][k]*(self.supervised_data[k] - self.output_layer[k])*delta_z
        return hidden_error

    def calculation(self):
        u = Symbol("u")
        hfunction = self.hfunction
        diff_hf = self.diff_hf

        for i in range(len(self.input_layer)):
            self.before_hidden_layer = np.matrix(self.w_ji).T*np.matrix(self.input_layer).T

        for i in range(len(self.hidden_layer)):
            self.hidden_layer[i] = hfunction.subs([(u, self.before_hidden_layer[i])])

        for i in range(len(self.before_output_layer)):
            self.before_output_layer = np.matrix(self.w_kj).T*np.matrix(self.hidden_layer).T

        for i in range(len(self.output_layer)):
            self.output_layer[i] = hfunction.subs([(u, self.before_output_layer[i])])

    def output_ad(self):
        u = Symbol("u")
        hfunction = self.hfunction
        diff_hf = self.diff_hf

        eta = self.eta
        for j in range(len(self.hidden_layer)):
            for k in range(len(self.output_layer)):
                delta_J = self.supervised_data[k] - self.output_layer[k]
                delta_z = self.output_layer[k]*(1-self.output_layer[k])
                delta_v = self.hidden_layer[j]
                self.w_kj[j][k] += eta*delta_J*delta_z*delta_v

    def input_ad(self):
        u = Symbol("u")
        hfunction = self.hfunction
        diff_hf = self.diff_hf

        eta = self.eta
        for i in range(len(self.input_layer)):
            for j in range(len(self.hidden_layer)):
                hidden_error = self.set_hidden_error(j)
                delta_y = self.hidden_layer[j]*(1-self.hidden_layer[j])
                delta_u = self.input_layer[i]
                self.w_ji[i][j] += eta*hidden_error*delta_y*delta_u

    def simulate(self, N=1, eta):
        self.eta = eta
        for i in range(N):
            self.calculation()
            self.output_ad()
            self.calculation()
            self.input_ad()
        return self.output_layer

    def main(self, f, idata, eta, N=1, i=2, h=2, o=1):
        self.set_input_layer(i)
        self.set_hidden_layer(h)
        self.set_output_layer(o)
        self.setup()
        self.initialize()
        self.supervised_function(f, idata)
        self.simulate(N, eta)
        return self.output_layer[0]

    def set_network(self, i=2, h=2, o=1):
        self.set_input_layer(i)
        self.set_hidden_layer(h)
        self.set_output_layer(o)
        self.setup()
        self.initialize()

    def main2(self, N, f, idata, eta):
        self.supervised_function(f, idata)
        self.simulate(N, eta)
        return self.output_layer[0]

    def realize(self, f, idata):
        self.supervised_function(f, idata)
        self.calculation()
        return self.output_layer[0]
```

```python

```


```python
import numpy as np
import scipy as sc
import random as rand
from sklearn import preprocessing, linear_model
import matplotlib.pyplot as plt

from core.controllers import PDController
from core.dynamics import LinearSystemDynamics, ConfigurationDynamics

from koopman_core.controllers import OpenLoopController, MPCController,BilinearFBLinController, PerturbedController, LinearLiftedController
from koopman_core.dynamics import LinearLiftedDynamics, BilinearLiftedDynamics
from koopman_core.learning import Edmd, FlBilinearLearner
from koopman_core.basis_functions import PolySineBasis
from koopman_core.learning.utils import differentiate_vec
from koopman_core.systems import PlanarQuadrotorForceInput

class QuadrotorPdOutput(ConfigurationDynamics):
    def __init__(self, dynamics, xd, t_d, n, m):
        ConfigurationDynamics.__init__(self, dynamics, 1)
        self.xd = xd
        self.t_d = t_d
        self.xd_dot = differentiate_vec(self.xd, self.t_d)
        self.n = n
        self.m = m

    def proportional(self, x, t):
        q, q_dot = x[:int(n/2)], x[int(n/2):]
        return self.y(q) - self.y_d(t)

    def derivative(self, x, t):
        q, q_dot = x[:int(n/2)], x[int(n/2):]
        return self.dydq(q)@q_dot - self.y_d_dot(t)

    def y(self, q):
        return q

    def dydq(self, q):
        return np.eye(int(self.n/2))

    def d2ydq2(self, q):
        return np.zeros((int(self.n/2), int(self.n/2), int(self.n/2)))

    def y_d(self, t):
        return self.desired_state_(t)[:int(self.n/2)]

    def y_d_dot(self, t):
        return self.desired_state_(t)[int(self.n/2):]

    def y_d_ddot(self, t):
        return self.desired_state_dot_(t)[int(self.n/2):]

    def desired_state_(self, t):
        return [np.interp(t, self.t_d.flatten(),self.xd[:,ii].flatten()) for ii in range(self.xd.shape[1])]

    def desired_state_dot_(self, t):
        return [np.interp(t, self.t_d.flatten(),self.xd_dot[:,ii].flatten()) for ii in range(self.xd_dot.shape[1])]

class QuadrotorTrajectoryOutput(ConfigurationDynamics):
    def __init__(self, bilinear_dynamics, y_d, y_d_dot, y_d_ddot, dt, z_d=None, z_d_dot=None, z_d_ddot=None, C_h=None):
        ConfigurationDynamics.__init__(self, bilinear_dynamics, 2)
        self.bilinear_dynamics = bilinear_dynamics
        self.ref = y_d
        self.ref_dot = y_d_dot
        self.ref_ddot = y_d_ddot

        self.ref_z = z_d
        self.ref_dot_z = z_d_dot
        self.ref_ddot_z = z_d_ddot

        self.C_h = C_h
        self.dt = dt
        self.t_d = self.dt * np.arange(0, self.ref.shape[1])

    def eval_z(self, x, t):
        z = self.bilinear_dynamics.phi_fun(x.reshape(1,-1)).squeeze()
        return z - self.z_d(t)

    def y(self, q):
        return q

    def dydq(self, q):
        return np.array([[1, 0], [0, 1]])

    def d2ydq2(self, q):
        return np.zeros((1, 2, 2))

    def y_d(self, t):
        return self.interpolate_ref_(self.ref, t)

    def y_d_dot(self, t):
        return self.interpolate_ref_(self.ref_dot, t)

    def y_d_ddot(self, t):
        return self.interpolate_ref_(self.ref_ddot, t)

    def z_d(self, t):
        return self.interpolate_ref_(self.ref_z, t)

    def z_d_dot(self, t):
        return self.interpolate_ref_(self.ref_dot_z, t)

    def z_d_ddot(self, t):
        return self.interpolate_ref_(self.ref_ddot_z, t)

    def interpolate_ref_(self, ref, t):
        return np.array([np.interp(t, self.t_d, ref[ii, :]) for ii in range(ref.shape[0])])
```

## Planar Quadrotor Example

Consider a planar quadrotor with states $\mathbf{x} = [y \, z \, \theta \, \dot{y} \, \dot{z} \, \dot{\theta}]^T$ and continuous-time dynamics

\begin{equation}
    \begin{bmatrix} \ddot{y} \\ \ddot{z} \\ \ddot{\theta} \end{bmatrix}
    = \begin{bmatrix}
    0\\-g\\0
    \end{bmatrix} +
    \begin{bmatrix}
    -\frac{1}{m}\text{sin}\theta & -\frac{1}{m}\text{sin}\theta\\
    \frac{1}{m}\text{cos}\theta & \frac{1}{m}\text{cos}\theta\\
    -\frac{l_{arm}}{I_{xx}} & \frac{l_{arm}}{I_{xx}}
    \end{bmatrix}
    \begin{bmatrix}
    T_1 \\ T_2
    \end{bmatrix}
\end{equation}

where $y,z$ describe the position of the vehicle in a fixed reference frame, $\theta$ is the orientation of the vehicle,
$T_1, T_2$ are the thrust from each of the propellers, $g$ is the gravitational acceleration, $m$ is the vehicle mass,
$l_{arm}$ is the distance from the vehicle's center of mass to the center of the propeller, and $I_{xx}$ is the inertia
around the x-axis.

```python
# Cart pole system parameters
mass = 2.
inertia = 1.
prop_arm = 0.2
gravity = 9.81
quadrotor = PlanarQuadrotorForceInput(mass, inertia, prop_arm, g=gravity)

# Linearized system specification:
n, m = 6, 2                                                         # Number of states, number of control inputs
A_nom = np.array([[0., 0., 0., 1., 0., 0.],                         # Linearization of the true system around the origin
                  [0., 0., 0., 0., 1., 0.],
                  [0., 0., 0., 0., 0., 1.],
                  [0., 0., -gravity, 0., 0., 0.],
                  [0., 0., 0., 0., 0., 0.],
                  [0., 0., 0., 0., 0., 0.]])
B_nom = np.array([[0., 0.],                                         # Linearization of the true system around the origin
                  [0., 0.],
                  [0., 0.],
                  [0., 0.],
                  [1./mass, 1./mass],
                  [-prop_arm/inertia, prop_arm/inertia]])

hover_thrust = mass*gravity/m
```

### Collect data for learning

To collect data, a nominal controller is designed with LQR on the dynamics's linearization around hover. However, any
controller can be used and the method does not require the knowledge of model's linearization. In addition, a
exploratory white noise is added to the controller to ensure that the data is sufficiently excited. Note that the system
is underactuated and that trajectory optimization is necessary to control the position of the vehicle. We use a
simplified trajectory generator based on a model predictive controller for the linearized dynamics. More careful design
of the desired trajectory may be necessary for more demanding applications and this is readily compatible with our method.

```python
q_dc, r_dc = 1e2, 1                                                 # State and actuation penalty values, data collection
Q_dc = q_dc * np.identity(n)                                        # State penalty matrix, data collection
R_dc = r_dc*np.identity(m)                                          # Actuation penalty matrix, data collection
P_dc = sc.linalg.solve_continuous_are(A_nom, B_nom, Q_dc, R_dc)     # Algebraic Ricatti equation solution, data collection
K_dc = np.linalg.inv(R_dc)@B_nom.T@P_dc                             # LQR feedback gain matrix, data collection
K_dc_p = K_dc[:,:int(n/2)]                                          # Proportional control gains, data collection
K_dc_d = K_dc[:,int(n/2):]                                          # Derivative control gains, data collection
nominal_sys = LinearSystemDynamics(A=A_nom, B=B_nom)

# Data collection parameters:
dt = 1.0e-2                                                         # Time step length
traj_length_dc = 2.                                                 # Trajectory length, data collection
n_pred_dc = int(traj_length_dc/dt)                                  # Number of time steps, data collection
t_eval = dt * np.arange(n_pred_dc + 1)                              # Simulation time points
n_traj_dc = 50                                                      # Number of trajectories to execute, data collection
noise_var = 1.                                                      # Exploration noise to perturb controller, data collection

xmax = np.array([2, 2, np.pi/3, 2.,2.,2.])                          # State constraints, trajectory generation
xmin = -xmax
umax = np.array([50., 50.]) - hover_thrust                          # Actuation constraint, trajectory generation
umin = np.array([0., 0.]) - hover_thrust
x0_max = np.array([xmax[0], xmax[1], xmax[2], 1., 1., 1.])          # Initial value limits
Q_trajgen = sc.sparse.diags([0,0,0,0,0,0])                          # State penalty matrix, trajectory generation
QN_trajgen = sc.sparse.diags([5e1,5e1,5e1,1e1,1e1,1e1])             # Final state penalty matrix, trajectory generation
R_trajgen = sc.sparse.eye(m)                                        # Actuation penalty matrix, trajectory generation
sub_sample_rate = 5                                                 # Rate to subsample data for training
model_fname = 'examples/planar_quad_models'                         # Path to save learned models
n_cols = 10                                                         # Number of columns in training data plot
```

```python
xd = np.empty((n_traj_dc, n_pred_dc + 1, n))
xs = np.empty((n_traj_dc, n_pred_dc + 1, n))
us = np.empty((n_traj_dc, n_pred_dc, m))

plt.figure(figsize=(12, 12 * n_traj_dc / (n_cols ** 2)))
for ii in range(n_traj_dc):
    x0 = np.asarray([rand.uniform(l, u) for l, u in zip(-x0_max, x0_max)])
    set_pt_dc = np.asarray([rand.uniform(l, u) for l, u in zip(-x0_max, x0_max)])
    mpc_trajgen = MPCController(nominal_sys, n_pred_dc, dt, umin, umax, xmin, xmax, QN_trajgen, R_trajgen,
                                QN_trajgen, set_pt_dc)
    mpc_trajgen.eval(x0, 0)
    xd[ii, :, :] = mpc_trajgen.parse_result().T
    while abs(x0[0]) < 1.25 or np.any(np.isnan(xd[ii, :, :])):
        x0 = np.asarray([rand.uniform(l, u) for l, u in zip(-x0_max, x0_max)])
        set_pt_dc = np.asarray([rand.uniform(l, u) for l, u in zip(-x0_max, x0_max)])
        mpc_trajgen = MPCController(nominal_sys, n_pred_dc, dt, umin, umax, xmin, xmax, QN_trajgen, R_trajgen,
                                    QN_trajgen, set_pt_dc)
        mpc_trajgen.eval(x0, 0)
        xd[ii, :, :] = mpc_trajgen.parse_result().T

    output = QuadrotorPdOutput(quadrotor, xd[ii, :, :], t_eval, n, m)
    pd_controller = PDController(output, K_dc_p, K_dc_d)
    perturbed_pd_controller = PerturbedController(quadrotor, pd_controller, noise_var, const_offset=hover_thrust)
    xs[ii, :, :], us[ii, :, :] = quadrotor.simulate(x0, perturbed_pd_controller, t_eval)

    plt.subplot(int(np.ceil(n_traj_dc / n_cols)), n_cols, ii + 1)
    plt.plot(t_eval, xs[ii, :, 0], 'b', label='$y$')
    plt.plot(t_eval, xs[ii, :, 1], 'g', label='$z$')
    plt.plot(t_eval, xs[ii, :, 2], 'r', label='$\\theta$')
    plt.plot(t_eval, xd[ii, :, 0], '--b', label='$y_d$')
    plt.plot(t_eval, xd[ii, :, 1], '--g', label='$z_d$')
    plt.plot(t_eval, xd[ii, :, 2], '--r', label='$\\theta_d$')

plt.suptitle(
    'Training data \nx-axis: time (sec), y-axis: state value, $x$ - blue, $xd$ - dotted blue, $\\theta$ - red, $\\theta_d$ - dotted red',
    y=0.94)
plt.show()
```

### Learn a linear model with dynamic mode decomposition (DMD)

To compare our method with existing techniques, we first learn a linear state space model from data. This is dubbed
dynamic mode decomposition. I.e. we use linear regression with LASSO regularization to learn an approximate linear model
with model structure

\begin{equation}
    \mathbf{\dot{x}} = A_{dmd}\mathbf{x} + B_{dmd}\mathbf{u}
\end{equation}

```python
#DMD parameters:
alpha_dmd = 1.4e-2                                                  # Regularization strength (LASSO) DMD
tune_mdl_dmd = False
```

```python
basis = lambda x: x
C_dmd = np.eye(n)

optimizer_dmd = linear_model.MultiTaskLasso(alpha=alpha_dmd, fit_intercept=False, selection='random')
cv_dmd = linear_model.MultiTaskLassoCV(fit_intercept=False, n_jobs=-1, cv=3, selection='random')
standardizer_dmd = preprocessing.StandardScaler(with_mean=False)

model_dmd = Edmd(n, m, basis, n, n_traj_dc, optimizer_dmd, cv=cv_dmd, standardizer=standardizer_dmd, C=C_dmd, first_obs_const=False)
xdmd, y_dmd = model_dmd.process(xs, us-hover_thrust, np.tile(t_eval,(n_traj_dc,1)), downsample_rate=sub_sample_rate)
model_dmd.fit(xdmd, y_dmd, cv=tune_mdl_dmd, override_kinematics=True)
sys_dmd = LinearLiftedDynamics(model_dmd.A, model_dmd.B, model_dmd.C, model_dmd.basis)
if tune_mdl_dmd:
    print('$\\alpha$ DMD: ',model_dmd.cv.alpha_)
```

### Learn a lifted linear model with extended dynamic mode decomposition (EDMD)

In addition, we compare our method with the current state of the art of Koopman based learning, the extended dynamic mode
decomposition. We use a dictionary of nonlinear functions $\boldsymbol{\phi(x)}$ to lift the state variables and learn a lifted state space model
of the dynamics. I.e. we first lift and then use linear regression with LASSO regularization to learn an approximate
lifted linear model with model structure

\begin{equation}
    \mathbf{\dot{z}} = A_{edmd}\mathbf{z} + B_{edmd}\mathbf{u}, \qquad \mathbf{z} = \boldsymbol{\phi(x)}
\end{equation}

```python
#EDMD parameters:
alpha_edmd = 1.1e-1                                                 # Regularization strength (LASSO) EDMD
tune_mdl_edmd = False
```

```python
basis = PolySineBasis(n, poly_deg=2, cross_terms=False)
basis.construct_basis()
poly_sine_features = preprocessing.FunctionTransformer(basis.basis)
poly_sine_features.fit(np.zeros((1,n)))
n_lift_edmd = poly_sine_features.transform((np.zeros((1,n)))).shape[1]
C_edmd = np.zeros((n,n_lift_edmd))
C_edmd[:,1:n+1] = np.eye(n)

optimizer_edmd = linear_model.MultiTaskLasso(alpha=alpha_edmd, fit_intercept=False, selection='random')
cv_edmd = linear_model.MultiTaskLassoCV(fit_intercept=False, n_jobs=-1, cv=3, selection='random')
standardizer_edmd = preprocessing.StandardScaler(with_mean=False)

model_edmd = Edmd(n, m, basis.basis, n_lift_edmd, n_traj_dc, optimizer_edmd, cv=cv_edmd, standardizer=standardizer_edmd, C=C_edmd)
X_edmd, y_edmd = model_edmd.process(xs, us-hover_thrust, np.tile(t_eval,(n_traj_dc,1)), downsample_rate=sub_sample_rate)
model_edmd.fit(X_edmd, y_edmd, cv=tune_mdl_edmd, override_kinematics=True)
model_edmd.reduce_mdl()
sys_edmd = LinearLiftedDynamics(model_edmd.A, model_edmd.B, model_edmd.C, model_edmd.basis_reduced)
if tune_mdl_edmd:
    print('$\\alpha$ EDMD: ',model_edmd.cv.alpha_)

```

### Learn a lifted bilinear model with bilinear extended mode decomposition (bEDMD)

Finally, we use the method developed in the paper to learn a lifted bilinear model of the dynamics, dubbed bilinear
extended mode decomposition (bEDMD). I.e. we first lift and then use linear regression with LASSO regularization to learn an approximate
lifted linear model with model structure

\begin{equation}
    \mathbf{\dot{z}}=F\mathbf{z}+\sum_{i=1}^m G_i\mathbf{z}\mathbf{u}_i, \qquad \mathbf{z} = \boldsymbol{\phi(x)}
\end{equation}

```python
#Bilinear EDMD parameters:
alpha_bedmd_init = 1.9e-2                                           # Regularization strength (LASSO) bEDMD
alpha_bedmd = 1.9e-2
tune_mdl_bedmd = False
```

```python
n_lift_bedmd = n_lift_edmd
output_inds = np.array([1, 2])                                      # Output states, feedback linearizing controller
C_x_bedmd = np.zeros((n, n_lift_bedmd))
C_x_bedmd[:, 1:n + 1] = np.eye(n)
C_h_bedmd = C_x_bedmd[output_inds, :]

basis_bedmd = lambda x: poly_sine_features.transform(x)
optimizer_bedmd = linear_model.MultiTaskLasso(alpha=alpha_bedmd_init, fit_intercept=False, selection='random')
cv_bedmd = linear_model.MultiTaskLassoCV(fit_intercept=False, n_jobs=-1, cv=3, selection='random')
standardizer_bedmd = preprocessing.StandardScaler(with_mean=False)

model_bedmd = FlBilinearLearner(n, m, basis_bedmd, n_lift_bedmd, n_traj_dc, optimizer_bedmd, C_h_bedmd,
                                cv=cv_bedmd, standardizer=standardizer_bedmd, C=C_x_bedmd)
X_bedmd, y_bedmd = model_bedmd.process(xs, us - hover_thrust, np.tile(t_eval, (n_traj_dc, 1)),
                                       downsample_rate=sub_sample_rate)
model_bedmd.fit(X_bedmd, y_bedmd, cv=tune_mdl_bedmd, override_kinematics=True, l1_reg=alpha_bedmd)
sys_bedmd = BilinearLiftedDynamics(model_bedmd.n_lift, m, model_bedmd.A, model_bedmd.B, model_bedmd.C,
                                   model_bedmd.basis)
if tune_mdl_bedmd:
    print('$\\alpha$ bilinear EDMD: ', model_bedmd.cv.alpha_)

```

    /Users/carlaxelfolkestad/.conda/envs/keedmd/lib/python3.7/site-packages/sklearn/linear_model/_coordinate_descent.py:1790: ConvergenceWarning: Objective did not converge. You might want to increase the number of iterations. Duality gap: 311.7818376414043, tolerance: 47.53595774775596
      check_random_state(self.random_state), random)

    Iteration  0: Norm of the residual =  589.24  comp. time =  432.79
    Iteration  1 : Norm of the residual =  367.61  comp. time =  479.68 , iteration improvement:  37.61 %
    Iteration  2 : Norm of the residual =  275.41  comp. time =  18.06 , iteration improvement:  25.08 %
    Iteration  3 : Norm of the residual =  216.80  comp. time =  19.87 , iteration improvement:  21.28 %
    Iteration  4 : Norm of the residual =  176.68  comp. time =  20.85 , iteration improvement:  18.51 %
    Iteration  5 : Norm of the residual =  148.58  comp. time =  19.23 , iteration improvement:  15.91 %
    Iteration  6 : Norm of the residual =  128.75  comp. time =  20.74 , iteration improvement:  13.34 %
    Iteration  7 : Norm of the residual =  114.80  comp. time =  19.38 , iteration improvement:  10.83 %
    Iteration  8 : Norm of the residual =  105.10  comp. time =  20.40 , iteration improvement:  8.46 %
    Iteration  9 : Norm of the residual =  98.41  comp. time =  19.16 , iteration improvement:  6.36 %
    Iteration  10 : Norm of the residual =  93.84  comp. time =  20.61 , iteration improvement:  4.65 %
    Iteration  11 : Norm of the residual =  90.73  comp. time =  20.39 , iteration improvement:  3.32 %
    Iteration  12 : Norm of the residual =  88.61  comp. time =  20.59 , iteration improvement:  2.33 %
    Iteration  13 : Norm of the residual =  87.18  comp. time =  20.11 , iteration improvement:  1.62 %
    Iteration  14 : Norm of the residual =  86.21  comp. time =  21.00 , iteration improvement:  1.11 %
    Iteration  15 : Norm of the residual =  85.56  comp. time =  20.27 , iteration improvement:  0.76 %
    Iteration  16 : Norm of the residual =  85.12  comp. time =  21.18 , iteration improvement:  0.52 %
    Iteration  17 : Norm of the residual =  84.82  comp. time =  20.47 , iteration improvement:  0.35 %

### Evaluate open loop prediction performance

We first evaluate the open loop prediction performance of the proposed method.
This is done by generating a new data set in the same way as the training set, predicting the evolution of the system
with the control sequence of each trajectory executed in the data set with each of the models, and finally comparing
the mean and standard deviation of the error between the true and predicted evolution over the trajectories. The
experimental results support what is to be expected from the theory as the error in the $y$ and $z$ terms are
significantly lower for the bEDMD method than both DMD and EDMD. The reason for this
improvement is that the bEDMD method can capture the nonlinearities present in the actuation matrix of the
$(y,z)$-dynamics.

```python
# Prediction performance evaluation parameters:
folder_plots = 'examples/figures/'                                  # Path to save plots
n_traj_ol = 100                                                      # Number of trajectories to execute, open loop
```

```python
xs_ol = np.empty((n_traj_ol, t_eval.shape[0], n))
xs_dmd_ol = np.empty((n_traj_ol, t_eval.shape[0]-1, n))
xs_edmd_ol = np.empty((n_traj_ol, t_eval.shape[0]-1, n))
xs_bedmd_ol = np.empty((n_traj_ol, t_eval.shape[0]-1, n))
us_test = np.empty((n_traj_ol, t_eval.shape[0]-1, m))

for ii in range(n_traj_ol):
    x0 = np.asarray([rand.uniform(l, u) for l, u in zip(-x0_max, x0_max)])
    set_pt_dc = np.asarray([rand.uniform(l, u) for l, u in zip(-x0_max, x0_max)])
    mpc_trajgen = MPCController(nominal_sys, n_pred_dc, dt, umin, umax, xmin, xmax, QN_trajgen, R_trajgen,
                                QN_trajgen, set_pt_dc)
    mpc_trajgen.eval(x0, 0)
    xd = mpc_trajgen.parse_result().T

    while xd[0,0] is None:
        x0 = np.asarray([rand.uniform(l, u) for l, u in zip(-x0_max, x0_max)])
        set_pt_dc = np.asarray([rand.uniform(l, u) for l, u in zip(-x0_max, x0_max)])
        mpc_trajgen = MPCController(nominal_sys, n_pred_dc, dt, umin, umax, xmin, xmax, QN_trajgen, R_trajgen,
                                    QN_trajgen, set_pt_dc)
        mpc_trajgen.eval(x0, 0)
        xd = mpc_trajgen.parse_result().T

    output = QuadrotorPdOutput(quadrotor, xd, t_eval, n, m)
    pd_controller = PDController(output, K_dc_p, K_dc_d)
    perturbed_pd_controller = PerturbedController(quadrotor, pd_controller, noise_var, const_offset=mass * gravity / 2)

    xs_ol[ii,:,:], us_test[ii,:,:] = quadrotor.simulate(x0, perturbed_pd_controller, t_eval)
    ol_controller_nom = OpenLoopController(sys_bedmd, us_test[ii,:,:]-hover_thrust, t_eval[:-1])

    xs_dmd_ol[ii,:,:], _ = sys_dmd.simulate(x0, ol_controller_nom, t_eval[:-1])

    z_0_edmd = sys_edmd.phi_fun(np.atleast_2d(x0)).squeeze()
    zs_edmd_tmp, _ = sys_edmd.simulate(z_0_edmd, ol_controller_nom, t_eval[:-1])
    xs_edmd_ol[ii,:,:] = np.dot(model_edmd.C, zs_edmd_tmp.T).T

    z_0_bedmd = sys_bedmd.phi_fun(np.atleast_2d(x0)).squeeze()
    zs_bedmd_tmp, _ = sys_bedmd.simulate(z_0_bedmd, ol_controller_nom, t_eval[:-1])
    xs_bedmd_ol[ii,:,:] = np.dot(model_bedmd.C, zs_bedmd_tmp.T).T

error_dmd = xs_ol[:,:-1,:] - xs_dmd_ol
error_dmd_mean = np.mean(error_dmd, axis=0).T
error_dmd_std = np.std(error_dmd, axis=0).T
mse_dmd = np.mean(np.mean(np.mean(np.square(error_dmd))))

error_edmd = xs_ol[:,:-1,:] - xs_edmd_ol
error_edmd_mean = np.mean(error_edmd, axis=0).T
error_edmd_std = np.std(error_edmd, axis=0).T
mse_edmd = np.mean(np.mean(np.mean(np.square(error_edmd))))

error_bedmd = xs_ol[:,:-1,:] - xs_bedmd_ol
error_bedmd_mean = np.mean(error_bedmd, axis=0).T
error_bedmd_std = np.std(error_bedmd, axis=0).T
mse_bedmd = np.mean(np.mean(np.mean(np.square(error_bedmd))))

print('\nOpen loop performance statistics:')
print('   MSE DMD:   ', "{:.3f}".format(mse_dmd),
      '\n   MSE EDMD:  ', "{:.3f}".format(mse_edmd),
      '\n   MSE bEDMD: ', "{:.3f}".format(mse_bedmd))
print('   Improvement DMD -> EDMD:   ', "{:.2f}".format((1 - mse_edmd / mse_dmd) * 100), ' %'
      '\n   Improvement DMD -> bEDMD:  ', "{:.2f}".format((1 - mse_bedmd / mse_dmd) * 100), ' %'
      '\n   Improvement EDMD -> bEDMD: ', "{:.2f}".format((1 - mse_bedmd / mse_edmd) * 100), ' %')
```

    Open loop performance statistics:
       MSE DMD:    0.211
       MSE EDMD:   0.119
       MSE bEDMD:  0.037
       Improvement DMD -> EDMD:    43.48  %
       Improvement DMD -> bEDMD:   82.27  %
       Improvement EDMD -> bEDMD:  68.62  %

```python
import matplotlib.pyplot as plt
import matplotlib

figwidth = 12
lw = 2
fs = 16
y_lim_gain = 1.2

#Plot open loop results:
ylabels = ['$e_{y}$', '$e_z$', '$e_{\\theta}$']
plt.figure(figsize=(figwidth,3))
for ii in range(3):
    plt.subplot(1,3,ii+1)
    plt.plot(t_eval[:-1], error_dmd_mean[ii,:], linewidth=lw, label='DMD')
    plt.fill_between(t_eval[:-1], error_dmd_mean[ii,:] - error_dmd_std[ii,:], error_dmd_mean[ii,:] + error_dmd_std[ii,:], alpha=0.2)
    plt.plot(t_eval[:-1], error_edmd_mean[ii, :], linewidth=lw, label='EDMD')
    plt.fill_between(t_eval[:-1], error_edmd_mean[ii, :] - error_edmd_std[ii, :],error_edmd_mean[ii, :] + error_edmd_std[ii, :], alpha=0.2)
    plt.plot(t_eval[:-1], error_bedmd_mean[ii, :], linewidth=lw, label='bEDMD')
    plt.fill_between(t_eval[:-1], error_bedmd_mean[ii, :] - error_bedmd_std[ii, :],error_bedmd_mean[ii, :] + error_bedmd_std[ii, :], alpha=0.2)
    ylim = max(max(np.abs(error_bedmd_mean[ii, :] - error_bedmd_std[ii, :])), max(np.abs(error_bedmd_mean[ii, :] + error_bedmd_std[ii, :])))
    plt.ylim([-ylim * y_lim_gain, ylim * y_lim_gain])
    plt.xlabel('$t$ (sec)', fontsize=fs)
    plt.ylabel(ylabels[ii], fontsize=fs)
    plt.grid()

plt.legend(loc='upper left', fontsize=fs-4)
suptitle = plt.suptitle('Open loop prediction error of DMD, EDMD and bilinear EDMD models', y=1.05, fontsize=18)
matplotlib.rcParams['pdf.fonttype'] = 42
matplotlib.rcParams['ps.fonttype'] = 42
plt.tight_layout()
plt.savefig(folder_plots + 'planar_quad_prediction.pdf', format='pdf', dpi=2400, bbox_extra_artists=(suptitle,), bbox_inches="tight")
plt.show()

```

### Evaluate closed loop performance

We now study the closed loop performance of the control design. A trajectory is designed
to move the system from $(y_0,z_0) = (-1,0)$ to $(y_f,z_f) = (1,1)$. Then, the feedback linearizing controller is
designed to track the trajectory in the $z,\theta$ coordinates. These coordinates are chosen to avoid singularities
present in the $y,z$ coordinates. Comparing the closed loop performance of the feedback linearization based on the
bEDMD with LQR controllers designed using the DMD and EDMD models, we observe that the trajectory tracking error is
significantly reduced while expending only somewhat more control effort.

```python
#Closed loop performance evaluation parameters:
x0_cl = np.array([-1., 0., 0., 0., 0., 0.])                         # Initial value, closed loop trajectory
set_pt_cl = np.array([1., 1., 0., 0., 0., 0.])                      # Desired final value, closed loop trajectory
t_eval_cl = dt * np.arange(201)                                     # Simulation time points, closed loop
Q_trajgen_cl = sc.sparse.diags([0,0,0,0,0,0])                       # State penalty matrix, trajectory generation
QN_trajgen_cl = sc.sparse.diags([3e2,3e2,3e2,1e2,1e2,1e2])          # Final state penalty matrix, trajectory generation
R_trajgen_cl = sc.sparse.eye(m)                                     # Actuation penalty matrix, trajectory generation
mpc_trajgen_cl = MPCController(nominal_sys,t_eval_cl.size,dt,umin,umax,xmin,xmax,QN_trajgen_cl,R_trajgen_cl,QN_trajgen_cl,set_pt_cl)
q_cl, r_cl = 2e1, 1                                                  # State and actuation penalty values, closed loop
output_inds = np.array([1, 2])                                      # Output states, feedback linearizing controller

# Generate trajectory:
mpc_trajgen_cl.eval(x0_cl, 0)
xr_cl = mpc_trajgen_cl.parse_result()[:,:-1]
ur_cl = mpc_trajgen_cl.get_control_prediction()
xr_cl_dot = nominal_sys.eval_dot(xr_cl,ur_cl,0.)

# Define outputs:
y_d = xr_cl[output_inds,:]
y_d_dot = xr_cl[output_inds+int(n/2),:]
y_d_ddot = xr_cl_dot[output_inds+int(n/2),:]
```

##### Design LQR controller based on DMD model

```python
Q_dmd = q_cl*np.identity(n)
R_dmd = r_cl*np.identity(m)
P_dmd = sc.linalg.solve_continuous_are(model_dmd.A, model_dmd.B, Q_dmd, R_dmd)
K_dmd = np.linalg.inv(R_dmd)@model_dmd.B.T@P_dmd
K_dmd_p, K_dmd_d = K_dmd[:,:int(n/2)], K_dmd[:,int(n/2):]
output_dmd = QuadrotorPdOutput(sys_dmd, xr_cl.T, t_eval_cl, n, m)
controller_dmd = PDController(output_dmd, K_dmd_p, K_dmd_d)
controller_dmd = PerturbedController(sys_dmd,controller_dmd,0.,const_offset=hover_thrust, umin=umin, umax=umax)
```

##### Design LQR controller based on EDMD model

```python
z_d_edmd = np.array([sys_edmd.phi_fun(x.reshape(1,-1)).squeeze() for x in xr_cl.T]).T
z_d_dot_edmd = differentiate_vec(z_d_edmd.T, t_eval_cl).T
z_d_ddot_edmd = differentiate_vec(z_d_dot_edmd.T, t_eval_cl).T
output_edmd = QuadrotorTrajectoryOutput(sys_edmd, y_d, y_d_dot, y_d_ddot, dt, z_d_edmd, z_d_dot_edmd, z_d_ddot_edmd, model_edmd.C[output_inds,:])

Q_edmd = q_cl*np.identity(sys_edmd.n)
R_edmd = r_cl*np.identity(m)
P_edmd = sc.linalg.solve_continuous_are(sys_edmd.A, sys_edmd.B, Q_edmd, R_edmd)
K_edmd = np.linalg.inv(R_edmd)@model_edmd.B.T@P_edmd
controller_edmd = LinearLiftedController(output_edmd, K_edmd)
controller_edmd = PerturbedController(quadrotor, controller_edmd,0.,const_offset=hover_thrust, umin=umin, umax=umax)

```

##### Design feedback linearizing controller based on bEDMD model

```python
k = m
n_lift_bedmd = sys_bedmd.n
Q_bedmd = q_cl*np.eye(int(2*n_lift_bedmd))
R_bedmd = r_cl*np.eye(n_lift_bedmd)
C_h = model_bedmd.C[output_inds,:]

z_d_bedmd = np.array([sys_bedmd.phi_fun(x.reshape(1,-1)).squeeze() for x in xr_cl.T]).T
z_d_dot_bedmd = differentiate_vec(z_d_bedmd.T, t_eval_cl).T
z_d_ddot_bedmd = differentiate_vec(z_d_dot_bedmd.T, t_eval_cl).T
output_bedmd = QuadrotorTrajectoryOutput(sys_bedmd, y_d, y_d_dot, y_d_ddot, dt, z_d_bedmd, z_d_dot_bedmd, z_d_ddot_bedmd, C_h)

f_eta = np.concatenate((np.zeros((n_lift_bedmd,n_lift_bedmd)), np.eye(n_lift_bedmd)), axis=1)
f_eta_dot = np.concatenate((sys_bedmd.F@sys_bedmd.F, np.zeros((n_lift_bedmd,n_lift_bedmd))), axis=1)
F_lin = np.concatenate((f_eta, f_eta_dot), axis=0)
G_lin = np.concatenate((np.zeros((n_lift_bedmd,n_lift_bedmd)), np.eye(n_lift_bedmd)), axis=0)

P_bedmd = sc.linalg.solve_continuous_are(F_lin, G_lin, Q_bedmd, R_bedmd)
K_bedmd = np.linalg.inv(R_bedmd)@G_lin.T@P_bedmd
controller_bedmd = BilinearFBLinController(sys_bedmd, output_bedmd, K_bedmd)
controller_bedmd = PerturbedController(sys_bedmd, controller_bedmd,0.,const_offset=hover_thrust, umin=umin, umax=umax)

```

##### Compare closed loop performance

```python
# Simulate the system under closed loop control:
xs_cl_dmd, us_cl_dmd = quadrotor.simulate(x0_cl, controller_dmd, t_eval_cl)
xs_cl_edmd, us_cl_edmd = quadrotor.simulate(x0_cl, controller_edmd, t_eval_cl)
xs_cl_bedmd, us_cl_bedmd = quadrotor.simulate(x0_cl, controller_bedmd, t_eval_cl)

mse_cl_dmd = np.linalg.norm(xs_cl_dmd[1:,output_inds]-xr_cl[output_inds,1:].T, ord='fro')**2
mse_cl_edmd = np.linalg.norm(xs_cl_edmd[1:,output_inds]-xr_cl[output_inds,1:].T, ord='fro')**2
mse_cl_bedmd = np.linalg.norm(xs_cl_bedmd[1:,output_inds]-xr_cl[output_inds,1:].T, ord='fro')**2
ctrl_cost_dmd = np.linalg.norm(us_cl_dmd, ord='fro')**2
ctrl_cost_edmd = np.linalg.norm(us_cl_edmd, ord='fro')**2
ctrl_cost_bedmd = np.linalg.norm(us_cl_bedmd, ord='fro')**2

print('\nClosed loop performance statistics:')
print(' -Tracking error:')
print('   Tracking MSE DMD:   ', "{:.3f}".format(mse_cl_dmd),
      '\n   Tracking MSE EDMD:  ', "{:.3f}".format(mse_cl_edmd),
      '\n   Tracking MSE bEDMD: ', "{:.3f}".format(mse_cl_bedmd))
print('   Improvement DMD -> EDMD:   ', "{:.2f}".format(100*(1-(mse_cl_edmd)/(mse_cl_dmd))), ' %'
      '\n   Improvement DMD -> bEDMD:  ', "{:.2f}".format(100*(1-(mse_cl_bedmd)/(mse_cl_dmd))), ' %'
      '\n   Improvement EDMD -> bEDMD: ', "{:.2f}".format(100*(1-(mse_cl_bedmd)/(mse_cl_edmd))), ' %')
print(' -Control effort:')
print('   Control effort DMD:   ', "{:.3f}".format(ctrl_cost_dmd),
      '\n   Control effort EDMD:  ', "{:.3f}".format(ctrl_cost_edmd),
      '\n   Control effort bEDMD: ', "{:.3f}".format(ctrl_cost_bedmd))
print('   Improvement DMD -> EDMD:   ', "{:.2f}".format(100*(1-(ctrl_cost_edmd)/(ctrl_cost_dmd))), ' %'
      '\n   Improvement DMD -> bEDMD:  ', "{:.2f}".format(100*(1-(ctrl_cost_bedmd)/(ctrl_cost_dmd))), ' %'
      '\n   Improvement EDMD -> bEDMD: ', "{:.2f}".format(100*(1-(ctrl_cost_bedmd)/(ctrl_cost_edmd))), ' %')

```

    Closed loop performance statistics:
     -Tracking error:
       Tracking MSE DMD:    2.865
       Tracking MSE EDMD:   17336.392
       Tracking MSE bEDMD:  7954.371
       Improvement DMD -> EDMD:    -605100.31  %
       Improvement DMD -> bEDMD:   -277581.05  %
       Improvement EDMD -> bEDMD:  54.12  %
     -Control effort:
       Control effort DMD:    47205.375
       Control effort EDMD:   252.228
       Control effort bEDMD:  491735.020
       Improvement DMD -> EDMD:    99.47  %
       Improvement DMD -> bEDMD:   -941.69  %
       Improvement EDMD -> bEDMD:  -194856.82  %

```python
x_index = output_inds[0]
y_index = output_inds[1]
plt.figure(figsize=(figwidth, 4))

plt.subplot(2, 2, 1)
plt.plot(t_eval_cl, xr_cl[x_index,:], '--r', linewidth=2, label='Reference')
plt.plot(t_eval_cl,xs_cl_dmd[:, x_index], linewidth=lw, label='DMD')
plt.plot(t_eval_cl,xs_cl_edmd[:, x_index], linewidth=lw, label='EDMD')
plt.plot(t_eval_cl,xs_cl_bedmd[:, x_index], linewidth=lw, label='bEDMD')
plt.ylabel('$z$', fontsize=fs)
plt.title('Output states', fontsize=fs)
plt.grid()

plt.subplot(2, 2, 3)
plt.plot(t_eval_cl, xr_cl[y_index, :], '--r', linewidth=2, label='Reference')
plt.plot(t_eval_cl, xs_cl_dmd[:, y_index], linewidth=lw, label='DMD')
plt.plot(t_eval_cl, xs_cl_edmd[:, y_index], linewidth=lw, label='EDMD')
plt.plot(t_eval_cl, xs_cl_bedmd[:, y_index], linewidth=lw, label='bEDMD')
plt.ylabel('$\\theta$', fontsize=fs)
plt.xlabel('Time (sec)')
plt.grid()

plt.subplot(2, 2, 2)
plt.plot(t_eval_cl[:-1], us_cl_dmd[:, 0], linewidth=lw, label='DMD')
plt.plot(t_eval_cl[:-1], us_cl_edmd[:, 0], linewidth=lw, label='EDMD')
plt.plot(t_eval_cl[:-1], us_cl_bedmd[:, 0], linewidth=lw, label='bEDMD')
plt.ylabel('$u_1$', fontsize=fs)
plt.title('Control action', fontsize=fs)
plt.grid()

plt.subplot(2, 2, 4)
plt.plot(t_eval_cl[:-1], us_cl_dmd[:, 1], linewidth=lw, label='DMD')
plt.plot(t_eval_cl[:-1], us_cl_edmd[:, 1], linewidth=lw, label='EDMD')
plt.plot(t_eval_cl[:-1], us_cl_bedmd[:, 1], linewidth=lw, label='bEDMD')
plt.xlabel('Time (sec)', fontsize=fs)
plt.ylabel('$u_2$', fontsize=fs)
plt.grid()

suptitle = plt.suptitle('Trajectory tracking based on DMD, EDMD and bilinear EDMD models', y=1.05,fontsize=18)
matplotlib.rcParams['pdf.fonttype'] = 42
matplotlib.rcParams['ps.fonttype'] = 42
plt.tight_layout()
plt.savefig(folder_plots + 'planar_quad_closedloop.pdf', format='pdf', dpi=2400, bbox_extra_artists=(suptitle,),
            bbox_inches="tight")
plt.show()
```

```python

```


# Generic programming

From [Wikipedia](https://en.wikipedia.org/wiki/Generic_programming):
> **Generic programming** is a style of computer programming in which algorithms are written in terms of types *to-be-specified-later* that are then *instantiated* when needed for specific types provided as parameters.

# Example: Vandermonde matrix (revisited)

[Vandermonde matrix:](https://en.wikipedia.org/wiki/Vandermonde_matrix)
\begin{align}V=\begin{bmatrix}1&\alpha _{1}&\alpha _{1}^{2}&\dots &\alpha _{1}^{n-1}\\1&\alpha _{2}&\alpha _{2}^{2}&\dots &\alpha _{2}^{n-1}\\1&\alpha _{3}&\alpha _{3}^{2}&\dots &\alpha _{3}^{n-1}\\\vdots &\vdots &\vdots &\ddots &\vdots \\1&\alpha _{m}&\alpha _{m}^{2}&\dots &\alpha _{m}^{n-1}\end{bmatrix}\end{align}

### Keep function argument types generic if possible

```julia
function vander_naive(x::Vector)
    m = length(x)
    V = Matrix{Float64}(undef, m, m)
    for j = 1:m
        V[j,1] = 1.0
    end
    for i= 2:m
        for j = 1:m
            V[j,i] = x[j] * V[j,i-1]
            end
        end
    return V
end
```

```julia
x = rand(3)
vander_naive(x)
```

```julia
vander_naive(1:3)
```

The annotation `::Vector` in the the function signature is unnecessarily specific.

```julia
function vander_naive(x::AbstractVector)
    m = length(x)
    V = Matrix{Float64}(undef, m, m)
    for j = 1:m
        V[j,1] = 1.0
    end
    for i= 2:m
        for j = 1:m
            V[j,i] = x[j] * V[j,i-1]
            end
        end
    return V
end
```

```julia
vander_naive(1:3)
```

### Avoid explicit typing if possible

```julia
vander_naive([1,2,3])
```

Why is the result a matrix of floating point numbers....?

Even worse:

```julia
vander_naive(rand(ComplexF64, 3))
```

We can easily cover those cases as well by only slightly modifying our code.

```julia
function vander_almost_generic(x::AbstractVector)
    T = eltype(x)
    m = length(x)
    V = Matrix{T}(undef, m, m)
    for j = 1:m
        V[j,1] = 1.0
    end
    for i= 2:m
        for j = 1:m
            V[j,i] = x[j] * V[j,i-1]
            end
        end
    return V
end
```

```julia
vander_almost_generic([1,2,3])
```

```julia
vander_almost_generic(rand(ComplexF64, 3))
```

```julia
vander_almost_generic(["Stadt", "Land", "Fluss"])
```

```julia
function vander_generic(x::AbstractVector{T}) where T # this is the same as just x::AbstractVector
    m = length(x)
    V = Matrix{T}(undef, m, m)
    for j = 1:m
        V[j,1] = one(T)
    end
    for i= 2:m
        for j = 1:m
            V[j,i] = x[j] * V[j,i-1]
            end
        end
    return V
end
```

```julia
vander_generic(["Stadt", "Land", "Fluss"])
```

One more level of generality, just because we can. :)

```julia
vander_generic([3, "Stadt", 4 + 5im])
```

```julia
function vander_supergeneric(x::AbstractVector{T}) where T
    m = length(x)
    V = Matrix{T}(undef, m, m)
    for j = 1:m
        V[j,1] = one(x[j])
    end
    for i= 2:m
        for j = 1:m
            V[j,i] = x[j] * V[j,i-1]
            end
        end
    return V
end
```

```julia
vander_supergeneric([3, "Stadt", 4 + 5im])
```

### And all of this comes at no performance penality

```julia
using BenchmarkTools
```

```julia
x = rand(Float64, 100);
@btime vander_naive($x);
@btime vander_supergeneric($x);
```

Actually, for this specific example **our generic code is faster** in a few cases inasmuch as type conversions are unnecessary.

```julia
x = rand(Int, 100);
@btime vander_naive($x);
@btime vander_supergeneric($x);
```

```julia
x = rand(Bool, 100);
@btime vander_naive($x);
@btime vander_supergeneric($x);
```

On the other hand, sometimes it is worth converting to a different type to dispatch to a faster method or to utilize magic like compiler optimizations.

```julia
x = rand(Float32, 100);
@btime vander_naive($x);
@btime vander_supergeneric($x);
```

# Arbitrary precision computations

Let's say you have implemented the following crazily complex physics code as part of your thesis project which takes in a number and spits out the answer to life, the universe, and everything.

```julia
function answer_to_life_universe_and_everything(x::Integer)
    m = sin((2*x)^100)
    c = 42
    E = m*c^2
    a = sqrt(abs(E))
    b = atan(m)
    c = a^2 + b^2
    answer = sqrt(1764)/(1+exp(-c))
end
```

```julia
answer_to_life_universe_and_everything(2)
```

Alright, apparently the answer is 21!

The author: "I checked the code multiple times, it is correct. So let's publish."

Without changing a line of code we can check the *correctness* (in the numerical sense) of our result using [arbitrary precision arithmetics](https://docs.julialang.org/en/latest/manual/integers-and-floating-point-numbers/#Arbitrary-Precision-Arithmetic-1).

```julia
big(2)
```

```julia
typeof(big(2))
```

```julia
answer_to_life_universe_and_everything(big(2))
```

# Let's get a bit more fancy!

```julia
using Interact
```

```julia
@manipulate for n in 1:20
    [i*j for i in 1:n, j in 1:n]
end
```

```julia
function insert_block(A::AbstractMatrix, i, j, what=7)
    B = copy(A)
    B[i:i+2, j:j+2] .= what
    B
end
```

```julia
A = fill(0, 9, 9)
insert_block(A, 3, 5)  # this returns the new matrix
```

```julia
A = fill(0, 10, 10)
n = size(A, 1)

@manipulate for i in 1:n-2, j in 1:n-2
    insert_block(A, i, j)
end
```

### Let's add some color!

Our function `insert_block` is generic. Since the first argument `A isa AbstractArray`, we can index into it and set new values. Pretty much every value type is fine!

```julia
using Colors
```

```julia
@manipulate for n in 1:80
    distinguishable_colors(n)
end
```

```julia
colors = distinguishable_colors(10)
```

```julia
colors[1]
```

```julia
A = fill(colors[1], 10, 10)
n = size(A, 1)

@manipulate for i in 1:n-2, j in 1:n-2
    insert_block(A, i, j, colors[4])
end
```

# Generic Programming + Multiple Dispatch + JIT

The possibility to write generic algorithms that compile to fast machine code in combination with multiple dispatch leads to an ([unreasonable](https://pretalx.com/juliacon2019/talk/BCYWZJ/)) amount of code reuse. This sharing of code comes in two forms:
1. **Sharing types** among a wide variety of packages implementing different algorithms;
2. **Sharing generic algorithms** that work for different package-defined types implementing common abstractions.

1. **Sharing types:** DataStructures.jl, OrderedCollections.jl, StaticArrays.jl, Colors.jl, Measurements.jl ...

As of the time of this writing, **804 packages** depend on the data types provided in [DataStructures.jl](https://juliacollections.github.io/DataStructures.jl/latest/).

**851 packages** reuse type implementations in [OrderedCollections.jl](https://github.com/JuliaCollections/OrderedCollections.jl).

**That's about every third package!**

2. **Sharing generic algorithms:** StatsBase.jl, SortingAlgorithms.jl, GenericLinearAlgebra.jl, ...

# Emergent features

`Measurement` type from [Measurements.jl]() and differential equation solver from [OrdinaryDiffEq.jl](https://github.com/JuliaDiffEq/OrdinaryDiffEq.jl) (i.e. [DifferentialEquations.jl](https://github.com/JuliaDiffEq/DifferentialEquations.jl))

```julia
using OrdinaryDiffEq, Measurements, PyPlot

#Half-life of Carbon-14 is 5730 years.
c = 5.730 ± 2

#Setup
u0 = 1.0 ± 0.1
tspan = (0.0 ± 0.0, 1.0 ± 0.0)

#Define the problem
radioactivedecay(u,p,t) = -c*u

#Pass to solver
prob = ODEProblem(radioactivedecay,u0,tspan)
sol = solve(prob, Tsit5(), reltol=1e-8, abstol=1e-8)
```

```julia
# analytic solution
u = u0 .* exp.(-c .* sol.t);

# plot solution
ts = getfield.(sol.t, :val)
solvals = getfield.(sol, :val)
solerrs = getfield.(sol, :err);

errorbar(ts, solvals, yerr=solerrs)
plot(ts, getfield.(u, :val), color="red", lw=2)
ylabel("u(t)")
xlabel("t");
```

Note that, in some sense, **Julia implemented that feature by itself**.

The authors of Measurements.jl and DifferentialEquations.jl never had any collabration on this.

It **just works**.

# Core messages of this Notebook

* It is simple to write **type-generic code** in Julia and you should do it.
* Generally, **generic code is just as fast as specific code**.
* Generic Programming + Multiple Dispatch + JIT = **lots of code sharing and emergent features**