# Spline transformations of points

```python
import numpy as np
%matplotlib notebook
import matplotlib as mpl
import matplotlib.pyplot as plt
import summerschool as ss
import os
```

## Linear/Affine transformations have too few degrees of freedom
They cannot model spatially varying growth and atrophy.  Because they're linear, they look the same everywhere.

## Arbitrary vector fields have too many degrees of freedom

If we have an arbitrary vector field $v(x)$, and we want to match a set of points $X$ to a set of points $Y$, we can just define the vector field as
\begin{align*}
v(x) &=
\left\{
\begin{array}{ll}
Y_i-X_i, & x=X_i\\
\text{anything}, & \text{otherwise}
\end{array}
\right.
\end{align*}

Usually we want to use vector fields to find out how images are deforming at points other than directly on the labeled landmarks.  These vector fields have too many degres of freedom to be useful.

One way to effectively reduce the degrees of freedom is by enforcing the vector fields to be smooth.  This is natural because growth and atrophy of tissue in one position tends to be quite similar to that in nearby positions.

## Energy methods for enforcing smoothness
Let's define the energy of a vector field by
\begin{align*}
E &= \frac{1}{2}\int v^T(x) A v(x) dx
\end{align*}
where $A$ is a differential operator.

For example, if $v$ represents the displacement of points in an elastic body in one dimension, then elastic energy is given by
\begin{align*}
\frac{1}{2}\int k\left|\frac{d}{dx}v(x)\right|^2dx
\end{align*}
Just like a spring has potential energy according to Hooke's law, which is quadratic in displacement ($E = \frac{1}{2} k (\Delta x)^2$) https://en.wikipedia.org/wiki/Hooke%27s_law#Spring_energy , an elastic body has energy which is quadratic in local displacment (i.e. the derivative).

We can rewrite this energy in the above form
\begin{align*}
&= \int k\frac{d}{dx}v^T(x)\frac{d}{dx}v(x) dx\\
&= \frac{1}{2}\int  v^T(x) \left( -k \frac{d^2}{dx^2}\right)v(x) dx\\
&= \frac{1}{2}\int  v^T(x)Av(x) dx\\
\end{align*}
for $A = -k \frac{d^2}{dx^2}$.  Here we used the integration by parts, assuming 0 boundary conditions.

A common choice for $A$ used in the image registration community is identity minus Laplacian times a constant raised to some integer power: $A = (id - a^2\Delta)^{2p}$ for some choice of $a$ (with units of space) and $p$ (an integer, typically 2 or 4).

For a well defined positive energy, $A$ should be symmetric (i.e. a Hermitian operator) positive definite.

We can now pose a new landmark matching problem.

minimize $\frac{1}{2} \int v^T(x) A v(x)dx$ subject to the constraints $v(X_i) = Y_i-X_i$.

This is a constrained optimization problem.  We could also consider an unconstrained problem, where $X_i + v(X_i)$ should be close (but not exactly equal) to $Y_i$.

minimize $\frac{1}{2} \int v^T(x) A v(x)dx + \frac{1}{2\sigma^2} \sum_i |X_i + v(X_i) - Y_i|^2$

Below we will use the first (constrained) problems.

## Green's kernels for solutions to energy problems
Write down the minimization problem with constraints using Lagrange multipliers.  We minimize the cost
\begin{align*}
C = \frac{1}{2}\int v^T(x) A v(x) dx +  \sum_i \lambda_i^T[(Y_i - X_i) - v(X_i)]
\end{align*}
where $\lambda_i$ are the Lagrange multipliers.

We look for a stationary solution by taking perturbations $v \mapsto v + \epsilon \delta v$, and insiting the change be zero for all possible choices of perturbation.
\begin{align*}
&\frac{d}{d\epsilon}\frac{1}{2} \int (v+\epsilon \delta v)^T(x) A (v+\epsilon \delta v)(x) dx +  \sum_i \lambda_i^T[(Y_i - X_i) - v(X_i) - \epsilon \delta v(X_i)] \bigg|_{\epsilon = 0}\\
&= \int Av^T(x) \delta v(x) dx + \sum_i \lambda_i \delta v(X_i)
\end{align*}

Let's use a Dirac $\delta$ to put everything under the same integral.
\begin{align*}
&= \int Av^T(x) \delta v(x)  - \sum_i \lambda_i^T \delta v(x) \delta (x-X_i)dx\\
&=\int \left(Av(x) - \sum_i \lambda_i^T \delta(x-X_i)\right)^T \delta v(x) dx
\end{align*}
If this variation is to be 0 for any perturbation, we require
\begin{align}
Av(x) = \sum_i \lambda_i \delta(x-X_i)
\end{align}

To solve this, we need to invert the operator $A$.  We'll assume that there is a function $K(x,y)$ such that $A K(x,y) = \delta(x-y)$.  This is called the Green's function of the operator.  The Green's functions for many operators can be looked up https://en.wikipedia.org/wiki/Green%27s_function.  Sometimes a convenient way to derive Green's functions is by using Fourier transforms.

Let's plug in the Green's function
\begin{align*}
Av(x) &= \sum_i A K(x,X_i)  \lambda_i \\
&= A\left(\sum_i  K(x,X_i) \lambda_i\right)
\end{align*}
Since $A$ is positive definite, this will be true if
\begin{align*}
v(x) &= \sum_i  K(x,X_i)\lambda_i
\end{align*}

This gives us the FORM of the solution.  Now we have to find the values of the Lagrange multipliers that will satisfy the constraints.  We require
\begin{align*}
v(X_j) &= \sum_i  K(X_j,X_i) \lambda_i\\
&= Y_j-X_j
\end{align*}
This is just a linear equation, it can be written in matrix form.  We will write $X$ as a $3xN$ matrix, where the $i$th column is $X_i$, and similarly for $Y$ and $\lambda$.
\begin{align*}
\lambda \bar K = Y-X \implies \lambda = (Y-X)\bar K^{-1}
\end{align*}
where $\bar K$ is the $NxN$ matrix whose $i$th row and $j$th column is $K(X_i,X_j)$.

Note that the solution to the problem can be written in terms of $K$ only, not $A$.

In many cases we'll simply choose a Green's kernel, rather than an energy operator.

```python
# let's solve an example problem
f,ax = plt.subplots(1,3)
# simple example landmarks, match X to Y
X = np.array([[0,0],[1,0],[2,0]]).transpose()
Y = np.array([[0,1.1],[0.9,0.5],[3,-0.1]]).transpose()
n = X.shape[1]

# use a Gaussian kernel because it is "nice" (simple, smooth, separable, rotationally symmetric, etc.)
# we will define the kernel instead of defining the operator
# either choice is valid
k = lambda x, y: np.exp(-np.sum((x-y)**2)/(2.0*1.0**2))

# make a picture
dx = 0.05
stride = 8
x0 = np.arange(-1,4,dx)
x1 = np.arange(-1,4,dx)
X0,X1 = np.meshgrid(x0,x1,indexing='ij')
ss.plot_grid(X0,X1,ax=ax[0],rstride=stride,cstride=stride)
hx = ax[0].scatter(X[0,:],X[1,:],color='c',edgecolor='k')
hy = ax[0].scatter(Y[0,:],Y[1,:],color='r',edgecolor='k')
ax[0].set_aspect('equal')
ax[0].legend([hx,hy],['X','Y'])
ax[0].set_title('Matching problem')

# define a kernel matrix K
K = np.empty((n,n))
for i in range(n):
    for j in range(n):
        K[i,j] = k(X[:,i],X[:,j])
print('K matrix is \n{}'.format(K))

# solve the linear system
Lambda = np.linalg.solve(K,(Y-X).transpose()).transpose()

# we would like to see the smooth displacment field v(x)
# this defines a function v(x) = sum_i lambda_i k(x_i,x)
# let's use it to deform the grid for visualization
phiX0 = np.array(X0)
phiX1 = np.array(X1)
for i in range(len(x0)):
    for j in range(len(x1)):
        # this point
        p = np.array([X0[i,j],X1[i,j]])
        for k_ in range(n):
            phiX0[i,j] += Lambda[0,k_]*k(p,X[:,k_])
            phiX1[i,j] += Lambda[1,k_]*k(p,X[:,k_])
ss.plot_grid(phiX0,phiX1,ax=ax[1],rstride=stride,cstride=stride)
# the deformed landmarks X + v(X) should be exactly equal to Y, so we only plot one of them
hy = ax[1].scatter(Y[0,:],Y[1,:],color='r',edgecolor='k')
ax[1].set_aspect('equal')
ax[1].set_title('The deformation')

ax[2].quiver(X0[::stride,::stride],X1[::stride,::stride],phiX0[::stride,::stride]-X0[::stride,::stride],
             phiX1[::stride,::stride]-X1[::stride,::stride],
             scale=stride)
ax[2].set_aspect('equal')
ax[2].set_title('The vector field')
```

    <IPython.core.display.Javascript object>

    K matrix is
    [[1.         0.60653066 0.13533528]
     [0.60653066 1.         0.60653066]
     [0.13533528 0.60653066 1.        ]]

    Text(0.5,1,'The vector field')

```python
# now our 3D data
# daniel update
#fname = os.path.join('hipAmygLandmarks', 'hippocampus_01_landmarks.txt')
fname = os.path.join('mouse_hippo_landmarks', 'hippocampus_01_landmarks.txt')
X = ss.load_landmarks(fname)
#fname = os.path.join('hipAmygLandmarks', 'hippocampus_01_surface.byu')
fname = os.path.join('mouse_hippo_landmarks', 'hippocampus_01_surface.byu')
V0,F0 = ss.load_surface(fname)

# get a landmarks
#fname = os.path.join('hipAmygLandmarks', 'hippocampus_02_landmarks.txt')
fname = os.path.join('mouse_hippo_landmarks', 'hippocampus_02_landmarks.txt')
Y = ss.load_landmarks(fname)
#fname = os.path.join('hipAmygLandmarks', 'hippocampus_02_surface.byu')
fname = os.path.join('mouse_hippo_landmarks', 'hippocampus_02_surface.byu')
V1,F1 = ss.load_surface(fname)

n = X.shape[1]

# define a kernel matrix
k = lambda x, y: np.exp(-np.sum((x-y)**2)/(2.0*1.0**2))
K = np.empty((n,n))
for i in range(n):
    for j in range(n):
        K[i,j] = k(X[:,i],X[:,j])
print('K matrix is size {}x{}'.format(K.shape[0],K.shape[1]))
Lambda = np.linalg.solve(K,(Y-X).transpose()).transpose()

# calculate the deformed vertices of our triangulated surface
# these vertices are deformed smoothly based on the landmarks that they are close to
# so we expect the deformed template surface to look similar to the target surface
phiV0 = np.array(V0)
for i in range(V0.shape[1]):
    # this point
    for k_ in range(n):
        phiV0[:,i] += Lambda[:,k_]*k(V0[:,i],X[:,k_])
# plot our results
f = plt.figure()
ax = f.add_subplot(1,2,1,projection='3d')
ax.plot_trisurf(V0[0,:],V0[1,:],V0[2,:],triangles=F0,color='c',alpha=0.5)
hx = ax.scatter(X[0,:],X[1,:],X[2,:],color='c',edgecolor='k')
ax.plot_trisurf(V1[0,:],V1[1,:],V1[2,:],triangles=F1,color='r',alpha=0.5)
hy = ax.scatter(Y[0,:],Y[1,:],Y[2,:],color='r',edgecolor='k')
ax.legend([hx,hy],['X','Y'],loc=6)
ss.axis_equal(ax)
ax.set_title('X and Y points \nwith corresponding surfaces')

ax = f.add_subplot(1,2,2,projection='3d')
ax.plot_trisurf(V0[0,:],V0[1,:],V0[2,:],triangles=F0,color='c',alpha=0.1)
ax.plot_trisurf(phiV0[0,:],phiV0[1,:],phiV0[2,:],triangles=F0,color='b',alpha=0.5)
ax.plot_trisurf(V1[0,:],V1[1,:],V1[2,:],triangles=F1,color='r',alpha=0.5)
ss.axis_equal(ax)
ax.set_title('Deformed template surface (blue)')
```

    K matrix is size 38x38

    <IPython.core.display.Javascript object>

    Text(0.5,0.92,'Deformed template surface (blue)')


# Calculus

Boilerplate to make the doctester work.

```
import sys
import os
sys.path.insert(1, os.path.join(os.path.pardir, "ipython_doctester"))
from sympy import *
from ipython_doctester import test
# Work around a bug in IPython. This will disable the ability to paste things with >>>
def notransform(line): return line
from IPython.core import inputsplitter
inputsplitter.transform_classic_prompt = notransform
```

For each exercise, fill in the function according to its docstring. Execute the cell to see if you did it right.

```
x, y, z = symbols('x y z')
```

## Derivatives

Compute the following

$$ \frac{d}{dx}\sin(x)e^x$$
$$ \frac{\partial}{\partial x}\sin(xy)e^x $$
$$ \frac{\partial^2}{\partial x\partial y}\sin(xy)e^x $$

```

```

```

```

```

```

Recall l'Hopital's rule, which states that if $$\lim_{x\to x_0}\frac{f(x)}{g(x)}$$ is $\frac{0}{0}$, $\frac{\infty}{\infty}$, or $-\frac{\infty}{\infty}$, then it is equal to $$\lim_{x\to x_0} \frac{f'(x)}{g'(x)}$$ (we will not consider other indeterminate forms here).

Write a function that computes $\lim_{x\to x_0}\frac{f(x)}{g(x)}$. Use the `fraction` function to get the numerator and denominator of an expression, for example

```
fraction(x/y)
```

    (x, y)

You may assume that the only indeterminate forms are the ones mentioned above, and that l'Hopital's rule will terminate after a finite number of steps. Do not use `limit` (use `subs`). Remember that after taking the derivatives, you will need to put the expression into the form $\frac{f(x)}{g(x)}$ before applying l'Hopital's rule again (what function did we learn that does this?).

```
@test
def lhopital(expr, x, x0):
    """
    Computes limit(expr, x, x0) using l'Hopital's rule.

    >>> lhopital(sin(x)/x, x, 0)
    1
    >>> lhopital(exp(x)/x**2, x, oo)
    oo
    >>> lhopital((x**2 - 2*x + 4)/(2 - x), x, 2)
    -oo
    >>> lhopital(x/(2 - x), x, 2)
    -oo
    >>> lhopital(cos(x), x, 0)
    1
    >>> lhopital((x + sin(x))/x, x, 0)
    2
    """

```

## Integrals

Recall that the mean value of a function on an interval $[a, b]$ can be computed by the formula

$$\frac{1}{b - a}\int_{a}^{b} f{\left (x \right )} dx. % Why doesn't \, work? $$

Write a function that computes the mean value of an expression on a given interval.

```
@test
def average_value(expr, x, a, b):
    """
    Computes the average value of expr with respect to x on [a, b].

    >>> average_value(sin(x), x, 0, pi)
    2/pi
    >>> average_value(x, x, 2, 4)
    3
    >>> average_value(x*y, x, 2, 4)
    3*y
    """

```

Write a function that takes a list of expressions and produces an "integral table", like

1. $$\int \sin(x)dx = -\cos(x) + C$$
2. $$\int \cos(x)dx = \sin(x) + C$$
3. $$\int e^xdx = e^x + C$$
4. $$\int \log(x)dx = x(\log(x) - 1) + C$$

```
@test
def int_table(exprs, x):
    """
    Produces a nice integral table of the integrals of exprs

    (note, we have to use pprint(use_unicode=False) because unicode is not supported
    by the IPython doctester)

    >>> int_table([sin(x), cos(x), exp(x), log(x)], x)
      /
     |
     | sin(x) dx = C - cos(x)
     |
    /
      /
     |
     | cos(x) dx = C + sin(x)
     |
    /
      /
     |
     |  x           x
     | e  dx = C + e
     |
    /
      /
     |
     | log(x) dx = C + x*log(x) - x
     |
    /
    """

```

Now use your function to compute the integrals in this Mathematica ad.  Remember that the inverse trig functions are spelled like `asin` in SymPy.

The ad below probably has a typo, because one of the integrals is trivial to compute. Include what you think the integral should be, and see if SymPy can compute that as well.

```
from IPython.core.display import Image
Image(filename='../imgs/Mathematica-ring-a.png')
```

```
# Write the answer here

```

## Limits

Recall that the definition of the derivative of $f(x)$ at $x=x_0$ is $$f'(x_0) = \lim_{x\to x_0}\frac{f(x) - f(x_0)}{x - x_0}.$$  Write a function that computes the derivative using the limit definition, using `limit`.

```
@test
def lim_deriv(expr, x, x0):
    """
    Computes the derivative of expr with respect to x at x0 using the limit definition.

    >>> lim_deriv(x**2, x, 0)
    0
    >>> lim_deriv(cos(x*y), x, pi)
    -y*sin(pi*y)

    Note that we must use this trick to take the derivative without evaluating at a point.
    >>> lim_deriv(exp(x**2), x, y).subs(y, x)
    2*x*exp(x**2)
    """

```

The function you wrote above to compute limits using l'Hopital's rule is very fragile. And even if you try to make it sophisticated, it will still be unable to compute many limits.  Try it on the following limits, and see what happens. Then try computing the same limits with `limit`.

1. $$\lim_{x\to 0}\frac{\log(x)}{x}$$
2. $$\lim_{x\to \infty}\frac{2^x}{3^x} \textbf{Warning: Be sure to save the notebook before you test this one, and be prepared to kill the kernel!}$$
3. $$\lim_{x\to \infty}x\sin{\left(\frac{1}{x}\right)}$$
4. $$\lim_{x\to 1}\arctan\left(\frac{1}{1 - x}\right)\; \text{Remember that $\arctan$ is called }\mathtt{atan}\text{ in SymPy}$$

```

```

```

```

```

```

```

```

```

```

```

```

```

```

```

```

```

```

```

```

## Series

The Fibonicci sequence is rexcursively defined by

$$F_0 = 0,$$
$$F_1 = 1,$$
$$F_n = F_{n - 1} + F_{n - 2}.$$

The first few vales are 0, 1, 1, 2, 3, 5, 8, 13, 21, …

The Fibonicci sequence has a generating function given by $$s(x) = \frac{x}{1 - x - x^2}$$ (see http://en.wikipedia.org/wiki/Fibonacci_number#Power_series for a derivation). What this means is that if we expand $s(x)$ as a power series, the coefficients are the Fibonicci numbers, $$s(x) = \sum_{n=0}^\infty F_nx^n$$

Write a function that uses series to compute the nth Fibonicci number.

Hint: `expr.coeff(x, n)` will give the coefficient of $x^n$ in an expression. For example

```
(1 + 2*x - x**2).coeff(x, 0)
```

    1

```
(1 + 2*x - x**2).coeff(x, 1)
```

    2

```
(1 + 2*x - x**2).coeff(x, 2)
```

    -1

```
@test
def fib(n):
    """
    Uses series expansion and a generating function to compute the nth Fibonnicci number.

    >>> fib(0)
    0
    >>> fib(4)
    3
    >>> fib(9)
    34
    """

```

Note: if you really want to compute Fibonicci numbers, there is a function in SymPy called `fibonicci` that can do this far more efficiently.

```
[fibonacci(i) for i in range(10)]
```

    [0, 1, 1, 2, 3, 5, 8, 13, 21, 34]

`series` is nice if you want a fixed power series, but what if you don't know ahead of time how many terms you want? For that, there is the `lseries` method, which returns a generator of series terms.  This is more efficient than recomputing the whole series again if you determine you need more terms. Here is an example usage (**Warning**: since series are in general infinite, `lseries` will return an infinite generator. Here we use `zip` to limit the number of terms).

```
for term, _ in zip(cos(x).lseries(x, 0), range(10)):
    print term
```

    1
    -x**2/2
    x**4/24
    -x**6/720
    x**8/40320
    -x**10/3628800
    x**12/479001600
    -x**14/87178291200
    x**16/20922789888000
    -x**18/6402373705728000

Write a function that computes the number of terms of a series expansion of a given function are needed to compute the given value near the given point within the given accuracy. For example, in the expansion of cos(x) near $\pi$, suppode we wish to compute $\pi + 1$. Let us see if terms up to $O(x^6)$ are sufficient (remember that due to a limitation in SymPy, when computing the series away from 0 with `series`, we have to use a shift)

```
a = cos(x).series(x, pi, 5).removeO().subs(x, x - pi).evalf(subs={x: pi + 1})
a
```

    -0.541666666666667

```
b = cos(pi + 1).evalf()
b
```

    -0.540302305868140

So the expansion is accurate up to two places after the decimal point, i.e., within `0.01`.

```
abs(a - b) < 0.01
```

    True

Note, with `lseries`, we do not need to worry about shifts

```
for term, _ in zip(cos(x).lseries(x, pi), range(10)):
    print term
```

    -1
    (x - pi)**2/2
    -(x - pi)**4/24
    (x - pi)**6/720
    -(x - pi)**8/40320
    (x - pi)**10/3628800
    -(x - pi)**12/479001600
    (x - pi)**14/87178291200
    -(x - pi)**16/20922789888000
    (x - pi)**18/6402373705728000

Hint: to get the exponent from a term like `3*(x - 1)**5`, use `as_coeff_exponent`.

```
(3*(x - 1)**5).as_coeff_exponent(x - 1)
```

    (3, 5)

```
@test
def series_accuracy(func, expansion_point, evaluation_point, accuracy, x):
    """
    Returns n such that series terms up to and including (x - expansion_point)**n
    (i.e., O((x - expansion_point)**(n + 1)) are needed to compute func at
    evaluation_point within the given accuracy.

    >>> series_accuracy(cos(x), pi, pi + 1, 0.01, x)
    4
    >>> series_accuracy(exp(x), 1, 10, 1, x)
    23
    """

```


# 2021-10-01 Runge-Kutta methods

## Last time

* Stability for advection-diffusion
* Cost scaling

## Today

* Accuracy and stability barriers
* Runge-Kutta methods

```julia
using Plots
using LinearAlgebra
using SparseArrays
using Zygote

function vander(x, k=nothing)
    if k === nothing
        k = length(x)
    end
    V = ones(length(x), k)
    for j = 2:k
        V[:, j] = V[:, j-1] .* x
    end
    V
end

function fdstencil(source, target, k)
    "kth derivative stencil from source to target"
    x = source .- target
    V = vander(x)
    rhs = zero(x)'
    rhs[k+1] = factorial(k)
    rhs / V
end

function newton(residual, jacobian, u0; maxits=20)
    u = u0
    uhist = [copy(u)]
    normhist = []
    for k in 1:maxits
        f = residual(u)
        push!(normhist, norm(f))
        J = jacobian(u)
        delta_u = - J \ f
        u += delta_u
        push!(uhist, copy(u))
    end
    uhist, normhist
end

function plot_stability(Rz, title; xlims=(-6, 2), ylims=(-4, 4))
    x = LinRange(xlims[1], xlims[2], 100)
    y = LinRange(ylims[1], ylims[2], 100)
    heatmap(x, y, (x, y) -> abs(Rz(x + 1im*y)), c=:bwr, clims=(0, 2), aspect_ratio=:equal, title=title)
end
Rz_theta(z, theta) = (1 + (1 - theta)*z) / (1 - theta*z)
plot_stability(z -> Rz_theta(z, .5), "Midpoint method")

function ode_theta_linear(A, u0; forcing=zero, tfinal=1, h=0.1, theta=.5)
    u = copy(u0)
    t = 0.
    thist = [t]
    uhist = [u0]
    while t < tfinal
        tnext = min(t+h, tfinal)
        h = tnext - t
        rhs = (I + h*(1-theta)*A) * u .+ h*forcing(t+h*theta)
        u = (I - h*theta*A) \ rhs
        t = tnext
        push!(thist, t)
        push!(uhist, u)
    end
    thist, hcat(uhist...)
end

function advdiff_matrix(n; kappa=1, wind=1, upwind=0.)
    dx = 2 / n
    rows = [1]
    cols = [1]
    vals = [0.]
    wrap(j) = (j + n - 1) % n + 1
    for i in 1:n
        append!(rows, [i, i, i])
        append!(cols, wrap.(i-1:i+1))
        diffuse = [-1, 2, -1] * kappa / dx^2
        advect_upwind = [-1, 1, 0] * wind / dx
        advect_center = [-1, 0, 1] * wind / 2dx
        stencil = -diffuse - upwind * advect_upwind - (1 - upwind) * advect_center
        append!(vals, stencil)
    end
    sparse(rows, cols, vals)
end
advdiff_matrix(5, kappa=.1)
```

    5×5 SparseMatrixCSC{Float64, Int64} with 15 stored entries:
     -1.25   -0.625    ⋅       ⋅      1.875
      1.875  -1.25   -0.625    ⋅       ⋅
       ⋅      1.875  -1.25   -0.625    ⋅
       ⋅       ⋅      1.875  -1.25   -0.625
     -0.625    ⋅       ⋅      1.875  -1.25

# Stiffness

> Stiff equations are problems for which explicit methods don't work. (Hairer and Wanner, 2002)

* "stiff" dates to Curtiss and Hirschfelder (1952)

```julia
k = 5
thist, uhist = ode_theta_linear(-k, [.2], forcing=t -> k*cos(t), tfinal=5, h=.5, theta=1)
scatter(thist, uhist[1,:])
plot!(cos)
```

```julia
function mms_error(h; theta=.5, k=10)
    u0 = [.2]
    thist, uhist = ode_theta_linear(-k, u0, forcing=t -> k*cos(t), tfinal=3, h=h, theta=theta)
    T = thist[end]
    u_exact = (u0 .- k^2/(k^2+1)) * exp(-k*T) .+ k*(sin(T) + k*cos(T))/(k^2 + 1)
    uhist[1,end] .- u_exact]
end
```

    mms_error (generic function with 1 method)

```julia
hs = .5 .^ (1:8)
errors = mms_error.(hs, theta=0, k=10)
plot(hs, norm.(errors), marker=:auto, xscale=:log10, yscale=:log10)
plot!(hs, hs, label="\$h\$", legend=:topleft)
plot!(hs, hs.^2, label="\$h^2\$")
```

## Discuss: is advection-diffusion stiff?

```julia
theta=0
n = 20
dx = 2 / n
kappa = .01
lambda_min = -4 * kappa / dx^2
cfl = 1
h = min(-2 / lambda_min, cfl * dx)

plot_stability(z -> Rz_theta(z, theta),
    "\$\\theta=$theta, h=$h, Pe_{\\Delta x} = $(dx/kappa)\$", xlims=(-2, 1), ylims=(-2, 2))
ev = eigvals(Matrix(h*advdiff_matrix(n, kappa=kappa, wind=1, upwind=0)))
scatter!(real(ev), imag(ev))
```

# Cost scaling

* Spatial discretization with error $O((\Delta x)^p)$
* Time discretization with error $O((\Delta t)^q)$

# Runge-Kutta methods

The methods we have considered thus far can all be expressed as Runge-Kutta methods, which are expressed in terms of $s$ "stage" equations (possibly coupled) and a completion formula.  For the ODE

$$ \dot u = f(t, u) $$

the Runge-Kutta method is

$$\begin{split}
Y_i = u(t) + h \sum_j a_{ij} f(t+c_j h, Y_j) \\
u(t+h) = u(t) + h \sum_j b_j f(t+c_j h, Y_j)
\end{split}$$
where $c$ is a vector of *abscissa*, $A$ is a table of coefficients, and $b$ is a vector of completion weights.

These coefficients are typically expressed in a Butcher Table
$$ \left[ \begin{array}{c|c} c & A \\ \hline & b^T \end{array} \right] = \left[ \begin{array}{c|cc}
c_0 & a_{00} & a_{01} \\
c_1 & a_{10} & a_{11} \\
\hline
& b_0 & b_1
\end{array} \right] . $$
We will see that, for consistency, the abscissa $c$ are always the row sums of $A$ and that $\sum_i b_i = 1$.

If the matrix $A$ is strictly lower triangular, then the method is **explicit** (does not require solving equations).

# Past methods as Runge-Kutta

* Forward Euler
$$ \left[ \begin{array}{c|cc}
0 & 0 \\
\hline
& 1
\end{array} \right] ,$$
* Backward Euler
$$ \left[ \begin{array}{c|c}
1 & 1 \\
\hline
& 1
\end{array} \right] ,$$
* Midpoint
$$ \left[ \begin{array}{c|c}
\frac 1 2 & \frac 1 2 \\
\hline
& 1
\end{array} \right]. $$

Indeed, the $\theta$ method is
$$ \left[ \begin{array}{c|c}
\theta & \theta \\
\hline
& 1
\end{array} \right] $$
and an alternative "endpoint" variant of $\theta$ (a generalization of the trapezoid rule) is
$$ \left[ \begin{array}{c|cc}
0 & 0 & 0 \\
1 & 1-\theta & \theta \\
\hline
& 1-\theta & \theta
\end{array} \right]. $$

# Stability function for Runge-Kutta

To develop an algebraic expression for stability in terms of the Butcher Table, we consider the test equation

$$ \dot u = \lambda u $$

and apply the RK method to yield

$$ \begin{split} Y_i = u(0) + h \sum_j a_{ij} \lambda Y_j \\
u(h) = u(0) + h \sum_j b_j \lambda Y_j \end{split} $$

or, in matrix form,

$$ \begin{split} Y = \mathbb 1 u(0) + h \lambda A Y \\
u(h) = u(0) + h \lambda b^T Y \end{split} $$

where $\mathbb 1$ is a column vector of length $s$ consisting of all ones.
This reduces to
$$ u(h) = \underbrace{\Big( 1 + h\lambda b^T (I - h \lambda A)^{-1} \mathbb 1 \Big)}_{R(h\lambda)} u(0) . $$

# Plotting stability

$$ u(h) = \underbrace{\Big( 1 + h\lambda b^T (I - h \lambda A)^{-1} \mathbb 1 \Big)}_{R(h\lambda)} u(0) $$

```julia
struct RKTable
    A::Matrix
    b::Vector
    c::Vector
    function RKTable(A, b)
        s = length(b)
        A = reshape(A, s, s)
        c = vec(sum(A, dims=2))
        new(A, b, c)
    end
end

function rk_stability(z, rk)
    s = length(rk.b)
    1 + z * rk.b' * ((I - z*rk.A) \ ones(s))
end
```

    rk_stability (generic function with 1 method)

```julia
theta = .5
rk_theta(theta) = RKTable([theta], [1])
rk_theta_endpoint(theta) = RKTable([0 0;1-theta theta], [1-theta, theta])
table = rk_theta(theta)
plot_stability(z -> rk_stability(z, table), "\$\\theta = $theta\$")
```

# Heun's method and RK4

```julia
heun = RKTable([0 0; 1 0], [.5, .5])
plot_stability(z -> rk_stability(z, heun), "Heun's method")
```

```julia
rk4 = RKTable([0 0 0 0; .5 0 0 0; 0 .5 0 0; 0 0 1 0],
[1, 2, 2, 1] / 6)
plot_stability(z -> rk_stability(z, rk4), "RK4")
```

# An explicit RK solver

\begin{align}
Y_i &= u(t) + h \sum_j a_{ij} f(t+c_j h, Y_j) &
u(t+h) &= u(t) + h \sum_j b_j f(t+c_j h, Y_j)
\end{align}

```julia
function ode_rk_explicit(f, u0; tfinal=1, h=0.1, table=rk4)
    u = copy(u0)
    t = 0.
    n, s = length(u), length(table.c)
    fY = zeros(n, s)
    thist = [t]
    uhist = [u0]
    while t < tfinal
        tnext = min(t+h, tfinal)
        h = tnext - t
        for i in 1:s
            ti = t + h * table.c[i]
            Yi = u + h * sum(fY[:,1:i-1] * table.A[i,1:i-1], dims=2)
            fY[:,i] = f(ti, Yi)
        end
        u += h * fY * table.b
        t = tnext
        push!(thist, t)
        push!(uhist, u)
    end
    thist, hcat(uhist...)
end
```

    ode_rk_explicit (generic function with 1 method)

```julia
linear_oscillator(t, u) = [0 -1; 1 0] * u
thist, uhist = ode_rk_explicit(linear_oscillator, [1., 0], tfinal=6, h=2)
plot(thist, uhist', marker=:auto)
plot!([sin, cos])
```

# Measuring convergence and accuracy

```julia
function mms_error(h, f, u_exact; table=rk4)
    u0 = u_exact(0)
    thist, uhist = ode_rk_explicit(f, u0, tfinal=3, h=h, table=table)
    T = thist[end]
    uhist[:,end] - u_exact(T)
end

hs = .5 .^ (-3:8)
linear_oscillator_exact(t) = [cos(t), sin(t)]
```

    linear_oscillator_exact (generic function with 1 method)

```julia
heun_errors = mms_error.(hs, linear_oscillator, linear_oscillator_exact, table=heun)
rk4_errors = mms_error.(hs, linear_oscillator, linear_oscillator_exact, table=rk4)
plot(hs, [norm.(heun_errors) norm.(rk4_errors)], label=["heun" "rk4"], marker=:auto)
plot!(hs, [hs hs.^2 hs.^3 hs.^4], label=["\$h^$p\$" for p in [1 2 3 4]], legend=:topleft, xscale=:log10, yscale=:log10)
```

# Work-precision (or accuracy vs cost)

```julia
heun_nfeval = length(heun.c) ./ hs
rk4_nfeval = length(rk4.c) ./ hs
plot(heun_nfeval, norm.(heun_errors), marker=:auto, label="heun")
plot!(rk4_nfeval, norm.(rk4_errors), marker=:auto, label="rk4")
plot!(xscale=:log10, yscale=:log10)
```

# Effective RK stability diagrams

```julia
function rk_eff_stability(z, rk)
    s = length(rk.b)
    z = ]
    1 + z * rk.b' * ((I - z*rk.A) \ ones(s))
end
plot_stability(z -> rk_eff_stability(z, heun), "Heun's method", xlims=(-2, 2), ylims=(-2, 2))
```

```julia
plot_stability(z -> rk_eff_stability(z, rk4), "RK4", xlims=(-2, 2), ylims=(-2, 2))
```

# Runge-Kutta order conditions

We consider the autonomous differential equation

$$ \dot u = f(u) . $$

Higher derivatives of the exact soultion can be computed using the chain rule, e.g.,

\begin{align*}
\ddot u(t) &= f'(u) \dot u = f'(u) f(u) \\
\dddot u(t) &= f''(u) f(u) f(u) + f'(u) f'(u) f(u) . \\
\end{align*}

Note that if $f(u)$ is linear, $f''(u) = 0$.
Meanwhile, the numerical solution is a function of the time step $h$,

$$\begin{split}
Y_i(h) &= u(0) + h \sum_j a_{ij} f(Y_j) \\
U(h) &= u(0) + h \sum_j b_j f(Y_j).
\end{split}$$

We will take the limit $h\to 0$ and equate derivatives of the numerical solution.  First we differentiate the stage equations,

\begin{split}
Y_i(0) &= u(0) \\
\dot Y_i(0) &= \sum_j a_{ij} f(Y_j) \\
\ddot Y_i(0) &= 2 \sum_j a_{ij} \dot f(Y_j) \\
&= 2 \sum_j a_{ij} f'(Y_j) \dot Y_j \\
&= 2\sum_{j,k} a_{ij} a_{jk} f'(Y_j) f(Y_k) \\
\dddot Y_i(0) &= 3 \sum_j a_{ij} \ddot f (Y_j) \\
&= 3 \sum_j a_{ij} \Big( \sum_k f''(Y_j) \dot Y_j \dot Y_k + f'(Y_j) \ddot Y_j \Big) \\
&= 3 \sum_{j,k,\ell} a_{ij} a_{jk} \Big( a_{j\ell} f''(Y_j) f(Y_k) f(Y_\ell) + 2 a_{k\ell} f'(Y_j) f'(Y_k) f(Y_\ell)  \Big)
\end{split}

where we have used Liebnitz's formula for the $m$th derivative,
$$ (h \phi(h))^{(m)}|_{h=0} = m \phi^{(m-1)}(0) .$$
Similar formulas apply for $\dot U(0)$, $\ddot U(0)$, and $\dddot U(0)$, with $b_j$ in place of $a_{ij}$.

# Order conditions are nonlinear algebraic equations

Equating terms $\dot u(0) = \dot U(0)$ yields
$$ \sum_j b_j = 1, $$
equating $\ddot u(0) = \ddot U(0)$ yields
$$ 2 \sum_{j,k} b_j a_{jk} = 1 , $$
and equating $\dddot u(0) = \dddot U(0)$ yields the two equations
\begin{split}
  3\sum_{j,k,\ell} b_j a_{jk} a_{j\ell} &= 1 \\
  6 \sum_{j,k,\ell} b_j a_{jk} a_{k\ell} &= 1 .
\end{split}

# Observations

* These are systems of nonlinear equations for the coefficients $a_{ij}$ and $b_j$.  There is no guarantee that they have solutions.
* The number of equations grows rapidly as the order increases.

|  | $u^{(1)}$       | $u^{(2)}$   |  $u^{(3)}$ | $u^{(4)}$ | $u^{(5)}$ | $u^{(6)}$ | $u^{(7)}$ | $u^{(8)}$ | $u^{(9)}$ | $u^{(10)}$ |
| ----------- |---|---|---|---|---|----|----|-----|-----|-----|
| # terms     | 1 | 1 | 2 | 4 | 9 | 20 | 48 | 115 | 286 | 719 |
| cumulative | 1 | 2 | 4 | 8 | 17 | 37 | 85 | 200 | 486 | 1205 |

* Usually the number of order conditions does not exactly match the number of free parameters, meaning that the remaining parameters can be optimized (usually numerically) for different purposes, such as to minimize the leading error terms or to maximize stability in certain regions of the complex plane.  Finding globally optimal solutions can be extremely demanding.

* Rooted trees provide a convenient notation

# Theorem (from Hairer, Nørsett, and Wanner)

A Runge-Kutta method is of order $p$ if and only if
$$ \gamma(\mathcal t) \sum_{j} b_j \Phi_j(t) = 1 $$
for all trees $t$ of order $\le p$.

For a linear autonomous equation
$$ \dot u = A u $$
we only need one additional order condition per order of accuracy because $f'' = 0$.
These conditions can also be derived by equating derivatives of the stability function $R(z)$ with the exponential $e^z$.
For a linear non-autonomous equation
$$ \dot u = A(t) u + g(t) $$
or more generally, an autonomous system with quadratic right hand side,
$$ \dot u = B (u \otimes u) + A u + C $$
where $B$ is a rank 3 tensor, we have $f''' = 0$, thus limiting the number of order conditions.


## Segmentation of Signals

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).

This notebook illustrates the segmentation of signals using analytic signals and by varying the parameters of the segmentation. First the function `segments(signal, segment_size=32, hop_size=16, window=None)` is defined which segments a signal $x[k]$ into potentially overlapping and windowed segments $x_n[k]$ whereby

\begin{equation}
x_n[k] = w[k] \cdot x[k + nP] \qquad \text{for } k = 0,1, \dots, F-1
\end{equation}

with the segment length $F$, the hop size $P$ and the window function $w[k]$. The segments are returned as a matrix whereby the first dimension represents the segment index $n$.

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

```python
def segments(signal, segment_size=32, hop_size=16, window=None):
    '''Split signal into overlapping windowed segments.'''

    # calculate total number of segments
    nseg = (len(signal) - segment_size) // hop_size + 1
    # split signal into segments
    segments = [signal[n*hop_size:n*hop_size+segment_size]
                for n in range(nseg)]
    # window segments
    if window is not None:
        segments = window(segment_size).reshape(1, -1) * segments

    return np.array(segments)
```

### Non-Overlapping Segments

As initial example, the segmentation of a cosine signal $x[k] = \cos(\Omega_0 k)$ with a period of $N_0 = 32$ samples is considered $\Omega_0 = \frac{2 \pi}{N_0}$. The segmentation is performed with non-overlapping segments of length $F = 32$ and a rectangular window function $w[k] = \text{rect}_{F}[k]$. For non-overlapping consecutive segments the hop size is chosen as $P = F$.

```python
k = np.arange(128)
x = np.cos(k * 2*np.pi/32)

xn1 = segments(x, segment_size=32, hop_size=32)
```

```python
def plot_signal(x, ax, title):
    ax.stem(x)
    ax.set_xlabel(r'$k$')
    ax.set_title(title)
    ax.set_xlim([0, len(x)-1])
    ax.grid()

fig = plt.figure(figsize=(8, 8), constrained_layout=True)
gs = fig.add_gridspec(4, 3)

ax = fig.add_subplot(gs[0, :])
plot_signal(x[0:3*32], ax, r'signal $x[k]$')

for n in range(3):
    ax = fig.add_subplot(gs[n+1, n])
    plot_signal(xn1[n, :], ax, r'segment $x_{}[k]$'.format(n))
```

From non-overlapping consecutive segments $x_n[k]$ the original signal can be recovered by concatenating the segments

\begin{equation}
x_\text{r}[k] = \sum_{n=0}^{N-1} x_n[k - nP]
\end{equation}

```python
xr = np.hstack(xn1)

fig, ax = plt.subplots(figsize=(10, 2))
plot_signal(xr[0:3*32], ax, r'recovered signal $x_\mathrm{r}[k]$')
```

### Overlapping Segments

Now the case of overlapping segments with the same segment length $F = 32$ as before but an hop size of $P=16$ samples is chosen. This results in an overlap of $16$ samples or $50$% of the samples per segment.

```python
xn2 = segments(x, segment_size=32, hop_size=16)
```

```python
fig = plt.figure(figsize=(8, 10), constrained_layout=True)
gs = fig.add_gridspec(5, 6)

ax = fig.add_subplot(gs[0, :])
plot_signal(x[0:3*32], ax, r'signal $x[k]$')

for n in range(4):
    ax = fig.add_subplot(gs[n+1, n:n+2])
    plot_signal(xn2[n, :], ax, r'segment $x_{}[k]$'.format(n))
```

### Windowed Overlapping Segments

Finally, the case of overlapping segments is considered using the same parameters as before, but now the segments are additionally windowed by an [Hamming window](https://en.wikipedia.org/wiki/Window_function#Hann_and_Hamming_windows).

```python
xn3 = segments(x, segment_size=32, hop_size=16, window=np.hamming)
```

```python
fig = plt.figure(figsize=(8, 10), constrained_layout=True)
gs = fig.add_gridspec(5, 6)

ax = fig.add_subplot(gs[0, :])
plot_signal(x[0:3*32], ax, r'signal $x[k]$')

for n in range(4):
    ax = fig.add_subplot(gs[n+1, n:n+2])
    plot_signal(xn3[n, :], ax, r'segment $x_{}[k]$'.format(n))
```

### Alternative Implementation

As segmentation is a common operation applied to signals, its implementation is available in various toolkits. For instance in the audio and music processing toolkit [`librosa`](https://librosa.org/). Here the function [`librosa.util.frame`](https://librosa.org/doc/main/generated/librosa.util.frame.html) splits a given array (signal) into overlapping segments (*frames*). For instance for the above example of overlapping segments without windowing

```python
from librosa.util import frame

xn4 = frame(x, frame_length=32, hop_length=16)
```

Note, the segments are returned as a matrix whereby the second dimension represents the segment index $n$.

```python
fig = plt.figure(figsize=(8,10), constrained_layout=True)
gs = fig.add_gridspec(5, 6)

ax = fig.add_subplot(gs[0,:])
plot_signal(x[0:3*32], ax, r'signal $x[k]$')

for n in range(4):
    ax = fig.add_subplot(gs[n+1,n:n+2])
    plot_signal(xn4[:,n], ax, r'segment $x_{}[k]$'.format(n))
```

**Copyright**

This notebook is provided as [Open Educational Resource](https://de.wikipedia.org/wiki/Open_Educational_Resources).
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, Data driven audio signal processing - Lecture supplementals.


# Introduction

In this project, we will investigate adversarial perturbation attacks using the Fast Gradient Signed Method (FGSM) on Deep Neural Networks with the MNIST digits dataset as a benchmark. FGSM attack was one of the first and most popular attacks to fool a neural network. We will then evaluate adversarial retraining as a defense against adversarial perturbations.

## What is Adversarial Attacks?

Adversarial input attacks can be broadly classified into two types, one is non-targeted attack and the other is targeted attack.

* Non-targeted Attack: Aiming to fool the neural network and output a label different than the original one.
* Targeted Attack: Intentionally misleading the network to output a specific label designed by the attacker.

For example, a face recognition system for security entrance control:
* Non-targeted attacks should lead to denial of legal access.
* Targeted attacks bring the jeopardy of illegal entrance.

To fool the neural network, we need a special kind of input data called adversarial examples.

## What is Adversarial Example?

Adversarial examples are specialised inputs created with the purpose of confusing a neural network, resulting in the misclassification of a given input. These notorious inputs are indistinguishable to the human eye, but cause the network to fail to identify the contents of the image. There are several types of such attacks, however, here the focus is on the fast gradient sign method attack, which is a white box attack whose goal is to ensure misclassification. A white box attack is where the attacker has complete access to the model being attacked. One of the most famous examples of an adversarial image shown below is taken from the aforementioned paper.

Here, starting with the image of a panda, the attacker adds small perturbations (distortions) to the original image, which results in the model labelling this image as a gibbon, with high confidence. The process of adding these perturbations is FGSM, which will be explained below.

## Fast Gradient Sign Method

The fast gradient sign method works by using the gradients of the neural network to create an adversarial example. For an input image, the method uses the gradients of the loss with respect to the input image to create a new image that maximises the loss. This new image is called the adversarial image. This can be summarised using the following expression:

**Non-targeted Attack**:

\begin{equation}
adv\_x = clip(x + \epsilon * sign(\nabla_xJ(\theta, x, y^*))) \tag{0}
\end{equation}

where
* $adv\_x$ : Adversarial image.
* $x$ : Original input image.
* $y^*$ : Original input label (true label)
* $\epsilon$ : Multiplier to ensure the perturbations are small.
* $\theta$ : Model parameters.
* $J$ : Loss.

**Targeted Attack**:

\begin{equation}
adv\_x = clip(x - \epsilon * sign(\nabla_xJ(\theta, x, y')))  \tag{1}
\end{equation}

where
* $adv\_x$ : Adversarial image.
* $x$ : Original input image.
* $y'$ : Target label (fake label)
* $\epsilon$ : Multiplier to ensure the perturbations are small.
* $\theta$ : Model parameters.
* $J$ : Loss.

An intriguing property here, is the fact that the gradients are taken with respect to the input image. This is done because the objective is to create an image that maximises the loss. A method to accomplish this is to find how much each pixel in the image contributes to the loss value, and add a perturbation accordingly. This works pretty fast because it is easy to find how each input pixel contributes to the loss by using the chain rule and finding the required gradients. Hence, the gradients are taken with respect to the image. In addition, since the model is no longer being trained (thus the gradient is not taken with respect to the trainable variables, i.e., the model parameters), and so the model parameters remain constant. The only goal is to fool an already trained model.

In the experiments section, we will try and fool a pretrained model, the model is a 2-layer DNN, pretrained on MNIST.

# Dataset

The MNIST dataset will be used in this project. The MNIST dataset is a commonly used 'toy' benchmarks for machine learning. It contains a train set of 60000 28x28 grayscale images of hand-drawn digits from 0-9, along with the associated labels. Also, it contains a test set of 10000 images. The dataset is available as part of the tensorflow package, which we will be using extensively in this project. For more details, please see the [link](https://colab.research.google.com/github/tensorflow/docs/blob/master/site/en/tutorials/quickstart/beginner.ipynb#scrollTo=7NAbSZiaoJ4z) of how to use MNIST dataset.

Load and prepare the [MNIST dataset](http://yann.lecun.com/exdb/mnist/). Convert the samples from integers to floating-point numbers:

```
import tensorflow as tf
import numpy as np

mnist = tf.keras.datasets.mnist

(x_train, y_train), (x_test, y_test) = mnist.load_data()
x_train, x_test = x_train / 255.0, x_test / 255.0
```

```
print("trainset shape: {}".format(x_train.shape))
print("testset shape: {}".format(x_test.shape))
```

    trainset shape: (60000, 28, 28)
    testset shape: (10000, 28, 28)

# Experiments

## Pretrain DNN on MNIST

The sample Google Colab [notebook](https://colab.research.google.com/github/tensorflow/docs/blob/master/site/en/tutorials/quickstart/beginner.ipynb#scrollTo=7NAbSZiaoJ4z)
that implements a 2-layer DNN for MNIST digit classification. The DNN has a 784 (28x28) dimensional input, a 10-dimensional output (prediction probabilities for each of the 10 classes) and one hidden layer with 300 hidden neurons and ReLU activations. We will implement my attacks and defenses on this **baseline DNN**.

Let's build this baseline DNN. Build the `tf.keras.Sequential` model by stacking layers. Choose an optimizer and loss function for training:

```
baseline_dnn = tf.keras.models.Sequential([
  tf.keras.layers.Flatten(input_shape=(28, 28)),
  tf.keras.layers.Dense(128, activation='relu'),
  tf.keras.layers.Dropout(0.2),
  tf.keras.layers.Dense(10)
])
```

Use SparseCategoricalCrossentropy loss function. The `losses.SparseCategoricalCrossentropy` loss takes a vector of logits and a `True` index and returns a scalar loss for each example. This loss is equal to the negative log probability of the true class:
It is zero if the model is sure of the correct class.

```
loss_fn = tf.keras.losses.SparseCategoricalCrossentropy(from_logits=True)
```

Train the baseline 2-layer DNN model.

```
baseline_dnn.compile(optimizer='adam',
              loss=loss_fn,
              metrics=['accuracy'])

baseline_dnn.fit(x_train, y_train, epochs=5)
```

    Epoch 1/5
    1875/1875 [==============================] - 3s 2ms/step - loss: 0.2943 - accuracy: 0.9143
    Epoch 2/5
    1875/1875 [==============================] - 3s 2ms/step - loss: 0.1416 - accuracy: 0.9580
    Epoch 3/5
    1875/1875 [==============================] - 3s 2ms/step - loss: 0.1080 - accuracy: 0.9669
    Epoch 4/5
    1875/1875 [==============================] - 3s 2ms/step - loss: 0.0885 - accuracy: 0.9727
    Epoch 5/5
    1875/1875 [==============================] - 4s 2ms/step - loss: 0.0757 - accuracy: 0.9756

    <tensorflow.python.keras.callbacks.History at 0x7fe3ba4f9588>

Evaluate the baseline model.

```
baseline_dnn.evaluate(x_test,  y_test, verbose=2)
baseline_dnn.trainable = False
```

    313/313 - 0s - loss: 0.0703 - accuracy: 0.9799

The baseline image classifier is now trained to ~98% accuracy on MNIST dataset.

Since it is more intuitive for the baseline model to return a probability, we can wrap the trained model, and attach the softmax to it:

```
probability_baseline = tf.keras.Sequential([
  baseline_dnn,
  tf.keras.layers.Softmax()
])
```

```
# image: shape(1, 28, 28)
def baseline_model_predict_class(image):
  predition_prob = probability_baseline(image)
  prediction_class = tf.argmax(predition_prob, axis=1)
  return int(prediction_class)
```

Test baseline model on first 5 testcases in test set. We can see that all 5 images are classified correctly.

```
print('Baseline model prediction probability of first 5 testcases in test set:')
predition_prob = probability_baseline(x_test[:5])
print(predition_prob)

print('\nBaseline model prediction class of first 5 testcases in test set:')
prediction_class = tf.argmax(predition_prob, axis=1)
print(prediction_class)

for i in range(5):
  input_image = x_test[i:i+1]
  print("image_id: {} predict_class: {}".format(i, baseline_model_predict_class(input_image)))

print('\nTrue label of first 5 testcases:')
print(y_test[:5])
```

    Baseline model prediction probability of first 5 testcases in test set:
    tf.Tensor(
    [[3.9403730e-08 2.2203761e-08 1.3829144e-05 1.5300075e-03 2.9337849e-10
      3.1156193e-07 2.8648677e-14 9.9841511e-01 7.3573227e-07 3.9881008e-05]
     [9.0605084e-07 5.9127906e-05 9.9992740e-01 4.2145507e-06 1.5287710e-13
      8.8521330e-07 2.0462313e-07 1.4191555e-10 7.3120846e-06 1.6394674e-11]
     [3.9571495e-07 9.9873155e-01 2.0955322e-05 1.5677875e-05 1.8973144e-05
      6.1774854e-06 2.4941428e-06 1.1058919e-03 9.5100237e-05 2.7351059e-06]
     [9.9996150e-01 4.9886393e-09 8.2489887e-06 1.8178936e-08 9.8998150e-08
      2.5708783e-08 2.1443459e-05 5.9305075e-06 2.7853203e-10 2.7225194e-06]
     [1.8438075e-05 1.9627441e-10 4.8314578e-06 8.9459702e-09 9.8946804e-01
      1.3617678e-05 1.9838733e-07 3.8371782e-05 1.9332513e-06 1.0454663e-02]], shape=(5, 10), dtype=float32)

    Baseline model prediction class of first 5 testcases in test set:
    tf.Tensor([7 2 1 0 4], shape=(5,), dtype=int64)
    image_id: 0 predict_class: 7
    image_id: 1 predict_class: 2
    image_id: 2 predict_class: 1
    image_id: 3 predict_class: 0
    image_id: 4 predict_class: 4

    True label of first 5 testcases:
    [7 2 1 0 4]

## Implementing Fast Gradient Sign Method

Let's use a sample image from MNIST and create adversarial examples from it by FGSM. Here, I use the first image in train set.

```
# Helper function to preprocess the image
def preprocess(image):
  image = tf.cast(image, tf.float32)
  return image

# Helper function to get a 10-dim vector true label so that it can be inputted in loss function
def get_mnist_label(label):
  l = np.zeros((1, 10), dtype=float)
  l[0][label] = 1
  l = tf.convert_to_tensor(l)
  return l

print(get_mnist_label(5))
```

    tf.Tensor([[0. 0. 0. 0. 0. 1. 0. 0. 0. 0.]], shape=(1, 10), dtype=float64)

```
import matplotlib.pyplot as plt

image_index = 0
image = x_train[image_index]  # shape (28, 28)
plt.gray()
plt.title('ture label: {}'.format(y_train[image_index]))
plt.imshow(image)
```

To implement fast gradient sign method, the first step is to create perturbations which will be used to distort the original image resulting in an adversarial image. The gradients are taken with respect to the image.

```
cce_loss = tf.keras.losses.CategoricalCrossentropy()

def create_adversarial_pattern(input_image, input_label):
  with tf.GradientTape() as tape:
    tape.watch(input_image)
    prediction = probability_baseline(input_image)
    true_label = get_mnist_label(input_label)
    loss = cce_loss(true_label, prediction)

  # Get the gradients of the loss w.r.t to the input image.
  gradient = tape.gradient(loss, input_image)
  # Get the sign of the gradients to create the perturbation
  signed_grad = tf.sign(gradient)
  return signed_grad
```

The resulting perturbations can also be visualised.

```
image_index = 0
image = x_train[image_index:image_index+1]  # shape (1, 28, 28)
image = preprocess(image)                   # shape (1, 28, 28)
label = y_train[image_index]

perturbations = create_adversarial_pattern(image, label)  # shape (1, 28, 28)
plt.imshow(perturbations[0])
```

Let's try this out for different values of epsilon and observe the resultant image attacked by FGSM based non-targeted attack. You'll notice that as the value of epsilon is increased, it becomes easier to fool the network. However, this comes as a trade-off which results in the perturbations becoming more identifiable.

```
# image: shape(1, 28, 28)
def display_images(image, description):
  plt.figure()
  plt.gray()
  plt.title(description)
  plt.imshow(image[0])
  plt.show()
```

To ensure the perturbations are small, we use $\epsilon$ as a multiplier ranged from 0.01 to 0.5 .

```
epsilons = [0.01, 0.05, 0.1, 0.2, 0.3, 0.4, 0.5]

for i, eps in enumerate(epsilons):
  adv_x = image + eps * perturbations
  adv_x = tf.clip_by_value(adv_x, 0, 1) # images are normalized to be in [0,1]
  description = "Epsilon: {}".format(epsilons[i])
  display_images(adv_x, description)
```

## FGSM based Untargeted Attacks

Step 1, our first goal to implement FGSM based untargeted attacks using images from the test set on the baseline DNN. That is, our goal is to adversarially perturb each image in the **test set** using the following values of parameter $\epsilon = \{0.01, 0.05, 0.1, 0.2, 0.3, 0.4, 0.5\}$. Report the success rate of our attack, i.e., the fraction of test images that were correctly classified by the baseline DNN that are mis-classified after adversarial perturbation, as a function of $\epsilon$.

Implement FGSM based untargeted attacks according to [equation (0)](https://colab.research.google.com/drive/1hQTZLZ0KXHfWh0X2Ry-7BF8E5Wx7J5x0#scrollTo=XkzFmYcE4mYn&line=22&uniqifier=1).

```
# image: shape(1, 28, 28)
# label: int, 0~9
# adv_x: shape(1, 28, 28)
def fgsm_untargeted(image, true_label, eps=0.1):
  perturbations = create_adversarial_pattern(image, true_label)
  adv_x = image + eps * perturbations
  adv_x = tf.clip_by_value(adv_x, 0, 1)
  return adv_x
```

Adversarially perturb each image in the test set by FGSM based untargeted perturbations.. And calculate the success rate of FGSM based untargeted attack, i.e., the fraction of test images that were correctly classified by the baseline DNN that are mis-classified after adversarial perturbation.

```
def fgsm_untargeted_attack(epsilon):
  cnt_correct_classified_iamge = 0.0
  cnt_mis_classified_image = 0.0
  for i in range(x_test.shape[0]):
    image = x_test[i:i+1]
    before_predict_label = baseline_model_predict_class(image)
    image = preprocess(image)
    true_label = y_test[i]
    adv_x = fgsm_untargeted(image, true_label, eps=epsilon)  # shape (1, 28, 28)
    after_predict_label = baseline_model_predict_class(adv_x)

    if before_predict_label == true_label:
      cnt_correct_classified_iamge += 1.0
      if after_predict_label != true_label:
        cnt_mis_classified_image += 1.0

  attack_success_rate = cnt_mis_classified_image / cnt_correct_classified_iamge
  return cnt_correct_classified_iamge, cnt_mis_classified_image, attack_success_rate

untargeted_attack_success_rates = []
for eps in epsilons:
  cnt_correct_classified_iamge, cnt_mis_classified_image, attack_success_rate = fgsm_untargeted_attack(eps)
  print("Epsilon: {}\nNumber of correctly classified images: {}\nNumber of mis-classified images after attack: {}\nAttack Success Rate: {}\n".format(eps, cnt_correct_classified_iamge, cnt_mis_classified_image, attack_success_rate))
  untargeted_attack_success_rates.append(attack_success_rate * 100)
```

    Epsilon: 0.01
    Number of correctly classified images: 9799.0
    Number of mis-classified images after attack: 188.0
    Attack Success Rate: 0.019185631186855803

    Epsilon: 0.05
    Number of correctly classified images: 9799.0
    Number of mis-classified images after attack: 2663.0
    Attack Success Rate: 0.27176242473721807

    Epsilon: 0.1
    Number of correctly classified images: 9799.0
    Number of mis-classified images after attack: 7459.0
    Attack Success Rate: 0.7612001224614756

    Epsilon: 0.2
    Number of correctly classified images: 9799.0
    Number of mis-classified images after attack: 9499.0
    Attack Success Rate: 0.9693846310848045

    Epsilon: 0.3
    Number of correctly classified images: 9799.0
    Number of mis-classified images after attack: 9582.0
    Attack Success Rate: 0.977854883151342

    Epsilon: 0.4
    Number of correctly classified images: 9799.0
    Number of mis-classified images after attack: 9582.0
    Attack Success Rate: 0.977854883151342

    Epsilon: 0.5
    Number of correctly classified images: 9799.0
    Number of mis-classified images after attack: 9582.0
    Attack Success Rate: 0.977854883151342

Draw a line graph of epsilon versus attack success rate.

```
def plot_attack_success_rate(epsilons, attack_success_rates, title):
  assert len(epsilons) == len(attack_success_rates), 'Please check the length of attack_success_rates!'

  plt.figure()
  plt.plot(epsilons, attack_success_rates, marker='o')
  plt.xlabel("Epsilon")
  plt.ylabel("Attack Success Rate (%)")

  attack_success_rates = [float("{:.2f}".format(rate)) for rate in attack_success_rates]

  for a, b in zip(epsilons, attack_success_rates):
    plt.text(a, b, b, ha='center', va='bottom', fontsize=10)

  plt.title(title)
  plt.show()

title = "FGSM Untargeted Attack"
plot_attack_success_rate(epsilons, untargeted_attack_success_rates, title)
```

We can notice that as the value of epsilon is increased, the attack success rate is increased, it becomes easier to fool the network.

## FGSM based Targeted Attacks

Step 2, we will repeat Step 1 above, except this time perform *targeted* attacks where digit $i$ is classified as $(i+1)\%10$ on the baseline DNN. (Here, $i$ refers to the true ground-truth label of the test images, and we can assume that the attacker has access to these labels). Test image with label $i$ will be perturbed with label $(i+1)\%10$.

As before, use the following values of the parameter $\epsilon = \{0.01, 0.05, 0.1, 0.2, 0.3, 0.4, 0.5\}$. Report the attack’s success rate as a function of parameter $\epsilon$ where success rate is defined as the fraction of test images that were that were correctly classified by the baseline DNN that are mis-classified after adversarial perturbations with label $(i+1)\%10$.

Implement FGSM based targeted attacks according to [equation (1)](https://colab.research.google.com/drive/1hQTZLZ0KXHfWh0X2Ry-7BF8E5Wx7J5x0#scrollTo=XkzFmYcE4mYn&line=22&uniqifier=1).

```
# image: shape(1, 28, 28)
# label: int, 0~9
# adv_x: shape(1, 28, 28)
def fgsm_targeted(image, fake_label, eps=0.1):
  perturbations = create_adversarial_pattern(image, fake_label)
  adv_x = image - eps * perturbations
  adv_x = tf.clip_by_value(adv_x, 0, 1)
  return adv_x
```

Adversarially perturb each image in the test set by FGSM based targeted perturbations. And calculate the success rate of FGSM based untargeted attack, i.e., the fraction of test images that were correctly classified by the baseline DNN that are mis-classified after adversarial perturbation.

```
def fgsm_targeted_attack(epsilon):
  cnt_correct_classified_iamge = 0.0
  cnt_mis_classified_image = 0.0
  for i in range(x_test.shape[0]):
    image = x_test[i:i+1]
    before_predict_label = baseline_model_predict_class(image)
    image = preprocess(image)
    true_label = y_test[i]
    fake_label = (true_label + 1) % 10
    adv_x = fgsm_targeted(image, fake_label, eps=epsilon)  # shape (1, 28, 28)
    after_predict_label = baseline_model_predict_class(adv_x)

    if before_predict_label == true_label:
      cnt_correct_classified_iamge += 1.0
      if after_predict_label != true_label and after_predict_label == fake_label:
        cnt_mis_classified_image += 1.0

  attack_success_rate = cnt_mis_classified_image / cnt_correct_classified_iamge
  return cnt_correct_classified_iamge, cnt_mis_classified_image, attack_success_rate

targeted_attack_success_rates = []
for eps in epsilons:
  cnt_correct_classified_iamge, cnt_mis_classified_image, attack_success_rate = fgsm_targeted_attack(eps)
  print("Epsilon: {}\nNumber of correctly classified images: {}\nNumber of mis-classified images after attack: {}\nAttack Success Rate: {}\n".format(eps, cnt_correct_classified_iamge, cnt_mis_classified_image, attack_success_rate))
  targeted_attack_success_rates.append(attack_success_rate * 100)

title = "FGSM Targeted Attack"
plot_attack_success_rate(epsilons, targeted_attack_success_rates, title)

```

We can notice that as the value of epsilon is increased, the attack success rate goes up first and then goes down slowly.

## Adversarial Retraining against Untargeted FGSM Attacks

Step 3, we can assume $\epsilon = 0.1$ throughout. To defend against adversarial perturbations, the defender adversarially perturbs each image in her **training set** using the attacker’s strategy in Step 1. She then appends the adversarially perturbed images to her training set, but using their *correct* labels. Then, the defender retrains the baseline DNN with a new training dataset containing both images from the original training dataset and the new adversarially perturbed images. We call the new DNN the **adversarially retrained DNN**.

* Report the classification accuracy of the adversarially retrained DNN on the original test dataset that contains only clean inputs.
* Is the adversarially retrained DNN robust against adversarial perturbations? Implement FGSM based untargeted attacks using images from the clean test set on the adversarially retrained DNN. Report the success rate of your attack.

As a defender now, adversarially perturbs each image in the training set using the attacker's strategy in Step 1 and append the adversarially perturbed images to her training set with their true labels.

```
epsilon = 0.1
delta_x_train = np.zeros(x_train.shape)

for i in range(x_train.shape[0]):
  image = x_train[i:i+1]    # shape (1, 28, 28)
  image = preprocess(image)
  true_label = y_train[i]
  adv_x = fgsm_untargeted(image, true_label, eps=epsilon)  # shape (1, 28, 28)
  delta_x_train[i] = adv_x[0]

new_x_train = np.append(x_train, delta_x_train, axis=0)
new_y_train = np.append(y_train, y_train, axis=0)
assert new_x_train.shape[0] == 120000, "Please make sure new train set has 120000 samples!"
assert new_y_train.shape[0] == 120000, "Please make sure new train set has 120000 samples!"

print("new train set images shape: {}".format(new_x_train.shape))
print("new train set labels shape: {}".format(new_y_train.shape))
```

    new train set images shape: (120000, 28, 28)
    new train set labels shape: (120000,)

Retrain the baseline DNN with new training set containing both images from the original training dataset and the new adversarially perturbed images.

```
adv_retrained_dnn = tf.keras.models.Sequential([
  tf.keras.layers.Flatten(input_shape=(28, 28)),
  tf.keras.layers.Dense(128, activation='relu'),
  tf.keras.layers.Dropout(0.2),
  tf.keras.layers.Dense(10)
])

loss_fn = tf.keras.losses.SparseCategoricalCrossentropy(from_logits=True)

adv_retrained_dnn.compile(optimizer='adam',
              loss=loss_fn,
              metrics=['accuracy'])

adv_retrained_dnn.fit(new_x_train, new_y_train, epochs=5)
```

    Epoch 1/5
    3750/3750 [==============================] - 7s 2ms/step - loss: 0.1904 - accuracy: 0.9448
    Epoch 2/5
    3750/3750 [==============================] - 7s 2ms/step - loss: 0.0779 - accuracy: 0.9766
    Epoch 3/5
    3750/3750 [==============================] - 7s 2ms/step - loss: 0.0573 - accuracy: 0.9820
    Epoch 4/5
    3750/3750 [==============================] - 7s 2ms/step - loss: 0.0461 - accuracy: 0.9850
    Epoch 5/5
    3750/3750 [==============================] - 7s 2ms/step - loss: 0.0422 - accuracy: 0.9865

    <tensorflow.python.keras.callbacks.History at 0x7fe3ba4374e0>

Evaluate the classification accuracy of the adversarially retrained DNN on the original test dataset that contains only clean inputs.

```
adv_retrained_dnn.evaluate(x_test, y_test, verbose=2)
```

    313/313 - 1s - loss: 0.0754 - accuracy: 0.9782

    [0.07536563277244568, 0.9782000184059143]

Since it is more intuitive for the adversarially retrained DNN model to return a probability, we can wrap the retrained model, and attach the softmax to it:

```
probability_adv_retrained_dnn = tf.keras.Sequential([
  adv_retrained_dnn,
  tf.keras.layers.Softmax()
])
```

```
# image: shape(1, 28, 28)
def adv_retrained_dnn_predict_class(image):
  predition_prob = adv_retrained_dnn(image)
  prediction_class = tf.argmax(predition_prob, axis=1)
  return int(prediction_class)
```

Test adversarially retrained DNN model on first 5 testcases in test set. We can see that all 5 test images are classified correctly.

```
for i in range(5):
  input_image = x_test[i:i+1]
  print("image_id: {} predict_class: {} true_label: {}".format(i, adv_retrained_dnn_predict_class(input_image), y_test[i]))
```

    image_id: 0 predict_class: 7 true_label: 7
    image_id: 1 predict_class: 2 true_label: 2
    image_id: 2 predict_class: 1 true_label: 1
    image_id: 3 predict_class: 0 true_label: 0
    image_id: 4 predict_class: 4 true_label: 4

Let's see if the adversarially retrained DNN can resist attack from adversarial perturbations. Implement FGSM based untargeted attacks using images from the clean test set on the adversarially retrained DNN. Calculate the success rate of the attack.

```
def fgsm_untargeted_attack_retrained(epsilon):
  cnt_correct_classified_iamge = 0.0
  cnt_mis_classified_image = 0.0
  for i in range(x_test.shape[0]):
    image = x_test[i:i+1]
    before_predict_label = adv_retrained_dnn_predict_class(image)
    image = preprocess(image)
    true_label = y_test[i]
    adv_x = fgsm_untargeted(image, true_label, eps=epsilon)  # shape (1, 28, 28)
    after_predict_label = adv_retrained_dnn_predict_class(adv_x)

    if before_predict_label == true_label:
      cnt_correct_classified_iamge += 1.0
      if after_predict_label != true_label:
        cnt_mis_classified_image += 1.0

  attack_success_rate = cnt_mis_classified_image / cnt_correct_classified_iamge
  return cnt_correct_classified_iamge, cnt_mis_classified_image, attack_success_rate

untargeted_attack_success_rates_retrained = []
for eps in epsilons:
  cnt_correct_classified_iamge, cnt_mis_classified_image, attack_success_rate = fgsm_untargeted_attack_retrained(eps)
  print("Epsilon: {}\nNumber of correctly classified images: {}\nNumber of mis-classified images after attack: {}\nAttack Success Rate: {}\n".format(eps, cnt_correct_classified_iamge, cnt_mis_classified_image, attack_success_rate))
  untargeted_attack_success_rates_retrained.append(attack_success_rate * 100)

title = "FGSM Untargeted Attacks on Adversarially Retrained DNN"
plot_attack_success_rate(epsilons, untargeted_attack_success_rates_retrained, title)
```

We can notice that as the value of epsilon is increased, the attack success rate remains very low (under 4.2%). When $\epsilon = \{0.01, 0.05, 0.1\}$, the number of mis-classified images is 0, which means the adversarial attack didn't fool the dnn at all. The adversarially retrained DNN is robust against adversarial perturbations.

# References
* Goodfellow I J, Shlens J, Szegedy C. Explaining and harnessing adversarial examples[J]. arXiv preprint arXiv:1412.6572, 2014.
* https://www.tensorflow.org/tutorials/generative/adversarial_fgsm
* https://github.com/tensorflow/cleverhans
* https://harvard-iacs.github.io/2019-CS109B/lecture/lab21/AdversarialNN/


# Control Theory Tutorial  (DRAFT, NOT APPROVED YET)
## The Cart-Pole System - Python for System Analysis

## 1. Introduction
The goal of this tutorial is to teach the usage of the programming language Python as a
tool for developing and simulating control systems. The following topics are covered:

- Derivation of the equations of motion through Lagrangian mechanics and scientific computing
- Linearization of the resulting nonlinear system equations to obtain a linear system in state space form
- Investigation of the control theoretic properties of the system (equilibria, observability, etc.)

Please refer to the [Python List-Dictionary-Tuple tutorial](http://cs231n.github.io/python-numpy-tutorial/#python-containers) and the [NumPy Array tutorial](http://cs231n.github.io/python-numpy-tutorial/#numpy)
if you are not familiar with the handling of containers and arrays in Python. If you are
completely new to Python consult the very basic introduction on [tutorialspoint](https://www.tutorialspoint.com/python/index.htm). If you don't have any experience with Jupyter Notebook to get started it is recommended to watch the following tutorial:

```python
from IPython.lib.display import YouTubeVideo
YouTubeVideo('HW29067qVWk')
```

# 2. The Cart-Pole System
The cart-pole system, as shown in the figure is a common control theory benchmark system, and has different attributes, that make it particular interesting for teaching control theoretic principles and methods.

The pole is attached to a cart at its end. By applying a force $F$ to the cart through a moment $M$, the cart can be moved in the $x$-direction. The objective is to control the cart, such that the pole, starting in the upright position, doesn't fall over. Another objective is the swing-up manover, where the pole starts from the downright position and has to be brought in the upright position and kept there. The second objective involves nonlinear control theory and is much harder to accomplish than the first.

## 2.1 Derivation of the equations of motion
To derive the equations of motion, Lagrangian mechanics are used. At first the position vectors $\mathbf{p}_i$ of all $i$ rigid bodies' center of mass have to be defined (in this case $i=0,1$). Then the velocities $\dot{\mathbf{p}}_i$ are derived by differentiation. After that the kinetic energy $T$ and potential energy $V$ are described to form the Lagrangian $L$. In the last step the equations of motions are derived by Lagrange's equations of the second kind.

At first the necessary Python libraries are imported.

```python
import numpy as np
import sympy as sp
from sympy import sin, cos, pi, Function
from sympy.interactive import printing
printing.init_printing()
```

Then symbolic expressions for all system parameters, time and force are defined.

```python
t = sp.Symbol('t') # time
params = sp.symbols('m0, m1, J1, l1, a1, g, d0, d1') # system parameters
m0, m1, J1, l1, a1, g, d0, d1 = params
params_values = [(m0, 3.34), (m1, 0.3583), (J1, 0.0379999),
                 (l1, 0.5), (a1, 0.43), (g, 9.81), (d0, 0.1), (d1, 0.006588)]
# force
F = sp.Symbol('F')
```

The system has 2 degrees of freedom, $x_0$ and $\varphi_1$. The generelized coordinates $q_i$ are therefore:
\begin{align}
&q_0 =  x_0 &q_1 =  \varphi_1
\end{align}
Because these are time dependent, they and there derivative up to order 2 are implemented as functions.

```python
q0_t = Function('q0')(t)
dq0_t = q0_t.diff(t)
ddq0_t = q0_t.diff(t, 2)
q1_t = Function('q1')(t)
dq1_t = q1_t.diff(t)
ddq1_t = q1_t.diff(t, 2)
```

### 2.1.1 Position vectors $\mathbf{p}_i$
The two rigid bodies of the cart-pole system are as the name suggests, the cart and the pole. The position vectors of the center of masses is found by the follwing expressions:
\begin{align}
&\mathbf{p}_0 = \begin{pmatrix} x_0 \\ 0\end{pmatrix} =\begin{pmatrix} q_0 \\ 0\end{pmatrix} & \mathbf{p}_1 = \begin{pmatrix} x_0 - a_1 \sin \varphi_1 \\ a_1 \cos \varphi_1\end{pmatrix} = \begin{pmatrix} q_0 - a_1 \sin q_1 \\ a_1 \cos q_1\end{pmatrix}
\end{align}
The position vectors are functions of the generalized coordinates.

```python
p0 = sp.Matrix([q0_t, 0])
p1 = sp.Matrix([q0_t - a1*sin(q1_t), a1*cos(q1_t)])
p0, p1
```

### 2.1.2  Velocity vectors $\dot{\mathbf{p}}_i$

The velocity vectors can be obtained by the time derivative.
\begin{align}
&\dot{\mathbf{p}}_0 = \begin{pmatrix} \dot{x}_0 \\ 0\end{pmatrix} =\begin{pmatrix}
\dot{q}_0 \\ 0\end{pmatrix}
& \dot{\mathbf{p}}_1 = \begin{pmatrix} \dot{x}_0 - a_1 \dot{\varphi}_1 \cos \varphi_1
\\ - a_1 \dot{\varphi}_1 \sin \varphi_1\end{pmatrix}
= \begin{pmatrix} \dot{q}_0 - a_1 \dot{q}_1 \cos q_1 \\ - a_1 \dot{q}_1 \sin q_1\end{pmatrix}
\end{align}

```python
dp0 = p0.diff(t)
dp1 = p1.diff(t)
dp0, dp1
```

### 2.1.3 Kinetic energy $T$
The total kinetic energy of the system $T$ can be found by the sum of the kinetic energies $T_0$ and $T_1$ of the rigid bodies.

```python
T0 = m0/2*(dp0.T*dp0)[0]
T0
```

```python
T1 = (m1*(dp1.T*dp1)[0] + J1*dq1_t**2)/2
T1
```

```python
T = T0 + T1
T
```

### 2.1.3 Potential energy $V$
The total potential energy of the system $V$ can be found by the sum of the kinetic energies $V_0$ and $V_1$ of the rigid bodies. Because $V_0=0$, $V = V_1$.

```python
V = m1*g*p1[1]
V
```

### 2.1.4 Lagrangian $L$
The Lagrangian is defined as $L = T-V$

```python
L = T - V
L = L.expand()
L = sp.trigsimp(L)
L
```

### 2.1.5 Lagrange's equation of the second kind
To obtain the equations of motion, Lagrange's equation of the second kind is used:
\begin{align}
\frac{\mathrm{d}}{\mathrm{d} t}\left(\frac{\partial L}{\partial \dot{q}_i}\right) - \frac{\partial L}{\partial q_i} = Q_i
\quad i = 0,1\end{align}
with the generalized forces $Q_i$.
Substituting $L = T-V$ leads to:
\begin{align}
\frac{\mathrm{d}}{\mathrm{d} t}\left(\frac{\partial T}{\partial \dot{q}_i} \right) - \frac{\mathrm{d}}{\mathrm{d} t}\left(\frac{\partial V}{\partial \dot{q}_i}\right) - \frac{\partial T}{\partial q_i} + \frac{\partial V}{\partial q_i} = Q_i
\quad i = 0,1\end{align}
For mechanical systems like the cart-pole system $\frac{\mathrm{d}}{\mathrm{d} t}\frac{\partial V}{\partial \dot{q}_i} = 0$. The equation simplifies to:
\begin{align}
\frac{\mathrm{d}}{\mathrm{d} t}\left(\frac{\partial T}{\partial \dot{q}_i}\right) - \frac{\partial T}{\partial q_i} + \left(\frac{\partial V}{\partial q_i}\right) = Q_i
\quad i = 0,1
\end{align}

The generalized forces can be seperated to a term $B\mathbf{\tau}$ that expresses how the actuator forces $u$ act on the system and a term for the disspitave forces $R$:
\begin{align}
(Q_0,...,Q_n)^\textrm{T} = B\mathbf{\tau} - R
\end{align}

For the cart-pole system one obtains:
\begin{align}
Q_0 = F - \frac{1}{2}d_0 \dot{q}_0^2 \qquad Q_1 = - \frac{1}{2}d_1 \dot{q}_1^2
\end{align}

```python
Q0 = F - d0/2*dq0_t**2
```

```python
Q0 = F - d0*dq0_t
```

```python
Q1 =  - d1/2*dq1_t**2
```

```python
Q1 =  - d1*dq1_t
```

```python
Eq0 = L.diff(dq0_t, t) - L.diff(q0_t) - Q0 # = 0
```

```python
Eq1 = L.diff(dq1_t, t) - L.diff(q1_t) - Q1 # = 0
```

A mechanical system is described by the following equation:
\begin{align}
M(\mathbf{q})\ddot{\mathbf{q}}+C(\mathbf{q},\dot{\mathbf{q}})+K(\mathbf{q},\dot{\mathbf{q}}) - B(\mathbf{q})\mathbf{\tau} = \mathbf{0}
\end{align}

$M$ - mass matrix

$C$ - coriolis vector

$K$ - vector containing potential energy terms and $R$

This eqation is contained in the variable `Eq`.

```python
Eq = sp.Matrix([Eq0, Eq1])
Eq
```

The mass matrix $M$ can be found by applying the differential operator
$\frac{\mathrm{d}}{\mathrm{d}\mathbf{\ddot{q}}}$ to `Eq`.

```python
ddq_t = sp.Matrix([ddq0_t, ddq1_t])
M = Eq.jacobian(ddq_t)
M
```

The mass matrix of mechanical systems is always symmetric and invertible. Therefore an equation for $\ddot{\mathbf{q}}$ always exists.
\begin{align}
\ddot{\mathbf{q}} = M^{-1}(\mathbf{q})\left(-C(\mathbf{q},\dot{\mathbf{q}})-K(\mathbf{q},\dot{\mathbf{q}}) + B(\mathbf{q})\mathbf{\tau} \right)
\end{align}

```python
q_zeros = [(ddq0_t, 0), (ddq1_t, 0)]
ddq = M.inv() * -Eq.subs(q_zeros)
ddq
```

## 2.2 State Space Model
An input-affine, nonlinear system is given by the following equation:
\begin{align}
\dot{\mathbf{x}}=\mathbf{f}(\mathbf{x})+\mathbf{g}(\mathbf{x})\mathbf{u} \qquad \mathbf{x} \textrm{ - state vector}, \mathbf{u} \textrm{ - control vector}
\end{align}

To transform the system equations in state space form, the system equations $\ddot{\mathbf{q}}= \mathbf{f}_{\ddot{\mathbf{q}}}(\mathbf{q},\dot{\mathbf{q}}) + \mathbf{g}_{\ddot{\mathbf{q}}}(\mathbf{q})\mathbf{\tau}$, which are second order differential equations each have to be seperated to two differential equations of first order. This is done by introducing a state vector $\mathbf{x}$ and a control vector $\mathbf{u}$.
\begin{align}
&\mathbf{x} := \begin{pmatrix} \mathbf{q} \\ \dot{\mathbf{q}} \end{pmatrix}
&\dot{\mathbf{x}} = \begin{pmatrix} \dot{\mathbf{q}} \\ \ddot{\mathbf{q}} \end{pmatrix}
&\qquad \mathbf{u} := \mathbf{\tau}
\end{align}
Substituting $\ddot{\mathbf{q}}$, one obtains:
\begin{align}
\dot{\mathbf{x}} = \underbrace{\begin{pmatrix} \dot{\mathbf{q}} \\ \mathbf{f}_{\ddot{\mathbf{q}}}(\mathbf{q},\dot{\mathbf{q}}) \end{pmatrix}}_{=:\mathbf{f}(\mathbf{x})} + \underbrace{\begin{pmatrix} \mathbf{0} \\ \mathbf{g}_{\ddot{\mathbf{q}}}(\mathbf{q}) \end{pmatrix}}_{=:\mathbf{g}(\mathbf{x})}\underbrace{\mathbf{\tau}}_{=:\mathbf{u}}
\end{align}
The result is an input-affine system, an inhomogenous, multi-dimensional first order differential equation in $\mathbf{x}$.

### 2.2.1 Nonlinear system

In the cart-pole case, $\mathbf{q}$ is two dimensional, the state vector $\mathbf{x}$ is therefore four dimensional:
\begin{align}
\mathbf{x} = \begin{pmatrix}x_1 \\ x_2 \\ x_3 \\ x_4 \end{pmatrix} =
\begin{pmatrix}q_0 \\ q_1 \\ \dot q_0 \\ \dot q_1 \end{pmatrix}
=\begin{pmatrix}x_0 \\ \varphi_1 \\ \dot x_0 \\ \dot \varphi_1 \end{pmatrix}
\end{align}

```python
x1_t = sp.Function('x1')(t)
x2_t = sp.Function('x2')(t)
x3_t = sp.Function('x3')(t)
x4_t = sp.Function('x4')(t)
x_t = sp.Matrix([x1_t, x2_t, x3_t, x4_t])

u_t = sp.Function('u')(t)
```

$q_0,q_1$ and $\dot q_0,\dot q_1$ in $\ddot{\mathbf{q}}$ are replaced by $x_1, x_2$ and $x_3, x_4$. The force $F$ is replaced by the control input $u$.

```python
xu_subs = [(dq0_t, x3_t), (dq1_t, x4_t), (q0_t, x1_t), (q1_t, x2_t), (F, u_t)]
ddq = ddq.subs(xu_subs)
ddq = sp.simplify(ddq)
ddq
```

```python
dx_t = sp.Matrix([x3_t, x4_t, ddq[0], ddq[1]])
ff = dx_t.subs([(u_t, 0)])
gg = dx_t.diff(u_t)
```

## 2.3 System Analysis
### 2.3.1 Equilibria
An equilibrium is a point in the state space, where the trajectory of the system dynamics is constant. To find such a point, $\dot{\mathbf{x}}$ is set equal to the zero vector. The result is a nonlinear equation system:
\begin{align}
\mathbf{0} = \mathbf{f}(\mathbf{x}) + \mathbf{g}(\mathbf{x})\mathbf{u}
\end{align}
The equation is dependent on the control vector $\mathbf{u}$. But for now, only the equlibria of the autonomous system $\mathbf{f}(\mathbf{x})$ are of interest, therefore we set $\mathbf{u}:= \mathbf{0}$.
To find the equilibria $\mathbf{x}_{0,i}(i = 1,2,...)$ of the autonoums system, the following nonlinear equation system has to be solved:
\begin{align}
\mathbf{0} = \mathbf{f}(\mathbf{x})
\end{align}

```python
x0 = sp.solve(ff, x_t)
x0
```

The result are two equilbria $\mathbf{x}_{0,1}$ and $\mathbf{x}_{0,2}$ that are independent of the state variable $x_1$, the position of the cart. The two eqilibria are the pole in the upright and downright position at zero velocity of both cart and pole, as one would intuitively guess. But the equilibria of a system can not always be simply found by intuition, like in the cart-pole case, therefore the equilibria should always be found by the shown method.

### 2.3.2 Obtaining a linear state space model
To obtain a linear state space model, a taylor expansion of the nonlinear equation is used.
\begin{align}
\dot{\mathbf{x}}-\dot{\mathbf{x}}_{0,i} &\approx \underbrace{\mathbf{f}(\mathbf{x}_{0,i}) +\mathbf{g}(\mathbf{x}_{0,i})\mathbf{u}_0}_{=0}
+ \underbrace{\frac{\partial}{\partial \mathbf{x}}\left(\mathbf{f}(\mathbf{x}) +\mathbf{g}(\mathbf{x})\mathbf{u}\right)\Big|_{\substack{\mathbf{u} = \mathbf{u}_0 \\\mathbf{x}=\mathbf{x}_{0,i}}}}_{=:A_i}(\mathbf{x}-\mathbf{x}_{0,i})
+ \underbrace{\frac{\partial}{\partial \mathbf{u}}\left(\mathbf{f}(\mathbf{x}) +\mathbf{g}(\mathbf{x})\mathbf{u}\right)\Big|_{\substack{\mathbf{u} = \mathbf{u}_0 \\\mathbf{x}=\mathbf{x}_{0,i}}}}_{=:B_i}(\mathbf{u}-\mathbf{u}_0)+\mathcal{O}(2)
\end{align}
First the linearization is done for the general case. Then, specific linear system matrices at the eqilibria are evaluated.

```python
# separate ("unpack") equilibrium points
x01, x02 = x0

A = dx_t.jacobian(x_t)
B = dx_t.diff(u_t)

eql1_rplmts = list(zip(x_t, x01))
eql2_rplmts = list(zip(x_t, x02))

A1 = A.subs(eql1_rplmts) # pole upright
B1 = B.subs(eql1_rplmts) # pole upright
A2 = A.subs(eql2_rplmts) # pole downright
B2 = B.subs(eql2_rplmts) # pole upright
A1, A2, B1, B2
```

### 2.3.3 Controllability

A linear time-invariant (LTI) system
\begin{align}
\dot{\mathbf{x}} = A \mathbf{x} + B \mathbf{u},
\end{align}
with
\begin{align}
\mathbf{x} \in \mathbb{R}^n, \mathbf{u} \in \mathbb{R}^m  \quad A \in \mathbb{R}^{n \times n}, B \in \mathbb{R}^{n\times m}
\end{align}
is controlable if and only if the system states $\mathbf{x}$ can be changed by changing the system input $\mathbf{u}$.
To investigate the controllability of the system, we use the Kalman criterion of controllabilty, which says that a LTI system is controllable if the Kalman controllabilty matrix $Q_c$ is full rank.
\begin{align}
Q_c := \begin{pmatrix}B, AB,..., A^{n-1}B\end{pmatrix}
\end{align}
\begin{align}
\mathrm{rank} (Q_c) = n
\end{align}

### Concatenating column vectors to a matrix with SymPy
Given the to column vectors $a, b \in \mathbb{R}^n$, concatenating them to a matrix $(a,b)\in \mathbb{R}^{n\times2}$ is not trivial in SymPy. Have a look at the following example:

```python
a = sp.Matrix([1, 2, 3])
b = sp.Matrix([4, 5, 6])
a, b
```

The resulting matrix should have two columns and three rows. But calling `sp.Matrix([a,b])` returns a column vector:

```python
sp.Matrix([a,b])
```

Instead sympy provides `row_join`:

```python
a.row_join(b)
```

---
With the shown method, the Kalman controllability matrix can be computed.

```python
Q_c1 = B1.row_join(A1*B1).row_join(A1**2*B1).row_join(A1**3*B1)
```

To obtain a real valued matrix, the previously defined parameter values are substituted.

```python
Q_c1 = Q_c1.subs(params_values)
Q_c1
```

Then the rank of $Q_{c,1}$ is computed:

```python
Q_c1.rank(simplify=True)
```

**Note**: Calculating the rank of a numerical matrix can be tricky, due to the fact that the computer has to decide whether a small numerical value is equal to or different from zero.

For the (educated) user to be sure the best is to have a look at the singular values.

```python
sp.Matrix(Q_c1.singular_values()).evalf()
```

The smallest singular value is $\approx 0.27 \gg 0$ which means the matrix is regular. The very small imaginary
parts are result of numerical calculation and can be neglected.

---

$\mathrm{rank} (Q_{c,1}) = 4 = n$, the system is controllable in the upper equilibrium.

## 2.4 Linear Quadratic Regulator (LQR)

The linear quadratic regulator (LQR) is a linear control scheme often used in pracitcal applications. With this method, an optimal state feedback of the closed loop system can be designed. Instead of placing the poles of the closed loop system manually (i.e. by Ackermann's formula), a linear feedback control law $\mathbf{u}=-K\mathbf{x}$ can be derived, such that the cost function $J$ is minimized.

\begin{align}
J &= \int_{t_0=0}^\infty \mathbf{x}(t)^\textrm{T}Q\mathbf{x}(t)+\mathbf{u}(t)^\textrm{T}R\mathbf{u}(t) \mathrm{d}t \\
Q &\in \mathbb{R}^{n\times n}, R \in \mathbb{R}^{m\times m} - \textrm{ diagonal weight matrices}
\end{align}

$K=R^{-1}B^\textrm{T}P$, where $P$ is the solution to the continuous time algebraic Riccati equation (ARE):

\begin{align}
A^\textrm{T}P+PA-PBR^{-1}B^\textrm{T}P + Q = 0
\end{align}
$A, B$ are the system matrices.

The ARE can be solved in Python with the SciPy package:
At first, the weight matrices are choosen. A high value stands for high cost of the corresponding signal.

Note that if $Q \ge 0$ and $R > 0$ then the control law $\mathbf{u}=-K\mathbf{x}$ places the poles of the closed loop always in the open left half-plane.

```python
Q = 10*np.eye(4)
R = 0.1
Q, R
```

    (array([[ 10.,   0.,   0.,   0.],
            [  0.,  10.,   0.,   0.],
            [  0.,   0.,  10.,   0.],
            [  0.,   0.,   0.,  10.]]), 0.1)

The parameters are substituted into the system matrices of the upper equilibrium and the resulting matrices are converted to the correct type.

```python
A = A1.subs(params_values)
B = B1.subs(params_values)
A = np.array(A).astype(np.float64)
B = np.array(B).astype(np.float64)
A, B
```

    (array([[  0.00000000e+00,   0.00000000e+00,   1.00000000e+00,
               0.00000000e+00],
            [  0.00000000e+00,   0.00000000e+00,   0.00000000e+00,
               1.00000000e+00],
            [  0.00000000e+00,   6.43606254e-01,  -2.88134322e-02,
              -2.80536630e-03],
            [  0.00000000e+00,   1.54492403e+01,  -4.25829735e-02,
              -6.73405174e-02]]), array([[ 0.        ],
            [ 0.        ],
            [ 0.28813432],
            [ 0.42582974]]))

The SciPy package is imported and the ARE is solved to obtain $P$.

```python
import scipy as sci
from scipy import linalg
```

```python
P = sci.linalg.solve_continuous_are(A, B, Q, R)
P
```

    array([[  18.64638492,  -42.32305222,   12.38388353,  -10.72781326],
           [ -42.32305222,  343.63231708,  -68.00490028,   83.06410971],
           [  12.38388353,  -68.00490028,   18.74020643,  -17.0827318 ],
           [ -10.72781326,   83.06410971,  -17.0827318 ,   21.3363659 ]])

Finally the feedback matrix $K$ is computed.

```python
K = 1/R*B.T.dot(P)
K
```

    array([[ -10.        ,  157.76622042,  -18.74638492,   41.63537705]])

---
## Simulation

For the simulation SciPy's integrate package is used.

```python
from scipy import integrate
```

At first, all relevant simulation parameters are defined.

```python
t0 = 0 # start time
tf = 10 # final time
dt = 0.04 # stepsize
tt = np.arange(t0, tf, dt) # simulation time
x0 = np.array([1, 0.3, 0, 0]) # initial value
dx_t = dx_t.subs(params_values) # substitute the parameters in dx_t
```

The right hand side of $\dot x=f(x)$ has to be converted from a symbolic expression to a (fast) callable python function.

```python
x1, x2, x3, x4, u = sp.symbols("x1, x2, x3, x4, u")
xx = [x1, x2, x3, x4]

dx_t_with_symbols = dx_t.subs(list(zip(x_t, xx))).subs(u_t, u) # replacing all symbolic functions with symbols

dx_func = sp.lambdify((x1, x2, x3, x4, u), dx_t_with_symbols, modules="numpy") # creating a callable python function
```

```python
dx_t_with_symbols
```

---

```python
# plausibility check1: equilibrium 1 (return value should be (0, 0, 0, 0))
dx_func(0, 0, 0, 0, 0)
```

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

```python
# plausibility check1: equilibrium + initial force
dx_func(0, 0, 0, 0, 2)
```

    array([[ 0.        ],
           [ 0.        ],
           [ 0.57626864],
           [ 0.85165947]])

---
A wrapper function is needed to evaluate the control algorithm and have the right argument-signature (t, x) for simulation.

The differential equation of the system is implemented in a function `fbODE(t, x)` (feedback ODE).

```python
def fbODE(t, x):
    '''Nonlinear system of equations dx/dt = f(x,u) = f(x,-K*x) = f(x)'''
    u = -K.dot(x) # define the control law
    return dx_func(*x, u).T[0]
```

Finally the initial value problem is solved.

```python
sol = sci.integrate.solve_ivp(fbODE, (t0, tf), x0, t_eval=tt)
```

```python
xt = sol.y.T
```

---
## Plotting the results

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

```python
def cartPolePlot(xt):
    fig1, (ax1, ax2) = plt.subplots(2, 1, sharex=True)
    fig2, (ax3, ax4) = plt.subplots(2, 1, sharex=True)
    ax1.plot(tt, xt[:,0])
    ax2.plot(tt, xt[:,1])
    ax3.plot(tt, xt[:,2])
    ax4.plot(tt, xt[:,3])

    ax1.set(ylabel=r'$x_0$')
    ax2.set(ylabel=r'$\varphi_1$')
    ax3.set(ylabel=r'$\dot x_0$')
    ax4.set(ylabel=r'$\dot \varphi_1$')
    ax2.set(xlabel=r't in s')
    ax4.set(xlabel=r't in s')
    ax1.grid('on')
    ax2.grid('on')
    ax3.grid('on')
    ax4.grid('on')
```

```python
cartPolePlot(xt)
```

---
## Animating the results

```python
from matplotlib import animation
from IPython.display import HTML
# equivalent to rcParams['animation.html'] = 'html5'
mpl.rc('animation', html='html5')
import matplotlib.patches as patches
```

```python
def cartPoleAnimation(xt):
    # mapping from theta and s to the x,y-plane (definition of the line points, that represent the pole)
    def cart_pole_plot(l, xt):
        x_pole_end = -l * np.sin(xt[:, 1]) + xt[:, 0]
        x_cart = xt[:, 0]
        y_pole_end = l * np.cos(xt[:, 1])
        return x_pole_end, y_pole_end, x_cart

    # line and text
    def animate(t):
        thisx = [x_cart[t], x_pole_end[t]]
        thisy = [0, y_pole_end[t]]

        pole.set_data(thisx, thisy)
        cart.set_xy([x_cart[t]-0.1, -0.05])
        time_text.set_text(time_template%(t*dt))
        return pole, cart, time_text,

    [x_pole_end, y_pole_end, x_cart] = cart_pole_plot(0.5, xt)
    fig, ax = plt.subplots()
    ax.set_aspect('equal')
    ax.set(xlabel=r'$x_1$')
    plt.ylim((-0.1, 0.6))
    plt.title('Cart-Pole LQR')
    time_template = 'time = %.1fs'
    time_text = ax.text(0.05, 1.2, '', transform=ax.transAxes)
    rail, = ax.plot([min(-1, 1.2*min(x_cart)), max(1, 1.2*max(x_cart))], [0,0], 'ks-', zorder=0)
    pole, = ax.plot([], [], 'b-', zorder=1, lw=3)
    cart = patches.Rectangle((-0.1, -0.05), 0.2, 0.1, fc='b', zorder=1)
    ax.add_artist(cart)
    # animation using matplotlibs animation library
    ani = animation.FuncAnimation(fig, animate, np.arange(0, len(xt)), interval=dt*1000,
                          blit=True)
    return ani
```

```python
cartPoleAnimation(xt)
```

An animation can sometimes be a helpful tool to check, wether the model equations are implemented correctly. A strange behaviour is much easier to detect in a visual animation, compared to looking at the plotted state trajectories.

## Feedforward control
The designed feedback controller successfully stabilizes the system in the unstable equilibrium $(0, 0, 0, 0)^\textrm{T}$. To stabilize the cart at another $x_0$-position, it is necessary to design a feedforward control.

Recapture the linearized feedback control system dynamics:
\begin{align}
\dot x(t) &= (A -BK)x(t) \\
y(t) &= Cx(t)
\end{align}
A feedforward input $u_{ff}$ is introduced:
\begin{align}
\dot x(t) &= (A -BK)x(t) + B u_{ff}(t) \\
y(t) &= Cx(t)
\end{align}
with
\begin{align}
u_{ff}(t) = - V y_d
\end{align}
where $V$ is a feedforward filter and $y_d$ is the stationary desired system output.

This leads to new dynamics:
\begin{align}
\dot x(t) &= (A -BK)x(t) - B V y_d \\
y(t) &= Cx(t)
\end{align}

The Laplace transform of this new system is given by:
\begin{align}
sX(s) &= (A -BK)X(s) - B V Y_d \\
Y(s) &= CX(s)
\end{align}

Now it is easy to solve for $X(s)$
\begin{align}
&& sX(s) &= (A -BK)X(s) - B V Y_d \\
\Leftrightarrow \quad &&(sI - A +BK)X(s) &= - B V Y_d \\
\Leftrightarrow \quad &&X(s) &= - (sI - A +BK)^{-1}(B V Y_d) \\
\end{align}

Substituting the equation for $X(s)$ in the output equation of the system results in:
\begin{align}
Y(s) &= - C(sI - A +BK)^{-1}(B V Y_d)
\end{align}

---
If $t$ goes to infinity, the system output $y$ should be equal to the desired system output $y_d$:
$$\lim_{t\to\infty}y(t) \overset{!}{=} y_d$$

In the s-Domain this corresponds to:
$$\lim_{s\to0}Y(s) \overset{!}{=} Y_d$$
(An intuition: If $s=0$, the derivatives are equal to zero, which means the system is in steady state)

$$Y(0)=Y_d = - C(- A +BK)^{-1}(B V Y_d)$$

Solving for V results in:
$$V = [C(A - BK)^{-1}B]^{-1}$$

By setting $C$, the output is defined as $y=x_0$ (position of the cart).

```python
C = np.array([1, 0, 0, 0])
```

Finally the feedforward filter $V$ is computed:

```python
V = ((C.dot(np.linalg.inv((A-B.dot(K))))).dot(B))**(-1)
V
```

    array([ 10.])

The function of the control system (feedback and feedforward control) is defined:

```python
def ffODE(t, x, yd):
    '''Nonlinear system of equations dx/dt = f(x,u) = f(x,-K*x) = f(x)'''
    u = -K.dot(x) - V*yd # define the control law (feedback / feedforward)
    return dx_func(*x, u).T[0]
```

```python
sol = sci.integrate.solve_ivp(lambda t, x: ffODE(t, x, yd=2), (t0, tf), x0, t_eval=tt)
```

```python
xt = sol.y.T
```

```python
cartPolePlot(xt)
```

```python
cartPoleAnimation(xt)
```

# C code export

It is possible, to export SymPy expressions to a variety of other programming languages, for example C, C++, Fortran and JavaScript.

---
For a more detailed insight into code generation with sympy, have a look at the following tutorial:
https://www.sympy.org/scipy-2017-codegen-tutorial/

## System dynamics

The easiest way to convert an expression to C code is, to call the `ccode()`-printer:

```python
sp.ccode(dx_t_with_symbols)
```

    '// Not supported in C:\n// ImmutableDenseMatrix\nMatrix([\n[                                                                                x3],\n[                                                                                x4],\n[(-0.10424957*u + 0.010424957*x3 + 0.01606162700033*x4**2*sin(x2) - 0.154069*(-0.006588*x4 + 1.51141689*sin(x2))*cos(x2))/(0.023737256761*cos(x2)**2 - 0.385546184731)],\n[                    (0.0243644004*x4 + 0.154069*(-u + 0.1*x3 + 0.154069*x4**2*sin(x2))*cos(x2) - 5.589673084287*sin(x2))/(0.023737256761*cos(x2)**2 - 0.385546184731)]])'

Because the `ccode()`-printer does not support SymPy matrices, an array expression has to be defined, to which the values of the SymPy expression are assigned to. In the case of the pendulums ODE, this is a 4 by 1 matrix symbol:

```python
dx_t_ccode = sp.MatrixSymbol('dx_t_ccode', 4, 1)
```

```python
dx_t_ccode
```

Now the `ccode()`-printer is called again, but with the argument `assign_to`:

```python
sp.ccode(dx_t_with_symbols, assign_to=dx_t_ccode)
```

    'dx_t_ccode[0] = x3;\ndx_t_ccode[1] = x4;\ndx_t_ccode[2] = (-0.10424957*u + 0.010424957*x3 + 0.016061627000330002*pow(x4, 2)*sin(x2) - 0.15406900000000001*(-0.0065880000000000001*x4 + 1.5114168900000002*sin(x2))*cos(x2))/(0.023737256760999997*pow(cos(x2), 2) - 0.38554618473099994);\ndx_t_ccode[3] = (0.0243644004*x4 + 0.15406900000000001*(-u + 0.10000000000000001*x3 + 0.15406900000000001*pow(x4, 2)*sin(x2))*cos(x2) - 5.589673084286999*sin(x2))/(0.023737256760999997*pow(cos(x2), 2) - 0.38554618473099994);'

This code can now be copied to a C code function, to simulate the system dynamics.

___

## Control law
To compute control signal, that could be run on the real system, we export the control signal expression $u = -K x - V y_d$ to C code. At first a symbolic expression of the desired output $y_d$ is introduced.

```python
yd = sp.Symbol('yd')
```

After that, the control law is converted to C code.

```python
sp.ccode(-K.dot(xx) - V*yd)
```

    '[10.0*x1 - 157.766220417435*x2 + 18.7463849197588*x3 - 41.635377052761*x4 - 10.0*yd]'