# Entscheidungsbäume

Alice beobachtet die Tennisspieler auf dem Tennisplatz vor ihrem Haus. Sie möchte herausfinden, wie die Spieler entscheiden, bei welchem Wetter die Spieler Tennis spielen und wann nicht. Sie macht die folgenden Beobachtungen:

Erstellen Sie einen Entscheidungsbaum auf Basis dieser Trainingsdaten. Notieren Sie an jedem Knoten die Anzahl der yes- und no-Instanzen. Benutzen Sie die Informationsentropie als Maß für Knotenreinheit.

# K-Nearest-Neighbor-Klassifikator

Betrachten sie folgende Teilmenge des Iris-Datensatzes:

| Sepal.Length        | Sepal.Width   | Petal.Length |   Petal.Width |    Species|
| ------------- |-------------| -----|------|------|
|           5.1 |        3.5   |       1.4  |       0.2  |   setosa
|           4.9 |       3.0   |       1.4    |     0.2  |   setosa
|           7.0 |        3.2  |        4.7   |      1.4 | versicolor
|           6.4 |        3.2  |        4.5   |      1.5 | versicolor

Klassifizieren Sie das Sample

|           6.9 |        3.0  |        3.5   |      1 |
|-|-|-|-|

mit einem k-nearest-Neighbor-Klassifikator. Verwenden Sie k=1 und die euklidische Distanz.

# Kerndichteschätzung
In der Vorlesung haben Sie Kerndichteschätzer als eine nichtparametrische Methode zum Darstellen einer Verteilung kennen gelernt. Dabei wird eine Verteilung an einem Punkt $t$ wie folgt dargestellt:
\begin{equation}
p(t) = \frac{1}{n \, h} \sum_{i=1}^n \varphi\left(\frac{t-x_i}{h}\right)
\end{equation}
Hierbei ist $\varphi$ eine Fenster-Funktion, z.B. das Gauß-Fenster
\begin{equation}
\varphi(u) = \frac{1}{\sqrt{2\pi}} e^{-u^2/2}
\end{equation}
## Aufgabe 1
Implementieren Sie die Funktion `kde(t,h,x)`, die für einen Punkt $t$, eine Fenster-Breite $h$ und ein Array von Trainings-Pukten $x$, die Kerndichteschätzung für $p(t)$ berechnet.

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

def kde(t,h,x):
    #TODO
    return None

def k1(t):
    return 1/math.sqrt(2*math.pi) * math.exp(-1/2 * t**2)

example = np.concatenate((np.random.normal(0,1,100),np.random.normal(5,1,100)))

dens = [kde(t,0.5,example) for t in np.arange(-2,8,0.05)]

plt.plot(dens)
```

## Aufgabe 2
Implementieren Sie die Funktion `classify_kde(xnew,x,classes)`, die eine Klassifikation mit dem Kerndichteschätzer durchführt. Das bedeutet, es handelt sich um einen Bayes-Klassifikator, bei dem die Likelihood mit dem Kerndichteschätzer geschätzt wird.

```python
import pandas as pd
from scipy.io import arff

def classify_kde(xnew,x,classes):
    #TODO
    return None

data = arff.loadarff('features1.arff')
df = pd.DataFrame(data[0])
feat = df["AccX_mean"]
cl = df["class"]

p = [classify_kde(x,feat,cl) for x in feat]
np.mean(p == cl)
```


```python
import numpy as np
import matplotlib.pyplot as plt
plt.style.use('seaborn-poster')
```

# Sistemas de ordem superior

Neste tutorial, estenderemos as técnicas vistas até aqui para resolver alguns tipos de problemas de EDOs de ordem superior.

# Exemplo 1

Resolva a EDO de segunda ordem dada por:

\begin{equation}
\begin{aligned}
y^{\prime \prime}+y^{\prime}+y &=\cos (t) \\
y(0) &=1 \\
y^{\prime}(0) &=0
\end{aligned}
\end{equation}

Queremos transformar a equação diferencial em um sistema de equações de primeira ordem, portanto, fazemos $y^{\prime} =w$ e obtemos um sistema de duas EDOs de primeira ordem:

\begin{equation}
\begin{aligned}
y^{\prime} &=w \\
w^{\prime} &=-w-y+\cos (t) \\
y(0) &=1 \\
w(0) &=0
\end{aligned}
\end{equation}

```python
# Escrevemos o sistema de EDOs na nossa função derivadas:
def derivadas(t,X):
  F = np.zeros(2)
  F[0] = -X[1]
  F[1] = -X[1]-X[0]+np.cos(t)
  return F
```

```python
# Parametros

h = 1e-2
tfim = 10
nt = int(tfim/h)
y0 = 1
w0 = 0
s0 = [y0,w0]
```

```python
def Euler_geral(derivadas,h,s0,tmax=10):
  nt = int(tmax/h)
  X = np.zeros([nt,2])
  tempo = np.linspace(0,tmax,nt)
  X[0,:] = s0

  for k in np.arange(1,nt):
      F = derivadas(tempo[k-1],X[k-1,:])
      X[k,0] = X[k-1,0] + h*F[0]
      X[k,1] = X[k-1,1] + h*F[1]

  return tempo,X
```

```python
tempo,X = Euler_geral(derivadas,h,[1,0],3)
```

```python
  plt.figure(figsize = (12, 8))
  plt.plot(tempo,X[:,0], 'bo--', label='y')
  plt.plot(tempo,X[:,1], 'ko--', label='w')
  plt.xlabel('t')
  plt.ylabel('x,y')
  plt.grid()
  plt.legend(loc='lower left')

  plt.figure(figsize = (12, 8))
  plt.plot(X[:,0],X[:,1], 'bo--', label='Numérica com h={}'.format(h))
  plt.xlabel('x')
  plt.ylabel('y')
  plt.grid()
  plt.legend(loc='lower left')
  plt.show()
```

```python
def Euler_mod_geral(derivadas,h,s0,tmax=10):
  nt = int(tmax/h)
  X = np.zeros([nt,2])
  tempo = np.linspace(0,tmax,nt)
  X[0,:] = s0

  for k in np.arange(1,nt):
      F = derivadas(tempo[k-1],X[k-1,:])
      kx1 = F[0]
      ky1 = F[1]

      x_euler = X[k-1,0] + h*kx1
      y_euler = X[k-1,1] + h*ky1

      Fnew = derivadas(tempo[k-1],[x_euler,y_euler])

      kx2 = Fnew[0]
      ky2 = Fnew[1]

      X[k,0] = X[k-1,0] + h*(kx1+kx2)/2
      X[k,1] = X[k-1,1] + h*(ky1+ky2)/2
  return tempo,X
```

```python
tempo,X = Euler_mod_geral(derivadas,h,[1,0],3)
```

```python
  plt.figure(figsize = (12, 8))
  plt.plot(tempo,X[:,0], 'bo--', label='y')
  plt.plot(tempo,X[:,1], 'ko--', label='w')
  plt.xlabel('t')
  plt.ylabel('x,y')
  plt.grid()
  plt.legend(loc='lower left')

  plt.figure(figsize = (12, 8))
  plt.plot(X[:,0],X[:,1], 'bo--', label='Numérica com h={}'.format(h))
  plt.xlabel('x')
  plt.ylabel('y')
  plt.grid()
  plt.legend(loc='lower left')
  plt.show()
```

```python

```


## Objectives:
- Student should be able to Explain why we care about linear algebra in the scope of data science
- Student should be able to Conceptualize and utilize vectors and matrices through matrix operations and properties such as: square matrix, identity matrix, transpose and inverse

# Why Linear Algebra? (ELI5 + Soapbox)

Data Science, Machine Learning, and Artificial intelligence is all about getting computers to do things for us better, cheaper, and faster than we could do them ourselves.

How do we do that? Computers are good at doing small repetitive tasks (like arithmetic). if we tell them what small repetitive tasks to do in the right order then sometimes all of those combined behaviors will result in something that looks like a human's behavior (or at least the decisions/output look like something a human might decide to do/create).

[Le Comte de Belamy](https://obvious-art.com/le-comte-de-belamy.htm)

The set of instructions that we give to a computer to complete certain tasks is called an **algorithm**. The better that we can organize the set of instructions, the faster that computers can do them. The method that we use to organize and store our set of instructions so that the computer can do them super fast is called a **data structure**. The practice of optimizing the organization of our data structures so that they run really fast and efficiently is called **computer science**. (This is why we will have a unit dedicated solely to computer science in a few months). Data Scientists should care how fast computers can process their sets of instructions (algorithms).

## A set of ordered instructions

Here's a simple data structure, in Python it's known as a **list**. It's one of the simplest ways that we can store things (data) and maintain their order. When giving instructions to a computer, it's important that the computer knows in what order to execute them.

```python
selfDrivingCarInstructions = ["open door",
          "sit on seat",
          "put key in ignition",
          "turn key to the right until it stops",
          "push brake pedal",
          "change gear to 'Drive'",
          "release brake pedal",
          "push gas pedal",
          '''turn wheel to navigate streets with thousands of small rules and
          exeptions to rules all while avoiding collision with other
          objects/humans/cars, obeying traffic laws, not running out of fuel and
          getting there in a timely manner''',
          "close door"]

# We'll have self-driving cars next week for sure. NBD
```

# Maintaining the order of our sets of ordered instruction-sets
Here's another data structure we can make by putting lists inside of lists, this is called a two-dimensional list. Sometimes it is also known as a two-dimensional array or --if you put some extra methods on it-- a dataframe. As you can see things are starting to get a little bit more complicated.

```python
holdMyData = [
    [1,2,3],
    [4,5,6],
    [7,8,9]
]

# Disregard the quality of these bad instructions
```

## Linear Algebra - organize and execute big calculations/operations really fast

So why linear algebra? Because the mathematical principles behinds **vectors** and **matrices** (lists and 2D lists) will help us understand how we can tell computers how to do an insane number of calculations in a very short amount of time.

Remember when we said that computers are really good at doing small and repetitive tasks very quickly?

## I Give You... Matrix Multiplication:

If you mess up any of those multiplications or additions you're up a creek.

## I Give You... Finding the Determinant of a Matrix: (an introductory linear algebra topic)

## 2x2 Matrix

Just use the formula!

## 3x3 Matrix

Just calculate the determinant of 3 different 2x2 matrices and multiply them by 3 other numbers and add it all up.

## 4x4 Matrix

Just calculate 3 diferent 3x3 matrix determinants which will require the calculating of 9 different 2x2 matrix determinants, multiply them all by the right numbers and add them all up. And if you mess up any of those multiplications or additions you're up a creek.

## 5x5 Matrix!

## ...

## ...

Just kidding, any linear algebra professor who assigns the hand calculation of a 5x5 matrix determinant (or larger) is a sadist. This is what computers were invented for! Why risk so much hand calculation in order to do something that computers **never** make a mistake at?

By the way, when was the last time that you worked with a dataframe that was 4 rows x 4 columns or smaller?

Quick, find the determinant of this 42837x42837 dataframe by hand!

# Common Applications of Linear Algebra in Data Science:

- Vectors: Rows, Columns, lists, arrays

- Matrices: tables, spreadsheets, dataframes

- Linear Regression: (You might remember from the intro course)

```python
# Linear Regression Example
import matplotlib.pyplot as plt
import numpy as np
import pandas as pd

# Read CSV
df = pd.read_csv('https://raw.githubusercontent.com/ryanleeallred/datasets/master/Ice_Cream_Sales.csv')

# Create Column of 1s
df['Ones'] = np.ones(11)

# Format X and Y Matrices
X = df[['Ones', 'Farenheit']].as_matrix()
Y = df['Dollars'].as_matrix().reshape(-1, 1)

# Calculate Beta Values
beta = np.matmul(np.linalg.inv(np.matmul(np.transpose(X), X)), np.matmul(np.transpose(X), Y))

print(beta)
```

    [[-596.20648399]
     [  24.68849397]]

```python
# Assign Beta Values to Variables
beta_0 = beta[0,0]
beta_1 = beta[1,0]

# Plot points with line of best fit
plt.scatter(df['Farenheit'], df['Dollars'])
axes = plt.gca()
x_vals = np.array(axes.get_xlim())
y_vals = beta_0 + beta_1 * x_vals
plt.plot(x_vals, y_vals, '-', color='b')
plt.title('Ice Cream Sales Regression Line')
plt.xlabel('Farenheit')
plt.ylabel('Dollars')
plt.show()
```

- Dimensionality Reduction Techniques: Principle Component Analysis (PCA) and Singular Value Decomposition (SVD)

Take a giant dataset and distill it down to its important parts. (typically as a pre-processing step for creating visualizations or putting into other models.)

- Deep Learning: Convolutional Neural Networks, (Image Recognition)

"Convolving" is the process of passing a filter/kernel (small matrix) over the pixels of an image, multiplying them together, and using the result to create a new matrix. The resulting matrix will be a new image that has been modified by the filter to emphasize certain qualities of an image. This is entirely a linear algebra-based process. A convolutional neural network learns the filters that help it best identify certain aspects of images and thereby classify immages more accurately.

```python
!pip install imageio
```

    Requirement already satisfied: imageio in /usr/local/lib/python3.6/dist-packages (2.4.1)
    Requirement already satisfied: numpy in /usr/local/lib/python3.6/dist-packages (from imageio) (1.14.6)
    Requirement already satisfied: pillow in /usr/local/lib/python3.6/dist-packages (from imageio) (4.0.0)
    Requirement already satisfied: olefile in /usr/local/lib/python3.6/dist-packages (from pillow->imageio) (0.46)

```python
# Convolution in action
import imageio
import matplotlib.pyplot as plt
import numpy as np
import scipy.ndimage as nd
from skimage.exposure import rescale_intensity
from skimage import color

img = imageio.imread('https://www.dropbox.com/s/dv3vtiqy439pzag/all_the_things.png?raw=1')
plt.axis('off')
plt.imshow(img);
```

```python
# Convert I to grayscale, so it will be MxNx1 instead of MxNx4
grayscale = color.rgb2gray(img)
plt.axis('off')
plt.imshow(grayscale, cmap=plt.cm.gray);
```

```python
laplacian = np.array([[0,0,1,0,0],
              [0,0,2,0,0],
              [1,2,-16,2,1],
              [0,0,2,0,0],
              [0,0,1,0,0]])

laplacian_image = nd.convolve(grayscale, laplacian)
plt.axis('off')
plt.imshow(laplacian_image, cmap=plt.cm.gray);
```

```python
sobel_x = np.array([
    [-1,0,1],
    [-2,0,2],
    [-1,0,1]
])

sobel_x_image = nd.convolve(grayscale, sobel_x)
plt.axis('off')
plt.imshow(sobel_x_image, cmap=plt.cm.gray);
```

```python
sobel_y = np.array([
    [1,2,1],
    [0,0,0],
    [-1,-2,-1]
])

sobel_y_image = nd.convolve(grayscale, sobel_y)
plt.axis('off')
plt.imshow(sobel_y_image, cmap=plt.cm.gray);
```

## Are we going to learn to do Linear Algebra by hand?

Let me quote your seventh grade math teacher:

> "ArE yOu GoInG To CaRrY a CaLcUlAtOr ArOuNd wItH yOu EvErYwHeRe YoU gO???"

Of course you're going to carry a calculator around everywhere, so mostly **NO**, we're not going to do a lot of hand calculating. We're going to try and refrain from calculating things by hand unless it is absolutely necessary in order to understand and implement the concepts.

We're not trying to re-invent the wheel.

We're learning how to  **use** the wheel.

# Linear Algebra Overview/Review:

## Scalars:

A single number. Variables representing scalars are typically written in lower case.

Scalars can be whole numbers or decimals.

\begin{align}
a = 2
\qquad
b = 4.815162342
\end{align}

They can be positive, negative, 0 or any other real number.

\begin{align}
c = -6.022\mathrm{e}{+23}
\qquad
d = \pi
\end{align}

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

# Start with a simple vector
blue = [.5, .5]

# Then multiply it by a scalar

green = np.multiply(2, blue)

red = np.multiply(math.pi, blue)

orange = np.multiply(-0.5, blue)

# Plot the Scaled Vectors
plt.arrow(0,0, red[0], red[1],head_width=.05, head_length=0.05, color ='red')
plt.arrow(0,0, green[0], green[1],head_width=.05, head_length=0.05, color ='green')
plt.arrow(0,0, blue[0], blue[1],head_width=.05, head_length=0.05, color ='blue')
plt.arrow(0,0, orange[0], orange[1],head_width=.05, head_length=0.05, color ='orange')
plt.xlim(-1,2)
plt.ylim(-1,2)
plt.title("Scaled Vectors")
plt.show()
```

## Vectors:

### Definition
A vector of dimension *n* is an **ordered** collection of *n* elements, which are called **components** (Note, the components of a vector are **not** referred to as "scalars"). Vector notation variables are commonly written as a bold-faced lowercase letters or italicized non-bold-faced lowercase characters with an arrow (→) above the letters:

Written: $\vec{v}$

Examples:

\begin{align}
   \vec{a} =
   \begin{bmatrix}
           1\\
           2
    \end{bmatrix}
    \qquad
    \vec{b} =
    \begin{bmatrix}
          -1\\
           0\\
           2
    \end{bmatrix}
    \qquad
    \vec{c} =
    \begin{bmatrix}
           4.5
    \end{bmatrix}
    \qquad
    \vec{d} =
    \begin{bmatrix}
           Pl\\
           a\\
           b\\
           \frac{2}{3}
    \end{bmatrix}
\end{align}

The above vectors have dimensions 2, 3, 1, and 4 respectively.

Why do the vectors below only have two components?

```python
# Vector Examples
yellow = [.5, .5]
red = [.2, .1]
blue = [.1, .3]

plt.arrow(0, 0, .5, .5, head_width=.02, head_length=0.01, color = 'y')
plt.arrow(0, 0, .2, .1, head_width=.02, head_length=0.01, color = 'r')
plt.arrow(0, 0, .1, .3,  head_width=.02, head_length=0.01, color = 'b')
plt.title('Vector Examples')
plt.show()
```

In domains such as physics it is emphasized that vectors have two properties: direction and magnitude. It's rare that we talk about them in that sense in Data Science unless we're specifically in a physics context. We just note that the length of the vector is equal to the number of dimensions of the vector.

What happens if we add a third component to each of our vectors?

```python
from mpl_toolkits.mplot3d import Axes3D
import numpy as np

yellow = [.5, .5, .5]
red = [.2, .1, .0]
blue = [.1, .3, .3 ]

vectors = np.array([[0, 0, 0, .5, .5, .5],
                    [0, 0, 0, .2, .1, .0],
                    [0, 0, 0, .1, .3, .3]])

X, Y, Z, U, V, W = zip(*vectors)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.quiver(X, Y, Z, U, V, W, length=1)
ax.set_xlim([0, 1])
ax.set_ylim([0, 1])
ax.set_zlim([0, 1])
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
plt.show()
```

### Norm of a Vector (Magnitude or length)

The *Norm* or *Magnitude* of a vector is nothing more than the **length** of the vector. Since a vector is just a line (essentially) if you treat it as the hypotenuse of a triangle you could use the pythagorean theorem to find the equation for the norm of a vector. We're essentially just generalizing the equation for the hypotenuse of a triangle that results from the pythagorean theorem to n dimensional space.

We denote the norm of a vector by wrapping it in double pipes (like double absolute value signs)

\begin{align}
||v|| =
\sqrt{v_{1}^2 + v_{2}^2 + \ldots + v_{n}^2}
\\
\vec{a} =
\begin{bmatrix}
3 & 7 & 2 & 4
\end{bmatrix}
\\
||a|| = \sqrt{3^2 + 7^2 + 2^2 + 4^2} \\
||a|| = \sqrt{9 + 49 + 4 + 16} \\
||a|| = \sqrt{78}
\end{align}

The Norm is the square root of the sum of the squared elements of a vector.

Properties of the Norm:

The norm is always positive or zero $||x|| \geq 0$

The norm is only equal to zero if all of the elements of the vector are zero.

The Triangle Inequality: $|| x + y ||\leq ||x|| + ||y||$

### Dot Product

The dot product of two vectors $\vec{a}$ and $\vec{b}$ is a scalar quantity that is equal to the sum of pair-wise products of the components of vectors a and b. An example will make this make much more sense:

\begin{align} \vec{a} \cdot \vec{b} = (a_{1} \times b_{1}) + (a_{2} \times b_{2}) + \ldots + ( a_{n} \times b_{n}) \end{align}

Example:

\begin{align}
\vec{a} =
\begin{bmatrix}
3 & 7 & 2 & 4
\end{bmatrix}
\qquad
\vec{b} =
\begin{bmatrix}
4 & 1 & 12 & 6
\end{bmatrix}
\end{align}

The dot product of two vectors would be:
\begin{align}
a \cdot b = (3)(4) + (7)(1) + (2)(12) + (4)(6) \\
= 12 + 7 + 24 + 24  \\
= 67
\end{align}

The dot product is commutative: $ a \cdot b = b \cdot a$

The dot product is distributive: $a \cdot (b + c) = a \cdot b + a \cdot c$

Two vectors must have the same number of components in order for the dot product to exist. If they lengths differ the dot product is undefined.

### Cross Product

The Cross Product is the vector equivalent of multiplication. The result is a third vector that is perpendicular to the first two vectors.

It is written with a regular looking multiplication sign like $a \times b$ but it is read as "a cross b"

The cross product equation is a little complicated, and gaining an intuition for it is going to take a little bit more time than we have here. I think it's the least useful of the vector operations, but I'll give you a short example anyway.

Assume that we have vectors $x$ and $y$.

\begin{align}
x = \begin{bmatrix} x_1 & x_2 & x_3 \end{bmatrix}
\qquad
y = \begin{bmatrix} y_1 & y_2 & y_3 \end{bmatrix}
\end{align}

The cross product can be found by taking these two vectors and adding a third unit vector to create a 3x3 matrix and then finding the determinant of the 3x3 matrix like follows:

\begin{align}
x = \begin{vmatrix}
  i & j & k \\
  x_1 & x_2 & x_3 \\
  y_1 & y_2 & y_3
  \end{vmatrix}
\end{align}
\begin{align} =
i\begin{vmatrix}
x_2 & x_3 \\
y_2 & y_3
\end{vmatrix}
+ j\begin{vmatrix}
x_1 & x_3 \\
y_1 & y_3
\end{vmatrix}
+ k\begin{vmatrix}
x_1 & x_2 \\
y_1 & y_2
\end{vmatrix}
\end{align}

## Matrices:

A **matrix** is a rectangular grid of numbers arranged in rows and columns. Variables that represent matrices are typically written as capital letters (boldfaced as well if you want to be super formal).

\begin{align}
A =
    \begin{bmatrix}
           1 & 2 & 3\\
           4 & 5 & 6\\
           7 & 8 & 9
    \end{bmatrix}
    \qquad
    B = \begin{bmatrix}
           1 & 2 & 3\\
           4 & 5 & 6
    \end{bmatrix}
 \end{align}

### Dimensionality

The number of rows and columns that a matrix has is called its **dimension**.

When listing the dimension of a matrix we always list rows first and then columns.

The dimension of matrix A is 3x3. (Note: This is read "Three by Three", the 'x' isn't a multiplication sign.)

What is the Dimension of Matrix B?

### Matrix Equality

In order for two Matrices to be equal the following conditions must be true:

1) They must have the same dimensions.

2) Corresponding elements must be equal.

\begin{align}
    \begin{bmatrix}
           1 & 4\\
           2 & 5\\
           3 & 6
    \end{bmatrix}
    \neq
    \begin{bmatrix}
           1 & 2 & 3\\
           4 & 5 & 6
    \end{bmatrix}
\end{align}

### Matrix Multiplication

You can multipy any two matrices where the number of  columns of the first matrix is equal to the number of rows of the second matrix.

The unused dimensions of the factor matrices tell you what the dimensions of the product matrix will be.

There is no commutative property of matrix multiplication (you can't switch the order of the matrices and always get the same result).

Matrix multiplication is best understood in terms of the dot product. Remember:

\begin{align} \vec{a} \cdot \vec{b} = (a_{1} \times b_{1}) + (a_{2} \times b_{2}) + \ldots + ( a_{n} \times b_{n}) \end{align}

To multiply to matrices together, we will take the dot product of each row of the first matrix with each column of the second matrix. The position of the resulting entries will correspond to the row number and column number of the row and column vector that were used to find that scalar. Lets look at an example to make this more clear.

\begin{align}
\begin{bmatrix}
1 & 2 & 3 \\
4 & 5 & 6
\end{bmatrix}
\times
\begin{bmatrix}
7 & 8 \\
9 & 10 \\
11 & 12
\end{bmatrix}
=
\begin{bmatrix}
(1)(7)+(2)(9)+(3)(11) & (1)(8)+(2)(10)+(3)(12)\\
(4)(7)+(5)(9)+(6)(11) & (4)(8)+(5)(10)+(6)(12)
\end{bmatrix}
=
\begin{bmatrix}
(7)+(18)+(33) & (8)+(20)+(36)\\
(28)+(45)+(66) & (32)+(50)+(72)
\end{bmatrix}
=
\begin{bmatrix}
58 & 64\\
139 & 154
\end{bmatrix}
\end{align}

## Transpose

A transposed matrix is one whose rows are the columns of the original and whose columns are the rows of the original.

Common notation for the transpose of a matrix is to have a capital $T$ superscript or a tick mark:

\begin{align}
B^{T}
\qquad
B^{\prime}
\end{align}

The first is read "B transpose" the second is sometimes read as "B prime" but can also be read as "B transpose".

The transpose of any matrix can be found easily by fixing the elements on the main diagonal and flipping the placement of all other elements across that diagonal.

\begin{align}
  B =
\begin{bmatrix}
  1 & 2 & 3 \\
  4 & 5 & 6
\end{bmatrix}
  \qquad
  B^{T} =
  \begin{bmatrix}
  1 & 4 \\
  2 & 5 \\
  3 & 6
  \end{bmatrix}
\end{align}

## Square Matrix:

In a true linear algebra class after the first few weeks you would deal almost exclusively with square matrices. They have very nice properties that their lopsided sisters and brothers just don't possess.

A square matrix is any matrix that has the same number of rows as columns:

\begin{align}
A =
\begin{bmatrix}
  a_{1,1}
\end{bmatrix}
\qquad
B =
\begin{bmatrix}
b_{1,1} & b_{1,2} \\
b_{2,1} & b_{2,2}
\end{bmatrix}
\qquad
C =
\begin{bmatrix}
c_{1,1} & c_{1,2} & c_{1,3} \\
c_{2,1} & c_{2,2} & c_{2,3} \\
c_{3,1} & c_{3,2} & c_{3,3}
\end{bmatrix}
\end{align}

### Special Kinds of Square Matrices

**Diagonal:** Values on the main diagonal, zeroes everywhere else.

\begin{align}
A =
\begin{bmatrix}
a_{1,1} & 0 & 0 \\
0 & a_{2,2} & 0 \\
0 & 0 & a_{3,3}
\end{bmatrix}
\end{align}

**Upper Triangular:** Values on and above the main diagonal, zeroes everywhere else.

\begin{align}
B =
\begin{bmatrix}
b_{1,1} & b_{1,2} & b_{1,3} \\
0 & b_{2,2} & b_{2,3} \\
0 & 0 & b_{3,3}
\end{bmatrix}
\end{align}

**Lower Triangular:** Values on and below the main diagonal, zeroes everywhere else.

\begin{align}
C =
\begin{bmatrix}
c_{1,1} & 0 & 0 \\
c_{2,1} & c_{2,2} & 0 \\
c_{3,1} & c_{3,2} & c_{3,3}
\end{bmatrix}
\end{align}

**Identity Matrix:** A diagonal matrix with ones on the main diagonal and zeroes everywhere else. The product of the any square matrix and the identity matrix is the original square matrix $AI == A$. Also, any matrix multiplied by its inverse will give the identity matrix as its product.  $AA^{-1} = I$

\begin{align}
D =
\begin{bmatrix}
  1
\end{bmatrix}
\qquad
E =
\begin{bmatrix}
1 & 0 \\
0 & 1
\end{bmatrix}
\qquad
F =
\begin{bmatrix}
1 & 0 & 0 \\
0 & 1 & 0 \\
0 & 0 & 1
\end{bmatrix}
\end{align}

**Symmetric:** The numbers above the main diagonal are mirrored below/across the main diagonal.

\begin{align}
G =
\begin{bmatrix}
1 & 4 & 5 \\
4 & 2 & 6 \\
5 & 6 & 3
\end{bmatrix}
\end{align}

## Determinant

The determinant is a property that all square matrices possess and is denoted $det(A)$ or using pipes (absolute value symbols) $|A|$

The equation given for finding the determinant of a 2x2 matrix is as follows:

\begin{align}
A = \begin{bmatrix}
a & b \\
c & d
\end{bmatrix}
\qquad
|A| = ad-bc
\end{align}

The determinant of larger square matrices recursively by finding the determinats of the smaller matrics that make up the large matrix.

For example:

The above equation is **very** similar to the equation that we use to find the cross-product of a 3x3 matrix. The only difference is the negative sign in front of the $b$.

## Inverse

 There are multiple methods that we could use to find the inverse of a matrix by hand. I would suggest you explore those methods --if this content isn't already overwhelming enough. The inverse is like the reciprocal of the matrix that was used to generate it. Just like $\frac{1}{8}$ is the reciprocal of 8, $A^{-1}$ acts like the reciprocal of $A$.  The equation for finding the determinant of a 2x2 matrix is as follows:

 \begin{align}
A = \begin{bmatrix}
a & b \\
c & d
\end{bmatrix}
\qquad
A^{-1} = \frac{1}{ad-bc}\begin{bmatrix}
d & -b\\
-c & a
\end{bmatrix}
\end{align}

 ### What happens if we multiply a matrix by its inverse?

The product of a matrix multiplied by its inverse is the identity matrix of the same dimensions as the original matrix. There is no concept of "matrix division" in linear algebra, but multiplying a matrix by its inverse is very similar since $8\times\frac{1}{8} = 1$.

\begin{align}
A^{-1}A = I
\end{align}

### Not all matrices are invertible

Matrices that are not square are not invertible.

 A matrix is invertible if and only if its determinant is non-zero. You'll notice that the fraction on the left side of the matrix is $\frac{1}{det(A)}$.

 As you know, dividing anything by 0 leads to an undefined quotient. Therefore, if the determinant of a matrix is 0, then the entire inverse becomes undefined.

### What leads to a 0 determinant?

 A square matrix that has a determinant of 0 is known as a "singular" matrix. One thing that can lead to a matrix having a determinant of 0 is if two rows or columns in the matrix are perfectly collinear. Another way of saying this is that the determinant will be zero if the rows or columns of a matrix are not linearly dependent.

One of the most common ways that a matrix can end up having rows that are linearly dependent is if one column a multiple of another column. Lets look at an example:

\begin{align}
C =\begin{bmatrix}
  1 & 5 & 2 \\
  2 & 7 & 4 \\
  3 & 2 & 6
\end{bmatrix}
\end{align}

Look at the columns of the above matrix, column 3 is exactly double column 1. (could be any multiple or fraction) Think about if you had some measure in a dataset of distance in miles, but then you also wanted to convert its units to feet, so you create another column and multiply the mile measure by 5,280 (Thanks Imperial System). But then you forget to drop one of the columns so you end up with two columns that are linearly dependent which causes the determinant of your dataframe to be 0 and will cause certain algorithms to fail. We'll go deeper into this concept next week (this can cause problems with linear regression) so just know that matrices that have columns that are a multiple or fraction of another column will cause the determinant of that matrix to be 0.

 For more details about when a matrix is invertible google the "Invertible Matrix Theorem" but be prepared for some heavy linear algebra jargon.

## Who's ready to get familiar with NumPy???

[Helpful NumPy Linear Algebra Functions](https://docs.scipy.org/doc/numpy-1.15.1/reference/routines.linalg.html)

```python

### What should we do first? :) Want to see anything demonstrated?
```


# Forward kinematics of 2dof planar robots

## Case 1) Two revolute joints

## Case 2) Revolute joint followed by prismatic joint

```python
import numpy as np
import pandas as pd
import plotly.express as px
import plotly.graph_objects as go
import doctest
import spatialmath as sm
import sympy as sy
import sys
```

```python
 def fwd_kinematics_2rev(th1, th2, l1=2, l2=1):
    '''
    Implements the forward kinematics of a robot with two revolute joints.

    Arguments
    ---------
    th1, th2 : float
       Angle in radians of the two degree of freedoms, respectively.
    l1, l2 : float
       Length of the two links, respectively.

    Returns
    -------
    x : float
       The position in the global x-direction of the end-effector (tool point)
    y : float
       The position in the global y-direction of the end-effector (tool point)
    theta : float
       The orientation of the end-effector with respect to the positive global x-axis.
       The angle returned is in the range [-np.pi, np.pi]
   j : tuple with 2 elements
       The position of the joint between the two links

    Tests
    ------
    1) End-effector pose at default position
    >>> x, y, th, j = fwd_kinematics_2rev(0, 0)
    >>> "(%0.2f, %0.2f, %0.2f)" %(x, y, th)
    '(3.00, 0.00, 0.00)'

    2) End-effector pose at 90 degrees in both joints
    >>> x, y, th, j = fwd_kinematics_2rev(np.pi/2, np.pi/2)
    >>> "(%0.2f, %0.2f, %0.2f)" %(x, y, th)
    '(-1.00, 2.00, 3.14)'

    3) End-effector pose at 0 degress in first joint and 90 degress in second
    >>> x, y, th, j = fwd_kinematics_2rev(0, np.pi/2)
    >>> "(%0.2f, %0.2f, %0.2f)" %(x, y, th)
    '(2.00, 1.00, 1.57)'

    4) End-effector position is always inside a circle of a certain radius
    >>> poses = [fwd_kinematics_2rev(th1_, th2_, 3, 2)
    ... for th1_ in np.arange(0, 2*np.pi, 0.2)
    ... for th2_ in np.arange(0, 2*np.pi, 0.2)]
    >>> distances = np.array([np.sqrt(x_**2 + y_**2) for x_, y_, th_, j_ in poses])
    >>> max_radius = 5 + 1e-12 # Add a small tolerance
    >>> np.any(distances > max_radius)
    False

    5) Joint is always at constant distance from the origin
    >>> poses = [fwd_kinematics_2rev(th1_, 0, 3, 2)
    ... for th1_ in np.arange(0, 2*np.pi, 0.2) ]
    >>> distances = np.array([np.sqrt(j_[0]**2 + j_[1]**2) for x_, y_, th_, j_ in poses])
    >>> np.any(np.abs(distances - 3) > 1e-12)
    False

    '''

    # Static transformation between frame 2 and frame E
    g_2e = sm.SE3()
    g_2e.A[0,3] = l1+l2

    # Transformation betwen frame 1 and frame 2
    g_I = sm.SE3() # Identity transformation
    g_12 = sm.SE3.Rz(th2) # Rotation about z-axis
    q = [l1,0,0]
    d_12 = g_I*q - g_12*q
    g_12.A[:3, 3] = d_12.ravel()

    # Transformation between frame S and frame 1
    g_s1 = sm.SE3.Rz(th1)

    # Chain of transformations
    g_se = g_s1 * g_12 * g_2e

    x = g_se.A[0,3]
    y = g_se.A[1, 3]

    theta = th1+th2
    #print(np.arccos(g_se[0,0]), theta)

    #assert(np.abs(theta-np.arccos(g_se[0,0])) < 1e-8)

    j_s = g_s1 * [l1,0,0]

    j = tuple(j_s[:2])

    return (x, y, theta, j)

```

```python
# Case 1)
doctest.run_docstring_examples(fwd_kinematics_2rev, globals(), verbose=True)
```

    Finding tests in NoName
    Trying:
        x, y, th, j = fwd_kinematics_2rev(0, 0)
    Expecting nothing
    ok
    Trying:
        "(%0.2f, %0.2f, %0.2f)" %(x, y, th)
    Expecting:
        '(3.00, 0.00, 0.00)'
    ok
    Trying:
        x, y, th, j = fwd_kinematics_2rev(np.pi/2, np.pi/2)
    Expecting nothing
    ok
    Trying:
        "(%0.2f, %0.2f, %0.2f)" %(x, y, th)
    Expecting:
        '(-1.00, 2.00, 3.14)'
    ok
    Trying:
        x, y, th, j = fwd_kinematics_2rev(0, np.pi/2)
    Expecting nothing
    ok
    Trying:
        "(%0.2f, %0.2f, %0.2f)" %(x, y, th)
    Expecting:
        '(2.00, 1.00, 1.57)'
    ok
    Trying:
        poses = [fwd_kinematics_2rev(th1_, th2_, 3, 2)
        for th1_ in np.arange(0, 2*np.pi, 0.2)
        for th2_ in np.arange(0, 2*np.pi, 0.2)]
    Expecting nothing
    ok
    Trying:
        distances = np.array([np.sqrt(x_**2 + y_**2) for x_, y_, th_, j_ in poses])
    Expecting nothing
    ok
    Trying:
        max_radius = 5 + 1e-12 # Add a small tolerance
    Expecting nothing
    ok
    Trying:
        np.any(distances > max_radius)
    Expecting:
        False
    ok
    Trying:
        poses = [fwd_kinematics_2rev(th1_, 0, 3, 2)
        for th1_ in np.arange(0, 2*np.pi, 0.2) ]
    Expecting nothing
    ok
    Trying:
        distances = np.array([np.sqrt(j_[0]**2 + j_[1]**2) for x_, y_, th_, j_ in poses])
    Expecting nothing
    ok
    Trying:
        np.any(np.abs(distances - 3) > 1e-12)
    Expecting:
        False
    ok

```python
 def fwd_kinematics_2rev_symbolic(th1, th2,
                                  l1=sy.symbols('l1'), l2=sy.symbols('l2')):
    '''
    Implements the forward kinematics of a robot with two revolute joints.

    Arguments
    ---------
    th1, th2 : sympy symbols
       Symbol representing the angle in radians of the two degree of freedoms, respectively.
    l1, l2 : sympy symbols
       Symbol representing the length of the two links, respectively.

    Returns
    -------
    x : sympy expression
       The position in the global x-direction of the end-effector (tool point)
    y : sympy expression
       The position in the global y-direction of the end-effector (tool point)
    theta : sympy expression
       The orientation of the end-effector with respect to the positive global x-axis.
    j : tuple with 2 elements, each is a sympy expression
       The position of the joint between link1 and link2

    Tests
    ------
    1) End-effector pose at default position
    >>> th1, th2, l1, l2 = sy.symbols('th1, th2, l1, l2')
    >>> x, y, th, j = fwd_kinematics_2rev_symbolic(th1, th2, l1, l2)
    >>> subsdict = {th1: 0, th2: 0, l1: 2, l2: 1}
    >>> xn = x.evalf(subs=subsdict)
    >>> yn = y.evalf(subs=subsdict)
    >>> thn = th.evalf(subs=subsdict)
    >>> "(%0.2f, %0.2f, %0.2f)" %(xn, yn, thn)
    '(3.00, 0.00, 0.00)'

    2) End-effector pose at 90 degrees in both joints
    >>> th1, th2, l1, l2 = sy.symbols('th1, th2, l1, l2')
    >>> x, y, th, j = fwd_kinematics_2rev_symbolic(th1, th2, l1, l2)
    >>> subsdict = {th1: np.pi/2, th2: np.pi/2, l1: 2, l2: 1}
    >>> xn = x.evalf(subs=subsdict)
    >>> yn = y.evalf(subs=subsdict)
    >>> thn = th.evalf(subs=subsdict)
    >>> "(%0.2f, %0.2f, %0.2f)" %(xn, yn, thn)
    '(-1.00, 2.00, 3.14)'
    '''

    # Static transformation between frame 2 and frame E
    g = sy.eye(4)
    g[0, 3] = l1+l2
    g_2e = sm.SE3(np.array(g), check=False)

    # Transformation betwen frame 1 and frame 2
    g_I = sm.SE3(np.array(sy.eye(4)), check=False) # Identity transformation
    g_12 = sm.SE3.Rz(th2) # Rotation about z-axis
    q = [l1,0,0]
    d_12 = g_I*q - g_12*q
    g_12.A[:3, 3] = d_12.ravel()

    # Transformation between frame S and frame 1
    g_s1 = sm.SE3.Rz(th1)

    # Chain of transformations
    g_se = g_s1 * g_12 * g_2e

    x = g_se.A[0,3]
    y = g_se.A[1, 3]

    theta = th1+th2
    #print(np.arccos(g_se[0,0]), theta)

    #assert(np.abs(theta-np.arccos(g_se[0,0])) < 1e-8)

    j_s = g_s1 * [l1,0,0]

    j = tuple(j_s[:2])

    return (x, y, theta, j)

```

```python
doctest.run_docstring_examples(fwd_kinematics_2rev_symbolic, globals())
```

    **********************************************************************
    File "__main__", line 43, in NoName
    Failed example:
        "(%0.2f, %0.2f, %0.2f)" %(xn, yn, thn)
    Expected:
        '(-1.00, 2.00, 3.14)'
    Got:
        '(-1.00, 2.00, 3.14)'

```python
def fwd_kinematics_rev_prism(th1, th2, l1=2):
    '''
    Implements the forward kinematics of a robot with one revolute joint and one prismatic.

    Arguments
    ---------
    th1 : float
       Angle in radians of the first degree of freedom.
    th2 : float
       Displacement in meter of the second degree of freedom.
    l1 : float
       Length of the first link.

    Returns
    -------
    x : float
       The position in the global x-direction of the end-effector (tool point)
    y : float
       The position in the global y-direction of the end-effector (tool point)
    theta : float
       The orientation of the end-effector with respect to the positive global x-axis

    Tests
    ------
    1) End-effector pose at default position
    >>> "(%0.2f, %0.2f, %0.2f)" %fwd_kinematics_rev_prism(0, 0)
    '(2.00, 0.00, 0.00)'

    2) End-effector pose at 90 degrees in first joint and 0.6m in second
    >>> "(%0.2f, %0.2f, %0.2f)" %fwd_kinematics_rev_prism(np.pi/2, 0.6)
    '(0.00, 2.60, 1.57)'

    4) End-effector orientation is always the same as the angle of the first dof
    >>> angles = np.array( [th1_ for th1_ in np.arange(0, 2*np.pi, 0.2)
    ... for th2_ in np.arange(-1, 1, 0.2)])
    >>> poses = [fwd_kinematics_rev_prism(th1_, th2_)
    ... for th1_ in np.arange(0, 2*np.pi, 0.2)
    ... for th2_ in np.arange(-1, 1, 0.2)]
    >>> orientations = np.array([th_ for x_, y_, th_ in poses])
    >>> np.any(np.abs(angles-orientations) > 1e-12)
    False
    '''
    x = 0
    y = 0
    theta = 0
    return (x, y, theta)

```

## Run doctests
If tests pass, no output is generated.

```python
# Case 2)
doctest.run_docstring_examples(fwd_kinematics_rev_prism, globals())
```

## Visualize the work space of the robot

```python
th1 = np.arange(0, 2*np.pi, 0.1)
th2 = np.arange(-np.pi, np.pi, 0.1)
xythetaj =[ fwd_kinematics_2rev(th1_, th2_) for th1_ in th1 for th2_ in th2]
xytheta = np.array([ (x_, y_, th_) for x_, y_, th_, j_ in xythetaj])
df = pd.DataFrame(data=np.reshape(xytheta, (-1,3)), columns=['x', 'y', 'theta'])
```

```python
fig = px.scatter_3d(df, x='x', y='y', z='theta')
camera = dict(
    up=dict(x=0, y=1, z=0),
    center=dict(x=0, y=0, z=0),
    eye=dict(x=0, y=0, z=4)
)
fig.update_scenes(camera_projection_type="orthographic")
fig.update_layout(scene_camera=camera)
fig.show()

```

## Visualize movement of the manipulator

```python
poses = [ fwd_kinematics_2rev(th1_, th2_) for th1_, th2_ in zip(th1, th2)]
endeff_trajectory = np.array([ [x_, y_]  for x_, y_, th_, j_ in poses])
joint_trajectory = np.array([ j_  for x_, y_, th_, j_ in poses])
```

```python
fig = go.Figure(
    data=[go.Scatter(x=[0, joint_trajectory[0,0]], y=[0, joint_trajectory[0,1]],
          name="First link", mode="lines",
          line=dict(width=6, color="blue")),
          go.Scatter(x=[joint_trajectory[0,0], endeff_trajectory[0,0]],
                     y=[joint_trajectory[0,1], endeff_trajectory[0,1]],
          name="Second link", mode="lines",
          line=dict(width=5, color="red")),
         go.Scatter(x=joint_trajectory[:,0], y=joint_trajectory[:,1],
          name="Joint trajectory", mode="lines",
          line=dict(width=1, color="lightblue")),
          go.Scatter(x=endeff_trajectory[:,0], y=endeff_trajectory[:,1],
          name="End-point trajectory", mode="lines",
          line=dict(width=1, color="red"))],
    layout=go.Layout( width=700, height=600,
        xaxis=dict(range=[-4, 4], autorange=False),
        yaxis=dict(range=[-4, 4], autorange=False),
        title="End-effector trajectory",
        updatemenus=[dict(
            type="buttons",
            buttons=[dict(label="Play",
                          method="animate",
                          args=[None])])]
    ),
    frames=[go.Frame(data=[go.Scatter(x=[0, xj_], y=[0, yj_]),
                          go.Scatter(x=[xj_, xe_], y=[yj_, ye_])])
            for xj_, yj_, xe_, ye_ in np.hstack((joint_trajectory, endeff_trajectory))]
)

fig.show()
```

```python
?px.scatter_3d
```

```python

```

```python

```


# Determine derivatives of angular sequence to rotation matrix

Peter Corke 2021

SymPy code to evaluate the mappings from rotation angle sequences to rotation matrix, then compute the derivatives to determine the mapping from angular rates to angular velocity.

Adjust the next cell to choose the particular angle sequence.

```python
from spatialmath.base import *
from sympy import *
```

```python
angle_names = ('alpha', 'beta', 'gamma')
func = eul2r

#angle_names =  ('phi', 'theta', 'psi')
#func = lambda Gamma: rpy2r(Gamma, order='xyz')
```

Define a symbol for time

```python
t = symbols('t')
```

Define arrays of symbols for the angles, angle as function of time, time derivative of angle as a function of time

```python
angle = []  # names of angles, eg. theta
anglet = []  # angles as function of time, eg. theta(t)
angled = []  # derivative of above, eg. d theta(t) / dt
angledn = []  # symbol to represent above, eg. theta_dot
for i in angle_names:
    angle.append(symbols(i))
    anglet.append(Function(i)(t))
    angled.append(anglet[-1].diff(t))
    angledn.append(i + '_dot')
```

Compute the rotation matrix

```python
R = Matrix(func(anglet))
R
```

$\displaystyle \left[\begin{matrix}- \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} + \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} & - \sin{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} - \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} & \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)}\\\sin{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} + \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} & - \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} + \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} & \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\beta{\left(t \right)} \right)}\\- \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} & \sin{\left(\beta{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} & \cos{\left(\beta{\left(t \right)} \right)}\end{matrix}\right]$

Compute its time derivative

```python
Rdot = Matrix(R).diff(t)
Rdot
```

$\displaystyle \left[\begin{matrix}- \sin{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} - \sin{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} - \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} - \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} - \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} & \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} + \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} + \sin{\left(\beta{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} - \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} - \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} & - \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} + \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)}\\- \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} - \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} - \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} + \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} + \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} & \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\beta{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} - \sin{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} - \sin{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} - \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} - \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} & \sin{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} + \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)}\\\sin{\left(\beta{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} - \cos{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} & \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} + \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} & - \sin{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)}\end{matrix}\right]$

Get angular velocity vector in terms of angles and angle rates

```python
omega = Matrix(vex(Rdot * R.T))
```

For each element of this 3x1 matrix get the coefficients of each angle derivative

```python
A = Matrix.zeros(3,3)
for i in range(3):
    e = omega[i,0].expand()
    for j in range(3):
        A[i, j] = e.coeff(angled[j])
```

The result is a 3x3 matrix. Mapping from angle rates to angular velocity. We subsitute angle as a function of time to plain angle, then simplify.

```python
A = trigsimp(A.subs(a for a in zip(anglet, angle)))
```

Compute the inverse and simplify.  Mapping from angular velocity to angle rates.

```python
Ai = trigsimp(A.inv())
```

Render as code

```python
pycode(A).replace('ImmutableDenseMatrix', 'np.array')
```

    'np.array([[0, -math.sin(alpha), math.sin(beta)*math.cos(alpha)], [0, math.cos(alpha), math.sin(alpha)*math.sin(beta)], [1, 0, math.cos(beta)]])'

```python
pycode(Ai).replace('ImmutableDenseMatrix', 'np.array')
```

    'np.array([[-math.cos(alpha)/math.tan(beta), -math.sin(alpha)/math.tan(beta), 1], [-math.sin(alpha), math.cos(alpha), 0], [math.cos(alpha)/math.sin(beta), math.sin(alpha)/math.sin(beta), 0]])'

Compute the time derivative of `Ai`, from angular acceleration to angle acceleration

```python
Ai = Ai.subs(a for a in zip(angle, anglet))
```

```python
Ai_dot = trigsimp(Ai.diff(t).subs(a for a in zip(angled, angledn)).subs(a for a in zip(anglet, angle)))
Ai_dot
```

$\displaystyle \left[\begin{matrix}\frac{\alpha_{dot} \sin{\left(\alpha \right)}}{\tan{\left(\beta \right)}} + \frac{\beta_{dot} \cos{\left(\alpha \right)}}{\sin^{2}{\left(\beta \right)}} & - \frac{\alpha_{dot} \cos{\left(\alpha \right)}}{\tan{\left(\beta \right)}} + \frac{\beta_{dot} \sin{\left(\alpha \right)}}{\sin^{2}{\left(\beta \right)}} & 0\\- \alpha_{dot} \cos{\left(\alpha \right)} & - \alpha_{dot} \sin{\left(\alpha \right)} & 0\\- \frac{\alpha_{dot} \sin{\left(\alpha \right)} + \frac{\beta_{dot} \cos{\left(\alpha \right)} \cos{\left(\beta \right)}}{\sin{\left(\beta \right)}}}{\sin{\left(\beta \right)}} & \frac{\alpha_{dot} \cos{\left(\alpha \right)} - \frac{\beta_{dot} \sin{\left(\alpha \right)} \cos{\left(\beta \right)}}{\sin{\left(\beta \right)}}}{\sin{\left(\beta \right)}} & 0\end{matrix}\right]$

```python
pycode(Ai_dot).replace('ImmutableDenseMatrix', 'np.array')
```

    'np.array([[alpha_dot*math.sin(alpha)/math.tan(beta) + beta_dot*math.cos(alpha)/math.sin(beta)**2, -alpha_dot*math.cos(alpha)/math.tan(beta) + beta_dot*math.sin(alpha)/math.sin(beta)**2, 0], [-alpha_dot*math.cos(alpha), -alpha_dot*math.sin(alpha), 0], [-(alpha_dot*math.sin(alpha) + beta_dot*math.cos(alpha)*math.cos(beta)/math.sin(beta))/math.sin(beta), (alpha_dot*math.cos(alpha) - beta_dot*math.sin(alpha)*math.cos(beta)/math.sin(beta))/math.sin(beta), 0]])'

```python

```


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

data=pd.read_csv('test.txt',sep=',',header=None)
data=data.values
data

mu_1=np.matrix([0,0])
mu_2=np.matrix([2,2])

sigma_1=np.matrix([[.25,.3],[.3,1]])
sigma_2=np.matrix([[.5,0],[0,.5]])

# print('Mu 1: ',mu_1)
# print('Mu 2: ',mu_2)
# print('Sigma 1: ',sigma_1)
# print('Sigma 2: ',sigma_2)

list=[]
for sample in data:
    temp=sample
    sample=np.matrix(sample)

    for i in range(2):
        if i==0:
            g1=-np.log(2*np.pi)-0.5*np.log(np.linalg.det(sigma_1))-0.5*(sample-mu_1)*np.linalg.inv(sigma_1)*(sample-mu_1).T+np.log(0.5)
#             print(g1)
        if i==1:
            g2=-np.log(2*np.pi)-0.5*np.log(np.linalg.det(sigma_2))-0.5*(sample-mu_2)*np.linalg.inv(sigma_2)*(sample-mu_2).T+np.log(0.5)
#             print(g2)

    print(sample,'g1:-',g1,'g2: ',g2)
    if g1>g2:
        plt.scatter(temp[0],temp[1],marker="+",color='blue')
        list.append([temp[0],temp[1],1])
    else:
        plt.scatter(temp[0],temp[1],marker=".",color='green')
        list.append([temp[0],temp[1],2])

plt.show()

```

```python
import numpy as np
import matplotlib.pyplot as plt
from scipy.stats import multivariate_normal
from mpl_toolkits.mplot3d import Axes3D
import sympy as sp
from matplotlib import cm

from sympy import lambdify

#Parameters to set
mu_1=np.array([0,0])
mu_2=np.array([2,2])

sigma_1=np.array([[.25,.3],[.3,1]])
sigma_2=np.array([[.5,0],[0,.5]])

#Create grid and multivariate normal
x = np.linspace(-6,6,500)
y = np.linspace(-6,6,500)
X, Y = np.meshgrid(x,y)
pos = np.empty(X.shape + (2,))
pos[:, :, 0] = X; pos[:, :, 1] = Y
rv = multivariate_normal(mu_1, sigma_1)
rv2 = multivariate_normal(mu_2,sigma_2)

#Make a 3D plot

fig = plt.figure(figsize=(10,8))
ax = fig.gca(projection='3d')

ax.plot_surface(X, Y, rv.pdf(pos),rstride=3, cstride=3, linewidth=1, antialiased=True, cmap=cm.plasma, alpha=0.4)
ax.plot_surface(X, Y, rv2.pdf(pos),rstride=3, cstride=3, linewidth=1, antialiased=True,cmap=cm.plasma, alpha=0.4)

cset = ax.contourf(X, Y, rv.pdf(pos), zdir='z',  offset=-1, cmap=cm.plasma, alpha=0.4)
cset = ax.contourf(X, Y, rv2.pdf(pos), zdir='z',  offset=-1, cmap=cm.plasma, alpha=0.4)
cset = ax.contourf(X, Y, rv.pdf(pos)-rv2.pdf(pos), zdir='z', offset=-1, cmap='Blues', alpha=0.2)

# ax.scatter(-4,2,-1,color='green',marker='+')
# ax.plot_surface(X, Y, rv.pdf(pos))
# ax.plot_surface(X, Y, rv2.pdf(pos))

ax.set_xlabel('X axis')
ax.set_ylabel('Y axis')
ax.set_zlabel('Z axis')

ax.set_zlim3d(-1,1)

ax.view_init(azim=240)

for x,y,l in list:
    if l==1:
        ax.scatter(x,y,-1,color='red',marker='+')
    else:
        ax.scatter(x,y,-1,color='blue',marker='.')

plt.show()

```

```python
from numpy import *
import math
%matplotlib inline

mu_1=np.matrix([0,0])
mu_2=np.matrix([2,2])

sigma_1=np.matrix([[.25,.3],[.3,1]])
sigma_2=np.matrix([[.5,0],[0,.5]])

x = np.linspace(-6,6,500)
y = np.linspace(-6,6,500)
X, Y = np.meshgrid(x,y)
# print(X)
# print(Y)

def norm_pdf_multivariate(x, mu, sigma):
    size = len(x)
#     print('sX,sM,sigmaShape:',size,len(mu),sigma.shape)

    det = linalg.det(sigma)

    norm_const = 1.0/ ( math.pow((2*pi),float(size)/2) * math.pow(det,1.0/2) )
    x_mu = matrix(x - mu)
    inv = np.linalg.inv(sigma)
    result = math.pow(math.e, -0.5 * (x_mu * inv * x_mu.T))
    return norm_const * result

# print(norm_pdf_multivariate(np.matrix([1,2]),mu_1,sigma_1))
pdf1=[]
pdf2=[]
for i in range(len(X)):
    temp1=[]
    temp2=[]
    for j in range(len(X)):
            n1=norm_pdf_multivariate(np.matrix([X[i,j],Y[i,j]]),mu_1,sigma_1)
            n2=norm_pdf_multivariate(np.matrix([X[i,j],Y[i,j]]),mu_2,sigma_2)

            temp1.append(n1)
            temp2.append(n2)
    pdf1.append(temp1)
    pdf2.append(temp2)
Z1=np.array(pdf1)
Z2=np.array(pdf2)
# print(Z1)

fig = plt.figure(figsize=(10,8))
ax = fig.gca(projection='3d')

ax.plot_surface(X, Y, Z1,rstride=3, cstride=3, linewidth=1, antialiased=True, cmap=cm.plasma, alpha=0.4)
ax.plot_surface(X, Y, Z2,rstride=3, cstride=3, linewidth=1, antialiased=True,cmap=cm.plasma, alpha=0.4)

cset = ax.contourf(X, Y, Z1, zdir='z',  offset=-1, cmap=cm.plasma, alpha=0.4)
cset = ax.contourf(X, Y, Z2, zdir='z',  offset=-1, cmap=cm.plasma, alpha=0.4)
cset = ax.contourf(X, Y, Z1-Z2, zdir='z', offset=-1, cmap='Reds', alpha=0.2)

# ax.scatter(-4,2,-1,color='green',marker='+')
# ax.plot_surface(X, Y, rv.pdf(pos))
# ax.plot_surface(X, Y, rv2.pdf(pos))

ax.set_xlabel('X axis')
ax.set_ylabel('Y axis')
ax.set_zlabel('Z axis')

ax.set_zlim3d(-1,1)

ax.view_init(azim=240)

for x,y,l in list:
    if l==1:
        ax.scatter(x,y,-1,color='red',marker='+')
    else:
        ax.scatter(x,y,-1,color='blue',marker='.')

plt.show()

```

```python
%matplotlib qt

fig = plt.figure(figsize=(15,10))
ax = fig.gca(projection='3d')

ax.plot_surface(X, Y, Z1,rstride=3, cstride=3, linewidth=1, antialiased=True, cmap=cm.plasma, alpha=0.4)
ax.plot_surface(X, Y, Z2,rstride=3, cstride=3, linewidth=1, antialiased=True,cmap=cm.plasma, alpha=0.4)

cset = ax.contourf(X, Y, Z1, zdir='z',  offset=-1, cmap=cm.plasma, alpha=0.4)
cset = ax.contourf(X, Y, Z2, zdir='z',  offset=-1, cmap=cm.plasma, alpha=0.4)
cset = ax.contourf(X, Y, Z1-Z2, zdir='z', offset=-1, cmap='Blues', alpha=0.2)

# ax.scatter(-4,2,-1,color='green',marker='+')
# ax.plot_surface(X, Y, rv.pdf(pos))
# ax.plot_surface(X, Y, rv2.pdf(pos))

ax.set_xlabel('X axis')
ax.set_ylabel('Y axis')
ax.set_zlabel('Z axis')

ax.set_zlim3d(-1,1)

for x,y,l in list:
    if l==1:
        ax.scatter(x,y,-1,color='red',marker='P')
    else:
        ax.scatter(x,y,-1,color='blue',marker='^')

for angle in range(0, 360,3):
    ax.view_init(22, angle)
#     print(angle)
    plt.draw()
    plt.pause(0.001)
    if angle==359:
        angle=0

```

```python
import numpy as np
import matplotlib.pyplot as plt
from scipy.stats import multivariate_normal
from mpl_toolkits.mplot3d import Axes3D
import sympy as sp

from sympy import lambdify

#Parameters to set
mu_1=np.array([0,0])
mu_2=np.array([2,2])

sigma_1=np.array([[.25,.3],[.3,1]])
sigma_2=np.array([[.5,0],[0,.5]])

#Create grid and multivariate normal
x = np.linspace(-6,6,4)
y = np.linspace(-6,6,4)
X, Y = np.meshgrid(x,y)
print(X)
print(Y)

pos = np.empty(X.shape + (2,))
pos[:, :, 0] = X; pos[:, :, 1] = Y
rv = multivariate_normal(mu_1, sigma_1)
rv2 = multivariate_normal(mu_2,sigma_2)

print(rv.pdf(pos))

pdf1=[]
pdf2=[]
for i in range(len(X)):
    temp1=[]
    temp2=[]
    for j in range(len(X)):
            n1=norm_pdf_multivariate(np.matrix([X[i,j],Y[i,j]]),mu_1,sigma_1)
            n2=norm_pdf_multivariate(np.matrix([X[i,j],Y[i,j]]),mu_2,sigma_2)

            temp1.append(n1)
            temp2.append(n2)
    pdf1.append(temp1)
    pdf2.append(temp2)
Z1=np.array(pdf1)
Z2=np.array(pdf2)
print(Z1)

```

    [[-6. -2.  2.  6.]
     [-6. -2.  2.  6.]
     [-6. -2.  2.  6.]
     [-6. -2.  2.  6.]]
    [[-6. -6. -6. -6.]
     [-2. -2. -2. -2.]
     [ 2.  2.  2.  2.]
     [ 6.  6.  6.  6.]]
    [[6.94986152e-33 5.34776920e-09 1.53080887e-28 1.63012570e-91]
     [1.43247322e-41 1.17792455e-04 3.60329854e-11 4.10048122e-61]
     [4.10048122e-61 3.60329854e-11 1.17792455e-04 1.43247322e-41]
     [1.63012570e-91 1.53080887e-28 5.34776920e-09 6.94986152e-33]]
    [[1.74207194e-32 1.34048695e-08 3.83716880e-28 4.08611917e-91]
     [3.59067789e-41 2.95261899e-04 9.03213001e-11 1.02783822e-60]
     [1.02783822e-60 9.03213001e-11 2.95261899e-04 3.59067789e-41]
     [4.08611917e-91 3.83716880e-28 1.34048695e-08 1.74207194e-32]]

```python
sample.shape
```

    (1, 2)


## Approximate Dynamic Programming Example

We consider an optimal control problem that is similar to that described in the [model predictive control example](https://github.com/cvxgrp/codegen/blob/main/examples/MPC.ipynb). However, we assume that the system matrices are functions of the state $x \in \mathbb{R}^n$, i.e., we have the nonlinear system $\left( A(x), B(x)\right) \in \left( \mathbb{R}^{n \times n}, \mathbb{R}^{n \times m} \right)$ with control input $u \in \mathbb{R}^m$ subject to optimization. The dynamics equation $x_{i+1} = A(x_i) x_i + B(x_i) u_i$ would be a non-convex constraint. Thus, we perform Approximate Dynamic Programming (ADP) [1] by predicting just one time step ahead and approximating the infinite-horizon cost as $\left(A(x) x + B(x) u\right)^T P \left(A(x) x + B(x) u\right)$ with measurement $x$. We solve the optimization problem

\begin{equation}
\begin{array}{ll}
\text{minimize} \quad & \left(A(x) x + B(x) u\right)^T P \left(A(x) x + B(x) u\right) + u^T R u\\
\text{subject to} \quad & \Vert u \Vert_2 \leq 1,
\end{array}
\end{equation}

where $u \in \mathbb{R}^m$ is the variable and constrained within a euclidean ball of size $1$. The cost matrices are positive definite: $P, R \in \mathbb{S}_{++}^n, \mathbb{S}_{++}^m$. We reformulate the problem to be [DPP-compliant](https://www.cvxpy.org/tutorial/advanced/index.html#disciplined-parametrized-programming), i.e.,

\begin{equation}
\begin{array}{ll}
\text{minimize} \quad & \Vert f+ G u \Vert_2^2 + \Vert R^{1/2} u \Vert_2^2\\
\text{subject to} \quad &\Vert u \Vert_2 \leq 1,
\end{array}
\end{equation}

where the new set of parameters contains $f = P^{1/2} A(x) x$, $G = P^{1/2} B(x)$, and $R^{1/2}$. Let's define the corresponding CVXPY problem.

```python
import cvxpy as cp

# define dimensions
n, m = 6, 3

# define variables
u = cp.Variable(m, name='u')

# define parameters
Rsqrt = cp.Parameter((m, m), name='Rsqrt')
f = cp.Parameter(n, name='f')
G = cp.Parameter((n, m), name='G')

# define objective
objective = cp.Minimize(cp.sum_squares(f+G@u) + cp.sum_squares(Rsqrt@u))

# define constraints
constraints = [cp.norm(u, 2) <= 1]

# define problem
problem = cp.Problem(objective, constraints)
```

Assign parameter values and solve the problem. In this case, the state $x = \left[p^T v^T\right]^T$ consists of position $p \in \mathbb{R}^3$ and velocity $v \in \mathbb{R}^3$ of some rigid body in three-dimensional space. The control input $u$ represents aerodynamic actuation. Multiplied with velocity, the force vector that acts on the body's center of mass results. Rotational dynamics are not considered. The discretization step is denoted by $t_d \in \mathbb{R}_{++}$. Air resistance forces relate to the squared velocity.

```python
import numpy as np

def dynamics(x):

    # continuous-time dynmaics
    A_cont = np.array([[0, 0, 0, 1, 0, 0],
                       [0, 0, 0, 0, 1, 0],
                       [0, 0, 0, 0, 0, 1],
                       [0, 0, 0, -x[3], 0, 0],
                       [0, 0, 0, 0, -x[4], 0],
                       [0, 0, 0, 0, 0, -x[5]]])
    mass = 1
    B_cont = np.concatenate((np.zeros((3,3)),
                             (1/mass)*np.diag(x[3:])), axis=0)

    # discrete-time dynamics
    td = 0.1
    A = np.eye(n)+td*A_cont
    B = td*B_cont

    return A, B

# cost
Rsqrt.value = np.sqrt(0.1)*np.eye(m)
Psqrt = np.eye(n)
x = np.array([2, 2, 2, -1, -1, 1])
A, B = dynamics(x)
f.value = np.matmul(Psqrt, np.matmul(A, x))
G.value = np.matmul(Psqrt, B)

val = problem.solve()
```

Generating C source for the problem is as easy as:

```python
from cvxpygen import cpg

cpg.generate_code(problem, code_dir='ADP_code')
```

Now, you can use a python wrapper around the generated code as a custom CVXPY solve method.

```python
from ADP_code.cpg_solver import cpg_solve
import numpy as np
import pickle
import time

# load the serialized problem formulation
with open('ADP_code/problem.pickle', 'rb') as f:
    prob = pickle.load(f)

# assign parameter values
prob.param_dict['Rsqrt'].value = np.sqrt(0.1)*np.eye(m)
prob.param_dict['f'].value = np.matmul(Psqrt, np.matmul(A, x))
prob.param_dict['G'].value = np.matmul(Psqrt, B)

# solve problem conventionally
t0 = time.time()
# CVXPY chooses eps_abs=eps_rel=1e-5, max_iter=10000, polish=True by default,
# however, we choose the OSQP default values here, as they are used for code generation as well
val = prob.solve()
t1 = time.time()
print('\nCVXPY\nSolve time: %.3f ms' % (1000 * (t1 - t0)))
print('Objective function value: %.6f\n' % val)

# solve problem with C code via python wrapper
prob.register_solve('CPG', cpg_solve)
t0 = time.time()
val = prob.solve(method='CPG')
t1 = time.time()
print('\nCVXPYgen\nSolve time: %.3f ms' % (1000 * (t1 - t0)))
print('Objective function value: %.6f\n' % val)
```

\[1\] Wang, Yang, Brendan O'Donoghue, and Stephen Boyd. "Approximate dynamic programming via iterated Bellman inequalities." International Journal of Robust and Nonlinear Control 25.10 (2015): 1472-1496.


# "Symbolic Euler's Method"
> "Applying Euler's Method to ODE , but with a twist: we're going to call method with Symbolic variables"
- toc: true
- badges: true
- comments: true
- categories: [jupyter, math, calculus, symbolics, julialang]

```julia
#collapse-show

# load dependacies

using MyCalculus
using Plots
using Symbolics
```

## Appply Euler's method for the following First-Order Differential Equation:

$fun(t, y) = y^2 -t$

```julia
# function definition : RHS of ODE

fun(t, y) = y^2 -t
```

    fun (generic function with 1 method)

## Definition of Euler's Method function:

$EulerMethod(f, x₀, y₀, step= 0.5, n=100.0)$

Function to approximate solution to an ordinary differential equation using the Euler's Method.
dy/dx = f(x, y) where y(x₀) = y₀ and x₀ = x₀.

 __Arguments__

- $f:$ function to approximate solution to, of the form f(x, y).
        This is the right hand side of the differential equation.
- $x₀:$ initial $x$ value condition
- $y₀:$ initial $y$ value condition
- $step:$ step size for the Euler's Method
- $n:$ number of steps to take

__Returns__
- $x:$ array of x values
- $y:$ array of y values

```julia
x, y = EulerMethod(fun, -1.0, -0.5, 0.5, 5)
```

    ([-1.0, -0.5, 0.0, 0.5, 1.0], [-0.5, 0.125, 0.3828125, 0.456085205078125, 0.31009206222370267])

```julia
plot(x,y, ylim=(-0.5,0.5), xlim=(-1,1), framestyle = :, ylabel = "y", xlabel = "x")

```

### We can also use Symbolics by passing Symbolics variables to the function:

This is a good way to "peek" into the inner workings of the Euler's Method and see what it's doing for every iteration.

It also showcases the power of Symbolics. Due to  highly composable julia code, we can use Symbolics everywhere a number is expected.

```julia
@variables x₀, y₀, step #defines the symbollics variables
t, y =  EulerMethod(fun, x₀, y₀, step, 5)
```

    (Num[x₀, step + x₀, x₀ + 2step, x₀ + 3step, x₀ + 4step], Num[y₀, y₀ + step*(y₀^2 - x₀), y₀ + step*(y₀^2 - x₀) + step*((y₀ + step*(y₀^2 - x₀))^2 - step - x₀), y₀ + step*(y₀^2 - x₀) + step*((y₀ + step*(y₀^2 - x₀))^2 - step - x₀) + step*((y₀ + step*(y₀^2 - x₀) + step*((y₀ + step*(y₀^2 - x₀))^2 - step - x₀))^2 - 2step - x₀), y₀ + step*(y₀^2 - x₀) + step*((y₀ + step*(y₀^2 - x₀))^2 - step - x₀) + step*((y₀ + step*(y₀^2 - x₀) + step*((y₀ + step*(y₀^2 - x₀))^2 - step - x₀))^2 - 2step - x₀) + step*((y₀ + step*(y₀^2 - x₀) + step*((y₀ + step*(y₀^2 - x₀))^2 - step - x₀) + step*((y₀ + step*(y₀^2 - x₀) + step*((y₀ + step*(y₀^2 - x₀))^2 - step - x₀))^2 - 2step - x₀))^2 - 3step - x₀)])

```julia
#collapse-output
t;
```

\begin{equation} \left[ \begin{array}{c} x{_0} \\ step + x{_0} \\ x{_0} + 2 step \\ x{_0} + 3 step \\ x{_0} + 4 step \\ \end{array} \right] \end{equation}

```julia
#collapse-output
y;
```

\begin{equation} \left[ \begin{array}{c} y{_0} \\ y{_0} + step \left( - x{_0} + y{_0}^{2} \right) \\ y{_0} + step \left( - x{_0} + y{_0}^{2} \right) + step \left( - step - x{_0} + \left( y{_0} + step \left( - x{_0} + y{_0}^{2} \right) \right)^{2} \right) \\ y{_0} + step \left( - x{_0} + y{_0}^{2} \right) + step \left( - step - x{_0} + \left( y{_0} + step \left( - x{_0} + y{_0}^{2} \right) \right)^{2} \right) + step \left( - 2 step - x{_0} + \left( y{_0} + step \left( - x{_0} + y{_0}^{2} \right) + step \left( - step - x{_0} + \left( y{_0} + step \left( - x{_0} + y{_0}^{2} \right) \right)^{2} \right) \right)^{2} \right) \\ y{_0} + step \left( - x{_0} + y{_0}^{2} \right) + step \left( - step - x{_0} + \left( y{_0} + step \left( - x{_0} + y{_0}^{2} \right) \right)^{2} \right) + step \left( - 2 step - x{_0} + \left( y{_0} + step \left( - x{_0} + y{_0}^{2} \right) + step \left( - step - x{_0} + \left( y{_0} + step \left( - x{_0} + y{_0}^{2} \right) \right)^{2} \right) \right)^{2} \right) + step \left( - 3 step - x{_0} + \left( y{_0} + step \left( - x{_0} + y{_0}^{2} \right) + step \left( - step - x{_0} + \left( y{_0} + step \left( - x{_0} + y{_0}^{2} \right) \right)^{2} \right) + step \left( - 2 step - x{_0} + \left( y{_0} + step \left( - x{_0} + y{_0}^{2} \right) + step \left( - step - x{_0} + \left( y{_0} + step \left( - x{_0} + y{_0}^{2} \right) \right)^{2} \right) \right)^{2} \right) \right)^{2} \right) \\ \end{array} \right] \end{equation}

```julia

```

```julia

```

```julia

```


# Best Approximation in Hilbert Spaces

```python
%matplotlib inline
import sympy as sym
import pylab as pl
import numpy as np
import numpy.polynomial.polynomial as n_poly
import numpy.polynomial.legendre as leg
```

## Mindflow

We want the best approximation (in Hilbert Spaces) of the function $f$, on the space $V = \mathrm{span}\{v_i\}$. Remember that $p\in V$ is best approximation of $f$ if and only if:

$$
(p-f,q)=0, \quad \forall q\in V.
$$

Focus one second on the fact that both $p$ and $q$ belong to $V$. We know that any $q$ can be expressed as a linear combination of the basis functions $v_i$:

$$
(p-f,v_i)=0, \quad \forall v_i\in V.
$$

Moreover $p$ is uniquely defined by the coefficients $p^j$ such that $p = p^j\,v_j$. Collecting this information together we get:

$$
(v_j,v_i) p^j = (f,v_i),\quad \forall v_i\in V.
$$

Now that we know our goal (finding these $p^j$ coefficients) we do what the rangers do: we explore!

We understand that we will need to invert the matrix:

$$
M_{ij} = (v_j,v_i) = \int v_i\cdot v_j
$$

What happens if we choose basis functions such that $(v_j,v_i) =  \delta_{ij}$?

How to construct numerical techniques to evaluate integrals in an efficient way?

Evaluate the $L^2$ projection.

## Orthogonal Polynomials

Grham Schmidt

$$
p_0(x) = 1, \qquad p_k(x) = x^k - \sum_{j=0}^{k-1} \frac{(x^k,p_j(x))}{(p_j(x),p_j(x))}
$$

or, alternatively

$$
p_0(x) = 1, \qquad p_k(x) = x\,p_{k-1}(x) - \sum_{j=0}^{k-1} \frac{(x p_{k-1}(x),p_j(x))}{(p_j(x),p_j(x))}
$$

```python
def scalar_prod(p0,p1,a=0,b=1):
    assert len(p0.free_symbols) <= 1, "I can only do this for single variable functions..."
    t = p0.free_symbols.pop() if len(p0.free_symbols) == 1 else sym.symbols('t')
    return sym.integrate(p0*p1,(t,a,b))
```

```python
t = sym.symbols('t')
k = 3

Pk = [1+0*t] # Force it to be a sympy expression

for k in range(1,5):
    s = 0
    for j in range(0,k):
        s+= scalar_prod(t**k,Pk[j])/scalar_prod(Pk[j],Pk[j])*Pk[j]
    pk = t**k-s
    # pk = pk/sym.sqrt(scalar_prod(pk,pk))
    pk = pk/pk.subs(t,1.)
    Pk.append(pk)

M = []
for i in range(len(Pk)):
    row = []
    for j in range(len(Pk)):
        row.append(scalar_prod(Pk[i],Pk[j]))
    M.append(row)

M = sym.Matrix(M)

print(M)

x = np.linspace(0,1,100)

for p in Pk:
    if p != 1 :
        fs = sym.lambdify(t, p, 'numpy')
        #print x.shape
        #print fs(x)
        _ = pl.plot(x,fs(x))
```

## Theorem

Le $q$ be nonzero polynomial of degree $n+1$ and $\omega(x)$ a positive weight function, s. t.:

$$
\int_a^b x^k q(x)\, \omega(x) = 0, \quad k = 0,\ldots, n
$$

If $x_i$ are zeros of $q(x)$, then:

$$
\int_a^b f(x)\, \omega(x)\approx \sum_{i=0}^nw_i\, f(x_i)
$$

with:

$$
w_i = \int_a^b l_i(x)\, \omega(x)
$$

is exact for all polynomials of degree at most $2n+1$. Here $l_i(x)$ are the usual Lagrange interpolation polynomials.

**Proof:** assume $f(x)$ is a polynomial of degree at most $2n+1$ and show:
$$
\int_a^b f(x)\, \omega(x) = \sum_{i=0}^nw_i\, f(x_i).
$$
Usign the polynomial division we have:
$$
\underbrace{f(x)}_{2n+1} = \underbrace{q(x)}_{n+1}\, \underbrace{p(x)}_{n} + \underbrace{r(x)}_{n}.
$$
By taking $x_i$ as zeros of $q(x)$ we have:
$$
f(x_i) = r(x_i)
$$
Now:
$$
\int_a^b f(x)\, \omega(x) = \int_a^b [q(x)\, p(x) + r(x)]\, \omega(x)
$$
$$
 = \underbrace{\int_a^b q(x)\, p(x) \, \omega(x)}_{=0} + \int_a^b r(x)\, \omega(x)
$$
Since $r(x)$ is a polynomial of order $n$ this is exact:

$$
\int_a^b f(x)\, \omega(x) = \int_a^b r(x)\, \omega(x) = \sum_{i=0}^nw_i\, r(x_i)
$$

But since we chose $x_i$ such that $f(x_i) = r(x_i)$, we have:

$$
\int_a^b f(x)\, \omega(x) = \int_a^b r(x)\, \omega(x) = \sum_{i=0}^nw_i\, f(x_i)
$$

This completes the proof.

## Legendre Polynomial

Two term recursion, to obtain the same orthogonal polynomials above (defined between [-1,1]), normalized to be one in $x=1$:

$$
(n+1) p^{n+1}(x) = (2n+1)\, x\, p^n(x) - n\, p^{n-1}(x)
$$

```python
Pn = [1.,t]

#Pn = [1.,x, ((2*n+1)*x*Pn[n] - n*Pn[n-1])/(n+1.) for n in range(1,2)]

for n in range(1,5):
    pn1 = ((2*n+1)*t*Pn[n] - n*Pn[n-1])/(n+1.)
    Pn.append(sym.simplify(pn1))

print(Pn)

#print(sym.poly(p))
#print(sym.real_roots(sym.poly(p)))
print(sym.integrate(Pn[4]*Pn[3],(t,-1,1)))

x = np.linspace(-1,1,100)
for p in Pn:
    if p != 1. :
        fs = sym.lambdify(t, p, 'numpy')
        #print x.shape
        #print fs(x)
        _ = pl.plot(x,fs(x))

```

In our proof we selected to evaluate $x_i$ at the zeros of the legendre polynomials, this is why we need to evaluate the zeros of the polynomials.

```python
print(sym.real_roots(sym.poly(Pn[2])))

#q = [-1.]+sym.real_roots(sym.poly(Pn[2]))+[1.]
q = sym.real_roots(sym.poly(Pn[3]))
print(q)

#for p in Pn:
#    if p != 1. :
#        #print(sym.poly(p))
#        #print(sym.real_roots(sym.poly(p)))
#        print(sym.nroots(sym.poly(p)))

```

    [-sqrt(3)/3, sqrt(3)/3]
    [-sqrt(15)/5, 0, sqrt(15)/5]

$$
w_i = \int_{-1}^{1} l_i(x)
$$

```python
Lg = [1. for i in range(len(q))]
print(Lg)

#for i in range(n+1):
for i in range(len(q)):
    for j in range(len(q)):
        if j != i:
            Lg[i] *= (t-q[j])/(q[i]-q[j])

print(Lg)

x = np.linspace(-1,1,100)

for l in Lg:
    fs = sym.lambdify(t, l, 'numpy')
    _ = pl.plot(x,fs(x))
```

```python
for l in Lg:
    print(sym.integrate(l,(t,-1,1)))
```

    0.555555555555555
    0.888888888888889
    0.555555555555555

### Hint
Proiezione usando polinomi LEGENDRE (f,v_i)

# Now let's get Numerical

From now on I work on the $[0,1]$ interval, becouse i like it this way :)

In the previus section we explored what sympbolically was happening, now we implement things on the computer. We saw how important are the legendre plynomials. Here a little documentation on that. I pont it out not because you need to read it all, but because I would like you get some aquitance with this criptic documentation pages [doc](https://docs.scipy.org/doc/numpy/reference/generated/numpy.polynomial.legendre.legroots.html#numpy.polynomial.legendre.legroots).

The problem we aim at solving is finding the coefficents $p_j$ such that:
$$
(v_j,v_i) p^j = (f,v_i),\quad \forall v_i\in V.
$$
Remind in this section the einstein notation holds.

We can expand the compact scalar product notation:
$$
p^j \int_0^1 v_i\, v_j = \int_0^1 f\, v_i,\quad \forall v_i\in V.
$$

We consider $V = \mathrm{span}\{l_i\}$. Our problem becomes:
$$
p^j \int_0^1 l_i\, l_j = \int_0^1 f\, l_i,\quad \mathrm{for}\ i = 0,\ldots,\mathtt{deg}
$$
Let's focus on mass matrix:
$$
\int_0^1 l_i(x)\, l_j(x) = \sum_k l_i(x_k)\, w_k\, l_j(x_k) =
$$

$$
=
\left(
\begin{array}{c c c c}
l_0(x_0) & l_0(x_1) & \ldots & l_0(x_q) \\
l_1(x_0) & l_1(x_1) & \ldots & l_1(x_q) \\
 & \ldots & \ldots  & \\
l_n(x_0) & l_n(x_1) & \ldots & l_n(x_q) \\
\end{array}
\right)
\left(
\begin{array}{c c c c}
w_0 &   0 & \ldots & 0 \\
  0 & w_1 & \ldots & 0 \\
   & \ldots & \ldots  & \\
  0 &   0 & \ldots & w_q \\
\end{array}
\right)
\left(
\begin{array}{c c c c}
l_0(x_0) & l_1(x_0) & \ldots & l_n(x_0) \\
l_0(x_1) & l_1(x_1) & \ldots & l_n(x_1) \\
 & \ldots & \ldots  & \\
l_0(x_q) & l_1(x_q) & \ldots & l_n(x_q) \\
\end{array}
\right)
= B\, W\, B^T
$$

A piece of curiosity, how the the two functions to find theros in two different ways

```python
print sym.nroots(sym.poly(Pn[-1]))
coeffs = np.zeros(6)
coeffs[-1] = 1.
print(leg.legroots(coeffs))
```

    [-0.906179845938664, -0.538469310105683, 0, 0.538469310105683, 0.906179845938664]
    [ -9.06179846e-01  -5.38469310e-01  -5.96500148e-17   5.38469310e-01
       9.06179846e-01]

```python
print gauss_points(3)
print(np.sqrt(3./5.)*.5)+.5
```

    [ 0.11270167  0.5         0.88729833]
    0.887298334621

```python
def define_lagrange_basis_set(q):
    n = q.shape[0]
    L = [n_poly.Polynomial.fromroots([xj for xj in q if xj != q[i]]) for i in range(n)]
    L = [L[i]/L[i](q[i]) for i in range(n)]
    return L
```

differenza fra le roots "simboliche" e non

```python
deg = 4
Nq = deg+1
p,w = leg.leggauss(Nq)
w = .5 * w
p = .5*(p+1)
#print p
#print w
W = np.diag(w)
#print W
```

```python
int_p = np.linspace(0,1,deg+1)
L = define_lagrange_basis_set(int_p)

print(len(L))

x = np.linspace(0,1,1025)
for f in L:
    _ = pl.plot(x, f(x))
_ = pl.plot(int_p, 0*int_p, 'ro')
```

```python
B = np.zeros((0,Nq))
for l in L:
    B = np.vstack([B,l(p)])
```

Recall:
$$
B\, W\, B^T p = B W f
$$

$$
B\, W\, B^T =
\left(
\begin{array}{c c c c}
l_0(x_0) & l_0(x_1) & \ldots & l_0(x_q) \\
l_1(x_0) & l_1(x_1) & \ldots & l_1(x_q) \\
 &  & \ddots  & \\
l_n(x_0) & l_n(x_1) & \ldots & l_n(x_q) \\
\end{array}
\right)
\left(
\begin{array}{c c c c}
w_0 &   0 & \ldots & 0 \\
  0 & w_1 & \ldots & 0 \\
   &  & \ddots  & \\
  0 &   0 & \ldots & w_q \\
\end{array}
\right)
\left(
\begin{array}{c c c c}
l_0(x_0) & l_1(x_0) & \ldots & l_n(x_0) \\
l_0(x_1) & l_1(x_1) & \ldots & l_n(x_1) \\
 & & \ddots  & \\
l_0(x_q) & l_1(x_q) & \ldots & l_n(x_q) \\
\end{array}
\right)
$$

```python
print(B.shape)
_ = pl.plot(B.T)
M = B.dot(W.dot(B.T))
print np.linalg.matrix_rank(M)
print np.linalg.cond(M)
```

```python
def step_function():
    def sf(x):
        index = where((x>.3) & (x<.7))
        step = zeros(x.shape)
        step[index] = 1
        return step
    return lambda x : sf(x)
```

$$
B\, W\, f =
\left(
\begin{array}{c c c c}
l_0(x_0) & l_0(x_1) & \ldots & l_0(x_q) \\
l_1(x_0) & l_1(x_1) & \ldots & l_1(x_q) \\
 & \ldots & \ldots  & \\
l_n(x_0) & l_n(x_1) & \ldots & l_n(x_q) \\
\end{array}
\right)
\left(
\begin{array}{c c c c}
w_0 &   0 & \ldots & 0 \\
  0 & w_1 & \ldots & 0 \\
   & \ldots & \ldots  & \\
  0 &   0 & \ldots & w_q \\
\end{array}
\right)
\left(
\begin{array}{c}
f(x_0) \\
f(x_1) \\
\vdots\\
f(x_q) \\
\end{array}
\right)
$$

```python
g = lambda x: np.sin(2*np.pi*x)
#g = step_function()
p = p.reshape((p.shape[0],1))

G = g(p)
print G.shape
print B.shape
print W.shape
G = B.dot(W.dot(G))
```

    (5, 1)
    (5, 5)
    (5, 5)

```python
u = np.linalg.solve(M, G)
print u
```

    [[ -1.92161045e-01]
     [  9.79052672e-01]
     [ -1.35712301e-15]
     [ -9.79052672e-01]
     [  1.92161045e-01]]

```python
def get_interpolating_function(LL,ui):
    def func(LL,ui,x):
        acc = 0
        for L,u in zip(LL,ui):
            #print(L,u)
            acc+=u*L(x)
        return acc
    return lambda x : func(LL,ui,x)
```

```python
I = get_interpolating_function(L,u)
sampling = np.linspace(0,1,101)
_= pl.plot(sampling, I(sampling))
#plot(xp, G,'ro')
```

## Diference in between projection and interpolation runge example

Proiezione usando polinomi LEGENDRE (f,v_i) con quadratura con 18 punti

Interpolazione usando polinomi LAGRANGE (sui punti di quadratura che sono i punti di gauss della funzione sopra)

```python

```


# Nonlinear Schrodinger equation (NLS)

* Physical space
\begin{align}
u_t = i \Delta_{\perp}u + 2i|u|^{2}u
\end{align}
* Spectral space $\hat{u} = \mathscr{F}\{u\}$
\begin{align}
\hat{u_t} = -i k_{\perp}^2 \hat{u} + 2i \mathscr{F} \{ |\mathscr{F}^{-1} \{ \hat{u} \}|^2 \mathscr{F}^{-1} \{ \hat{u} \} \}
\end{align}

# Imports

```python
import numpy as np
import matplotlib.pyplot as plt
from rkstiff.grids import HankelTransform, mirrorGrid
from rkstiff.etd35 import ETD35
from rkstiff.grids import construct_x_kx_fft
from matplotlib import animation
from matplotlib import cm
%matplotlib inline
```

# Helper functions

```python
def waterfall(x,t,u,**kwargs):
    if 'figsize' in kwargs:
        fig = plt.figure(figsize=kwargs['figsize'])
    else:
        fig = plt.figure(figsize=(6,6))
    ax = fig.add_subplot(1,1,1,projection='3d')
    ax.w_xaxis.set_pane_color((0,0,0,0))
    ax.w_yaxis.set_pane_color((0,0,0,0))
    ax.w_zaxis.set_pane_color((0,0,0,0))
    for i,snapshot in enumerate(u):
        ax.plot(x,t[i]*np.ones_like(x),snapshot,color='black')
    plt.xlim([x[0],x[-1]])
    plt.ylim([t[0],t[-1]])
    plt.tight_layout()
    return ax
```

# Radial grid setup with Hankel Transform

```python
nr = 100
rmax = 5
HT = HankelTransform(nr=nr,rmax=rmax)
```

# NLS linear and nonlinear operators

```python
L = -1j*HT.kr**2
gamma = 2
def NL(g):
    f = HT.iht(g)
    f2 = f.real**2 + f.imag**2
    return 1j*gamma*HT.ht(f2*f)
```

# Initial value profile

```python
# If A0 is large enough then the initial profile with collapse into singularity (otherwise it will spread out)
a = 1
A0 = 2
u0 = A0*np.exp(-a*HT.r**2)
u0HT = HT.ht(u0)
r,u0full = mirrorGrid(HT.r,u0)
kr,u0HTfull = mirrorGrid(HT.kr,u0HT)
fig,ax = plt.subplots(1,2,figsize=(12,4))
ax[0].plot(r,np.abs(u0full)**2)
ax[0].set_xlabel('r')
ax[0].set_ylabel('|u0|^2')
ax[1].plot(kr,np.log(np.abs(u0HTfull)**2))
ax[1].set_xlabel('kr')
ax[1].set_ylabel('log|u0HT|^2')
```

# Setup ETD35 solver and propagate

```python
solver = ETD35(linop=L,NLfunc=NL,epsilon=1e-5)
uHT = solver.evolve(u0HT,t0=0,tf=0.8,store_data=True,store_freq=10)

U = []
for u in solver.u:
    uRS = HT.iht(u)
    umag = uRS.real**2 + uRS.imag**2
    r,u = mirrorGrid(HT.r,umag) # extends r grid from [0,rmax] to [-rmax,rmax] by mirroring arrays
    U.append(u)
U = np.array(U)
t = np.array(solver.t)
ax = waterfall(r,t,U,figsize=(8,8))
ax.grid(False)
ax.axis(False)
ax.set_title('Magnitude')
```

# Radial and temporal grid setup for multi-variable NLS

```python
nr = 80
rmax = 4
HT.rmax = rmax
HT.nr = nr

nt = 512
a, b = -6, 6
t,omega = construct_x_kx_fft(nt,a,b)

# 2D matrices for real-space and spectral space components
R,T = np.meshgrid(HT.r,t,indexing='ij')
KR,OMEGA = np.meshgrid(HT.kr,omega,indexing='ij')
```

# Initial value profile

```python
R0 = 1
phaseR = 0
T0 = 0.5
phaseT = 0
A0 = 4
U0 = A0*np.exp(-(R/R0)**2- (T/T0)**2)*np.exp(1j*(phaseR*R + phaseT*T))
# Hankel transform taken over axis 0, FFT taken over axis 1
U0FFT_HT = np.fft.fft(HT.ht(U0),axis=1)

fig, ax = plt.subplots(1,2,figsize=(12,4))
rm, UM = mirrorGrid(HT.r,U0,axis=0)
RM,TM = np.meshgrid(rm,t,indexing='ij')
ax[0].contourf(RM,TM,UM.real**2 + UM.imag**2)
ax[0].set_xlabel('r')
ax[0].set_ylabel('t')
ax[0].set_title('initial function')

# complex fft omega grid is arranged with positive frequencies first in the array followed by negative frequencies
# put these in increasing order to plot results
omega_ordered = np.hstack([omega[nt//2:], omega[:nt//2]])
U0OT = np.hstack([U0FFT_HT[:,nt//2:],U0FFT_HT[:,:nt//2]])
krm, U0OT = mirrorGrid(HT.kr,U0OT,axis=0)

KRM,OMEGAM = np.meshgrid(krm,omega_ordered,indexing='ij')
mag = U0OT.real**2 + U0OT.imag**2
# plotting the log, so to avoid taking a log of 0 we need this
mag[mag < 1e-12] = 1e-12
ax[1].contourf(KRM,OMEGAM,np.log(mag))
ax[1].set_xlabel('kr')
ax[1].set_ylabel('omega')
ax[1].set_title('initial spectrum')

```

# NLS with spatial (r) and temporal terms (t)

```python
sigma = 0.5
gamma = 2

L = -1j*KR**2 + 1j*sigma*OMEGA**2
# Flatten L since modes are independent and we have a diagonal system
L = L.flatten()

def NL(g):
    # Expand the 1D array to a 2D array with radius varying across rows and time across columns
    G = g.reshape(nr,nt)
    # Now we can take the inverse Hankel transform and inverse FFT to recover a real-space form
    # appropriate for computing the nonlinear response
    F = np.fft.ifft(HT.iht(G),axis=1)
    F2 = F.real**2 + F.imag**2
    # Compute NL response, then flatten back to a 1D array for use with RKstiff PDE solver
    return 1j*gamma*np.fft.fft(HT.ht(F2*F),axis=1).flatten()
```

# Initialize solver and propagate from z=0 to z=zf

```python
z0, zf = 0, 0.3
store_freq = 4
solver = ETD35(linop=L,NLfunc=NL,epsilon=1e-5)
# flatten initial spectral array for solver
uHT = solver.evolve(U0FFT_HT.flatten(),t0=z0,tf=zf,store_data=True,store_freq=store_freq)
```

# Extract magnitudes for plotting

```python
# Helper function
def extractRTForPlotting(t,omega,HT,solverU,rlimit,tlimits,freq=1):
    SU = np.array(solverU[0:-1:freq])
    SU = SU.reshape(len(SU),len(HT.kr),len(omega))
    RU = np.zeros(shape = (len(SU),len(HT.r),len(t)),dtype=np.complex128)
    for i in range(len(SU)):
        RU[i] = np.fft.ifft(HT.iht(SU[i]),axis=1)
    rnew = HT.r[HT.r < rlimit]
    tnew = t[(t > tlimits[0]) & (t < tlimits[1])]
    RU = RU[:,HT.r < rlimit,:]
    RU = RU[:,:,(t > tlimits[0]) & (t < tlimits[1])]
    rm_new = np.hstack([-np.flipud(rnew),rnew])
    UM = np.zeros(shape=(RU.shape[0],2*RU.shape[1],RU.shape[2]),dtype=np.complex128)
    for i in range(len(RU)):
        UM[i] = np.vstack([np.flipud(RU[i]),RU[i]])
    RM, TM = np.meshgrid(rm_new,tnew,indexing='ij')
    return RM,TM,UM
```

```python
freq = 1
rlimit = 2
tlimits = [-3,3]
RM,TM,UM = extractRTForPlotting(t,omega,HT,solver.u,rlimit=rlimit,tlimits=tlimits,freq=freq)
MAG = UM.real**2 + UM.imag**2
MAG = MAG/MAG[0].max()
z = np.array(solver.t)
```

# Surface animation of propagation pulse splitting

```python
%matplotlib notebook
zmin, zmax = 0,20
interval = 200

fig = plt.figure(figsize=(10,6))
ax = fig.add_subplot(111,projection='3d')
ax.set_axis_off()
ax.set_facecolor('white')

ax.set_xlim(-rlimit,rlimit)
ax.set_ylim(tlimits[0],tlimits[1])
ax.set_zlim(zmin,zmax)

surface_opts = dict(
    cmap=cm.bone,edgecolors=None,antialiased=False,alpha=1
)

ax.plot_surface(RM,TM,MAG[0],**surface_opts)
ax.set_xlabel('r')
ax.set_ylabel('t')
fig.tight_layout()

def updateSurface(i):
    ax.collections = []
    ax.plot_surface(RM,TM,MAG[i],**surface_opts)
    ax.set_xlabel('r')
    ax.set_ylabel('t')
    #ax.set_title('z = {:.2f}'.format(z[i]))

anim = animation.FuncAnimation(fig, updateSurface,
                        frames=len(MAG),interval=interval,repeat=True,blit=False)
fig.show()
```

    <IPython.core.display.Javascript object>

# 2D Cartesian setup for multi-variable NLS

```python
%matplotlib inline
nx, ny = 128,128
ax,bx = -6,6
ay,by = -6,6
x,kx = construct_x_kx_fft(nx,ax,bx)
y,ky = construct_x_kx_fft(ny,ay,by)

# 2D matrices for real-space and spectral space components
X,Y = np.meshgrid(x,y,indexing='ij')
KX,KY= np.meshgrid(kx,ky,indexing='ij')

X0, Y0 = 1, 1
phaseX, phaseY = 0, 0
A0 = 2
U0 = A0*np.exp(-(X/X0)**2- (Y/Y0)**2)*np.exp(1j*(phaseX*X + phaseY*Y))
# Hankel transform taken over axis 0, FFT taken over axis 1
U0FFT = np.fft.fft2(U0)

fig, ax = plt.subplots(1,2,figsize=(12,4))
ax[0].contourf(X,Y,U0.real**2 + U0.imag**2)
ax[0].set_xlabel('x')
ax[0].set_ylabel('y')
ax[0].set_title('initial function')

# complex fft kx/ky grids are arranged with positive frequencies first in the array followed by negative frequencies
# put these in increasing order to plot results
kx_ordered = np.hstack([kx[nx//2:], kx[:nx//2]])
ky_ordered = np.hstack([ky[ny//2:], ky[:ny//2]])
nx_neg = np.hstack([U0FFT[nx//2:,ny//2:],U0FFT[nx//2:,:ny//2]])
nx_pos = np.hstack([U0FFT[:nx//2,ny//2:],U0FFT[:nx//2,:ny//2]])
U0OT = np.vstack([nx_neg,nx_pos])
KXO,KYO = np.meshgrid(kx_ordered,ky_ordered,indexing='ij')

mag = U0OT.real**2 + U0OT.imag**2
# plotting the log, so to avoid taking a log of 0 we need this
mag[mag < 1e-12] = 1e-12
ax[1].contourf(KXO,KYO,np.log(mag))
ax[1].set_xlabel('kx')
ax[1].set_ylabel('ky')
ax[1].set_title('initial spectrum')

```

# Operators and propagation of 2D-NLS

```python
gamma = 2
L = -1j*(KX**2 + KY**2)
# Flatten L since modes are independent and we have a diagonal system
L = L.flatten()

def NL(g):
    # Expand the 1D array to a 2D array with radius varying across rows and time across columns
    G = g.reshape(nx,ny)
    # Now we can take the inverse Hankel transform and inverse FFT to recover a real-space form
    # appropriate for computing the nonlinear response
    F = np.fft.ifft2(G)
    F2 = F.real**2 + F.imag**2
    # Compute NL response, then flatten back to a 1D array for use with RKstiff PDE solver
    return 1j*gamma*np.fft.fft2(F2*F).flatten()

solver = ETD35(linop=L,NLfunc=NL,epsilon=1e-4)
uHT = solver.evolve(U0FFT.flatten(),t0=0,tf=0.8,store_data=True,store_freq=4)
```

# Process data for animation

```python
# Helper function
def extractXYForPlotting(x,y,kx,ky,solverU,xlimits,ylimits,freq=1):
    SU = np.array(solverU[0:-1:freq])
    SU = SU.reshape(len(SU),len(kx),len(ky))
    UE = np.zeros(shape = (len(SU),len(x),len(y)),dtype=np.complex128)
    for i in range(len(SU)):
        UE[i] = np.fft.ifft2(SU[i])
    x_idx = (x > xlimits[0]) & (x < xlimits[1])
    y_idx = (y > ylimits[0]) & (y < ylimits[1])
    xnew = x[x_idx]
    ynew = y[y_idx]
    UE = UE[:,x_idx,:]
    UE = UE[:,:,y_idx]
    XE,YE = np.meshgrid(xnew,ynew,indexing='ij')
    return XE,YE,UE

freq = 1
xlimits = [-2,2]
ylimits = [-2,2]
XE,YE,UE = extractXYForPlotting(x,y,kx,ky,solver.u,xlimits=xlimits,ylimits=ylimits,freq=freq)
MAGXY = UE.real**2 + UE.imag**2
MAGXY = MAGXY/MAGXY[0].max()
t = np.array(solver.t)
```

# Blow-up for 2D-NLS in Cartesian coordinates

```python
%matplotlib notebook
zmin, zmax = 0,30
interval = 200

fig = plt.figure(figsize=(10,6))
ax = fig.add_subplot(111,projection='3d')
ax.set_axis_off()
ax.set_facecolor('gray')

ax.set_xlim(xlimits[0],xlimits[1])
ax.set_ylim(ylimits[0],ylimits[1])
ax.set_zlim(zmin,zmax)

surface_opts = dict(
    cmap=cm.hot,edgecolors=None,antialiased=False,alpha=1
)

ax.plot_surface(XE,YE,MAGXY[0],**surface_opts)
ax.set_xlabel('x')
ax.set_ylabel('y')

def updateSurface(i):
    ax.collections = []
    ax.plot_surface(XE,YE,MAGXY[i],**surface_opts)
    ax.set_xlabel('x')
    ax.set_ylabel('y')
    ax.set_title('t = {:.2f}'.format(t[i]))

anim = animation.FuncAnimation(fig, updateSurface,
                        frames=len(MAGXY),interval=interval,repeat=True,blit=False)
fig.show()
```

    <IPython.core.display.Javascript object>

    /opt/anaconda3/envs/rkstiff-env/lib/python3.9/site-packages/matplotlib/animation.py:973: UserWarning: Animation was deleted without rendering anything. This is most likely unintended. To prevent deletion, assign the Animation to a variable that exists for as long as you need the Animation.
      warnings.warn(

```python

```