# Vortex panel solution method

This section will use vortex panels to solve to the flow around general objects by setting up and solving a system of linear equations.

## General vortex sheets

A curved vortex sheet with a variable strength can describe the flow around any immersed object. This is achieved by having the sheet act as an infinitely thin version of the boundary layer to enforce the no-slip boundary condition.

---

---

In other words we use the sheets to force the tangential velocity $u_s$ to zero at every point $s$ on the body surface $\cal S$

\begin{equation}
u_s = \vec u \cdot \hat s = 0 \quad s \in \cal S
\end{equation}

Substituting the definition of velocity induced by the sheet, the tangential velocity condition is

\begin{equation}
\left[\vec U+\frac{\partial}{\partial \vec x}\oint_{\cal S} \frac{\gamma(s')}{2\pi}\theta(s,s')\ ds'\right] \cdot\hat s = 0
\end{equation}

where $\vec U$ is the background velocity that has been added by superposition.

**If we can use this equation to determine the strength distribution $\gamma(s)$ along the sheet then we will have solved for the potential flow around the body!**

## Discrete vortex panels

For general body surface shapes the velocity is a highly nonlinear function of $\gamma(s)$, rendering analytic solution unlikely. We could attempt some complex analytic expansions, but why would we want to do that?

##### Numerical fundamental: Discretization
##### Replace continuous functions with linear approximations

We already know that the velocity depends **linearly** on $\gamma$ for a vortex panel. This makes it easy to solve for $\gamma$ as a function of $u_s$.

If we break up the continuous sheet into a series of vortex panels, we can add their influence together using superposition. We can then apply the no slip boundary condition, and use it to solve for $\gamma$.

This is the essence of the *vortex panel method*.

---

## Array of panels

To help make this more concrete, lets use a unit circle as an example. The `make_circle` function has already been implemented in the `VortexPanel` module. Since we're going to use many functions in that module lets import the whole thing (like we do for `numpy`), and since it has a long name, lets give it the nick-name `vp`:

```python
# Annoying stuff to find files in other directories (only needed once)
import os
import sys
module_path = os.path.abspath(os.path.join('..'))
if module_path not in sys.path:
    sys.path.append(module_path)

# Actual import code
import numpy as np
from matplotlib import pyplot as plt
%matplotlib inline
from vortexpanel import VortexPanel as vp
```

Since you need to build your own shapes for the project, lets `inspect` how `make_circle` works

```python
from inspect import getsource
print(getsource(vp.make_circle))
```

Most of that is a comment block explaining the inputs and giving an example of using the function. Only the last three lines are code. An array of `theta` values are defined and then converted into `x,y` positions. The last line converts those points into an array of vortex panels using a function called `panelize`.

Lets use the two lines in the example to make a circle of panels and plot it:

```python
circle = vp.make_circle(N=32)  # make
circle.plot(style='o-')        # plot
plt.axis('equal');             # show its a circle
```

Play around with the `style` argument to see the points and panels that make up the shape.

Note that in addition to holding the array of `panels`, the `PanelArray` object also has some really useful functions like `PanelArray.plot()`.

The `PanelArray` class (and `Panel` within it) is written in an [Object Oriented Programming](https://realpython.com/python3-object-oriented-programming/#what-is-object-oriented-programming-oop) style.

##### Coding fundamental: Objects
##### Object-Oriented Programming helps organize data and functions

Lets see what the help says

```python
help(vp.PanelArray)
```

From the help, we can see that `PanelArray` contains the array of panels as well as an angle off attack. `Panel` also contains functions (the `methods`) which can be applied to it`self` such as plotting the panel and determining the panel's induced velocity. This avoids needing to pass the data about the geometry to the function - it's already all built-in!

---
Now that we have an example, what is the velocity induced by these panels and the uniform flow?

Using superposition, the total velocity at any point $x,y$ is simply

\begin{equation}
\vec u(x,y) = \vec U+\sum_{j=0}^{N-1} \vec u_j(x,y)
\end{equation}

where we use the index  $j$ to label each of the $N$ panels. This is easily implemented for an array of panels:

```python
print(getsource(vp.PanelArray.velocity))
```

where the `for-in` notation loops through every panel in the array.

---
The function `PanelArray.plot_flow(size=2)` uses this superposition code to visualize the flow:

```python
circle.plot_flow()
```

##### Quiz

Why is the flow going through the body above?

1. We haven't applied the flow tangency condition
1. We haven't applied the no-slip condition
1. We haven't determined the correct `gamma` for each panel

## System of linear equations

One key to the solution method is that the velocity $\vec u_j$ induced by vortex panel $j$ depends __linearly__ on $\gamma_j$. So lets write $\vec u_j$ in a way that makes the linearity explicit:

$$ \vec u_j(x,y)=\gamma_j\ \vec f_j(x,y)$$

where $\vec f_j$ is the part of the velocity function which depends only on the panel geometry.

Substituting this linear relation for the velocity $\vec u_j$ into the total velocity equation and applying the no-slip boundary condition we have:

$$ u_s = \left[\vec U + \sum_{j=0}^{N-1} \gamma_j \ \vec f_j(x,y)\right]\cdot\hat s=0 $$

So the goal is to set $\gamma$ on each panel such that this condition is enforced on the body.

##### Quiz

How many unknowns are there?

1. $1$
1. $N$
1. $N^2$

But we only have one equation, the no-slip condition... right?

##### Numerical fundamental: Consistency
##### Develop enough equations to match the unknowns

We need to have as many equations as we have unknowns to be consistent and to be able to determine a solution.

Luckily the no-slip condition is a continuous equation - it applies to *every* point on the body. Therefore, we can evaluate the boundary equation **on each panel**. Then we will have a consistent linear system.

---

Let's start with one panel, say panel $i$, and set the total tangential velocity at the center of the panel to zero:

$$ \frac 12 \gamma_i + \vec U\cdot\hat s_i + \sum_{j=0, j\ne i}^{N-1} \gamma_j \ \vec f_j(x_i,y_i)\cdot\hat s_i= 0 $$

Note that we've used the simple relation for the velocity a panel induces on itself

$$ \vec u_i(x_i,y_i) \cdot \hat s_i = \frac 12 \gamma_i $$

for the tangential velocity that the panel induces on itself.

Next, let's write the summation as an inner product of two arrays in order to separate out the knowns from the unknowns:

$$
\begin{bmatrix} \vec f_0(x_i,y_i)\cdot\hat s_i & \vec f_1(x_i,y_i)\cdot\hat s_i & \cdots & \frac 12 & \cdots & \vec f_{N-1}(x_i,y_i)\cdot\hat s_i\end{bmatrix} \times \begin{bmatrix} \gamma_0 \\ \gamma_1 \\ \vdots \\ \gamma_i \\ \vdots \\ \gamma_{N-1} \end{bmatrix} + \vec U \cdot \hat s_i = 0
$$

Written like this, we can see two things:

 - The no-slip condition on panel $i$ depends on the strength at every panel, and
 - This is just the $i$th row of a matrix of equations

\begin{equation}
\begin{bmatrix}
\frac 12  & \vec f_1(x_0,y_0)\cdot\hat s_0 & \cdots & \vec f_{N-1}(x_0,y_0)\cdot\hat s_0\\[0.5em]
\vec f_0(x_1,y_1)\cdot\hat s_1 & \frac 12 & \cdots & \vec f_{N-1}(x_1,y_1)\cdot\hat s_1 \\[0.5em]
\vdots & \vdots & \ddots & \vdots \\[0.5em]
\vec f_0(x_{N-1},y_{N-1})\cdot\hat s_{N-1} & \vec f_1(x_{N-1},y_{N-1})\cdot\hat s_{N-1} & \cdots & \frac 12
\end{bmatrix}
\times \begin{bmatrix} \gamma_0 \\[0.9em] \gamma_1 \\[0.9em] \vdots \\[0.9em] \gamma_{N-1} \end{bmatrix}
= -\begin{bmatrix} \vec U\cdot\hat s_0 \\[0.7em] \vec U\cdot\hat s_1 \\[0.7em] \vdots \\[0.7em] \vec U\cdot\hat s_{N-1} \end{bmatrix}
\end{equation}

which we can summarize as:

$$ A \gamma = b$$

this defines the complete linear system.

---

## Summary

Lets review the key concepts:

1. The no-slip vortex sheet equation implicitly defines the analytic $\gamma(s)$, but is highly nonlinear.
1. Breaking the sheet up into a set of panels ensures the velocity is a linear function of $\gamma_i$.
1. Applying the no-slip condition to each panel results in a linear system of equations for $\gamma_i$.

Therefore, the solution $\gamma_i$ of $A\gamma=b$ is a numerical approximation to the analytic solution $\gamma(s)$.

*This same basic procedure is applied whenever numerically approximating partial differential equations!*

---

## Implementation

We can construct `A` and `b` in only a few lines of code:
```python
    # construct the matrix `A`
    for j, p_j in enumerate(panels):      # loop through columns
        fx,fy = p_j.constant(xc,yc)           # f_j at all panel centers
        A[:,j] = fx*sx+fy*sy                  # tangential component
    np.fill_diagonal(A, 0.5)              # fill diagonal with 1/2

    # construct the vector `b`
    b = -(np.cos(alpha)*sx+np.sin(alpha)*sy)
```

That seems like the easy part. After all, this is a dense linear algebra problem. How in the world will we determine `gamma`?

In fact, this is trivial:
```python
    gamma = np.linalg.solve(A, b)
```
##### Numerical fundamental: Linear Algebra Packages
##### Never write your own matrix solver

Every worthwhile numerical language has a set of linear algebra solution routines like the [linalg package](http://docs.scipy.org/doc/numpy/reference/routines.linalg.html). Use them!

---

The function `PanelArray.solve_gamma(alpha=0)` combines the construction and solution code above. After defining __any__ `PanelArray` and an angle of attack `alpha`, this function sets $\gamma_i$ on each panel such that the no-slip condition is enforced.

Let's put all the `VortexPanel` functions together and test them out!

```python
circle = vp.make_circle(N=10)   #1) define geometry
circle.solve_gamma()            #2) solve for gamma
circle.plot_flow()              #3) compute flow field and plot
```

Much better! The flow is generally going around the shape. And the flow is (pretty much) stationary within the body. But the external flow doesn't look __exactly__ like the potential flow solution...

##### Numerical Fundamental: Convergence with resolution
##### The numerical solution is improved by using more panels

---

## Quantitative testing

Let's make this explicitly clear by plotting the distance around the shape $s$ versus $\gamma$ for a number of resolutions.

The function `PanelArray.get_array` lets you get any attribute from the panels. What attributes are there?

```python
help(vp.Panel)
```

So each panel knows all about its geometry (size, location, orrientation) as well as the strength $\gamma$.

Let's try getting this information. Say, the half-width of each panel:

```python
print(circle.get_array('S'))
```

The `distance` function uses this to get the cumulative distance.

```python
print(getsource(circle.distance))
```

The two functions `get_array` and `distance` are all we need to make a quantitative plot of our results:

```python
# Exact solution
s = np.linspace(0,2*np.pi)
plt.plot(s,2*np.sin(s),'k',label='exact')

# Loop over resolutions
for N in 2**np.arange(6,2,-1):         # N in powers of 2
    circle = vp.make_circle(N)         # define geometry
    s = circle.distance()              # get distance array

    circle.solve_gamma()                # solve for gamma
    gamma = circle.get_array('gamma')   # get gamma array
    plt.plot(s,gamma,'--',label=N)      # plot

# finish gamma(s) plot
plt.legend(title='N')
plt.xlabel(r'$s$', fontsize=20)
plt.ylabel(r'$\gamma$', fontsize=20, rotation=0)
plt.show()
```

All of the results for $N>32$ panels look pretty much identical, meaning our results __converge__ as we increase the number of panels. But we can't judge if the converged solution is __correct__ unless we compare to the known result.

##### Numerical fundamental: Validation
##### A method is validated by comparing the result to a known solution

That's easy in this case. The exact solution from analytic potential flow around a circle is

$$\tilde\gamma = -\tilde u_e = 2\sin\theta $$

which is also in the plot and matches the converged solution perfectly.

## Other shapes

Notice that there was nothing special to the circle in this set-up. By giving a different set of points to `panelize`, we can make any shape we need.

##### Quiz

This vortex panel method can be used to solve for the flow around:

1. an ellipse
1. a pair of tandem bodies
1. a rudder

---
##### Your turn

 - ** Modify ** the `make_ellipse` function below to generate an ellipse instead of a circle when supplied with an aspect ratio `t_c`=$t/c$.
 - ** Discuss ** whether the maximum speed around the ellipse is greater or less than that around the circle.
 - ** Move ** a 2:1 ellipse geometry by giving it a different center
 - ** Combine ** the circle and ellipse geometry together into one set of panels using `vp.concatenate` and solve for the flow.
 - ** Discuss ** if there is a *wake* between the bodies. Why or why not?

---

##### Solution

```python
def make_ellipse(N, t_c, xcen=0, ycen=0):
    theta = np.linspace(0, -2*np.pi, N+1)
    # your code here to define the points for an ellipse
    x = xcen+np.cos(theta)  # adjust?
    y = ycen+np.sin(theta)  # adjust?
    return vp.panelize(x,y)

# ellipse = make_ellipse(N=32,t_c=2,xcen=2)     # make the shape
# ellipse.solve_gamma()                         # solve for gamma
# ellipse.plot_flow(size=4)                     # compute flow field and plot
```

```python
# pair = ?   your code using vp.concatenate(a1,a2)
```


### Smooth transition between analytic functions

The idea is to create a smooth transition in a place where a function is discontinuous.

This way to create smoothness is useful because it doesn't have to evaluate the original piecewise functions outside their original domains.

The original discontinuity usually happens because a function is already defined like this:

$$
  f(x) = \left\{
     \begin{array}{r}
       f_{left}(x)  &: x < x_{threshold}\\
       f_{right}(x)  &: x \geq x_{threshold}\\
     \end{array}
   \right.
$$

For example:

```python
import sympy
from sympy import Piecewise
import numpy as np

# For example:
x_ = sympy.symbols('x', real=True)

f_left_ = x_**1.2
f_right_ = 10.0 / x_**0.2

x_threshold = 10.0 ** (5 / 7)

f_ = Piecewise(
    (f_left_, x_ < x_threshold),
    (f_right_, True)
)

f = sympy.lambdify(x_, f_)
```

```python
import seaborn
import matplotlib.pyplot as plt
from ipywidgets import interact, widgets

x = np.linspace(0.001, 20, 1000)
y = f(x)

plt.plot(x, y)
plt.show()
```

What we want is $C^1$ continuity, i.e., first-derivatives are continuous, without needing values of `f_left` and `f_right` outside where they are defined.

To accomplish this, we'll join the second derivatives of both functions by a line, around a region $[x_0, x_1]$ that contains the threshold:

```python
d_dx_f_left_ = sympy.diff(f_left_, x_, 1)
d_dx_f_right_ = sympy.diff(f_right_, x_, 1)

d_dx_f_ = Piecewise(
    (d_dx_f_left_, x_ < x_threshold),
    (d_dx_f_right_, True)
)

d_dx_f = sympy.lambdify(x_, d_dx_f_)

d_dx_f_left = sympy.lambdify(x_, d_dx_f_left_)
d_dx_f_right = sympy.lambdify(x_, d_dx_f_right_)

plt.plot(x, d_dx_f(x))
plt.xlim((x_threshold * 0.8, x_threshold * 1.2))
plt.ylim((-0.4, 1.8))
plt.show()
```

The derivative is indeed very discontinuous! Let's fix this!

They do not need to be symmetric around the threshold

```python
x_0 = x_threshold * 0.8
x_1 = x_threshold * 1.2

d_dx_at_x_0 = d_dx_f_left_.evalf(subs={x_: x_0})
d_dx_at_x_1 = d_dx_f_right_.evalf(subs={x_: x_1})

d_dx_f_center_ = d_dx_at_x_0 + ((x_ - x_0) / (x_1 - x_0)) * (d_dx_at_x_1 - d_dx_at_x_0)

d_dx_f_smooth_ = Piecewise(
    (d_dx_f_left_, x_ < x_0),
    (d_dx_f_center_, x_ < x_1),
    (d_dx_f_right_, True)
)

d_dx_f_smooth = sympy.lambdify(x_, d_dx_f_smooth_)

```

```python
plt.plot(x, d_dx_f_smooth(x))
plt.xlim((x_threshold * 0.6, x_threshold * 1.4))
plt.ylim((-0.4, 1.8))
plt.show()
```

So now we just have to integrate `d_dx_f_center` and adjust it's integral constant to create a better piecewise `f_smooth`

```python
f_center_ = sympy.integrate(d_dx_f_center_, x_)

f_left = sympy.lambdify(x_, f_left_)
f_center = sympy.lambdify(x_, f_center_)

# f_center(x0) == f_left(x0)
f_center_ = f_center_ + (f_left(x_0) - f_center(x_0))

f_smooth_ = Piecewise(
    (f_left_, x_ < x_0),
    (f_center_, x_ < x_1),
    (f_right_, True)
)

f_smooth = sympy.lambdify(x_, f_smooth_)
```

```python
plt.plot(x, f_smooth(x))
plt.show()
```

Much better!
Now let's generalize a way to create `f_center`:

```python
sympy.init_printing()

x_0_, x_1_, f_0_ = sympy.symbols('x_0,x_1,f_{left}(x_0)', real=True)

# df_0 is f_left'(x_0)
# df_1 is f_right'(x_1)

df_0_, df_1_ = sympy.symbols('df_0,df_1', real=True)

a = (x_ - x_0_) / (x_1_ - x_0_)

# d_dx_f_center_ = d_dx_at_x_0 + a * (d_dx_at_x_1 - d_dx_at_x_0)
d_dx_f_center_ = (1 - a) * df_0_ + a * df_1_
f_center_ = sympy.integrate(d_dx_f_center_, x_)

# f_center_ = f_center_ + f_0_ - f_center_.subs(x_, x_0_)

f_center_
```

```python
# Simplifying f_center to be an equation like x * (a*x + b) + c:

def get_f_center_coefficients(x0, x1, f_left, df_left, df_right):
    dx = x1 - x0
    df0 = df_left(x0)
    df1 = df_right(x1)
    a = 0.5 * (df1 - df0) / dx
    b = (df0*x1 - df1*x0) / dx
    c = f_left(x0) - (x0 * (a*x0 + b))
    return a, b, c

x_0 = x_threshold * 0.8
x_1 = x_threshold * 1.2

# So, for example:
a, b, c = get_f_center_coefficients(
    x_0,
    x_1,
    f_left=lambda x: x**1.2,
    df_left=lambda x: 1.2 * x**0.2,
    df_right=lambda x: -2.0*x**-1.2,
)

print (a, b, c)
```

    -0.4387286573389664 5.230431342173345 -8.63388046349694

```python
def f_smooth(x):
    return np.piecewise(x, [x < x_0, (x_0 <= x) & (x <= x_1), x > x_1], [
        lambda x: x**1.2,
        lambda x: x * (-0.4387286573389664 * x + 5.230431342173345) - 8.63388046349694,
        lambda x: 10.0 / x**0.2,
    ])

```

```python
plt.plot(x, f_smooth(x))
plt.show()
```


# Lecture 14, Algebraic Modeling Languages

Algebraic Modeling Languages (AML) are high-level computer programming languages for describing and solving high complexity problems for large scale mathematical computation (i.e. large scale optimization type problems).  Their syntax mimics the mathematical notation of optimization problems, which allows one to express optimization problems in a familiar, concise and readable way.

**AMLs do not directly solve the problem, but they call appropriate external solvers to find the solution.**

Examples of AMLs are
* A Mathematical Programming Language (AMPL),
* General Algebraic Modeling System (GAMS),
* Optimization Programming Language (OPL),
* Advanced Interactive Multidimensional Modeling System (AIMMS), and
* Pyomo.

In addition to the ease of modelling, one of the advantages of AMLs is that you can model the problem once and then solve it with multiple solvers.

## Pyomo

On this course, we use Pyomo as an example of AMLs. Pyomo is a Python-based, open-source optimization modeling language with a diverse set of optimization capabilities.

Pyomo may not be a completely typical AML, because Pyomo's modeling objects are embedded within a full-featured high-level programming language providing a rich set of supporting libraries, which distinguishes Pyomo from other AMLs.

Pyomo supports a wide range of problem types, including:
* Linear programming
* Quadratic programming
* Nonlinear programming
* Mixed-integer linear programming
* Mixed-integer quadratic programming
* Mixed-integer nonlinear programming
* Stochastic programming
* Generalized disjunctive programming
* Differential algebraic equations
* Bilevel programming
* Mathematical programs with equilibrium constraints

# Installing Pyomo

The easiest way to install Pyomo is to call
```
pip install pyomo
```
when pip has been installed on your machine.

## Example 1, linear optimization

Let us start with a very simple linear problem
$$
\begin{align}
\min &\qquad   2x_1+3x_2\\
\text{s.t. }& \qquad 2x_1+3x_2\geq 1\\
& \qquad x_1,x_2\geq 0.
\end{align}
$$

```python
from pyomo.environ import *

model = ConcreteModel()

model.x = Var([1,2], domain=NonNegativeReals) #Non-negative variables x[1] and x[2]

model.OBJ = Objective(expr = 2*model.x[1] + 3*model.x[2]) #Objective function

model.Constraint1 = Constraint(expr = 3*model.x[1] + 4*model.x[2] >= 1) #Constraint

```

Once we have defined the problem, we can solve it. Let us start by using glpk, which is an open source linear programming program.

You need to have glpk installed on your system. For details, see https://www.gnu.org/software/glpk/#TOCdownloading. For many Linux distributions, you can install glpk from the repositories by typing
```
sudo yum install glpk
```
```
sudo apt-get install glpk,
```
or whatever your distribution needs.

```python
from pyomo.opt import SolverFactory #Import interfaces to solvers
opt = SolverFactory("glpk") #Use glpk
res = opt.solve(model, tee=True) #Solve the  problem and print the output
print "Solution:"
print "========="
model.x.display() #Print values of x
```

Now, if you have other linear solvers installed on your system, you can use them too. Let us use Cplex, which is a commercial solvers (academic license available).

```python
opt = SolverFactory("cplex")
res = opt.solve(model, tee=True)
print "Solution:"
model.x.display()
```

We can use also gurobi, which is another commercial solver with academic license.

```python
opt = SolverFactory("gurobi")
res = opt.solve(model, tee=True)
print "Solution:"
model.x.display()
```

## Example 2, nonlinear optimization

Let use define optimization problem
$$
\begin{align}
\min &\qquad c_d\\
\text{s.t. }& \qquad c_{af}s_v - s_vc_a-k_1c_a=0\\
&\qquad s_vc_b+k_1c_a-k_2c_b=0\\
&\qquad s_vc_c+k_2c_b=0\\
&\qquad s_vc_d+k_3c_a^2=0,\\
&\qquad s_v,c_a,c_b,c_c,c_d\geq0
\end{align}
$$
where $k_1=5/6$, $k_2=5/3$, $k_3=1/6000$, and $c_{af}=10000$.

```python
from pyomo.environ import *
# create the concrete model
model = ConcreteModel()
# set the data
k1 = 5.0/6.0
k2 = 5.0/3.0
k3 = 1.0/6000.0
caf = 10000.0
# create the variables
model.sv = Var(initialize = 1.0, within=PositiveReals)
model.ca = Var(initialize = 5000.0, within=PositiveReals)
model.cb = Var(initialize = 2000.0, within=PositiveReals)
model.cc = Var(initialize = 2000.0, within=PositiveReals)
model.cd = Var(initialize = 1000.0, within=PositiveReals)

# create the objective
model.obj = Objective(expr = model.cb, sense=maximize)
# create the constraints
model.ca_bal = Constraint(expr = (0 == model.sv * caf \
    - model.sv * model.ca - k1 * model.ca \
    - 2.0 * k3 * model.ca ** 2.0))
model.cb_bal = Constraint(expr=(0 == -model.sv * model.cb \
    + k1 * model.ca - k2 * model.cb))
model.cc_bal = Constraint(expr=(0 == -model.sv * model.cc \
    + k2 * model.cb))
model.cd_bal = Constraint(expr=(0 == -model.sv * model.cd \
    + k3 * model.ca ** 2.0))
```

## Solving with Ipopt

Install IPopt following http://www.coin-or.org/Ipopt/documentation/node10.html.

```python
opt = SolverFactory("ipopt",solver_io="nl")

opt.solve(model,tee=True)

print "Solution is "
model.sv.display()
model.ca.display()
model.cb.display()
model.cc.display()
model.cd.display()
```

# Example 3, Nonlinear multiobjective optimization

Let us study optimization problem
$$
\begin{align}
\min \ & \left(\sum_{i=1}^{48}\frac{\sqrt{1+x_i^2}}{v_i},\sum_{i=1}^{48}\left(\left(\frac{x_iv_i}{\sqrt{1+x_i^2}}+v_w\right)^2+\frac{v_i^2}{1+x_i^2}\right)\right., \\
&\qquad\left.\sum_{i=1}^{47}\big|(x_{i+1}-x_i\big|\right)\\
\text{s.t. } & \sum_{i=1}^{j}x_i\leq -1\text{ for all }j=10,11,12,13,14\\
& \left|\sum_{i=1}^{j}x_i\right|\geq 2\text{ for all }j=20,21,22,23,24\\
& \sum_{i=1}^{j}x_i\geq 1\text{ for all }j=30,31,32,33,34\\
&\sum_{i=1}^{48}\frac{\sqrt{1+x_i^2}}{v_i} \leq 5\\
&\sum_{i=1}^{48}x_i=0\\
&-10\leq\sum_{i=1}^{j}x_i\leq10\text{ for all }j=1,\ldots,48
&0\leq v_i\leq 25\text{ for all }i=1,\ldots,48\\
&-10\leq x_i\leq 10\text{ for all }i=1,\ldots,48\\
\end{align}
$$

```python

from pyomo.environ import *
# create the concrete model9
def solve_ach(reference,lb,ub):
    model = ConcreteModel()

    vwind = 5.0
    min_speed = 0.01

    #f1, time used
    def f1(model):
        return sum([sqrt(1+model.y[i]**2)/model.v[i] for i in range(48)])
    #f2, wind drag, directly proportional to square of speed wrt. wind
    def f2(model):
        return sum([((model.y[i]*model.v[i])/sqrt(1+model.y[i]**2)+vwind)**2/
                    +model.v[i]**2*((1+model.y[i])**2) for i in range(48)])
    #f3, maximal course changes
    def f3(model):
        return sum([abs(model.y[i+1]-model.y[i]) for i in range(47)])

    def h1_rule(model,i):
        return sum(model.y[j] for j in range(i))<=-1
    def h2_rule(model,i):
        return abs(sum(model.y[j] for j in range(i)))>=2
    def h3_rule(model,i):
        return sum(model.y[j] for j in range(i))>=1
    def h4_rule(model):
        return sum([sqrt(1+model.y[i]**2)/model.v[i] for i in range(48)])<=25
    def h5_rule(model):
        return sum(model.y[i] for i in range(48))==0

    def f_rule(model):
        return t

    def y_init(model,i):
        if i==0:
            return -1
        if i==18:
            return -1
        if i==24:
            return 1
        if i==25:
            return 1
        if i==26:
            return 1
        if i==34:
            return -1
        return 0
    model.y = Var(range(48),bounds = (-10,10),initialize=y_init)
    model.v = Var(range(48),domain=NonNegativeReals,bounds=(min_speed,25),initialize=25)
    model.t = Var()
    model.h1=Constraint(range(9,14),rule=h1_rule)
    model.h2=Constraint(range(19,24),rule=h2_rule)
    model.h3=Constraint(range(29,34),rule=h3_rule)
    model.h4=Constraint(rule=h4_rule)
    model.h5=Constraint(rule=h5_rule)

    def h6_rule(model,i):
        return -10<=sum([model.y[j] for j in range(i)])<=10

    model.h6 = Constraint(range(1,48),rule=h6_rule)
    def t_con_f1_rule(model):
        return model.t>=(f1(model)-reference[0]-lb[0])/(ub[0]-lb[0])
    model.t_con_f1 = Constraint(rule = t_con_f1_rule)
    def t_con_f2_rule(model):
        return model.t>=(f2(model)-reference[1]-lb[1])/(ub[1]-lb[1])
    model.t_con_f2 = Constraint(rule = t_con_f2_rule)
    def t_con_f3_rule(model):
        return model.t>=(f3(model)-reference[2]-lb[2])/(ub[2]-lb[2])
    model.t_con_f3 = Constraint(rule = t_con_f3_rule)
    model.f = Objective(expr = model.t+1e-10*(f1(model)+f2(model)+f3(model)))
    tee =False
    opt = SolverFactory("ipopt",solver_io="nl")
    opt.options.max_iter=100000
    #opt.options.constr_viol_tol=0.01
    #opt.options.halt_on_ampl_error = "yes"

    opt.solve(model,tee=tee)
    return [[value(f1(model)),value(f2(model)),value(f3(model))],[model.y,model.v]]

```

```python
lb_ = [0,0,0]
ub_ = [1,1,1]
values =[]
for i in range(3):
    reference = [1e10,1e10,1e10]
    reference[i]=0
    values.append(solve_ach(reference,ub_,lb_)[0])
print values
```

    WARNING - Loading a SolverResults object with a warning status into model=unknown; message from solver=Ipopt 3.12\x3a Maximum Number of Iterations Exceeded.
    WARNING - Loading a SolverResults object with a warning status into model=unknown; message from solver=Ipopt 3.12\x3a Maximum Number of Iterations Exceeded.
    [[25133.93493837363, 759264160.1226324, 860.0], [25.000000247539568, 11188.12151236145, 147.55358533550918], [12673.93579526275, 289156195.6539606, 151.2406436251979]]

```python
lb = [0,0,0]
ub = [1,1,1]
for i in range(3):
    lb[i] = min([values[j][i] for j in range(3)])
    ub[i] = max([values[j][i] for j in range(3)])
print lb
print ub
```

    [25.000000247539568, 11188.12151236145, 147.55358533550918]
    [25133.93493837363, 759264160.1226324, 860.0]

```python
[f,x] = solve_ach([(a+b)/2 for (a,b) in zip(lb,ub)],lb,ub) #Compromise solution
#[f,x] = solve_ach([1e10,1e10,0],lb,ub) #Minimize the third objective

```

```python
import matplotlib.pyplot as plt
from matplotlib.patches import Rectangle
plt.plot([sum(value(x[0][j]) for j in range(i)) for i in range(49)])
currentAxis = plt.gca()
currentAxis.add_patch(Rectangle((10, -1),4,10))
currentAxis.add_patch(Rectangle((20, -2),4,4))
currentAxis.add_patch(Rectangle((30, -10),4,11))
plt.show()
```

## Black-box optimization problem

```python
import sys
from pyomo.opt.blackbox import RealOptProblem

class RealProblem1(RealOptProblem):

    def __init__(self):
        RealOptProblem.__init__(self)
        self.lower=[0.0, -1.0, 1.0, None]
        self.upper=[None, 0.0, 2.0, -1.0]
        self.nvars=4

    def function_value(self, point):
        self.validate(point)
        return point.vars[0] - point.vars[1] + (point.vars[2]-1.5)**2 + (point.vars[3]+2)**4

problem = RealProblem1()

```

Lecture 14, Algebraic Modeling Languages, especially Pyomo.ipynb


# Item III

*Solving a very (un)known problem*

1. Implement a function that finds the two roots of the quadratic equation $a x^2 + b x + c = 0$ given $a$,$b$, and $c$, i.e. implement $x_{\pm} = \frac{-b \pm \sqrt{b^2-4ac}}{2a}$.
2. What are the roots of $2x^2+10^9+1 = 0$? How many digits of significance can you get for the two roots? Is there any problem?
3. Design a code that finds the correct roots for $x^2+Bx+C=0$, given $B \gg C$ to at least 2 digits of significance.
4. Solve the previous equation using this new code and find the new roots. *I hope it works!*
5. From the well-known solution $x_{\pm}$ of the quadratic equation design an algorithm that approximates the two roots of $x^2+Bx+C = 0$, given $B \gg C$. Hint: *A Taylor expansion may work*.
---

## Part 1

```python
def solve_quadratic(a,b,c):
    assert(a!=0)
    disc = (b**2-4*a*c+0j)**0.5
    x1 = (-b-disc)/(2*a)
    x2 = (-b+disc)/(2*a)
    return (x1,x2)
```

## Part 2

The analytical solution is:
$$
x_{\pm} = \frac{-10^{9}\pm\sqrt{10^{18}-8}}{4}
$$

```python
# if we use the solver
x1,x2 = solve_quadratic(2,1e9,1)
print("x-:",x1)
print("x+:",x2)
```

    x-: (-500000000+0j)
    x+: 0j

The approximation given for $x_{-}$ is good since the relative error is very little (the approximation $\sqrt{10^{18}-8} \approx 10^{9}$ that the computer does because of the *absorption* of the much smaller $-8$ doesn't affect the relative error too much).
<!-- we have as many digits of significance as the double's allow, since -->

For $x_{+}$ however, the error is equal to the value of the root, since

## Part 3

We make the following change to the equation:
\begin{align}
x_{\pm} = \frac{-b \pm \sqrt{b^2-4ac}}{2a} &= \frac{-b \pm \sqrt{b^2-4ac}}{2a} \cdot \frac{-b \mp \sqrt{b^2-4ac}}{-b \mp \sqrt{b^2-4ac}}
\\ &= \frac{4ac}{2a \left(-b \mp \sqrt{b^2-4ac}\right)}
\\ &= \frac{-2c}{\left(b \pm \sqrt{b^2-4ac}\right)}
\end{align}
we can use it for $x_{+}$ if $b>0$ or $x_{-}$ otherwise.

```python
def solve_quadratic_2(a,b,c):
    assert(a!=0)
    disc = (b**2-4*a*c+0j)**0.5
    if b>0:
        x1 = (-b-disc)/(2*a)
        x2 = -2*c/(b+disc)
    else:
        x1 = -2*c/(b-disc)
        x2 = (-b+disc)/(2*a)
    return (x1,x2)
```

## Part 4

We solve using the new method and see that it works.

```python
x1,x2 = solve_quadratic_2(2,1e9,1)
print("x-:",x1)
print("x+:",x2)
```

    x-: (-500000000+0j)
    x+: (-1e-09+0j)

## Part 5

If $C$ is small, we can approximate the solution:
$$
x = x_0 + C x_1 + C^2 x_2 + ...
$$

Then
$$
\begin{align}
x^2 + B x + C &= 0
\\ (x_0^2 + C (2x_0x_1) + C^2 (x_1^2 + 2x_0x_2) + \dots) + (Bx_0 + C B x_1 + C^2 B x_2 + \dots) + C &= 0
\end{align}
$$
And we have the following equations:
\begin{align}
O(C^0) :  &\qquad x_0^2 + B x_0 = 0
\\ O(C^1) :  &\qquad 2x_0x_1 + B x_1 +1 = 0
\\ O(C^2) :  &\qquad x_1^2 + 2 x_0x_2 + B x_2 = 0
\end{align}
which have the following solutions:
$$
(x_0,x_1,x_2)_1 = \left(0,\frac{-1}{B},\frac{-1}{B^3}\right)
$$
$$
(x_0,x_1,x_2)_2 = \left(-B,\frac{1}{B},\frac{1}{B^3}\right)
$$
which result in the following approximations for $x$:
$$
\begin{align}
x_{1} &= 0 + C \frac{-1}{B} + C^2 \frac{-1}{B^3} + \cdots
\\ x_{2} &= -B + C \frac{1}{B} + C^2 \frac{1}{B^3} + \cdots
\end{align}
$$

```python
def solve_quadratic_3(a,b,c):
    # In case a!=1 we just have to scale the equation:
    b /= a
    c /= a
    # Approximations:
    x1 = 0+c*(-1/b)+c**2*(-1/b**3)
    x2 = -b+c*(1/b)+c**2*(1/b**3)
    return (x1,x2)
```

```python
x1,x2 = solve_quadratic_3(2,1e9,1)
print("x1:",x1)
print("x2:",x2)
```

    x1: -1e-09
    x2: -500000000.0

```python

```


# integrate functions

```python
from scipy import integrate
import numpy as np
import matplotlib.pyplot as plt
import sympy as sy
sy.init_printing()
```

```python
a,b=-1,1
x=np.linspace(a,b,17)
y=np.exp(-x)
```

```python
val_trapz=integrate.trapz(y,x)
val_trapz
```

```python
val_simps=integrate.simps(y,x)
val_simps
```

```python
val_true=-np.exp(-b)+np.exp(-a)
```

```python
print(val_true-val_trapz)
```

    -0.00305962308718

```python
print(val_true-val_simps)
```

    -3.18201703609e-06

```python
x=np.linspace(a,b,1+2**4)
y=np.exp(-x)
dx=x[1]-x[0]
val_romb=integrate.romb(y,dx=dx)
val_romb
```

```python
print(val_true-val_romb)
```

    -4.20898871312e-11

```python
f=lambda x:np.exp(-x**2) * (x**12-x**5)
val,err=integrate.quad(f,0,np.inf)
val
```

```python
err
```

# 多重積分
$$\int_{x=a}^b \int_{y=g_1(x)}^{g_2(x)} f(x,y) dxdy$$
where $g_1=-1$ and $g_2=0$

```python
def f(x,y):
    return 4-x**2-y**2
a,b=0,1
g1,g2=lambda x:-1,lambda x:0
integrate.dblquad(f,a,b,g1,g2)
```

```python
val,err=integrate.dblquad(f,0,1,lambda x:x-1,lambda x:1-x)
val,err
```

$$\int_{a}^{b} \int_{y=g_1(x)}^{g_2(x)} \int_{z=h_1(x,y)}^{h_2(x,y)} f(x,y,z)dxdydz$$

```python
ff=lambda x,y,z:(x+y+z)**2
a,b=-1,1
g1,g2=lambda x:-1,lambda x:1
h1,h2=lambda x,y:-1,lambda x,y:1
var,err=integrate.tplquad(ff,a,b,g1,g2,h1,h2)
val
```

```python
val,err=integrate.nquad(f,[(-1,1),(-1,1),(-1,1)])
val
```

```python

```


# Solving equations and inequalities

```
from sympy import *
init_printing()
```

```
var('x y z a')
```

```
solve(x**2 - a, x)
```

```
x = Symbol('x')
solve_univariate_inequality(x**2 > 4, x)
```

```
solve([x + 2*y + 1, x - 3*y - 2], x, y)
```

```
solve([x**2 + y**2 - 1, x**2 - y**2 - S(1) / 2], x, y)
```

```
solve([x + 2*y + 1, -x - 2*y - 1], x, y)
```

```
var('a b c d u v')
```

```
M = Matrix([[a, b, u], [c, d, v]])
M
```

```
solve_linear_system(M, x, y)
```

```
det(M[:2, :2])
```


# Runge-Kutta methods and higher-order ODEs

Runge-Kutta methods are a broad class of useful ODE solvers. In this notebook we look at a few of them, their convergence rates, and how to apply them to second-order equations

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

# The below commands make the font and image size bigger
plt.rcParams.update({'font.size': 22})
plt.rcParams["figure.figsize"] = (15,10)
```

We will define an `ODESolve` function that can use different methods. First let's define the stepping functions for the Euler, RK2 and RK4 methods.

```python
def EulerStep(f, dx, xi, yi):
    return yi + dx*f(xi, yi)
```

```python
def RK2Step(f, dx, xi, yi):
    k1 = dx*f(xi, yi)
    k2 = dx*f(xi + dx, yi + k1)

    return yi + 0.5*(k1 + k2)
```

```python
def RK4Step(f, dx, xi, yi):
        k1 = dx*f(xi,yi)
        k2 = dx*f(xi + 0.5*dx, yi + 0.5*k1)
        k3 = dx*f(xi + 0.5*dx, yi + 0.5*k2)
        k4 = dx*f(xi + dx, yi + k3)

        return yi + 1/6*(k1 + 2*k2 + 2*k3 + k4)
```

The method in the below function can be set using the optional 6th argument.

```python
def ODESolve(f, dx, x0, y0, imax, method='RK4', plotSteps=False):

    xi = x0
    yi = y0

    # Create arrays to store the steps in
    steps = np.zeros((imax+1,2))
    steps[0,0] = x0
    steps[0,1] = y0

    i = 0
    while i < imax:
        if(method == 'RK4'):
            yi = RK4Step(f, dx, xi, yi)
        elif(method == 'RK2'):
            yi = RK2Step(f, dx, xi, yi)
        elif(method == 'Euler'):
            yi = EulerStep(f, dx, xi, yi)

        xi += dx
        i  += 1

        # Store the steps for plotting
        steps[i, 0] = xi
        steps[i, 1] = yi

    if(plotSteps):
        plt.scatter(steps[:,0], steps[:,1], color='red', linewidth='10')

    return [xi, yi]
```

```python
def dydx(x,y):
    return -2*x - y
```

```python
def yExact(x):
    return - 3*np.exp(-x) - 2*x + 2
```

```python
x = np.linspace(0, 0.5, 100)
y = yExact(x)
plt.xlabel('x')
plt.ylabel('y')
plt.grid(True)
plt.plot(x,y)

ODESolve(dydx, 0.1, 0, -1, 5, 'RK2', True)
```

## Convergence of the methods
Let's look at the rate of convergence of the three methods: Euler, RK2 and RK4

```python
nmax = 12

diffEuler = np.zeros(nmax)
diffRK2 = np.zeros(nmax)
diffRK4 = np.zeros(nmax)

n = 1
while n < nmax:
    deltax       = 0.1/2**n
    nsteps       = 5*2**n
    resEuler     = ODESolve(dydx, deltax, 0, -1, nsteps, 'Euler')
    resRK2       = ODESolve(dydx, deltax, 0, -1, nsteps, 'RK2')
    resRK4       = ODESolve(dydx, deltax, 0, -1, nsteps, 'RK4')

    diffEuler[n] = np.abs(resEuler[1] - yExact(0.5))
    diffRK2[n]   = np.abs(resRK2[1] - yExact(0.5))
    diffRK4[n]   = np.abs(resRK4[1] - yExact(0.5))
    n += 1

# Plot the results
plt.grid(True)
plt.yscale('log')
plt.xlabel('n')
plt.ylabel('y_i - y(0.5)')
plt.ylim([1e-24, 1])

# Compute and plot reference curves for the convergence rate
x           = np.linspace(1, nmax, 12)
deltax      = (0.1/2**x)
firstOrder  = deltax**1
secondOrder = deltax**2
forthOrder  = deltax**4

plt.plot(x, firstOrder)
plt.plot(x, secondOrder)
plt.plot(x, forthOrder)

plt.scatter(np.arange(1,nmax+1), diffEuler, color='red', linewidth='10')
plt.scatter(np.arange(1,nmax+1), diffRK2, color='orange', linewidth='10')
plt.scatter(np.arange(1,nmax+1), diffRK4, color='green', linewidth='10')

plt.legend(['First-order convergence reference',
            'Second-order convergence reference',
            'Forth-order convergence reference',
            'Convergence of Euler method',
            'Convergence of the RK2 method',
            'Convergence of the RK4 method'
           ]);
```

Thus we see that the RK4 method is rapidly convergent. It does require 4 evaluations of the right-hand side of the equations. The RK4 method has a good balance between between taking the least number of steps and achieving the highest accuracy.

## Second-order ODEs

As we discussed in the lectures we can write any an $n^\text{th}$-order ODE as a coupled system of $n$ first-order ODEs. Let's look at implementing it in practice.

Let's consider a second-order ODE. We want to write this in the form:

$$\begin{align}
    y_0'(x) &= f_0(x, y_0, y_1)\\
    y_1'(x) &= f_1(x, y_0, y_1)
\end{align}$$

We thus want to write a function that when passed $x$ and an array $\textbf{y} = [y_0, y_1]$ returns an array $\textbf{f} = [f_0, f_1]$. Let's look at a specific example. Consider $y''(x) = -y$ with $y(0) = 1$. This has the analytic solution $y(x) = \cos(x)$. Let's write this in first-order form.

Let $y_0(x) = y(x)$ and $y_1 = y_0'(x)$. Then we have $y_1' = -y_0$. Thus

$$\begin{align}
    y_0'(x) &= y_1\\
    y_1'(x) &= -y_0
\end{align}$$

so $\textbf{f} = [y_1, -y_0]$. Let's define a Python function for this.

```python
def f2(x, y):
    return np.array([y[1], -y[0]])
```

```python
def SecondOrderRK2(f, dx, x0, y0, imax):
    output = np.empty((imax, 3))
    i = 0
    xi = x0
    yi = y0
    while(i < imax):
        k1 = dx*f(xi, yi)
        k2 = dx*f(xi + dx, yi + k1)
        yi = yi + 0.5*(k1 + k2)
        xi += dx
        output[i, 0] = xi
        output[i, 1] = yi[0]
        output[i, 2] = yi[1]
        i += 1
    return output
```

```python
res = SecondOrderRK2(f2, 0.1, 0, [1,0], 200);
```

```python
x = np.linspace(0,20,400)
y = np.cos(x)

plt.grid(True)
plt.scatter(res[:,0], res[:,1], color='r')
plt.plot(x, y);
```

```python

```


# Discrete Fourier Transform in Python

This notebook is a quick refresher on how to perform FFT in python/scipy.

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

```python
from scipy.fftpack import fft
```

We define:

- $N$: number of samples
- $f_s$: sampling frequency/rate in samples/second

```python
N = 1000
f_s = 100
```

Period between samples $T_s$:

```python
T_s = 1/f_s
print(T_s, "seconds")
print(T_s*1000, "ms")
```

    0.01 seconds
    10.0 ms

Create time vector, each element corresponds to a measurement

```python
t = np.linspace(0, T_s*N, N)
```

The signal which we are sampling:

\begin{align}
s(t) = 0.1 sin(2\pi 5t) + sin(2\pi 3t - 0.25\pi)
\end{align}

```python
x_t = 0.1*np.sin(2*np.pi*5*t) + np.sin(2*np.pi*3*t-np.pi/4)
```

```python
plt.figure(figsize=(15,5))
plt.plot(t, x_t)
plt.plot(t, x_t, "k+")
plt.xlabel("time [s]")
plt.xlim([0, 2])
plt.grid()
plt.title("Visualizing samples")
```

Note that we can describe the **period** of each sinus component in number of samples:

- $0.1 sin(2\pi 5t)$: **20** samples ($f=5Hz$ leads to $T=1/5Hz=200ms$ with $T_s = 10ms$, $T/T_s = 20$)
- $sin(2\pi 3t - 0.25\pi)$ : **33** samples

Alternatively we can express the frequency in the reciprocal:

- $0.1 sin(2\pi 5t)$: **1/20 = 0.05**
- $sin(2\pi 3t - 0.25\pi)$ : **1/33 = 0.0303**

Alternatively we can express the frequency relative to the number of samples $N=1000$:

- $0.1 sin(2\pi 5t)$: **1000/20 = 50**
- $sin(2\pi 3t - 0.25\pi)$ : **1000/33 = 30.30**

You can think of the last representation as a reference of the highest $T_s$ (or lowest $f_s$) we can extract from FFT. I.e. the FFT method cannot extract frequency information lower than $\frac{f_s}{2}$ (ignore the $\frac{1}{2}$ for now).

## FFT

We perform the FFT on the sample array, note that the time vector ${t}$ is not used in the `fft` call:

```python
a_f = fft(x_t)
```

```python
a_f.dtype
```

    dtype('complex128')

FFT returns a symmetric shape with positive frequencies on the right side and negative on the left:

```python
plt.figure(figsize=(10,5))
plt.plot(np.abs(a_f)) # we take abs in order to get the magnitude of a complex number
plt.axvline(N//2, color="red", label="left: positive frequencies | right: negative, from high to low")
plt.xlabel("index k")
plt.legend();
```

The index $k$ represents a frequency component.

Because we are interested in positive frequencies for now we cut the returned array in half:

```python
a_f_positive = a_f[:N//2]
```

```python
a_f_positive.shape
```

    (500,)

Each element in `a_f` represents the real and imaginary part (amplitude $A_i$ and phase $\phi_i$) for a specific frequency $f_i$.

The "frequency" after the FFT is defined as $\frac{N}{s_i}$ in the period of specific sinus component. The period $s_i$ is expressed in number of samples.

I.e. a sinus component with a frequency of $5 Hz$ or period of $\frac{1}{5Hz} = 0.2s$ is $\frac{0.2s}{T_s} = \frac{0.2s}{0.01s} = 20$ samples long. Thus its magnitude peak should appear at $\frac{N}{s_i} = \frac{1000}{20} = 50$.

- $0.1 sin(2\pi 5t)$: low peak (because of $0.1$) at $k=50$
- $sin(2\pi 3t - 0.25\pi)$: greater peak at $k= 30.303 \approx 30$

```python
plt.figure(figsize=(10,5))
plt.plot(np.abs(a_f_positive))
plt.xlim([0, 100])
plt.xticks(range(0, 101, 10))
plt.grid()
plt.xlabel("frequency in $k = N/s_i$")
```

In order to relate the sample-frequencies (as $N/1$) into time domain we need to convert the $k$ into frequencies as $1/s$.

\begin{align}
k = \frac{N}{s_i} = \frac{N}{T_i/T_s} = \frac{N f_i}{1/T_s} = \frac{N f_i}{f_s}
\end{align}

Our translation formula from $k$ to frequency is the following

\begin{align}
\Rightarrow f_i =& f_s\frac{k}{N}
\end{align}

```python
f_i = np.arange(0, N//2)*f_s/N
```

```python
plt.figure(figsize=(10,5))
plt.plot(f_i, np.abs(a_f_positive))
plt.grid()
plt.xlabel("frequency in $1/s$")
plt.xticks(range(0, f_s//2, 1));
plt.xlim([0, 10]);
```

We need to normalize the magnitude of the peaks by the factor of $\frac{2}{N}$:

```python
plt.figure(figsize=(10,5))
plt.plot(f_i, 2/N*np.abs(a_f_positive))
plt.grid()
plt.xlabel("frequency in $1/s$ (Hz)")
plt.ylabel("amplitude [1]")
plt.xticks(range(0, f_s//2, 1));
plt.xlim([0, 10]);
plt.ylim([-0.2, 1.2]);
plt.title("Final DFT result.")
plt.text(3, 1.02, "$sin(2\pi 3t - 0.25\pi)$", fontdict={"size": 15})
plt.text(5, 0.12, "$0.1 sin(2\pi 5t)$", fontdict={"size": 15});
```

As you can see we found both sinus components.

## Phase

We could find the magnitudes and the frequencies of both signals but not the $45^\circ$ phase of the slower $3Hz$ signal.

In the previous section we saw that the result of the FFT algorithm is a complex array. Let's plot the real and imaginary parts relative to frequency.

```python
plt.figure(figsize=(15, 5))
plt.subplot(2, 1, 1)
plt.title("real")
plt.plot(f_i, 2/N*np.real(a_f_positive))
plt.grid()
plt.xlim([0, 10])
plt.subplot(2, 1, 2)
plt.title("imag")
plt.plot(f_i, 2/N*np.imag(a_f_positive))
plt.grid()
plt.xlim([0, 10])
```

Lets calculate the angle of the complex number:

\begin{align}
\alpha = \text{arctan} \frac{imag}{real}
\end{align}

There is a handy function: `np.angle` which does it for us.

```python
angle = np.angle(a_f_positive, deg=True)

# OR manually
# angle = np.arctan2(2/N*np.imag(a_f_positive),(2/N*np.real(a_f_positive)))*grad_to_degree_factor
```

and plot it again

```python
plt.figure(figsize=(15, 10))
plt.subplot(3, 1, 1)
plt.ylabel("real-component [1]")
plt.plot(f_i, 2/N*np.real(a_f_positive))
plt.grid()
plt.xlim([0, 10])
plt.subplot(3, 1, 2)
plt.ylabel("imag component [1]")
plt.plot(f_i, 2/N*np.imag(a_f_positive))
plt.grid()
plt.xlim([0, 10])
plt.subplot(3, 1, 3)
plt.plot(f_i, angle)
plt.grid()
plt.ylabel("phase [°]")
plt.xlabel("frequency [Hz]")
plt.xlim([0, 10])

plt.scatter(f_i[[30, 50]], angle[[30, 50]], color="k")
plt.text(f_i[30] + 0.1 , angle[30], "%d°" % int(angle[30]))
plt.text(f_i[50] + 0.1 , angle[50], "%d°" % int(angle[50]))
plt.ylim([-150, 100])
```

The $5Hz$ sinus wave with zero phase has an $\alpha \approx -90^\circ$, since a sine wave is a $90^\circ$-shifted cos wave.

The $3Hz$ sinus component with $45^\circ$ phase has an $\alpha \approx -90^\circ-45^\circ = -135^\circ$

## FFT on complex numbers

Because within the multi-chirp FMCW algorithm we do a FFT on a series of complex numbers we want to make a simple example here.

Our example function of interest will be:

\begin{align}
f(t) = 0.25\text{sin}(2\pi 5 t + \phi) \\
\phi = \phi(t) = -\frac{\pi}{8}t = vt
\end{align}

The phase shift is time dependent in this example.

**Goal**: find parameter $v$ via FFT.

```python
def f(t, phi=0):
    return 0.25*np.sin(2*np.pi*5*t + phi)
```

Let's visualize how the sinus wave develops over time ...

```python
t = np.linspace(0, 10, 10000)
plt.figure(figsize=(15,5))
plt.plot(t, f(t), label="$\phi=0$")
plt.plot(t, f(t, -np.pi/8*t), label="$\phi=-\pi/8 \cdot t$")
plt.xlim([0, 4])
plt.xlabel("$t$ [s]")
plt.grid()
plt.legend();
```

For the sake of our example we will run the FFT each $T_{cycle}$ seconds.

```python
T_cycle = 2 # seconds
n_cycles = 200
f_cycle = 1/T_cycle
```

Per cycle FFT config

```python
f_s = 100
T_s = 1/f_s
N = int(T_cycle/T_s)
print("Sample frequency:", f_s, "Hz")
print("Sample period:", T_s, "sec")
print("Number samples:", N)
```

    Sample frequency: 100 Hz
    Sample period: 0.01 sec
    Number samples: 200

We run FFT in each cycle and save the results in a list.

```python
fft_cycle_results = list() # result list

# for each cycle
for c in range(n_cycles):

    # determine start and end of a cycle
    t_start = c*T_cycle
    t_end = (c+1)*T_cycle

    # sample the signal at according timesteps
    t_sample = np.arange(t_start, t_end, T_s)
    f_sample = f(t_sample, -np.pi/8*t_sample)

    # run FFT and append results
    fft_res = fft(f_sample)
    fft_cycle_results.append(fft_res)
```

We cut the positive frequency range and normalize the amplitudes (see introdcutory example above).

```python
fft_cycle_results = [2/N*r[:N//2] for r in fft_cycle_results]
```

```python
freq = np.arange(0, N//2)*f_s/N
```

```python
freq
```

    array([ 0. ,  0.5,  1. ,  1.5,  2. ,  2.5,  3. ,  3.5,  4. ,  4.5,  5. ,
            5.5,  6. ,  6.5,  7. ,  7.5,  8. ,  8.5,  9. ,  9.5, 10. , 10.5,
           11. , 11.5, 12. , 12.5, 13. , 13.5, 14. , 14.5, 15. , 15.5, 16. ,
           16.5, 17. , 17.5, 18. , 18.5, 19. , 19.5, 20. , 20.5, 21. , 21.5,
           22. , 22.5, 23. , 23.5, 24. , 24.5, 25. , 25.5, 26. , 26.5, 27. ,
           27.5, 28. , 28.5, 29. , 29.5, 30. , 30.5, 31. , 31.5, 32. , 32.5,
           33. , 33.5, 34. , 34.5, 35. , 35.5, 36. , 36.5, 37. , 37.5, 38. ,
           38.5, 39. , 39.5, 40. , 40.5, 41. , 41.5, 42. , 42.5, 43. , 43.5,
           44. , 44.5, 45. , 45.5, 46. , 46.5, 47. , 47.5, 48. , 48.5, 49. ,
           49.5])

**Note**: The FFT frequency resolution is at 1Hz. That's important because the frequency shift by $-\frac{1}{8}Hz$ introduced by $\phi(t)$ is not visible in the FFT!

The FFT will show a peak at 5Hz with a different phase each time.

Because the frequency is almost the same in each cycle, we expect the same behaviour in each result:

```python
n_cycles_to_display = 4
fft_res_display = fft_cycle_results[:n_cycles_to_display]

fig, ax = plt.subplots(ncols=len(fft_res_display), figsize=(15, 3), sharex=True, sharey=True)
for i, ax, res in zip(range(n_cycles_to_display), ax, fft_res_display):
    res_abs = np.abs(res)
    ax.plot(freq, res_abs)
    ax.grid(True)
    ax.set_xlim([0, 10])
    ax.set_xlabel("frequency [Hz]")

    k = np.argmax(res_abs)
    magn_max = res_abs[k]
    freq_max = freq[k]

    ax.set_title("Cycle %d:\n%.2f at %.2f Hz" % (i, magn_max, freq_max))
```

Looks fine for the first 4 cycles ... Let's look at all cycles by picking the frequency with max. magnitude from each cycle:

```python
freq_list = list()
for res in fft_cycle_results:
    res_abs = np.abs(res)
    k = np.argmax(res_abs)
    freq_list.append(freq[k])

plt.figure(figsize=(15,3))
plt.plot(freq_list)
plt.xlabel("cycle nr.")
plt.ylabel("frequency [Hz]")
plt.title("Frequency with max. peak in FFT domain vs. cycle");
```

It seems that the position (frequency) of the peaks remains **eqal**, despite the changing real and imaginary components.

Let's collect the max. frequency component from each cycle

```python
cycle_max_list = list()

for res in fft_cycle_results:
    # calc. the magnitude
    res_abs = np.abs(res)

    # find frequency index
    k = np.argmax(res_abs)
    cycle_max_list.append(res[k])
```

... and visualize the complex numbers:

```python
n_cycles_to_display = 4
cycle_max_list_display = cycle_max_list[:n_cycles_to_display]

fig, ax = plt.subplots(ncols=len(cycle_max_list_display), figsize=(15, 30),
                       subplot_kw={'projection': "polar"}, sharey=True)

for i, ax, res in zip(range(n_cycles_to_display), ax, cycle_max_list_display):
    ax.plot([0, np.angle(res)], [0, np.abs(res)], marker="o")
    ax.text(np.angle(res)+0.1, np.abs(res), "%d°" % int(np.angle(res, deg=True)))
    ax.set_ylim([0, 0.4])
    ax.set_title("Cycle %d:\n" % (i, ))
```

We can observe that the angle moves in negative direction with $-45^\circ = T_{cycle}v = 2\frac{\pi}{8} = \pi/4$ per cycle.

### Solution via phase differences

Now we could calculate ange velocity by taking differences between cycles and put them relative to cycle duration:

```python
angle_diff = np.diff(np.angle(cycle_max_list, deg=True))
angle_vel = angle_diff/T_cycle
print(angle_vel[:10])
```

    [-22.57811002 157.25241595 -22.41998496 -22.25432097 -22.57811002
     -22.74758405 -22.41998496 -22.25432097 -22.57811002 157.25241595]

Let's look at the parameter $v = -\frac{\pi}{8}$

```python
v = -np.pi/8*360/(2*np.pi)
print(v)
```

    -22.5

Let's calculate the differences right (to remove the $157^\circ-(-157^\circ)$ effect).

```python
angle_vel[angle_vel>0] -= 180
print("Angle velocities:", angle_vel[:10])
```

    Angle velocities: [-22.57811002 -22.74758405 -22.41998496 -22.25432097 -22.57811002
     -22.74758405 -22.41998496 -22.25432097 -22.57811002 -22.74758405]

```python
plt.figure(figsize=(15,3))
plt.plot(angle_vel)
plt.xlabel("cycle nr.")
plt.ylabel("°/s")
plt.title("angular velocity derived by cycle FFT phase differences")
plt.ylim([-40, 0]);
```

As you can see, the phases of the FFT output from each cycle give a hint over the phase velocity $v$ of the signal in time domain.

**Summary**: We found $v$!

### Solution via second FFT

The core idea of this alternative approach is to extract the periodic change of phase $\phi(t)$.

We can find the phase velocity via a **second FFT over the cycle results**, too. Consider the first FFT result as a measurement/sample for the second FFT.

Remember, those are our results (FFT-magnitude from the $5Hz$-component):

```python
cycle_max_list[:5]
```

    [(-0.09177962152642351-0.22644808906297761j),
     (-0.22334337399156695-0.09379786966319806j),
     (-0.22407560703847842+0.09379786966349972j),
     (-0.09354738847923255+0.22644808906302294j),
     (0.09177962152656505+0.22644808906289618j)]

```python
# here, we take only the positive side of fft
second_fft_res = fft(cycle_max_list)[:n_cycles//2]
```

```python
second_fft_res[:5]
```

    array([ 2.60416688e-13-3.46972451e-13j,  5.82752770e-12-1.78615130e-11j,
           -1.55815396e-11-1.21250472e-11j, -7.42998983e-13+9.69827294e-12j,
            1.79379736e-11-1.00089983e-11j])

Like in the introductory example, each element of `second_fft_res` represents a frequency component.

```python
freq_second = np.arange(0, n_cycles//2)*f_cycle/n_cycles
omega_second = 360*freq_second  # same as 2*np.pi*
```

```python
omega_second
```

    array([ 0. ,  0.9,  1.8,  2.7,  3.6,  4.5,  5.4,  6.3,  7.2,  8.1,  9. ,
            9.9, 10.8, 11.7, 12.6, 13.5, 14.4, 15.3, 16.2, 17.1, 18. , 18.9,
           19.8, 20.7, 21.6, 22.5, 23.4, 24.3, 25.2, 26.1, 27. , 27.9, 28.8,
           29.7, 30.6, 31.5, 32.4, 33.3, 34.2, 35.1, 36. , 36.9, 37.8, 38.7,
           39.6, 40.5, 41.4, 42.3, 43.2, 44.1, 45. , 45.9, 46.8, 47.7, 48.6,
           49.5, 50.4, 51.3, 52.2, 53.1, 54. , 54.9, 55.8, 56.7, 57.6, 58.5,
           59.4, 60.3, 61.2, 62.1, 63. , 63.9, 64.8, 65.7, 66.6, 67.5, 68.4,
           69.3, 70.2, 71.1, 72. , 72.9, 73.8, 74.7, 75.6, 76.5, 77.4, 78.3,
           79.2, 80.1, 81. , 81.9, 82.8, 83.7, 84.6, 85.5, 86.4, 87.3, 88.2,
           89.1])

```python
plt.figure(figsize=(10,5))
plt.plot(omega_second, np.abs(second_fft_res))
plt.grid()
plt.xlabel("angle velocity $\omega$ [°/s]")
plt.xticks(range(0, 90, 5));
```

As you could see we could detect the phase change $v=22.5^{\circ}$ with a second FFT on the results of the first FFT.

```python

```


## What is a Percepton

A percepton is a type of binary classifier. A binary classfier is a function
which can be decide weather or not an input, represented by a vector of numbers,
belong to a specific class.

The perceptron maps its input $x$ ( a real value vector ) to an output value $f(x)$ ( a single binary value ):

\begin{equation}
    f(x) = \chi(\langle w, x \rangle + b)
\end{equation}

where $w$ is the vector of the weights with real value, $\langle \cdot , \cdot \rangle$ is the scalar product,
$b$ is the bias,  a constant term that doesn't dipend on any input value and $\chi(x)$ is the output fuction, also called
activation function.
The most common choices for $\chi(x)$ are:

- $\chi(x)$ = $sing(x)$
- $\chi(x)$ = $Θ(x)$
- $\chi(x)$ = $x$

where $Θ(x)$ is the Heavside Function.

The perceptron works weel when the learning set is linearly separable, while when the learning isn't linearly separable
its learning algorithm doesn't terminate. If the vector are not linearly separable will never reach a point where all vectors are
classified properly. The most famous example of perceptron inability to solve problems with linearly separable vector is the
boolean **XOR** problem.

## How to train the Perceptron

There are various way to train a perceptron, one of the most effienct ( the one that used here )
is the **Delta Rule**

The Delta Rule is a gradient descend learning rule for updating the weights of the inputs of an artifical
neuron in a single-layer neural network. It is a special case of the more general backpropagation algorithm. For
a neuron $j$ with activation function $g(x)$, the delta rule for $j$'s weight $w_{ji}$ is given by:

\begin{equation}
\Delta w_{ji} = \eta (t_j - y_j)g'(h_i)x_i
\end{equation}

where:

- $\eta$ is a small constant, called learning rate
- $g(x)$ is the neuron activation function
- $g'$ is the derivative of $g$
- $t_j$ us the target output
- $h_i$ is the weighted sum of the neuron's inputs
- $y_j$ is the actual output
- $x_i$ is the ith input

It holds that $h_j = \sum x_i w_{ji}$ and $ y_j = g(h_j)$.
The delta rule is commonly stated is a simplified forn for a neuron with
a linear activation function as

\begin{equation}
\Delta w_{ji} = \eta (t_j - y_j)x_i
\end{equation}

While the delta rule is similar to the percetron's training rule, the derivation is different.
If the percepton uses the Heaviside step function as the activation function , it turn out that
$g(h)$ is no differantiable in zero, which make the direct application of the delta rule impossible.


# Van der Pol oscillator
We will look at the second order differentual equation (see https://en.wikipedia.org/wiki/Van_der_Pol_oscillator):

$$
{d^2y_0 \over dx^2}-\mu(1-y_0^2){dy_0 \over dx}+y_0= 0
$$

```python
from __future__ import division, print_function
import itertools
import numpy as np
import sympy as sp
import matplotlib.pyplot as plt
from pyodesys.symbolic import SymbolicSys
sp.init_printing()
%matplotlib inline
print(sp.__version__)
```

One way to reduce the order of our second order differential equation is to formulate a system of first order ODEs, using:

$$ y_1 = \dot y_0 $$

which gives us:

$$
\begin{cases}
\dot y_0 = y_1 \\
\dot y_1 = \mu(1-y_0^2) y_1-y_0
\end{cases}
$$

Let's call this system of ordinary differential equations vdp1:

```python
vdp1 = lambda x, y, p: [y[1], -y[0] + p[0]*y[1]*(1 - y[0]**2)]
```

```python
y0 = [0, 1]
mu = 2.5
tend = 25
```

```python
odesys1 = SymbolicSys.from_callback(vdp1, 2, 1, names='y0 y1'.split())
odesys1.exprs
```

```python
# Let us plot using 30 data points
res1 = odesys1.integrate(np.linspace(0, tend, 20), y0, [mu], name='vode')
res1.plot()
print(res1.yout.shape)
```

```python
# Let us interpolate between data points
res2 = odesys1.integrate(np.linspace(0, tend, 20), y0, [mu], integrator='cvode', nderiv=1)
res2.plot(m_lim=21)
print(res2.yout.shape)
```

```python
odesys1.integrate(np.linspace(0, tend, 20), y0, [mu], integrator='cvode', nderiv=2)
xplt, yplt = odesys1.plot_result(m_lim=21, interpolate=30)
print(odesys1._internal[1].shape, yplt.shape)
```

Equidistant points are not optimal for plotting this function. Using ``roots`` kwarg we can make the solver report the output where either the function value, its first or second derivative is zero.

```python
odesys2 = SymbolicSys.from_other(odesys1, roots=odesys1.exprs + (odesys1.dep[0],))
# We could also add a higher derivative: tuple(odesys1.get_jac().dot(odesys1.exprs)))
```

```python
# Let us plot using 10 data points
res2 = odesys2.integrate(np.linspace(0, tend, 20), y0, [mu], integrator='cvode',
                                     nderiv=1, atol=1e-4, rtol=1e-4)
xout, yout, info = res2
xplt, yplt = odesys2.plot_result(m_lim=21, interpolate=30, indices=[0])
xroots, yroots = info['roots_output'][0], info['roots_output'][1][:, 0]
plt.plot(xroots, yroots, 'bd')
print(odesys2._internal[1].shape, yplt.shape, xroots.size)
```

```python
odesys2.roots
```

```python
res2.plot(indices=[0])
plt.plot(xplt, [res2.at(_)[0][0, 0] for _ in xplt])
```

```python
res1.plot(indices=[0])
plt.plot(xplt, [res1.at(_, use_deriv=True)[0][0] for _ in xplt])
plt.plot(xplt, [res1.at(_, use_deriv=False)[0][0] for _ in xplt])
```


# Robust Kalman filtering for vehicle tracking

We will try to pinpoint the location of a moving vehicle with high accuracy from noisy sensor data. We'll do this by modeling the vehicle state as a discrete-time linear dynamical system. Standard **Kalman filtering** can be used to approach this problem when the sensor noise is assumed to be Gaussian. We'll use **robust Kalman filtering** to get a more accurate estimate of the vehicle state for a non-Gaussian case with outliers.

# Problem statement

A discrete-time linear dynamical system consists of a sequence of state vectors $x_t \in \mathbf{R}^n$, indexed by time $t \in \lbrace 0, \ldots, N-1 \rbrace$ and dynamics equations

\begin{align}
x_{t+1} &= Ax_t + Bw_t\\
y_t &=Cx_t + v_t,
\end{align}

where $w_t \in \mathbf{R}^m$ is an input to the dynamical system (say, a drive force on the vehicle), $y_t \in \mathbf{R}^r$ is a state measurement, $v_t \in \mathbf{R}^r$ is noise, $A$ is the drift matrix, $B$ is the input matrix, and $C$ is the observation matrix.

Given $A$, $B$, $C$, and $y_t$ for $t = 0, \ldots, N-1$, the goal is to estimate $x_t$ for $t = 0, \ldots, N-1$.

# Kalman filtering

A Kalman filter estimates $x_t$ by solving the optimization problem

\begin{array}{ll}
\mbox{minimize} & \sum_{t=0}^{N-1} \left(
\|w_t\|_2^2 + \tau \|v_t\|_2^2\right)\\
\mbox{subject to} & x_{t+1} = Ax_t + Bw_t,\quad t=0,\ldots, N-1\\
& y_t = Cx_t+v_t,\quad t = 0, \ldots, N-1,
\end{array}

where $\tau$ is a tuning parameter. This problem is actually a least squares problem, and can be solved via linear algebra, without the need for more general convex optimization. Note that since we have no observation $y_{N}$, $x_N$ is only constrained via $x_{N} = Ax_{N-1} + Bw_{N-1}$, which is trivially resolved when $w_{N-1} = 0$ and $x_{N} = Ax_{N-1}$. We maintain this vestigial constraint only because it offers a concise problem statement.

This model performs well when $w_t$ and $v_t$ are Gaussian. However, the quadratic objective can be influenced by large outliers, which degrades the accuracy of the recovery. To improve estimation in the presence of outliers, we can use **robust Kalman filtering**.

# Robust Kalman filtering

To handle outliers in $v_t$, robust Kalman filtering replaces the quadratic cost with a Huber cost, which results in the convex optimization problem

\begin{array}{ll}
\mbox{minimize} & \sum_{t=0}^{N-1} \left( \|w_t\|^2_2 + \tau \phi_\rho(v_t) \right)\\
\mbox{subject to} & x_{t+1} = Ax_t + Bw_t,\quad t=0,\ldots, N-1\\
& y_t = Cx_t+v_t,\quad t=0,\ldots, N-1,
\end{array}

where $\phi_\rho$ is the Huber function
$$
\phi_\rho(a)= \left\{ \begin{array}{ll} \|a\|_2^2 & \|a\|_2\leq \rho\\
2\rho \|a\|_2-\rho^2 & \|a\|_2>\rho.
\end{array}\right.
$$

The Huber penalty function penalizes estimation error linearly outside of a ball of radius $\rho$, whereas in standard Kalman filtering, all errors are penalized quadratically. Thus, large errors are penalized less harshly, making this model more robust to outliers.

# Vehicle tracking example

We'll apply standard and robust Kalman filtering to a vehicle tracking problem with state $x_t \in \mathbf{R}^4$, where
$(x_{t,0}, x_{t,1})$ is the position of the vehicle in two dimensions, and $(x_{t,2}, x_{t,3})$ is the vehicle velocity.
The vehicle has unknown drive force $w_t$, and we observe noisy measurements of the vehicle's position, $y_t \in \mathbf{R}^2$.

The matrices for the dynamics are

$$
A = \begin{bmatrix}
1 & 0 & \left(1-\frac{\gamma}{2}\Delta t\right) \Delta t & 0 \\
0 & 1 & 0 & \left(1-\frac{\gamma}{2} \Delta t\right) \Delta t\\
0 & 0 & 1-\gamma \Delta t & 0 \\
0 & 0 & 0 & 1-\gamma \Delta t
\end{bmatrix},
$$

$$
B = \begin{bmatrix}
\frac{1}{2}\Delta t^2 & 0 \\
0 & \frac{1}{2}\Delta t^2 \\
\Delta t & 0 \\
0 & \Delta t \\
\end{bmatrix},
$$

$$
C = \begin{bmatrix}
1 & 0 & 0 & 0 \\
0 & 1 & 0 & 0
\end{bmatrix},
$$
where $\gamma$ is a velocity damping parameter.

# 1D Model
The recurrence is derived from the following relations in a single dimension. For this subsection, let $x_t, v_t, w_t$ be the vehicle position, velocity, and input drive force. The resulting acceleration of the vehicle is $w_t - \gamma v_t$, with $- \gamma v_t$ is a damping term depending on velocity with parameter $\gamma$.

The discretized dynamics are obtained from numerically integrating:
$$
\begin{align}
x_{t+1} &= x_t + \left(1-\frac{\gamma \Delta t}{2}\right)v_t \Delta t + \frac{1}{2}w_{t} \Delta t^2\\
v_{t+1} &= \left(1-\gamma\right)v_t + w_t \Delta t.
\end{align}
$$

Extending these relations to two dimensions gives us the dynamics matrices $A$ and $B$.

## Helper Functions

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

def plot_state(t,actual, estimated=None):
    '''
    plot position, speed, and acceleration in the x and y coordinates for
    the actual data, and optionally for the estimated data
    '''
    trajectories = [actual]
    if estimated is not None:
        trajectories.append(estimated)

    fig, ax = plt.subplots(3, 2, sharex='col', sharey='row', figsize=(8,8))
    for x, w in trajectories:
        ax[0,0].plot(t,x[0,:-1])
        ax[0,1].plot(t,x[1,:-1])
        ax[1,0].plot(t,x[2,:-1])
        ax[1,1].plot(t,x[3,:-1])
        ax[2,0].plot(t,w[0,:])
        ax[2,1].plot(t,w[1,:])

    ax[0,0].set_ylabel('x position')
    ax[1,0].set_ylabel('x velocity')
    ax[2,0].set_ylabel('x input')

    ax[0,1].set_ylabel('y position')
    ax[1,1].set_ylabel('y velocity')
    ax[2,1].set_ylabel('y input')

    ax[0,1].yaxis.tick_right()
    ax[1,1].yaxis.tick_right()
    ax[2,1].yaxis.tick_right()

    ax[0,1].yaxis.set_label_position("right")
    ax[1,1].yaxis.set_label_position("right")
    ax[2,1].yaxis.set_label_position("right")

    ax[2,0].set_xlabel('time')
    ax[2,1].set_xlabel('time')

def plot_positions(traj, labels, axis=None,filename=None):
    '''
    show point clouds for true, observed, and recovered positions
    '''
    matplotlib.rcParams.update({'font.size': 14})
    n = len(traj)

    fig, ax = plt.subplots(1, n, sharex=True, sharey=True,figsize=(12, 5))
    if n == 1:
        ax = [ax]

    for i,x in enumerate(traj):
        ax[i].plot(x[0,:], x[1,:], 'ro', alpha=.1)
        ax[i].set_title(labels[i])
        if axis:
            ax[i].axis(axis)

    if filename:
        fig.savefig(filename, bbox_inches='tight')
```

## Problem Data

We generate the data for the vehicle tracking problem. We'll have $N=1000$, $w_t$ a standard Gaussian, and $v_t$ a standard Guassian, except $20\%$ of the points will be outliers with $\sigma = 20$.

Below, we set the problem parameters and define the matrices $A$, $B$, and $C$.

```python
n = 1000 # number of timesteps
T = 50 # time will vary from 0 to T with step delt
ts, delt = np.linspace(0,T,n,endpoint=True, retstep=True)
gamma = .05 # damping, 0 is no damping

A = np.zeros((4,4))
B = np.zeros((4,2))
C = np.zeros((2,4))

A[0,0] = 1
A[1,1] = 1
A[0,2] = (1-gamma*delt/2)*delt
A[1,3] = (1-gamma*delt/2)*delt
A[2,2] = 1 - gamma*delt
A[3,3] = 1 - gamma*delt

B[0,0] = delt**2/2
B[1,1] = delt**2/2
B[2,0] = delt
B[3,1] = delt

C[0,0] = 1
C[1,1] = 1
```

# Simulation

We seed $x_0 = 0$ (starting at the origin with zero velocity) and simulate the system forward in time. The results are the true vehicle positions `x_true` (which we will use to judge our recovery) and the observed positions `y`.

We plot the position, velocity, and system input $w$ in both dimensions as a function of time.
We also plot the sets of true and observed vehicle positions.

```python
sigma = 20
p = .20
np.random.seed(6)

x = np.zeros((4,n+1))
x[:,0] = [0,0,0,0]
y = np.zeros((2,n))

# generate random input and noise vectors
w = np.random.randn(2,n)
v = np.random.randn(2,n)

# add outliers to v
np.random.seed(0)
inds = np.random.rand(n) <= p
v[:,inds] = sigma*np.random.randn(2,n)[:,inds]

# simulate the system forward in time
for t in range(n):
    y[:,t] = C.dot(x[:,t]) + v[:,t]
    x[:,t+1] = A.dot(x[:,t]) + B.dot(w[:,t])

x_true = x.copy()
w_true = w.copy()

plot_state(ts,(x_true,w_true))
plot_positions([x_true,y], ['True', 'Observed'],[-4,14,-5,20],'rkf1.pdf')
```

# Kalman filtering recovery

The code below solves the standard Kalman filtering problem using CVXPY. We plot and compare the true and recovered vehicle states. Note that the recovery is distorted by outliers in the measurements.

```python
%%time
import cvxpy as cp

x = cp.Variable(shape=(4, n+1))
w = cp.Variable(shape=(2, n))
v = cp.Variable(shape=(2, n))

tau = .08

obj = cp.sum_squares(w) + tau*cp.sum_squares(v)
obj = cp.Minimize(obj)

constr = []
for t in range(n):
    constr += [ x[:,t+1] == A*x[:,t] + B*w[:,t] ,
                y[:,t]   == C*x[:,t] + v[:,t]   ]

cp.Problem(obj, constr).solve(verbose=True)

x = np.array(x.value)
w = np.array(w.value)

plot_state(ts,(x_true,w_true),(x,w))
plot_positions([x_true,y], ['True', 'Noisy'], [-4,14,-5,20])
plot_positions([x_true,x], ['True', 'KF recovery'], [-4,14,-5,20], 'rkf2.pdf')

print("optimal objective value: {}".format(obj.value))
```

# Robust Kalman filtering recovery

Here we implement robust Kalman filtering with CVXPY. We get a better recovery than the standard Kalman filtering, which can be seen in the plots below.

```python
%%time
import cvxpy as cp

x = cp.Variable(shape=(4, n+1))
w = cp.Variable(shape=(2, n))
v = cp.Variable(shape=(2, n))

tau = 2
rho = 2

obj = cp.sum_squares(w)
obj += cp.sum([tau*cp.huber(cp.norm(v[:,t]),rho) for t in range(n)])
obj = cp.Minimize(obj)

constr = []
for t in range(n):
    constr += [ x[:,t+1] == A*x[:,t] + B*w[:,t] ,
                y[:,t]   == C*x[:,t] + v[:,t]   ]

cp.Problem(obj, constr).solve(verbose=True)

x = np.array(x.value)
w = np.array(w.value)

plot_state(ts,(x_true,w_true),(x,w))
plot_positions([x_true,y], ['True', 'Noisy'], [-4,14,-5,20])
plot_positions([x_true,x], ['True', 'Robust KF recovery'], [-4,14,-5,20],'rkf3.pdf')

print("optimal objective value: {}".format(obj.value))
```


```python
import sympy as sp
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
```

```python
# import symbols
g, b, t, d, A, delt, l0, gam, A0 = sp.symbols(r'g \beta t \rho A \delta l_0, \gamma, A_0')
```

# The motion of the pendulum can be attributed to the superposition of 2 damped perpendicular SHMs who have angular frequencies as multiples of the variable angular frequency of the pendulum.

$$x = A_1(t)\;sin(n\omega t)\newline
y = A_2(t)\;sin(m\omega t + \delta)$$

```python
# Define the values of n amd m. By default, these are set two 6 and 5 respesctively.
try:
    n = int(input('Please enter n: '))
    m = int(input('Please enter m: '))
except:
    n = 6
    m = 5
```

### $$Let\; \delta = \text{initial phase difference of the perpendicular SHMs}, \newline
l_0 = \text{initial length of pendulum}\newline
\beta = \text{rate of mass outflow}\newline
\rho = \text{density of sand}\newline
A = \text{area of container}$$

```python
#Defining the substitutions for later use
subst = {A0 : 5, #initial amplitude of pendulum
         g: 9.8, # acceleration due to gravity
         d: 10, # density of sand
         l0: 1, #initial length of pendulum
         gam: 0.1, #damping coefficient
         b: 0.5, #rate of mass outflow
         A: 0.4, # c.s area of the container
         delt:0 # initial phase difference of the two SHMs
        }
```

$$\omega(t) =\sqrt{\frac{g}{l_0 + \frac{\beta t}{\rho a}}}\newline
x = A_1(t)\;sin(\omega t)\newline
y = A_2(t)\; sin(\omega t + \delta)\newline
\frac{x^2}{A_1^2} + \frac{y^2}{A_2^2} - \frac{2xy\;cos(\delta)}{A_1A_2} = sin^2(\delta) - Equation\;of\;the\;curves
$$

```python
# define the time dependent symbolic functions
w = sp.Function(r'\omega')(t)
A1 = sp.Function(r'A_1')(t)
A2 = sp.Function(r'A_2')(t)
```

```python
# Define the angular frequency and the amplitudes
w = sp.sqrt(g/(l0 + (b*t/(d*A))))
A1 = A0*sp.exp(-gam*t)
A2 = A0*sp.exp(-gam*t)
```

# The amplitudes of the perpendicular SHMs are given as
## $$A_1(t) = A_0e^{-\beta t}\newline
A_2(t) = A_0e^{-\beta t}$$
## The individual perpendicular SHMs are damped and hence follow the general equation:
## $$ \ddot{x} + 2\beta\dot{x} + kx = 0, \newline \;where\; a,and\; \beta\; are \;constants \;and\; \beta =\;damping\; parameter.$$

The working formula of the angular frequency of the pendulum is
$$ \omega(t) =\sqrt{\frac{g}{l_0 + \frac{\beta t}{\rho a}}}$$

The effective length of the pendulum is also changing. Hence, the working formula for the effective length of the pendulum is
$$ l_{eff}(t) = L_0 + \frac{\beta}{A\rho}t$$

## The x and y components of the curves are given by the individual damped SHMs:
### $$x = A_1(t)\;sin(n\omega t)\newline
y = A_2(t)\;sin(m\omega t + \delta)$$

```python
# Define the symbolic x values and the y values
x = A1*sp.sin(n*w*t)
y = A2*sp.sin(m*w*t + delt)
```

```python
# Convert the symbolic expressions to functions for plotting
w_f = sp.lambdify(t, w.subs(subst))
x_pts = sp.lambdify(t, x.subs(subst))
y_pts = sp.lambdify(t, y.subs(subst))

# Defining the time of plotting
time = np.linspace(0,20,1000)

# create the data points
x_div = x_pts(time)
y_div = y_pts(time)
```

```python
# animating the graph

#Set the figure
fig, ax = plt.subplots(figsize=(12,10))
ax.grid()
ax.set_xlabel(r'$x\rightarrow$')
ax.set_ylabel(r'$y\rightarrow$')
ax.set(xlim = (-5, 5), ylim = (-5,5))
line, =  ax.plot([],[], 'ro-') # blank line object for the lines

# define the animating function which append to the empty line object for every x and y value
def animate(i):
    line.set_data(x_div[:i], y_div[:i])
    return line,

# Create the animation
anim = FuncAnimation(fig, animate, frames = len(time), blit = True, interval = 50)

# Change the output format to JavaScript Html 'jshtml'
plt.rcParams['animation.html'] = 'jshtml'

plt.rcParams['animation.embed_limit'] = 50

# Call the animation
anim
```


### Evaluate a polynomial string

```python
def symbolize(s):
    """
    Converts a a string (equation) to a SymPy symbol object
    """
    from sympy import sympify
    s1=s.replace('.','*')
    s2=s1.replace('^','**')
    s3=sympify(s2)

    return(s3)
```

```python
def eval_multinomial(s,vals=None,symbolic_eval=False):
    """
    Evaluates polynomial at vals.
    vals can be simple list, dictionary, or tuple of values.
    vals can also contain symbols instead of real values provided those symbols have been declared before using SymPy
    """
    from sympy import Symbol
    sym_s=symbolize(s)
    sym_set=sym_s.atoms(Symbol)
    sym_lst=[]
    for s in sym_set:
        sym_lst.append(str(s))
    sym_lst.sort()
    if symbolic_eval==False and len(sym_set)!=len(vals):
        print("Length of the input values did not match number of variables and symbolic evaluation is not selected")
        return None
    else:
        if type(vals)==list:
            sub=list(zip(sym_lst,vals))
        elif type(vals)==dict:
            l=list(vals.keys())
            l.sort()
            lst=[]
            for i in l:
                lst.append(vals[i])
            sub=list(zip(sym_lst,lst))
        elif type(vals)==tuple:
            sub=list(zip(sym_lst,list(vals)))
        result=sym_s.subs(sub)

    return result
```

### Helper function for flipping binary values of a _ndarray_

```python
def flip(y,p):
    import numpy as np
    lst=[]
    for i in range(len(y)):
        f=np.random.choice([1,0],p=[p,1-p])
        lst.append(f)
    lst=np.array(lst)
    return np.array(np.logical_xor(y,lst),dtype=int)
```

### Classification sample generation based on a symbolic expression

```python
def gen_classification_symbolic(m=None,n_samples=100,n_features=2,flip_y=0.0):
    """
    Generates classification sample based on a symbolic expression.
    Calculates the output of the symbolic expression at randomly generated (Gaussian distribution) points and
    assigns binary classification based on sign.
    m: The symbolic expression. Needs x1, x2, etc as variables and regular python arithmatic symbols to be used.
    n_samples: Number of samples to be generated
    n_features: Number of variables. This is automatically inferred from the symbolic expression. So this is ignored
                in case a symbolic expression is supplied. However if no symbolic expression is supplied then a
                default simple polynomial can be invoked to generate classification samples with n_features.
    flip_y: Probability of flipping the classification labels randomly. A higher value introduces more noise and make
            the classification problem harder.
    Returns a numpy ndarray with dimension (n_samples,n_features+1). Last column is the response vector.
    """

    import numpy as np
    from sympy import Symbol,sympify

    if m==None:
        m=''
        for i in range(1,n_features+1):
            c='x'+str(i)
            c+=np.random.choice(['+','-'],p=[0.5,0.5])
            m+=c
        m=m[:-1]
    sym_m=sympify(m)
    n_features=len(sym_m.atoms(Symbol))
    evals=[]
    lst_features=[]
    for i in range(n_features):
        lst_features.append(np.random.normal(scale=5,size=n_samples))
    lst_features=np.array(lst_features)
    lst_features=lst_features.T
    for i in range(n_samples):
        evals.append(eval_multinomial(m,vals=list(lst_features[i])))

    evals=np.array(evals)
    evals_binary=evals>0
    evals_binary=evals_binary.flatten()
    evals_binary=np.array(evals_binary,dtype=int)
    evals_binary=flip(evals_binary,p=flip_y)
    evals_binary=evals_binary.reshape(n_samples,1)

    lst_features=lst_features.reshape(n_samples,n_features)
    x=np.hstack((lst_features,evals_binary))

    return (x)
```

```python
x=gen_classification_symbolic(m='2*x1+3*x2+5*x3',n_samples=10,flip_y=0.0)
```

```python
import pandas as pd
df=pd.DataFrame(x)
```

```python
df
```

      0
      1
      2
      3

      0
      -0.442614
      -0.523797
      2.300378
      1.0

      1
      -0.463687
      3.200522
      4.309449
      1.0

      2
      -2.580825
      -2.606877
      5.845246
      1.0

      3
      0.888534
      3.727563
      -3.174944
      0.0

      4
      -1.120233
      -2.737827
      -8.308171
      0.0

      5
      -4.832498
      -6.591497
      5.565251
      0.0

      6
      -2.326688
      4.751803
      3.309559
      1.0

      7
      -1.213919
      -10.209268
      2.447471
      0.0

      8
      -1.320891
      0.770222
      -8.735817
      0.0

      9
      2.150437
      6.061752
      -6.733177
      0.0

```python
x=gen_classification_symbolic(m='12*x1/(x2+5*x3)',n_samples=10,flip_y=0.2)
df=pd.DataFrame(x)
df
```

      0
      1
      2
      3

      0
      5.566707
      7.645691
      0.559811
      1.0

      1
      -10.528796
      2.241197
      -4.216359
      0.0

      2
      5.000866
      0.615997
      2.021055
      1.0

      3
      2.919458
      3.593280
      0.498438
      1.0

      4
      4.671159
      -0.679506
      -5.125242
      1.0

      5
      -4.484902
      -12.881067
      2.340985
      1.0

      6
      1.302499
      -5.054502
      -1.396578
      0.0

      7
      -2.321861
      -2.548314
      2.881654
      0.0

      8
      -7.021660
      4.530175
      2.701544
      0.0

      9
      -9.778964
      -0.132642
      4.021956
      0.0

### Regression sample generation based on a symbolic expression

```python
def gen_regression_symbolic(m=None,n_samples=100,n_features=2,noise=0.0,noise_dist='normal'):
    """
    Generates regression sample based on a symbolic expression. Calculates the output of the symbolic expression
    at randomly generated (drawn from a Gaussian distribution) points
    m: The symbolic expression. Needs x1, x2, etc as variables and regular python arithmatic symbols to be used.
    n_samples: Number of samples to be generated
    n_features: Number of variables. This is automatically inferred from the symbolic expression. So this is ignored
                in case a symbolic expression is supplied. However if no symbolic expression is supplied then a
                default simple polynomial can be invoked to generate regression samples with n_features.
    noise: Magnitude of Gaussian noise to be introduced (added to the output).
    noise_dist: Type of the probability distribution of the noise signal.
    Currently supports: Normal, Uniform, t, Beta, Gamma, Poission, Laplace

    Returns a numpy ndarray with dimension (n_samples,n_features+1). Last column is the response vector.
    """

    import numpy as np
    from sympy import Symbol,sympify

    if m==None:
        m=''
        for i in range(1,n_features+1):
            c='x'+str(i)
            c+=np.random.choice(['+','-'],p=[0.5,0.5])
            m+=c
        m=m[:-1]

    sym_m=sympify(m)
    n_features=len(sym_m.atoms(Symbol))
    evals=[]
    lst_features=[]

    for i in range(n_features):
        lst_features.append(np.random.normal(scale=5,size=n_samples))
    lst_features=np.array(lst_features)
    lst_features=lst_features.T
    lst_features=lst_features.reshape(n_samples,n_features)

    for i in range(n_samples):
        evals.append(eval_multinomial(m,vals=list(lst_features[i])))

    evals=np.array(evals)
    evals=evals.reshape(n_samples,1)

    if noise_dist=='normal':
        noise_sample=noise*np.random.normal(loc=0,scale=1.0,size=n_samples)
    elif noise_dist=='uniform':
        noise_sample=noise*np.random.uniform(low=0,high=1.0,size=n_samples)
    elif noise_dist=='beta':
        noise_sample=noise*np.random.beta(a=0.5,b=1.0,size=n_samples)
    elif noise_dist=='Gamma':
        noise_sample=noise*np.random.gamma(shape=1.0,scale=1.0,size=n_samples)
    elif noise_dist=='laplace':
        noise_sample=noise*np.random.laplace(loc=0.0,scale=1.0,size=n_samples)

    noise_sample=noise_sample.reshape(n_samples,1)
    evals=evals+noise_sample

    x=np.hstack((lst_features,evals))

    return (x)
```

#### Generate samples with a rational function as input
### $$\frac{10x_1}{(3x_2+4x_3)}$$

```python
x=gen_regression_symbolic(m='10*x1/(3*x2+4*x3)',n_samples=10,noise=0.1)
df=pd.DataFrame(x)
df
```

      0
      1
      2
      3

      0
      -7.08499
      -4.35839
      8.10732
      -3.67783156896633

      1
      -10.6684
      0.779056
      -6.64651
      4.45796155973331

      2
      -6.69542
      -5.307
      1.17935
      5.95108211351032

      3
      -3.83581
      6.46401
      -5.99678
      8.28104884665168

      4
      -1.50995
      -1.46056
      -4.10213
      0.493224249022704

      5
      2.22995
      -1.12163
      4.41009
      1.51213441402200

      6
      4.26261
      0.982687
      7.0676
      1.31581579041464

      7
      0.787195
      2.20071
      -4.68889
      -0.724650906335961

      8
      0.720728
      -2.32469
      -0.468637
      -0.917106940761226

      9
      -3.04485
      -3.26701
      -3.19083
      1.40239700700260

#### Generate samples with no symbolic input and with 10 features

```python
x=gen_regression_symbolic(n_features=10,n_samples=10,noise=0.1)
df=pd.DataFrame(x)
df
```

      0
      1
      2
      3
      4
      5
      6
      7
      8
      9
      10

      0
      -3.55302
      -3.60695
      -10.0729
      7.85252
      -3.76276
      7.78251
      -0.638193
      -4.75034
      -0.989603
      0.181414
      -10.2675547899900

      1
      -6.34778
      -2.74753
      -4.75187
      4.70489
      2.70014
      5.29475
      -0.190095
      4.85289
      -4.50207
      -1.22839
      -1.93507315813350

      2
      2.15057
      -0.43572
      -5.47805
      6.96074
      3.10096
      -1.50898
      1.09913
      4.73097
      -0.216513
      -3.25629
      4.91632094325713

      3
      1.11181
      5.62156
      0.0660036
      6.66565
      1.88656
      -4.57167
      -0.84389
      -2.44545
      1.66649
      7.36169
      18.0607243993836

      4
      -3.27857
      -4.13545
      4.36299
      -3.73405
      -1.3958
      -6.39237
      1.99202
      -0.216661
      -10.3905
      3.43896
      -23.7181534803553

      5
      -6.78726
      -7.76895
      -2.2653
      -3.4326
      -16.8937
      -1.79111
      4.79866
      -4.43692
      2.01342
      -3.46766
      -49.7886776301048

      6
      1.66157
      -4.34424
      6.69391
      -3.42321
      -8.63976
      0.862722
      2.70897
      -13.5047
      -7.14555
      -3.16143
      -33.7432854958296

      7
      3.46949
      7.11633
      -1.08523
      6.41709
      -1.23161
      7.07956
      6.6377
      6.06688
      -4.99262
      3.85005
      19.9862211512126

      8
      1.13155
      4.02597
      -3.05607
      4.9271
      -3.60454
      6.06252
      0.936673
      4.07096
      -2.04052
      5.11093
      15.8002295267657

      9
      -1.6051
      4.97768
      -7.69743
      -0.65865
      4.07053
      2.98333
      -10.9341
      4.9402
      2.9771
      0.68357
      21.5880195549236

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

#### Generate samples with less noise and plot: $0.2x^2+1.2x+6+f_{noise}(x\mid{N=0.1})$

```python
x=gen_regression_symbolic(m='0.2*x**2+1.2*x+6',n_samples=100,noise=0.1)
df=pd.DataFrame(x)
plt.scatter(df[0],df[1])
plt.show()
```

#### Generate samples with more noise and plo: $0.2x^2+1.2x+6+f_{noise}(x\mid{N=10})$

```python
x=gen_regression_symbolic(m='0.2*x**2+1.2*x+6',n_samples=100,noise=10)
df=pd.DataFrame(x)
plt.scatter(df[0],df[1])
plt.show()
```

#### Generate samples with larger coefficent for the quadratic term and plot: $1.3x^2+1.2x+6+f_{noise}(x\mid{N=10})$

```python
x=gen_regression_symbolic(m='1.3*x**2+1.2*x+6',n_samples=100,noise=10)
df=pd.DataFrame(x)
plt.scatter(df[0],df[1])
plt.show()
```

Random Function Generator/Symbolic regression classification generator.ipynb


# 2021-08-30 High order and boundaries

## Last time

* Stability
* Derivatives as matrices
* Boundary conditions
* Discrete eigenvalues/eigenvectors

## Today
* High order on arbitrary grids
* Method of manufactured solutions
* More satisfying approach to boundary conditions

```julia
using Plots
```

# A few methods on grids

```julia
diff1l(x, u) = x[2:end],   (u[2:end] - u[1:end-1]) ./ (x[2:end] - x[1:end-1])
diff1r(x, u) = x[1:end-1], (u[2:end] - u[1:end-1]) ./ (x[2:end] - x[1:end-1])
diff1c(x, u) = x[2:end-1], (u[3:end] - u[1:end-2]) ./ (x[3:end] - x[1:end-2])
difflist = [diff1l, diff1r, diff1c]

n = 20
h = 2 / (n - 1)
x = LinRange(-3, 3, n)
u = sin.(x)
fig = plot(cos, xlims=(-3, 3))
for d in difflist
    xx, yy = d(x, u)
    plot!(fig, xx, yy, marker=:circle, label=d)
end
```

```julia
fig
```

# Measuring error on grids

```julia
using LinearAlgebra

grids = 2 .^ (2:10)
hs = 1 ./ grids
function refinement_error(f, fprime, d)
    error = []
    for n in grids
        x = LinRange(-3, 3, n)
        xx, yy = d(x, f.(x))
        push!(error, norm(yy - fprime.(xx), Inf))
    end
    error
end
```

    refinement_error (generic function with 1 method)

```julia
fig = plot(xscale=:log10, yscale=:log10)
for d in difflist
    error = refinement_error(sin, cos, d)
    plot!(fig, hs, error, marker=:circle, label=d)
end
plot!(fig, hs, hs .^ 2)
```

# Stability

Are there "rough" functions for which these formulas estimate $u'(x_i) = 0$?

```julia
x = LinRange(-1, 1, 9)
f_rough(x) = cos(.1 + 4π*x)
fp_rough(x) = -4π*sin(.1 + 4π*x)

plot(x, f_rough, marker=:circle)
plot!(f_rough)
```

```julia
fig = plot(fp_rough, xlims=(-1, 1))
for d in difflist
    xx, yy = d(x, f_rough.(x))
    plot!(fig, xx, yy, label=d, marker=:circle)
end
fig
```

If we have a solution $u(x)$, then $u(x) + f_{\text{rough}}(x)$ is indistinguishable to our FD method.

# Second derivatives

We can compute a second derivative by applying first derivatives twice.

```julia
function diff2a(x, u)
    xx, yy = diff1c(x, u)
    diff1c(xx, yy)
end

function diff2b(x, u)
    xx, yy = diff1l(x, u)
    diff1r(xx, yy)
end

diff2list = [diff2a, diff2b]
n = 10
x = LinRange(-3, 3, n)
u = - cos.(x);
```

```julia
fig = plot(cos, xlims=(-3, 3))
for d2 in diff2list
    xx, yy = d2(x, u)
    plot!(fig, xx, yy, marker=:circle, label=d2)
end
fig
```

# How fast do these approximations converge?

```julia
grids = 2 .^ (3:10)
hs = 1 ./ grids
function refinement_error2(f, f_xx, d2)
    error = []
    for n in grids
        x = LinRange(-3, 3, n)
        xx, yy = d2(x, f.(x))
        push!(error, norm(yy - f_xx.(xx), Inf))
    end
    error
end
```

    refinement_error2 (generic function with 1 method)

```julia
fig = plot(xscale=:log10, yscale=:log10)
for d2 in diff2list
    error = refinement_error2(x -> -cos(x), cos, d2)
    plot!(fig, hs, error, marker=:circle, label=d2)
end
plot!(fig, hs, hs .^ 2)
```

# Differentiation matrices

All our `diff*` functions thus far have been linear in `u`, therefore they can be represented as matrices.
$$\frac{u_{i+1} - u_i}{x_{i+1} - x_i} = \begin{bmatrix} -1/h & 1/h \end{bmatrix} \begin{bmatrix} u_i \\ u_{i+1} \end{bmatrix}$$

```julia
function diff1_mat(x)
    n = length(x)
    D = zeros(n, n)
    h = x[2] - x[1]
    D[1, 1:2] = [-1/h  1/h]
    for i in 2:n-1
        D[i, i-1:i+1] = [-1/2h  0  1/2h]
    end
    D[n, n-1:n] = [-1/h  1/h]
    D
end
```

    diff1_mat (generic function with 1 method)

```julia
x = LinRange(-3, 3, 10)
plot(x, diff1_mat(x) * sin.(x), marker=:circle)
plot!(cos)
```

# How accurate is this derivative matrix?

```julia
fig = plot(xscale=:log10, yscale=:log10, legend=:topleft)
error = refinement_error(sin, cos, (x, u) -> (x, diff1_mat(x) * u))
plot!(fig, hs, error, marker=:circle)
plot!(fig, hs, hs, label="\$h\$")
plot!(fig, hs, hs .^ 2, label="\$h^2\$")
```

# Can we study it as a matrix?

```julia
D = diff1_mat(x)
spy(D, marker=(:square, 10), c=:bwr)
```

```julia
svdvals(D)
```

    10-element Vector{Float64}:
     2.268133218393964
     2.2674392839412794
     1.4265847744427302
     1.3683737968309653
     1.2135254915624207
     1.0228485194005281
     0.8816778784387094
     0.5437139466339259
     0.46352549156242107
     3.204643550177702e-17

# Second derivative with Dirichlet boundary conditions

The left endpoint in our example boundary value problem has a Dirichlet boundary condition,
$$u(-1) = a . $$
With finite difference methods, we have an explicit degree of freedom $u_0 = u(x_0 = -1)$ at that endpoint.
When building a matrix system for the BVP, we can implement this boundary condition by modifying the first row of the matrix,
$$ \begin{bmatrix} 1 & 0 & 0 & 0 & 0 \\ \\ & & A_{2:,:} & & \\ \\ \end{bmatrix} \begin{bmatrix} u_0 \\ \\ u_{2:} \\ \\ \end{bmatrix} = \begin{bmatrix} a \\ \\ f_{2:} \\ \\ \end{bmatrix} . $$

* This matrix is not symmetric even if $A$ is.

```julia
function laplacian_dirichlet(x)
    n = length(x)
    D = zeros(n, n)
    h = x[2] - x[1]
    D[1, 1] = 1
    for i in 2:n-1
        D[i, i-1:i+1] = (1/h^2) * [-1, 2, -1]
    end
    D[n, n] = 1
    D
end
```

    laplacian_dirichlet (generic function with 1 method)

# Laplacian as a matrix

```julia
L = laplacian_dirichlet(x)
spy(L, marker=(:square, 10), c=:bwr)
```

```julia
svdvals(L)
```

    10-element Vector{Float64}:
     8.745632414048261
     8.01252539624902
     6.887027178840083
     5.501492841921461
     4.019212498940009
     2.620230430399528
     1.5101748989145012
     0.9274512680423916
     0.630930249171091
     0.23924866094931405

# Shortcomings of our previous methods

* Only second order accurate (at best)
* Worse than second order on non-uniform grids
* Worse than second order at Neumann boundaries
* Boundary conditions break symmetry

# Interpolation by Vandermonde matrices

We can compute a polynomial

$$ p(x) = c_0 + c_1 x + c_2 x^2 + \dotsb $$

that assumes function values $p(x_i) = u_i$ by solving a linear system with the Vandermonde matrix.

$$ \underbrace{\begin{bmatrix} 1 & x_0 & x_0^2 & \dotsb \\
    1 & x_1 & x_1^2 & \dotsb \\
    1 & x_2 & x_2^2 & \dotsb \\
    \vdots & & & \ddots \end{bmatrix}}_V \begin{bmatrix} c_0 \\ c_1 \\ c_2 \\ \vdots \end{bmatrix} = \begin{bmatrix} u_0 \\ u_1 \\ u_2 \\ \vdots \end{bmatrix} .$$

```julia
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
```

    vander (generic function with 2 methods)

```julia
vander(LinRange(-1, 1, 5))
```

    5×5 Matrix{Float64}:
     1.0  -1.0  1.0   -1.0    1.0
     1.0  -0.5  0.25  -0.125  0.0625
     1.0   0.0  0.0    0.0    0.0
     1.0   0.5  0.25   0.125  0.0625
     1.0   1.0  1.0    1.0    1.0

# Fitting a polynomial

```julia
k = 4
x = LinRange(-2, 2, k)
u = sin.(x)
V = vander(x)
c = V \ u
scatter(x, u, label="\$u_i\$", legend=:topleft)
plot!(x -> (vander(x, k) * c)[1,1], label="\$p(x)\$")
plot!(sin, label=sin)
```

# Differentiating

We're given the coefficients $c = V^{-1} u$ of the polynomial
$$p(x) = c_0 + c_1 x + c_2 x^2 + \dotsb.$$
What is
\begin{align} p(0) &= c_0 \\
p'(0) &= c_1 \\
p''(0) &= c_2 \cdot 2\\
p^{(k)}(0) &= c_k \cdot k! .
\end{align}

```julia
function fdstencil1(source, target)
    "first derivative stencil from source to target"
    x = source .- target
    V = vander(x)]
    inv(V)[2, :]' # as a row vector
end
plot([z -> fdstencil1(x, z) * u, cos], xlims=(-3,3))
scatter!(x, 0*x, label="grid points")
```

# Arbitrary order

```julia
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
fdstencil(x, 0.5, 2)

```

    1×4 adjoint(::Vector{Float64}) with eltype Float64:
     0.0703125  0.351563  -0.914062  0.492188

```julia
plot([z -> fdstencil(x, z, 2) * u,
        z -> -sin(z)])
```

## We didn't call `inv(V)`; what's up?
$$p(0) = s_0^0 u_0 + s_1^0 u_1 + s_2^0 u_2 + \dotsb = e_0^T \underbrace{V^{-1} u}_c = \underbrace{e_0^T V^{-1}}_{s^0} u$$

# Convergence order

```julia
hs = 2 .^ -LinRange(-4, 10, 10)
function diff_error(u, du, h; n, k, z=0)
    x = LinRange(-h, h, n) .+ .5
    fdstencil(x, z, k) * u.(x) - du.(z)
end
errors = [diff_error(sin, t -> -sin(t), h, n=5, k=2, z=.5+0.1*h)
    for h in hs]
plot(hs, abs.(errors), marker=:circle)
plot!(h -> h^3, label="\$h^?\$", xscale=:log10, yscale=:log10)
```

## Observations

* When using $n=3$ points, we fit a polynomial of degree 2 and have error $O(h^3)$ for interpolation $p(0)$.
* Each derivative gives up one order of accuracy in general.
* Centered diff on uniform grids can have extra cancellation (superconvergence)
* The Vandermonde matrix is notoriously ill-conditioned with many points $n$. We recommend using a [stable algorithm from Fornberg](https://doi.org/10.1137/S0036144596322507).

# High order discretization of the Laplacian
## The Poisson problem $-u_{xx} = f$ with boundary conditions

```julia
function poisson(x, spoints, forcing; left=(0, zero), right=(0, zero))
    n = length(x)
    L = zeros(n, n)
    rhs = forcing.(x)
    for i in 2:n-1
        jleft = min(max(1, i-spoints÷2), n-spoints+1)
        js = jleft : jleft + spoints - 1
        L[i, js] = -fdstencil(x[js], x[i], 2)
    end
    L[1,1:spoints] = fdstencil(x[1:spoints], x[1], left[1])
    L[n,n-spoints+1:n] = fdstencil(x[n-spoints+1:n], x[n], right[1])
    rhs[1] = left[2](x[1])
    rhs[n] = right[2](x[n])
    L, rhs
end
```

    poisson (generic function with 1 method)

```julia
L, b = poisson(LinRange(-1, 1, 6), 3, zero, left=(1, zero))
L
```

    6×6 Matrix{Float64}:
     -3.75   5.0   -1.25   0.0           0.0           0.0
     -6.25  12.5   -6.25   0.0           0.0           0.0
      0.0   -6.25  12.5   -6.25          0.0           0.0
      0.0    0.0   -6.25  12.5          -6.25          0.0
      0.0    0.0    0.0   -6.25         12.5          -6.25
      0.0    0.0    0.0    9.25186e-18  -2.31296e-16   1.0

# Method of manufactured solutions

## Problem: analytic solutions to PDEs are hard to find

Let's choose a smooth function with rich derivatives,
$$ u(x) = \tanh(x) . $$
Then $$ u'(x) = \cosh^{-2}(x) $$ and $$ u''(x) = -2 \tanh(x) \cosh^{-2}(x) . $$

* This works for nonlinear too.

```julia
x = LinRange(-2, 2, 10)
L, rhs = poisson(x, 3,
    x -> 2 * tanh(x) / cosh(x)^2,
    left=(0, tanh),
    right=(1, x -> cosh(x)^-2))
u = L \ rhs
plot(x, u, marker=:circle, legend=:topleft)
plot!(tanh)
```

# Convergence rate

```julia
ns = 2 .^ (2:10)
hs = 1 ./ ns
function poisson_error(n)
    x = LinRange(-2, 2, n)
    L, rhs = poisson(x, 3, x -> 2 * tanh(x) / cosh(x)^2,
        left = (0, tanh),
        right = (1, x -> cosh(x)^-2))
    u = L \ rhs
    norm(u - tanh.(x), Inf)
end
```

    poisson_error (generic function with 1 method)

```julia
plot(hs, [poisson_error(n) for n in ns], marker=:circle)
plot!(h -> h^2, xscale=:log10, yscale=:log10)
```

# Symmetry in boundary conditions: Dirichlet

We have implemented Dirichlet conditions by modifying the first row of the matrix,
$$ \begin{bmatrix} 1 & 0 & 0 & 0 & 0 \\ \\ & & A_{1:,:} & & \\ \\ \end{bmatrix} \begin{bmatrix} u_0 \\ \\ u_{1:} \\ \\ \end{bmatrix} = \begin{bmatrix} a \\ \\ f_{1:} \\ \\ \end{bmatrix} . $$

* This matrix is not symmetric even if $A$ is.
* We can eliminate $u_0$ and create a reduced system for $u_{1:}$.
* Generalize: consider a $2\times 2$ block system
$$ \begin{bmatrix} I & 0 \\ A_{10} & A_{11} \end{bmatrix} \begin{bmatrix} u_0 \\ u_1 \end{bmatrix} = \begin{bmatrix} f_0 \\ f_1 \end{bmatrix} .$$

We can rearrange as
$$ A_{11} u_1 = f_1 - A_{10} f_0, $$
which is symmetric if $A_{11}$ is.
* This is called "lifting" and is often done implicitly in the mathematics literature.  It is convenient for linear solvers and eigenvalue solvers, but inconvenient for IO and postprocessing, as well as some nonlinear problems.
* Convenient alternative: write
$$ \begin{bmatrix} I & 0 \\ 0 & A_{11} \end{bmatrix} \begin{bmatrix} u_0 \\ u_1 \end{bmatrix} = \begin{bmatrix} f_0 \\ f_1 - A_{10} f_0 \end{bmatrix}, $$
which is symmetric and decouples the degrees of freedom associated with the boundary. This method applies cleanly to nonlinear problems.
* Optionally scale the identity by some scalar related to the norm of $A_{11}$.

# Symmetry in boundary conditions: Neumann

Consider FD discretization of the Neumann boundary condition
$$ \frac{du}{dx}(1) = b . $$
1. Use a one-sided difference formula as in
$$ \frac{u_n - u_{n-1}}{h} = b . $$
  * an extra discretization choice
  * may reduce order of accuracy compared to interior discretization, lose symmetry.
2. Temporarily introduce a ghost value $u_{n+1} = u(x_{n+1} = 1 + h)$ (possibly more) and define it to be a reflection of the values from inside the domain.  In the case $b=0$, this reflection is $u_{n+i} = u_{n-i}$.  More generally,
$$ u_{n+i} = u_{n-i} + 2b(x_n - x_{n-i}) . $$

After this definition of ghost values, we apply the interior discretization at the boundary. For our reference equation, we would write

$$ \frac{-u_{n-1} + 2 u_n - u_{n+1}}{h^2} = f(x_n) $$

which simplifies to $$ \frac{u_n - u_{n-1}}{h^2} = f(x_n)/2 + b/h $$
after dividing by 2 and moving the boundary term to the right hand side.

# Fourier analysis of stencils

Consider the plane waves $\phi(x, \theta) = e^{i\theta x}$.

Sample $\phi$ on a discrete grid $x = \mathbb Z$ and apply the stencil
\begin{align}
S \phi(x, \theta) &= s_{-1} \phi(x-1, \theta) + s_{0} \phi(x, \theta) + s_1 \phi(x+1,\theta) \\
&= \Big( s_{-1} e^{-i\theta} + s_0 + s_{1} e^{i\theta} \Big) \phi(x, \theta)
\end{align}
With $S = \begin{bmatrix} -1 & 2 & -1 \end{bmatrix}$, we get
$$S \phi(x, \theta) = \underbrace{(2 - 2 \cos\theta)}_{\hat S(\theta)} \phi(x, \theta)$$
We call $\hat S(\theta)$ the *symbol* of the operator.
What is the symbol of the continuous second derivative?

```julia
plot(theta -> 2 - 2*cos(theta), xlims=(-pi, pi))
```