# Bayesian Statistics

## Chapter Zero Review

Omission

## Chapter One Introduction

### Conditional Probability

As we have mentioned in `Using Python in Advanced Statistics`, **probability** is a number within in 0 to 1(included 0 and 1),and the meaning of probability is that the credibility(可信度) of a event or a prediction. According to this idea, **COnditional Probability** is the probability under some kinds of conditions. For example, if there is a rain tomorrow in Zhuhai. I check the data, found that in tomorrow's date, there are about 30% that will rain. **This is the probability**. If I want to know the possibility of rain tomorrow if it is already raining today. **This is the conditional probability**, which is a probability of an event happen based on a happened fact.

The usual notation for conditional probability is $p(A\mid B)$, which means under B event's condition, the probability of A.

*PS: if the probability of an event is 0, we says it is **false**; in the opposite, if the probability is 1, it is **true**.*

### Conjoint Probability

After known what is **conditional probability**, we can easily know what is **conjoint probability**, which is another way to say the probability that two things are true. This time, the notation is: $p(A B)$ or $P(A\ and \ B)$.

```python
import matplotlib.pyplot as plt
import numpy as np
import sympy
from matplotlib_venn import venn2, venn3_circles

plt.figure(figsize=(4,4))
v = venn2(subsets=(2,2,1), set_labels = ('A', 'B'))

plt.title("Sample Venn diagram - plot one")
plt.annotate('P(AB)', xy=v.get_label_by_id('11').get_position() - np.array([0, 0.05]), xytext=(-70,-70),
             ha='center', textcoords='offset points', bbox=dict(boxstyle='round,pad=0.5', fc='gray', alpha=0.1),
             arrowprops=dict(arrowstyle='->', connectionstyle='arc3,rad=0.5',color='gray'))
plt.show()
```

The conjointed part can represent the conjoint probability that $A$ and $B$ are both true. For instance, if $A$ and $B$ are independent, we know that the $P(A\ and \ B) = P(A)P(B) \Leftrightarrow P(A \mid B) = p(A)$, for whether A is true or not will not affect B; but if $A$ and $B$ are not independent, just like the weather, if we know that it raind today, it is more likely that it will rain tomorrow, so $p(A|B) > p(A)$.

In general, the probability of a conjunction is : $\mathbf  p(A\ and \ B) = p(A)p(B \mid A)$

### Bayes's theorem

Known what is conditional probability and cojoint probability, we can move on to the Headlight: **Bayes's theorem**. First, we should make sure we are in the same channel:
   1. conjoint probability: $p(A\ and \ B) = p(B\ and \ A)$
   2. probability of a conjunction: $p(A\ and \ B) = p(A)p(B \mid A)$
   3. interchange $A$ and $B$: $p(B\ and \ A) = p(B)p(A \mid B)$

In the light of point 2 and point 3, we can combine the two equations to one euqation: $\Rightarrow p(A) \ p(B \mid A) = p(B) \ p(A \mid B)$, divided by $p(B)$ in the meantime and we can get the **Bayes's theorem !** : $\Rightarrow \frac{p(A) \ p(B \mid A)}{p(B)} =  p(A \mid B)$

We can have a example, Allen B.Downey called it **The Cookie Problem**: Suppose there are two bowls of cookies. Bowl-1 contains $30$ vanilla cookies and $10$ chocolate cookies. Bowl-2 contains $20$ of each.
Now suppose you choose one of the bowls at random and, without looking, select a cookie at random. The cookie is vanilla. What is the probability that it came from Bowl-1?
This is a conditional probability; we want $p(Bowl-1|vanilla)$, but it is not obvious how to compute it. If I asked a different question — the probability of a vanilla cookie given Bowl-1 — it would be easy:

$$p(vanilla\mid Bowl-1) = \frac{3}{4}$$

Sadly, $p(A|B)$ is not the same as $p(B|A)$, but there is a way to get from one
to the other: Bayes’s theorem.

Using **Bayes's Theorem**, we can easily solve the cookie problem:

First, assume that $B_1$ represent the probility that cookie from Bowl-1 and $p(V)$ is the probability of taking out a Vanilla.

$$P(B_1\mid V) = \frac{p(B_1)p(V\mid B_1)}{p(V)}$$

There is another way to think of Bayes's theorem: it gives us a way to update the probability of a hypothesis, $H$, in light of some body of data,$D$.

This way of thinking about Bayes's theorem is called the **diachronic interpretation**. "Diachronic" means that something is happening over time; in this case the probability of the hypotheses changes, over time, as we see new data.

Rewriting Bayes's theorem with $H$ and $D$ yields:

$$P(H|D) = \frac{p(H)p(D|H)}{p(D)}$$

In this interpretation, each term has a name:

* $p(H)$ is the probability of the hypothesis before we see the data, called the prior probability, or just **prior**;
* $p(D)$ is the probability of the data under any hypothesis, called the normalizing constant(total probability);
* $P(H|D)$ is waht we want to compute, the probability of the hypothesis after we see the data, called the **posterior**;
* $P(D|H)$ is the probability of the data under the hypothesis, called the likelihood

In the classical Statistics theory, the data is the random variable. The the Cookie Problem for example, if we want to know the probability of taking out vanilla cookies from Bowl-1, the random variable is $1,2,3,\dots$, the data or the number. However, if we want to know the probability of which Bowls does the vanilla cookie come from, as we got a vanilla cookies already, this time, the random varaible is the probability, not the data. This is the biggest difference between Bayesian and Frequentist.

## Chapter Two Computational Statistics

In statistics, a **distribution** is a set of values and thier correponding probabilites.

You can get more information about Probabilities and Statistics here

Here is a example of rolling a dice, as we all know that the probability of each side is $\frac{1}{6}$, then, this is a Discrete Uniform Distribution.

```python
import numpy as np
from scipy import stats
import matplotlib.pyplot as plt
import matplotlib

fig, ax = plt.subplots(1, 1)
a = [1,2,3,4,5,6]
sample = np.random.randint(len(a), size = 10000)

dict = {}
for key in sample:
    dict[key] = dict.get(key, 0) + 1
len(dict)

x = [dict[i] for i in dict] # change dict to list
pmf = [dict[i]/len(sample) for i in dict] #calculate pmf of each element in list

ax.plot(a,pmf,'bo')
ax.vlines(a, 0, pmf, colors='b', lw=5, alpha=0.5)

line1 = [(0, 0.1666667), (10, 0.1666667)]
(line1_xs, line1_ys) = zip(*line1)
ax.add_line(matplotlib.lines.Line2D(line1_xs, line1_ys, linewidth=1, color='red') )
plt.show()
```

```python

```


# INF-482, v0.01, Claudio Torres, ctorres@inf.utfsm.cl. DI-UTFSM
### Textbook: Gregory E. Fasshauer, Meshfree Approximaition Methods with MatLab, Interdisciplinary Mathematical Sciences - Vol. 6, World Scientific Publishers, Singapore, 2007. Link: http://www.math.iit.edu/~fass/

# Mairhuber-Curtis Theorem, Halton, Distance Matrix and RBF Interpolation

```python
import numpy as np
import ghalton
import matplotlib.pyplot as plt
%matplotlib inline
from ipywidgets import interact
from scipy.spatial import distance_matrix
from mpl_toolkits.mplot3d import Axes3D
from matplotlib import cm
from matplotlib.ticker import LinearLocator, FormatStrFormatter
from ipywidgets import IntSlider
import sympy as sym
import matplotlib as mpl
mpl.rcParams['font.size'] = 14
mpl.rcParams['axes.labelsize'] = 20
mpl.rcParams['xtick.labelsize'] = 14
mpl.rcParams['ytick.labelsize'] = 14
sym.init_printing()
M=8
```

```python
def plot_matrices_with_values(ax,M):
    N=M.shape[0]
    cmap = plt.get_cmap('GnBu')
    ax.matshow(M, cmap=cmap)
    for i in np.arange(0, N):
        for j in np.arange(0, N):
            ax.text(i, j, '{:.2f}'.format(M[i,j]), va='center', ha='center', color='r')
```

## Mairhuber-Curtis Theorem

```python
# Initializing a R^2
sequencer = ghalton.Halton(2)
sequencer.reset()
xH=np.array(sequencer.get(9))
print(xH)
```

    [[ 0.5         0.33333333]
     [ 0.25        0.66666667]
     [ 0.75        0.11111111]
     [ 0.125       0.44444444]
     [ 0.625       0.77777778]
     [ 0.375       0.22222222]
     [ 0.875       0.55555556]
     [ 0.0625      0.88888889]
     [ 0.5625      0.03703704]]

```python
def show_MC_theorem(s_local=0):
    i=3
    j=4
    NC=40

    sequencer.reset()
    xH=np.array(sequencer.get(9))

    phi1= lambda s: (s-0.5)*(s-1)/((0-0.5)*(0-1))
    phi2= lambda s: (s-0)*(s-1)/((0.5-0)*(0.5-1))
    phi3= lambda s: (s-0)*(s-0.5)/((1-0)*(1-0.5))
    C1=lambda s: xH[i,:]*phi1(s)+np.array([0.45,0.55])*phi2(s)+xH[j,:]*phi3(s)
    C2=lambda s: xH[j,:]*phi1(s)+np.array([0.15,0.80])*phi2(s)+xH[i,:]*phi3(s)
    C1v=np.vectorize(C1,otypes=[np.ndarray])
    C2v=np.vectorize(C2,otypes=[np.ndarray])
    ss=np.linspace(0,1,NC).reshape((-1, 1))
    C1o=np.array(C1v(ss))
    C2o=np.array(C2v(ss))
    C1plot=np.zeros((NC,2))
    C2plot=np.zeros((NC,2))
    for k in np.arange(0,NC):
        C1plot[k,0]=C1o[k][0][0]
        C1plot[k,1]=C1o[k][0][1]
        C2plot[k,0]=C2o[k][0][0]
        C2plot[k,1]=C2o[k][0][1]

    plt.figure(figsize=(2*M,M))
    plt.subplot(121)
    plt.plot(C1plot[:,0],C1plot[:,1],'r--')
    plt.plot(C2plot[:,0],C2plot[:,1],'g--')
    plt.scatter(xH[:,0], xH[:,1], s=300, c="b", alpha=1.0, marker='.',
                label="Halton")
    plt.scatter(C1(s_local)[0], C1(s_local)[1], s=300, c="r", alpha=1.0, marker='d')
    plt.scatter(C2(s_local)[0], C2(s_local)[1], s=300, c="g", alpha=1.0, marker='d')
    plt.axis([0,1,0,1])
    plt.title(r'Quasi-random points (Halton)')
    plt.grid(True)

    xHm=np.copy(xH)
    xHm[i,:]=C1(s_local)
    xHm[j,:]=C2(s_local)
    R=distance_matrix(xHm, xH)
    det_s_local=np.linalg.det(R)

    plt.subplot(122)
    plt.title(r'det(R_fixed)='+str(det_s_local))
    det_s=np.zeros_like(ss)
    for k, s in enumerate(ss):
        xHm[i,:]=C1plot[k,:]
        xHm[j,:]=C2plot[k,:]
        R=distance_matrix(xHm, xH)
        det_s[k]=np.linalg.det(R)

    plt.plot(ss,det_s,'-')
    plt.plot(s_local,det_s_local,'dk',markersize=16)
    plt.grid(True)

    plt.show()
```

```python
interact(show_MC_theorem,s_local=(0,1,0.1))
```

## Halton points vs pseudo-random points in 2D

```python
def plot_random_vs_Halton(n=100):
    # Number of points to be generated
    # n=1000
    # I am reseting the sequence everytime I generated just to get the same points
    sequencer.reset()
    xH=np.array(sequencer.get(n))
    np.random.seed(0)
    xR=np.random.rand(n,2)

    plt.figure(figsize=(2*M,M))

    plt.subplot(121)
    plt.scatter(xR[:,0], xR[:,1], s=100, c="r", alpha=1.0, marker='.',
                label="Random", edgecolors='None')
    plt.axis([0,1,0,1])
    plt.title(r'Pseudo-random points')
    plt.grid(True)

    plt.subplot(122)
    plt.scatter(xH[:,0], xH[:,1], s=100, c="b", alpha=1.0, marker='.',
                label="Halton")
    plt.axis([0,1,0,1])
    plt.title(r'Quasi-random points (Halton)')
    plt.grid(True)

    plt.show()
```

```python
interact(plot_random_vs_Halton,n=(20,500,20))
```

## Interpolation with Distance Matrix from Halton points

```python
def show_R(mH=10):
    fig= plt.figure(figsize=(2*M*mH/12,M*mH/12))
    ax = plt.gca()
    sequencer.reset()
    X=np.array(sequencer.get(mH))
    R=distance_matrix(X, X)
    plot_matrices_with_values(ax,R)
```

```python
interact(show_R,mH=(2,20,1))
```

### Defining a test function

```python
# The function to be interpolated
f=lambda x,y: 16*x*(1-x)*y*(1-y)
```

```python
def showing_f(n=10, elev=40, azim=230):
    fig = plt.figure(figsize=(2*M,M))

    # Creating regular mesh
    Xr = np.linspace(0, 1, n)
    Xm, Ym = np.meshgrid(Xr,Xr)
    Z = f(Xm,Ym)

    # Wireframe
    plt.subplot(221,projection='3d')
    ax = fig.gca()
    ax.plot_wireframe(Xm, Ym, Z)
    ax.view_init(elev,azim)

    # imshow
    plt.subplot(222)
    #plt.imshow(Z,interpolation='none', extent=[0, 1, 0, 1])
    plt.contourf(Xm, Ym, Z, 20)
    plt.ylabel('$y$')
    plt.xlabel('$x$')
    plt.axis('equal')
    plt.xlim(0,1)
    plt.colorbar()

    # Contour plot
    plt.subplot(223)
    plt.contour(Xm, Ym, Z, 20)
    plt.axis('equal')
    plt.colorbar()

    # Surface
    plt.subplot(224,projection='3d')
    ax = fig.gca()
    surf = ax.plot_surface(Xm, Ym, Z, rstride=1, cstride=1, cmap=cm.coolwarm,
                           linewidth=0, antialiased=False)
    fig.colorbar(surf)
    ax.view_init(elev,azim)

    plt.show()

```

### Let's look at $f$

```python
elev_widget = IntSlider(min=0, max=180, step=10, value=40)
azim_widget = IntSlider(min=0, max=360, step=10, value=230)

interact(showing_f,n=(5,50,5),elev=elev_widget,azim=azim_widget)
```

```python
def eval_interp_distance_matrix(C,X,x,y):
    R=distance_matrix(X, np.array([[x,y]]))
    return np.dot(C,R)

def showing_f_interpolated(n=10, mH=10, elev=40, azim=230):
    fig = plt.figure(figsize=(2*M,M))

    ## Building distance matrix and solving linear system
    sequencer.reset()
    X=np.array(sequencer.get(mH))
    R=distance_matrix(X, X)
    Zs=f(X[:,0],X[:,1])
    C=np.linalg.solve(R,Zs)
    # f interpolated with distance function
    fIR=np.vectorize(eval_interp_distance_matrix, excluded=[0,1])

    # Creating regular mesh
    Xr = np.linspace(0, 1, n)
    Xm, Ym = np.meshgrid(Xr,Xr)
    Z = f(Xm,Ym)

    # Contour plot - Original Data
    plt.subplot(221)
    plt.contour(Xm, Ym, Z, 20)
    plt.colorbar()
    plt.axis('equal')
    plt.title(r'$f(x,y)$')

    # Surface - Original Data
    plt.subplot(222,projection='3d')
    ax = fig.gca()
    surf = ax.plot_surface(Xm, Ym, Z, rstride=1, cstride=1, cmap=cm.coolwarm,
                           linewidth=0, antialiased=False)
    fig.colorbar(surf)
    ax.view_init(elev,azim)
    plt.title(r'$f(x,y)$')

    # Contour plot - Interpolated Data
    plt.subplot(223)
    plt.contour(Xm, Ym, fIR(C,X,Xm,Ym), 20)
    plt.axis('equal')
    plt.colorbar()
    plt.scatter(X[:,0], X[:,1], s=100, c="r", alpha=0.5, marker='.',
                label="Random", edgecolors='None')
    plt.title(r'$fIR(x,y)$')

    # Surface - Interpolated Data
    plt.subplot(224,projection='3d')
    ax = fig.gca()
    surf = ax.plot_surface(Xm, Ym,  fIR(C,X,Xm,Ym), rstride=1, cstride=1, cmap=cm.coolwarm,
                           linewidth=0, antialiased=False)
    fig.colorbar(surf)
    ax.view_init(elev,azim)
    ax.set_zlim(0,1)
    plt.title(r'$fIR(x,y)$')

    plt.show()
```

### The interpolation with distance matrix itself

```python
interact(showing_f_interpolated,n=(5,50,5),mH=(5,80,5),elev=elev_widget,azim=azim_widget)
```

## RBF interpolation

```python
# Some RBF's
linear_rbf   = lambda r,eps: r
gaussian_rbf = lambda r,eps: np.exp(-(eps*r)**2)
MQ_rbf       = lambda r,eps: np.sqrt(1+(eps*r)**2)
IMQ_rbf      = lambda r,eps: 1./np.sqrt(1+(eps*r)**2)
# The chosen one! But please try all of them!
rbf          = lambda r,eps: MQ_rbf(r,eps)
```

```python
def eval_interp_rbf(C,X,x,y,eps):
    A=rbf(distance_matrix(X, np.array([[x,y]])),eps)
    return np.dot(C,A)

def showing_f_interpolated_rbf(n=10, mH=10, elev=40, azim=230, eps=1):
    fig = plt.figure(figsize=(2*M,M))

    # Creating regular mesh
    Xr = np.linspace(0, 1, n)
    Xm, Ym = np.meshgrid(Xr,Xr)
    Z = f(Xm,Ym)

    ########################################################
    ## Pseudo-random
    ## Building distance matrix and solving linear system
    np.random.seed(0)
    X=np.random.rand(mH,2)
    R=distance_matrix(X,X)
    A=rbf(R,eps)
    Zs=f(X[:,0],X[:,1])
    C=np.linalg.solve(A,Zs)
    # f interpolated with distance function
    fIR=np.vectorize(eval_interp_rbf, excluded=[0,1,4])

    # Contour plot - Original Data
    plt.subplot(231)
    plt.contour(Xm, Ym, fIR(C,X,Xm,Ym,eps), 20)
    plt.colorbar()
    plt.scatter(X[:,0], X[:,1], s=100, c="r", alpha=0.5, marker='.',
                label="Random", edgecolors='None')
    plt.title(r'$f(x,y)_{rbf}$ with Pseudo-random points')

    # Surface - Original Data
    plt.subplot(232,projection='3d')
    ax = fig.gca()
    surf = ax.plot_surface(Xm, Ym, fIR(C,X,Xm,Ym,eps), rstride=1, cstride=1, cmap=cm.coolwarm,
                           linewidth=0, antialiased=False)
    fig.colorbar(surf)
    ax.view_init(elev,azim)
    ax.set_zlim(0,1)
    plt.title(r'$f(x,y)_{rbf}$ with Pseudo-random points')

    # Contour plot - Original Data
    plt.subplot(233)
    plt.contourf(Xm, Ym, np.abs(f(Xm,Ym)-fIR(C,X,Xm,Ym,eps)), 20)
    #plt.imshow(np.abs(f(Xm,Ym)-fIR(C,X,Xm,Ym,eps)),interpolation='none', extent=[0, 1, 0, 1])
    plt.axis('equal')
    plt.xlim(0,1)
    plt.colorbar()
    plt.scatter(X[:,0], X[:,1], s=100, c="k", alpha=0.8, marker='.',
                label="Random", edgecolors='None')
    plt.title(r'Error with Pseudo-random points')

    ########################################################

    ## HALTON (Quasi-random)
    ## Building distance matrix and solving linear system
    sequencer.reset()
    X=np.array(sequencer.get(mH))
    R=distance_matrix(X,X)
    A=rbf(R,eps)
    Zs=f(X[:,0],X[:,1])
    C=np.linalg.solve(A,Zs)
    # f interpolated with distance function
    fIR=np.vectorize(eval_interp_rbf, excluded=[0,1,4])

    # Contour plot - Interpolated Data
    plt.subplot(234)
    plt.contour(Xm, Ym, fIR(C,X,Xm,Ym,eps), 20)
    plt.colorbar()
    plt.scatter(X[:,0], X[:,1], s=100, c="r", alpha=0.5, marker='.',
                label="Random", edgecolors='None')
    plt.title(r'$f_{rbf}(x,y)$ with Halton points')

    # Surface - Interpolated Data
    plt.subplot(235,projection='3d')
    ax = fig.gca()
    surf = ax.plot_surface(Xm, Ym,  fIR(C,X,Xm,Ym,eps), rstride=1, cstride=1, cmap=cm.coolwarm,
                           linewidth=0, antialiased=False)
    fig.colorbar(surf)
    ax.view_init(elev,azim)
    ax.set_zlim(0,1)
    plt.title(r'$f_{rbf}(x,y)$ with Halton points')

    # Contour plot - Original Data
    plt.subplot(236)
    plt.contourf(Xm, Ym, np.abs(f(Xm,Ym)-fIR(C,X,Xm,Ym,eps)), 20)
    #plt.imshow(np.abs(f(Xm,Ym)-fIR(C,X,Xm,Ym,eps)),interpolation='none', extent=[0, 1, 0, 1])
    plt.axis('equal')
    plt.xlim(0,1)
    plt.colorbar()
    plt.scatter(X[:,0], X[:,1], s=100, c="k", alpha=0.8, marker='.',
                label="Random", edgecolors='None')
    plt.title(r'Error with Halton points')

    plt.show()
```

```python
interact(showing_f_interpolated_rbf,n=(5,50,5),mH=(5,80,5),elev=elev_widget,azim=azim_widget,eps=(0.1,50,0.1))
```

```python

```


```python
import sympy as sp
from sympy.physics.mechanics import dynamicsymbols, mechanics_printing
mechanics_printing()
import modern_robotics as mr
import numpy as np
from Utilities.symbolicFunctions import exp6, Ad
from Utilities.kukaKinematics import Blist
```

### Functions & variables from former tasks

#### Jacobi and decoupling

```python
th1, th2, th3, th4, th5, th6 = dynamicsymbols('theta_1, theta_2, theta_3, theta_4, theta_5, theta_6')

def Jb_maker6x6(Blist): #Foreløpig for 6x6
    Jb = sp.zeros(6,6)
    Jb[:,5] =  Blist[:,5]
    Jb[:,4] =  Ad(exp6(-Blist[:,5], th6)) * Blist[:,4]
    Jb[:,3] =  Ad(exp6(-Blist[:,5], th6) * exp6(-Blist[:,4], th5)) * Blist[:,3]
    Jb[:,2] =  Ad(exp6(-Blist[:,5], th6) * exp6(-Blist[:,4], th5) * exp6(-Blist[:,3], th4)) * Blist[:,2]
    Jb[:,1] =  Ad(exp6(-Blist[:,5], th6) * exp6(-Blist[:,4], th5) * exp6(-Blist[:,3], th4) * exp6(-Blist[:,2], th3)) * Blist[:,1]
    Jb[:,0] =  Ad(exp6(-Blist[:,5], th6) * exp6(-Blist[:,4], th5) * exp6(-Blist[:,3], th4) * exp6(-Blist[:,2], th3) * exp6(-Blist[:,1], th2)) * Blist[:,0]

    return Jb

Jb = Jb_maker6x6(Blist)
Jb.simplify()
Jb
```

```python
#Jb = [[Jb11, Jb12],
#      [Jb21, Jb22]]
JbArm = sp.zeros(3,3)
JbWrist = sp.zeros(3,3)

JbWrist = Jb[0:3,3:6]  # Jb12 (rotation)
JbArm = Jb[3:6,0:3]    # Jb21 (position)

JbArm.simplify()
JbWrist.simplify()

JbWrist, JbArm
```

### Wrist:

```python
JbWrist
```

```python
DetJbWrist = JbWrist.det()
DetJbWrist

```

```python
sp.solve(DetJbWrist, th4, th5)
```

### Arm:

```python
detArm = JbArm.det()
simplJbArm = JbArm.subs({th1:0, th4:0, th5:0, th6:0}) # Simplifying the expression to shorten calculation time
simplJbArm
```

```python
detSimplJbArm = simplJbArm.det()
detSimplJbArm.simplify()

```

```python
sp.solve(detSimplJbArm, th2, th3, dict = True)

```

```python
t = -2*np.arctan(12-np.sqrt(145))
# t = np.rad2deg(t)
t
```

```python
t = -2*np.arctan(12+np.sqrt(145))
t = np.rad2deg(t)
t
```

```python
360+t
```

```python

```


# Importing Packages

```python
import numpy as np
import matplotlib.pyplot as plt
import sympy as sp
from numpy.linalg import inv

import time

from matplotlib.animation import FuncAnimation
from IPython import display
from matplotlib import animation
from IPython.display import HTML
```

```python
import warnings # Serve para ignorar mensagens de alerta que aparecem
warnings.simplefilter(action='ignore', category=FutureWarning)
```

```python
plt.rcParams['font.size'] = 14
```

# Objective

- Resolve the Kuramoto-Sivashinsky equation with a compact finite element scheme

#  Kuramoto-Sivashinky Equation (KSE)

Formula:

\begin{equation}
    \frac{\partial u}{\partial t} =  -\beta\frac{\partial^4 u}{\partial u^4}- \alpha\frac{\partial^2 u}{\partial x^2} - u\frac{\partial u}{\partial x}
\end{equation}

Where $\beta$ and $ \alpha, $ are constants

## Creating the domain

Based on reference [2], we consider a spatial domain $\Omega_x = [a,b]$ and a time domain $\Omega_T = [0,T]$

Defining the constant values:

$b = 200$

$a = 0$

$\beta = 1$

$\alpha = 1$

Uniform grids must be formed by a set of nodes $x$ and $t$. Following reference [1], we define the spatial spacing $\Delta_x$ from the length of the domain and the number of nodes $N$:

$\Delta_x = \frac{b-a}{N-1}$

The time step $\Delta_t$ is defined by the user.

```python
N = 256 # Number of nodes
a = 0
b = 200

dx = (b-a)/(N-1)
dt = 0.05
T = 2000

# Creating the domains

x = np.linspace(a,b,N)
t = np.arange(0,T+dt,dt)
```

## Inicial condition

The initial periodic condition of reference [2] is defined as:

\begin{equation}
    u_0 = cos \left(\frac{\pi x}{20}\right) \left(1+sin\frac{\pi x}{20}\right)
\end{equation}

```python
u0 = np.cos(np.pi*x/20)*(1+np.sin(np.pi*x/20))

plt.figure(figsize=(12, 4))
plt.plot(x,u0)
plt.grid()
plt.xlabel('x')
plt.ylabel('Initial condition')
```

```python
u_num = np.zeros((len(t),len(x))) # Matrix where I will save the solution at each time step as a row.

# Applying initial condition
u_num[0,:] = u0
u_num
```

    array([[1.        , 1.11437737, 1.20634371, ..., 0.73325016, 0.87046365,
            1.        ],
           [0.        , 0.        , 0.        , ..., 0.        , 0.        ,
            0.        ],
           [0.        , 0.        , 0.        , ..., 0.        , 0.        ,
            0.        ],
           ...,
           [0.        , 0.        , 0.        , ..., 0.        , 0.        ,
            0.        ],
           [0.        , 0.        , 0.        , ..., 0.        , 0.        ,
            0.        ],
           [0.        , 0.        , 0.        , ..., 0.        , 0.        ,
            0.        ]])

# Compact Finite Difference:

Using [3] as the main reference, we see that he didn't consider periodic boundary conditions, but [2] did. Comparing the difference between there matrixes, we can see that all of them are tridiagonal, however, the terms close to the boundary are different.

Therefore, we can use the same scheme as in [3], but applying the periodic boundary conditions the same way as in [2].

## First derivative

```python
# Creating the matrix of numbers
A = np.zeros((N,N))
i,j = np.indices(A.shape,dtype=int)

A[i==j-1] = 1
A[i==j] = 3
A[i==j+1] = 1

# Correcting boundary conditions for periodic case
A[0,-1] = A[0,1]
A[-1,0] = A[-1,-2]

print(A)
A2 = inv(A)
```

    [[3. 1. 0. ... 0. 0. 1.]
     [1. 3. 1. ... 0. 0. 0.]
     [0. 1. 3. ... 0. 0. 0.]
     ...
     [0. 0. 0. ... 3. 1. 0.]
     [0. 0. 0. ... 1. 3. 1.]
     [1. 0. 0. ... 0. 1. 3.]]

```python
#A[0,:]
```

```python
matrixpsi = np.zeros((N,N))
i,j = np.indices(matrixpsi.shape,dtype=int)

matrixpsi[i==j-2] = 1/(12*dx)
matrixpsi[i==j-1] = 28/(12*dx)
matrixpsi[i==j+1] = -28/(12*dx)
matrixpsi[i==j+2] = -1/(12*dx)

# Correcting boundary conditions for periodic case
matrixpsi[0,-1] = -matrixpsi[0,1]
matrixpsi[0,-2] = -matrixpsi[0,2]
matrixpsi[1,-1] = -matrixpsi[0,2]
matrixpsi[1,0] = -matrixpsi[0,1]

matrixpsi[-1,0] = -matrixpsi[-1,N-2]
matrixpsi[-1,1] = -matrixpsi[-1,N-3]
matrixpsi[-2,0] = -matrixpsi[-2,-4]
matrixpsi.view()
```

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

```python
def u_x(u):
    psi = np.matmul(matrixpsi,u)
    ux = np.matmul(A2,psi)
    return ux
```

## Second derivative

```python
B = np.zeros((N,N))

i,j = np.indices(A.shape,dtype=int)

B[i==j-1] = 1
B[i==j] = 10
B[i==j+1] = 1

# Correcting boundary conditions for periodic case
B[0,-1] = B[0,1]
B[-1,0] = B[-1,-2]
B2 = inv(B)
B
```

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

```python
matrixphi = np.zeros((N,N))
i,j = np.indices(matrixphi.shape,dtype=int)

matrixphi[i==j-1] = 12/(dx**2)
matrixphi[i==j] = -(12*2)/(dx**2)
matrixphi[i==j+1] = 12/(dx**2)

# Correcting boundary conditions for periodic case
matrixphi[0,-1] = matrixphi[0,1]
matrixphi[-1,0] = matrixphi[-1,-2]
matrixphi
```

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

```python
def u_xx(u):
    phi = np.matmul(matrixphi,u)
    uxx = np.matmul(B2,phi)
    return uxx
```

## Fourth derivative

```python
def u_xxxx(u):
    phi = np.matmul(matrixphi,u_xx(u))
    uxxxx = np.matmul(B2,phi)
    return uxxxx
```

# Plugging in the derivative values

```python
beta=1
alpha=1
```

```python
def Kuramoto(u):
    """
    Function to compute the right-hand side of the system.

    Parameters
    ----------
    u : numpy.ndarray
        Solution of time step t as a 1D array of floats

    Returns
    ----------
    u_t : numpy.ndarray
          The right-hand side of the system as a 1D array of floats
    """

    u_t = -beta*u_xxxx(u)-alpha*u_xx(u)-u*u_x(u)
    return u_t
```

# Solving the equation

```python
def SSP_RK43(u):

    u1 = u + (dt/2)*Kuramoto(u)
    u2 = u1 + (dt/2)*Kuramoto(u1)
    u3 = (2/3)*u + u2/3 + (dt/6)*Kuramoto(u2)
    u_tdt = u3 + (dt/2)*Kuramoto(u3)

    return u_tdt # solution at time step t+dt
```

```python
u_hist = u_num.copy()
u_hist.shape
```

    (40001, 256)

```python
from tqdm.notebook import trange, tqdm
from time import sleep

for i in tqdm(range(len(t)-1)):
    u_num[i+1,:] = SSP_RK43(u_num[i,:])
```

      0%|          | 0/40000 [00:00<?, ?it/s]

# Plots

```python
fig, ax = plt.subplots(figsize=(9, 5))

Xm, Tm = np.meshgrid(x,t)

surf = plt.contourf(Xm, Tm, u_num,15, cmap=plt.get_cmap("seismic"))

plt.colorbar()
plt.xlabel('x')
plt.ylabel('T')
plt.tight_layout()
```

```python
np.save('Kuramoto_dataset\Kuramoto_X', Xm)
np.save('Kuramoto_dataset\Kuramoto_T', Tm)
np.save('Kuramoto_dataset\Kuramoto_U', u_num)
```

# References

[1] - Backpropagation algorithms and reservoir computing in recurrent neural networks for the forecasting of complex spatiotemporal dynamics

[2] - A COMPACT FOURTH-ORDER IMPLICIT-EXPLICIT RUNGE-KUTTA TYPE SCHEME FOR NUMERICAL SOLUTION OF THE KURAMOTO-SIVASHINSKY EQUATION

[3] - A note on solving the fourth-order Kuramoto-Sivanshinsky equation by the compact finite difference scheme

[4] - A Reduced High-order Compact Finite Difference Scheme Based on Proper Orthogonal Decomposition for the Generalized Kuramoto-Sivashinsky Equation


# ***Introduction to Radar Using Python and MATLAB***
## Andy Harrison - Copyright (C) 2019 Artech House
<br/>

# Barker Ambiguity Function
***

Referring to Section 8.7.1, a common set of binary phase codes for radar applications is Barker codes, which have several unique properties.  The time sidelobe level of the normalized ambiguity function is $1/N$, where $N$ is the length of the code.  The ambiguity function has a length of $2N\tau_n$, the main lobe has a width of $2\tau_n$, and there are $(N-1)/2$ sidelobes on either side of the main lobe.  There are only seven known Barker codes, and these are given in Table 8.1.

While these are the only known Barker codes, much research has been performed to find longer binary phase codes with low time sidelobe levels.  Although the time sidelobe levels are not $1/N$ as with the Barker codes, some codes have been found to have quite small sidelobe levels.  A Barker code of length $4$ is illustrated in Figure 8.20.  Another approach to obtain longer codes is to combine or embed one code within another.  For example, a Barker code of length $5$ may be used with a Barker code of length $3$ to give (Equation 8.71)

\begin{equation}
    B_{MN} = [\; \; 0\;\; 0\;\; 0\;\; \pi\;\; 0\,,   \; 0\;\; 0\;\; 0\;\; \pi\;\; 0\,,   \; \pi\;\; \pi\;\; \pi\;\; 0\;\; \pi\;].
\end{equation}

The compression ratio of the combined Barker code is $MN$.  However, the sidelobe level of the combined code is not $1/MN$.  When using combined Barker codes, a number of sidelobes may be reduced to zero if the matched filter is followed by a linear transversal filter.
***

Begin by getting the library path

```python
import lib_path
```

Set the chip width (s) and the code length (2, 3, 4, 5, 7, 11, 13)

```python
chip_width = 0.1

code_length = 5
```

Set the Barker code

```python
if code_length == 2:

    code = [1, -1]

elif code_length == 3:

    code = [1, 1, -1]

elif code_length == 4:

    code = [1, 1, -1, 1]

elif code_length == 5:

    code = [1, 1, 1, -1, 1]

elif code_length == 7:

    code = [1, 1, 1, -1, -1, 1, -1]

elif code_length == 11:

    code = [1, 1, 1, -1, -1, -1, 1, -1, -1, 1, -1]

elif code_length == 13:

    code = [1, 1, 1, 1, 1, -1, -1, 1, 1, -1, 1, -1, 1]
```

Calculate the ambiguity function for the Barker code

```python
from Libs.ambiguity.ambiguity_function import phase_coded_wf

ambiguity, time_delay, doppler_frequency = phase_coded_wf(code, chip_width)
```

Display the zero-Doppler cut, the zero-range cut, and the 2D contour plot using the `matplotlib` routines

```python
from matplotlib import pyplot as plt

# Set the figure size

plt.rcParams["figure.figsize"] = (15, 10)

# Plot the ambiguity function

plt.plot(time_delay, ambiguity[round(len(doppler_frequency) / 2)], '')

# Set the time axis limits

plt.xlim(-len(code) * chip_width, len(code) * chip_width)

# Set the x and y axis labels

plt.xlabel("Time (s)", size=12)

plt.ylabel("Relative Amplitude", size=12)

# Turn on the grid

plt.grid(linestyle=':', linewidth=0.5)

# Set the plot title and labels

plt.title('Barker Code Ambiguity Function', size=14)

# Set the tick label size

plt.tick_params(labelsize=12)
```

Create the zero-range cut

```python
 # Plot the ambiguity function

plt.plot(doppler_frequency,  ambiguity[:, round(len(time_delay) / 2)], '')

# Set the x and y axis labels

plt.xlabel("Doppler (Hz)", size=12)

plt.ylabel("Relative Amplitude", size=12)

# Turn on the grid

plt.grid(linestyle=':', linewidth=0.5)

# Set the plot title and labels

plt.title('Barker Code Ambiguity Function', size=14)

# Set the tick label size

plt.tick_params(labelsize=12)
```

Create the two-dimensional contour plot

```python
from numpy import meshgrid

# Create the grid

t, f = meshgrid(time_delay, doppler_frequency)

# Plot the ambiguity function

plt.contour(t, f, ambiguity, 30, cmap='jet', vmin=-0.2, vmax=1.0)

# Set the time axis limits

plt.xlim(-len(code) * chip_width, len(code) * chip_width)

# Set the x and y axis labels

plt.xlabel("Time (s)", size=12)

plt.ylabel("Doppler (Hz)", size=12)

# Turn on the grid

plt.grid(linestyle=':', linewidth=0.5)

# Set the plot title and labels

plt.title('Barker Code Ambiguity Function', size=14)

# Set the tick label size

plt.tick_params(labelsize=12)
```

```python

```


```python
%matplotlib inline
```

# The Johnson-Lindenstrauss bound for embedding with random projections

The `Johnson-Lindenstrauss lemma`_ states that any high dimensional
dataset can be randomly projected into a lower dimensional Euclidean
space while controlling the distortion in the pairwise distances.

Theoretical bounds
==================

The distortion introduced by a random projection `p` is asserted by
the fact that `p` is defining an eps-embedding with good probability
as defined by:

\begin{align}(1 - eps) \|u - v\|^2 < \|p(u) - p(v)\|^2 < (1 + eps) \|u - v\|^2\end{align}

Where u and v are any rows taken from a dataset of shape [n_samples,
n_features] and p is a projection by a random Gaussian N(0, 1) matrix
with shape [n_components, n_features] (or a sparse Achlioptas matrix).

The minimum number of components to guarantees the eps-embedding is
given by:

\begin{align}n\_components >= 4 log(n\_samples) / (eps^2 / 2 - eps^3 / 3)\end{align}

The first plot shows that with an increasing number of samples ``n_samples``,
the minimal number of dimensions ``n_components`` increased logarithmically
in order to guarantee an ``eps``-embedding.

The second plot shows that an increase of the admissible
distortion ``eps`` allows to reduce drastically the minimal number of
dimensions ``n_components`` for a given number of samples ``n_samples``

Empirical validation
====================

We validate the above bounds on the digits dataset or on the 20 newsgroups
text document (TF-IDF word frequencies) dataset:

- for the digits dataset, some 8x8 gray level pixels data for 500
  handwritten digits pictures are randomly projected to spaces for various
  larger number of dimensions ``n_components``.

- for the 20 newsgroups dataset some 500 documents with 100k
  features in total are projected using a sparse random matrix to smaller
  euclidean spaces with various values for the target number of dimensions
  ``n_components``.

The default dataset is the digits dataset. To run the example on the twenty
newsgroups dataset, pass the --twenty-newsgroups command line argument to this
script.

For each value of ``n_components``, we plot:

- 2D distribution of sample pairs with pairwise distances in original
  and projected spaces as x and y axis respectively.

- 1D histogram of the ratio of those distances (projected / original).

We can see that for low values of ``n_components`` the distribution is wide
with many distorted pairs and a skewed distribution (due to the hard
limit of zero ratio on the left as distances are always positives)
while for larger values of n_components the distortion is controlled
and the distances are well preserved by the random projection.

Remarks
=======

According to the JL lemma, projecting 500 samples without too much distortion
will require at least several thousands dimensions, irrespective of the
number of features of the original dataset.

Hence using random projections on the digits dataset which only has 64 features
in the input space does not make sense: it does not allow for dimensionality
reduction in this case.

On the twenty newsgroups on the other hand the dimensionality can be decreased
from 56436 down to 10000 while reasonably preserving pairwise distances.

```python
print(__doc__)

import sys
from time import time
import numpy as np
import matplotlib.pyplot as plt
from sklearn.random_projection import johnson_lindenstrauss_min_dim
from sklearn.random_projection import SparseRandomProjection
from sklearn.datasets import fetch_20newsgroups_vectorized
from sklearn.datasets import load_digits
from sklearn.metrics.pairwise import euclidean_distances

# Part 1: plot the theoretical dependency between n_components_min and
# n_samples

# range of admissible distortions
eps_range = np.linspace(0.1, 0.99, 5)
colors = plt.cm.Blues(np.linspace(0.3, 1.0, len(eps_range)))

# range of number of samples (observation) to embed
n_samples_range = np.logspace(1, 9, 9)

plt.figure()
for eps, color in zip(eps_range, colors):
    min_n_components = johnson_lindenstrauss_min_dim(n_samples_range, eps=eps)
    plt.loglog(n_samples_range, min_n_components, color=color)

plt.legend(["eps = %0.1f" % eps for eps in eps_range], loc="lower right")
plt.xlabel("Number of observations to eps-embed")
plt.ylabel("Minimum number of dimensions")
plt.title("Johnson-Lindenstrauss bounds:\nn_samples vs n_components")

# range of admissible distortions
eps_range = np.linspace(0.01, 0.99, 100)

# range of number of samples (observation) to embed
n_samples_range = np.logspace(2, 6, 5)
colors = plt.cm.Blues(np.linspace(0.3, 1.0, len(n_samples_range)))

plt.figure()
for n_samples, color in zip(n_samples_range, colors):
    min_n_components = johnson_lindenstrauss_min_dim(n_samples, eps=eps_range)
    plt.semilogy(eps_range, min_n_components, color=color)

plt.legend(["n_samples = %d" % n for n in n_samples_range], loc="upper right")
plt.xlabel("Distortion eps")
plt.ylabel("Minimum number of dimensions")
plt.title("Johnson-Lindenstrauss bounds:\nn_components vs eps")

# Part 2: perform sparse random projection of some digits images which are
# quite low dimensional and dense or documents of the 20 newsgroups dataset
# which is both high dimensional and sparse

if '--twenty-newsgroups' in sys.argv:
    # Need an internet connection hence not enabled by default
    data = fetch_20newsgroups_vectorized().data[:500]
else:
    data = load_digits().data[:500]

n_samples, n_features = data.shape
print("Embedding %d samples with dim %d using various random projections"
      % (n_samples, n_features))

n_components_range = np.array([300, 1000, 10000])
dists = euclidean_distances(data, squared=True).ravel()

# select only non-identical samples pairs
nonzero = dists != 0
dists = dists[nonzero]

for n_components in n_components_range:
    t0 = time()
    rp = SparseRandomProjection(n_components=n_components)
    projected_data = rp.fit_transform(data)
    print("Projected %d samples from %d to %d in %0.3fs"
          % (n_samples, n_features, n_components, time() - t0))
    if hasattr(rp, 'components_'):
        n_bytes = rp.components_.data.nbytes
        n_bytes += rp.components_.indices.nbytes
        print("Random matrix with size: %0.3fMB" % (n_bytes / 1e6))

    projected_dists = euclidean_distances(
        projected_data, squared=True).ravel()[nonzero]

    plt.figure()
    plt.hexbin(dists, projected_dists, gridsize=100, cmap=plt.cm.PuBu)
    plt.xlabel("Pairwise squared distances in original space")
    plt.ylabel("Pairwise squared distances in projected space")
    plt.title("Pairwise distances distribution for n_components=%d" %
              n_components)
    cb = plt.colorbar()
    cb.set_label('Sample pairs counts')

    rates = projected_dists / dists
    print("Mean distances rate: %0.2f (%0.2f)"
          % (np.mean(rates), np.std(rates)))

    plt.figure()
    plt.hist(rates, bins=50, normed=True, range=(0., 2.), edgecolor='k')
    plt.xlabel("Squared distances rate: projected / original")
    plt.ylabel("Distribution of samples pairs")
    plt.title("Histogram of pairwise distance rates for n_components=%d" %
              n_components)

    # TODO: compute the expected value of eps and add them to the previous plot
    # as vertical lines / region

plt.show()
```

```python

```


# System Properties

*This Jupyter notebook is part of a [collection of notebooks](../index.ipynb) in the bachelors module Signals and Systems, Communications Engineering, Universität Rostock. Please direct questions and suggestions to [Sascha.Spors@uni-rostock.de](mailto:Sascha.Spors@uni-rostock.de).*

## Idealized Systems

Idealized systems are systems with idealized properties that typically render their practical implementation infeasible. They play an important role in various fields of signal processing as they allow a convenient formulation of concepts and principles. In the following, the ideal low-pass is introduced as prototype for an idealized frequency selective system. Other frequency selective system can be deduced directly from this prototype.

### Ideal Low-Pass

The transfer function $H(j \omega)$ of a real-valued [ideal low-pass](https://en.wikipedia.org/wiki/Sinc_filter) reads

\begin{equation}
H(j \omega) = \text{rect} \left( \frac{\omega}{2 \omega_\text{c}} \right)
\end{equation}

where $\omega_\text{c} > 0$ denotes its cut-off frequency. The ideal low-pass removes all frequency components above the cut-off frequency $\omega_\text{c}$, without affecting lower frequencies. The impulse response $h(t) = \mathcal{F}^{-1} \{ H(j \omega) \}$ is computed by applying the [duality principle](../fourier_transform/properties.ipynb#Duality) to the [Fourier transform of the rectangular signal](../fourier_transform/definition.ipynb#Transformation-of-the-Rectangular-Signal)

\begin{equation}
h(t) = \frac{\omega_\text{c}}{\pi} \cdot \text{sinc} ( \omega_\text{c} t )
\end{equation}

Since the impulse response is an acausal signal, the ideal low-pass is an [acausal system](causality_stability.ipynb#Condition-for-the-Impulse-Response). The sinc-function is furthermore not absolutely integrable. Hence the ideal-low pass is not a [stable system](causality_stability.ipynb#Condition-for-the-Impulse-Response) in the sense of the bounded-input bounded-output (BIBO) criterion. In conclusion, the ideal low-pass is not realizable. It can only be approximated in practical systems. It plays nevertheless an important role in the foundations of sampling and interpolation.

#### Realizable low-pass

Various techniques have been developed in order to approximate the ideal low-pass by a realizable system. One is the *windowed sinc filter*. In order to make the ideal-low pass filter causal and stable, its impulse response is windowed to a finite-length $T$ followed by a temporal shift of $\frac{T}{2}$. Using the [rectanglar signal](../continuous_signals/standard_signals.ipynb#Rectangular-Signal) to truncate (window) the impulse response, the impulse response of the realizable low-pass is given as

\begin{equation}
h(t) = \frac{\omega_\text{c}}{\pi} \cdot \text{sinc} \left( \omega_\text{c} \left(t - \frac{T}{2} \right) \right) \cdot \text{rect}\left( \frac{1}{T} \left( t - \frac{T}{2} \right) \right)
\end{equation}

Fourier transformation yields its transfer function

\begin{equation}
H(j \omega) = \frac{1}{2 \pi} e^{-j \omega \frac{T}{2}} \cdot \text{rect}\left( \frac{\omega}{2 \omega_c} \right) * T \cdot \text{sinc} \left( \frac{T}{2} \omega \right)
\end{equation}

The impulse response is plotted for $w_\text{c}=10$ and $T=5$

```python
%matplotlib inline
import sympy as sym
sym.init_printing()

t, w = sym.symbols('t omega', real=True)
wc = 10
T = 5

h = wc/sym.pi * sym.sinc(wc*(t-T/2))
sym.plot(h, (t, 0, T), xlabel='$t$', ylabel='$h(t)$');
```

**Exercise**

* Discuss the properties of the magnitude response $|H(j \omega)|$ of the realizable low-pass in dependence of its length $T$.

### Ideal Band-Pass

The transfer function $H(j \omega)$ of a real-valued ideal band-pass reads

\begin{equation}
H(j \omega) =
\begin{cases}
1 & \text{for }  \omega_\text{c} - \frac{\Delta \omega}{2} < |\omega| < \omega_\text{c} + \frac{\Delta \omega}{2} \\
0 & \text{otherwise}
\end{cases}
\end{equation}

The ideal band-pass does not affect the frequency components of a signal around a given center frequency $\omega_\text{c}$ where the total width of this transition band is $\Delta \omega$. Components outside the transition band are removed. The transfer function can be rewritten as

\begin{equation}
H(j \omega) =
\text{rect} \left( \frac{\omega - \omega_\text{c}}{\Delta \omega} \right) + \text{rect} \left( \frac{\omega + \omega_\text{c}}{\Delta \omega} \right)
= \text{rect} \left( \frac{\omega}{\Delta \omega} \right) * \left( \delta(\omega - \omega_\text{c}) + \delta(\omega + \omega_\text{c}) \right)
\end{equation}

Its impulse response is computed by inverse Fourier transformation

\begin{equation}
h(t) = \pi \Delta \omega \cdot \text{sinc} ( \frac{\Delta \omega}{2} t ) \cdot \cos(\omega_\text{c} t)
\end{equation}

The ideal band-pass can be interpreted as a modulated low-pass filter. Due to its relation to the ideal low-pass, it is neither causal nor stable. The ideal band-pass can only be approximated in practical realizations. Its plays an important role in the theoretical foundations of wireless communications.

**Example**

For illustration, the impulse response of the ideal band-pass for $\omega_\text{c} = 10$ and $\Delta \omega = 2$ is plotted

```python
wc = 10
dw = 2

h = sym.pi*dw * sym.sinc(dw/2*t) * sym.cos(wc*t)
sym.plot(h, (t, -10, 10), xlabel='$t$', ylabel='$h(t)$');
```

**Exercise**

* In the same manner as for the low-pass, derive the impulse response and transfer function of a realizable band-pass.

### Ideal High-Pass

The transfer function $H(j \omega)$ of a real-valued ideal high-pass reads

\begin{equation}
H(j \omega) = 1 - \text{rect} \left( \frac{\omega}{2 \omega_\text{c}} \right)
\end{equation}

where $\omega_\text{c} > 0$ denotes its cut-off frequency. The ideal high-pass removes all frequency components below the cut-off frequency $\omega_\text{c}$, without affecting higher frequencies. Its impulse response can be derived in a straightforward way from the impulse response of the ideal low-pass

\begin{equation}
h(t) = \delta(t) - \frac{\omega_\text{c}}{\pi} \cdot \text{sinc} ( \omega_\text{c} t )
\end{equation}

Due to its relation to the ideal low-pass, the ideal high-pass is neither causal nor stable. The ideal high-pass can only be approximated in practical realizations.

### Ideal Band-Stop

The transfer function $H(j \omega)$ of a real-valued ideal band-stop is derived from the transfer function of the ideal band-pass in the same manner as the ideal high-pass. It reads

\begin{equation}
H(j \omega) =  1 - \text{rect} \left( \frac{\omega - \omega_\text{c}}{\Delta \omega} \right) - \text{rect} \left( \frac{\omega + \omega_\text{c}}{\Delta \omega} \right)
\end{equation}

The ideal band-stop removes the frequency components of a signal around a given center frequency $\omega_\text{c}$ where the total width of this stop band is $\Delta \omega$. Components outside the stop band are not affected by the system.  The impulse response of the ideal band-stop can be derived in a straightforward way from the impulse response of the ideal band-pass as

\begin{equation}
h(t) = \delta(t) - \pi \Delta \omega \cdot \text{sinc} ( \frac{\Delta \omega}{2} t ) \cdot \cos(\omega_\text{c} t)
\end{equation}

Due to its relation to the ideal band-pass, the ideal band-stop is neither causal nor stable. The ideal band-stop can only be approximated in practical realizations. The ideal band-stop is for instance used to remove undesired signal components, e.g. [mains hum](https://en.wikipedia.org/wiki/Mains_hum).

**Copyright**

The notebooks are provided as [Open Educational Resource](https://de.wikipedia.org/wiki/Open_Educational_Resources). Feel free to use the notebooks for your own educational purposes. The text is licensed under [Creative Commons Attribution 4.0](https://creativecommons.org/licenses/by/4.0/), the code of the IPython examples under the [MIT license](https://opensource.org/licenses/MIT). Please attribute the work as follows: *Lecture Notes on Signals and Systems* by Sascha Spors.


```python
import numpy as np
import matplotlib.pyplot as plt
import matplotlib as mpl
import scipy
from scipy.integrate import odeint
from scipy.integrate import quad
from scipy.optimize import minimize
from scipy.optimize import fsolve
from scipy.optimize import least_squares
from scipy.interpolate import interp1d
```

Go slow, write the plan down, and check energy

```python
mpl.rc('figure',dpi=250)
mpl.rc('text',usetex=True)
```

```python
def add_labels(xlabel, ylabel, title):
    plt.xlabel(xlabel)
    plt.ylabel(ylabel)
    plt.title(title)
    plt.legend()
```

```python
def generate_samples(func, bounds, N):
    """
    Assignment 5: function for sampling from a custom 1d distribution.

    Input:  func - any function
            bounds - tuple (a, b)

    Output: 1d samples of size N
    """
    a, b = bounds
    x = np.linspace(a, b)

    f_max = max(func(x))

    x_rand = np.random.uniform(a, b, size=N)
    y_rand = np.random.uniform(0, f_max, size=N)

    samples = x_rand[y_rand < func(x_rand)]

    return samples
```

```python
def spherical_to_xyz(r, theta, phi):
    "Generate a vector in xyz coordinates from r, theta, phi"

    return np.array([r*np.sin(theta)*np.cos(phi), r*np.sin(theta)*np.sin(phi), r*np.cos(theta)])
```

Solves a problem of the form

\begin{equation}
\frac{dy}{dt} = f(y, t)
\end{equation}

```python
def main(y, t):
    "Main thing to integrate"

    position = y[0:3]
    velocity = y[3:6]

    dydt = np.empty(6)

    dydt[0:3] = velocity
    dydt[3:6] = np.array([0, 0, -9.81]) #force goes here...
    # Don't forget to divide by m

    return dydt

m = 1
theta = np.pi/3
t = np.linspace(0, 1)
y0 = np.array([0, 0, 0, 5*np.cos(theta), 0, 5*np.sin(theta)])
```

```python
y = odeint(main, y0, t)
plt.plot(t, y[:, 2])
```

```python
def df(f, x):
    h = 0.0001
    return (f(x + h) - f(x)/h)

# Whenever possible, use polyderiv if possible
```

```python
# If number of counts is n, then the uncertainty is sqrt(n) cough Poisson distribution
# Error of the mean is always stdev/sqrt(n) by CLT
```

## Curve Fitting Syntax

To use curve fit please pass in a function with params

```python
import numpy as np
from scipy.optimize import curve_fit

x = np.array([1, 2, 3, 9])
y = np.array([1, 4, 1, 3])

def fit_func(x, a, b):
    return a*x + b

popt = curve_fit(fit_func, x, y)
popt[0]

plt.scatter(x, y)

x_cont = np.linspace(1, 9)
plt.plot(x_cont, fit_func(x_cont, *popt[0]))
```

## Minimization Syntax

To use minimize must pass in a loss function. Note we already plug in the data

```python
import numpy as np
from scipy.optimize import minimize

x = np.array([1, 2, 3, 9])
y = np.array([1, 4, 1, 3])

def loss_func(params, x, y):
    a, b = params

    fit_func = lambda x: a*x + b
    return np.sum((fit_func(x) - y)**2)
```

```python
params0 = np.array([1, 1])
popt_minimization = minimize(loss_func, params0, args=(x, y))
```

```python
plt.scatter(x, y)
plt.plot(x_cont, fit_func(x_cont, *popt_minimization.x))
```

## Least Squares Syntax

```python
import numpy as np
from scipy.optimize import least_squares

def res_func(params, x, y):
    a, b = params
    fit_func = lambda x: a*x + b

    return (fit_func(x) - y)

params0 = np.array([1, 1])
popt_minimization = least_squares(res_func, params0, args=(x, y))
popt_minimization

plt.scatter(x, y)
plt.plot(x_cont, fit_func(x_cont, *popt_minimization.x))
```

## Polyfit Syntax

```python
import numpy as np

x = np.array([1, 2, 3, 9])
y = np.array([1, 4, 1, 3])

plt.scatter(x, y)

x_cont = np.linspace(1, 9)

popt = np.polyfit(x, y, 1)
popt

plt.plot(x_cont, np.poly1d(popt)(x_cont))

# Poly fit is the easiest to use
```

## Testing

```python

```

```python
myfunc()
```

    3

```python
a = np.arange(4, 10)
wh = np.where(a > 7)
a[wh]
```

    array([8, 9])

```python
a[a > 7]
```

    array([8, 9])

```python
b = np.arange(10, 30).reshape(-1, 4)
b
```

    array([[10, 11, 12, 13],
           [14, 15, 16, 17],
           [18, 19, 20, 21],
           [22, 23, 24, 25],
           [26, 27, 28, 29]])

```python
wh = np.where(b > 15)
b[wh[0], wh[1]]
```

    array([16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29])

```python
(b > 15)

```

    array([[False, False, False, False],
           [False, False,  True,  True],
           [ True,  True,  True,  True],
           [ True,  True,  True,  True],
           [ True,  True,  True,  True]])

```python
(b < 17) & (b > 15)
# & is a ufunc
# and comparing the whole array

```

    array([[False, False, False, False],
           [False, False,  True, False],
           [False, False, False, False],
           [False, False, False, False],
           [False, False, False, False]])

```python
xdata = np.linspace(1, 10, 10)
ydata = np.exp(xdata)

x_cont = np.linspace(1, 10)

plt.plot(x_cont, interp1d(xdata, ydata, kind='linear')(x_cont))
plt.plot(x_cont, interp1d(xdata, ydata, kind='cubic')(x_cont))
# sometimes need to turn on extrapolate if minimize is complaing
```

```python

```


```python
import math as ma  # import math library
ma.frexp(0.2) #find the mantissa and the exponent
```

    (0.8, -2)

```python
1. + 1.e-15 == 1 # machine accuracy below 10^-15
```

    False

```python
1. + 1.e-16 == 1. # machine accuracy above 10^-16
```

    True

```python
x=1.e-7
a=(1-x*x)**(0.5)
a
```

    0.999999999999995

```python
b=((1-x)/(1+x))**0.5
b+b*x
```

    0.9999999999999949

```python
import matplotlib.pyplot as plt # get matplotlib to plot
```

```python
t, h, n = 0.0, 0.1, 20          # init time, step size, number of ticks
```

```python
v0 = float(input("enter initial velocity:"))
```

    enter initial velocity:1.0

```python
ta, xa = [], []                 # time and position arrays for plotting
```

```python
for i in range(n):              # loop for n steps
    ta.append(t)                # record time
    xa.append(v0*t)             # update position
    t = t + h                   # update time
```

```python
plt.figure()                    # start a figure;
plt.plot(ta, xa, '-o')          # plot data, '-o' means blue
plt.xlabel('t (s)')             # add labels
plt.ylabel('x (m)')
plt.show()                      # show figure
```

```python
from pylab import *
x = linspace(-10.,10.,100)
plot(x,sin(x)/x)
```

```python
from sympy import *
x = symbols('x')
plot(sin(x)/x)
```

```python
import numpy as np
```

```python
help(np.array)
```

    Help on built-in function array in module numpy:

    array(...)
        array(object, dtype=None, *, copy=True, order='K', subok=False, ndmin=0,
              like=None)

        Create an array.

        Parameters
        ----------
        object : array_like
            An array, any object exposing the array interface, an object whose
            __array__ method returns an array, or any (nested) sequence.
        dtype : data-type, optional
            The desired data-type for the array.  If not given, then the type will
            be determined as the minimum type required to hold the objects in the
            sequence.
        copy : bool, optional
            If true (default), then the object is copied.  Otherwise, a copy will
            only be made if __array__ returns a copy, if obj is a nested sequence,
            or if a copy is needed to satisfy any of the other requirements
            (`dtype`, `order`, etc.).
        order : {'K', 'A', 'C', 'F'}, optional
            Specify the memory layout of the array. If object is not an array, the
            newly created array will be in C order (row major) unless 'F' is
            specified, in which case it will be in Fortran order (column major).
            If object is an array the following holds.

            ===== ========= ===================================================
            order  no copy                     copy=True
            ===== ========= ===================================================
            'K'   unchanged F & C order preserved, otherwise most similar order
            'A'   unchanged F order if input is F and not C, otherwise C order
            'C'   C order   C order
            'F'   F order   F order
            ===== ========= ===================================================

            When ``copy=False`` and a copy is made for other reasons, the result is
            the same as if ``copy=True``, with some exceptions for `A`, see the
            Notes section. The default order is 'K'.
        subok : bool, optional
            If True, then sub-classes will be passed-through, otherwise
            the returned array will be forced to be a base-class array (default).
        ndmin : int, optional
            Specifies the minimum number of dimensions that the resulting
            array should have.  Ones will be pre-pended to the shape as
            needed to meet this requirement.
        like : array_like
            Reference object to allow the creation of arrays which are not
            NumPy arrays. If an array-like passed in as ``like`` supports
            the ``__array_function__`` protocol, the result will be defined
            by it. In this case, it ensures the creation of an array object
            compatible with that passed in via this argument.

            .. note::
                The ``like`` keyword is an experimental feature pending on
                acceptance of :ref:`NEP 35 <NEP35>`.

            .. versionadded:: 1.20.0

        Returns
        -------
        out : ndarray
            An array object satisfying the specified requirements.

        See Also
        --------
        empty_like : Return an empty array with shape and type of input.
        ones_like : Return an array of ones with shape and type of input.
        zeros_like : Return an array of zeros with shape and type of input.
        full_like : Return a new array with shape of input filled with value.
        empty : Return a new uninitialized array.
        ones : Return a new array setting values to one.
        zeros : Return a new array setting values to zero.
        full : Return a new array of given shape filled with value.

        Notes
        -----
        When order is 'A' and `object` is an array in neither 'C' nor 'F' order,
        and a copy is forced by a change in dtype, then the order of the result is
        not necessarily 'C' as expected. This is likely a bug.

        Examples
        --------
        >>> np.array([1, 2, 3])
        array([1, 2, 3])

        Upcasting:

        >>> np.array([1, 2, 3.0])
        array([ 1.,  2.,  3.])

        More than one dimension:

        >>> np.array([[1, 2], [3, 4]])
        array([[1, 2],
               [3, 4]])

        Minimum dimensions 2:

        >>> np.array([1, 2, 3], ndmin=2)
        array([[1, 2, 3]])

        Type provided:

        >>> np.array([1, 2, 3], dtype=complex)
        array([ 1.+0.j,  2.+0.j,  3.+0.j])

        Data-type consisting of more than one element:

        >>> x = np.array([(1,2),(3,4)],dtype=[('a','<i4'),('b','<i4')])
        >>> x['a']
        array([1, 3])

        Creating an array from sub-classes:

        >>> np.array(np.mat('1 2; 3 4'))
        array([[1, 2],
               [3, 4]])

        >>> np.array(np.mat('1 2; 3 4'), subok=True)
        matrix([[1, 2],
                [3, 4]])

```python
np.array([1.,2.,3.])
```

    array([1., 2., 3.])

```python
a=np.arange(5)
a
```

    array([0, 1, 2, 3, 4])

```python
a[2]
```

    2

```python
a[-2]
```

    3

```python
a[1:3] #slicing an array
```

    array([1, 2])

```python
a[::-1]
```

    array([4, 3, 2, 1, 0])

```python
np.insert(a,1,9)
```

    array([0, 9, 1, 2, 3, 4])

```python
np.delete(a,2)
```

    array([0, 1, 3, 4])

```python
2*a
```

    array([0, 2, 4, 6, 8])

```python
a+3
```

    array([3, 4, 5, 6, 7])

```python
a[1:3]=[-1,-2]
a
```

    array([ 0, -1, -2,  3,  4])

```python
b=np.copy(a[1:3])
b
```

    array([-1, -2])

```python
d=np.array([[11,12,13],[21,22,23],[31,32,33]]) #multidimensional array
```

```python
d
```

    array([[11, 12, 13],
           [21, 22, 23],
           [31, 32, 33]])

```python
d.shape
```

    (3, 3)

```python
d[0]
```

    array([11, 12, 13])

```python
d[1,2]
```

    23


# The basics of `generalrelativity`

`generalrelativity.py` contains some algorithms related to the basics of multilinear algebra and differential geometry, algorithms which relate to some of the calculations that appear in the everyday work of a physicist or a mathematician who works in general relativity. These algorithms are built upon sympy.

In this first tutorial, we deal with multilinear algebra and the manipulation of the `Tensor` object. In the next one we learn about index manipulation, and in the third one we use these algorithms to implement the Schwarzschild metric.

```python
import generalrelativity as gr
import sympy
sympy.init_printing(use_latex=True)
```

```python
from pprint import pprint
```

# Multiindices

The main object of the library (i.e. the `Tensor` object) relies heavily on multiindices and their manipulation. For us, a $p$-multiindex is just a tuple $(a_1, \dots, a_p)$ in which every $a_i$ is between $0$ and $n-1$ for some $n$ (the dimension of a vector space in our particular case).

This library includes a function for getting all multiindices of certain length $p$ and certain dimension $n$:

```python
n = 4
p = 2
list_of_multiindices = gr.get_all_multiindices(p, n)
list_of_multiindices
```

## Special types of multiindices

The only $0$-multiindex is the python object `None`, and $1$-multiindices must still be tuples, so one must write them like `(a_1, )` in python.

```python
n = 4
p = 1
list_of_multiindices = gr.get_all_multiindices(p, n)
print(list_of_multiindices)
```

    [(0,), (1,), (2,), (3,)]

# The `Tensor` object

`generalrelativity` implements the tensor object in a very algebraic and generic sense. Sadly, we have the bound of dealing always in coordinates (that is, always talking about the coefficients with respect to some basis). To create a tensor $\Gamma \in T^{(p,q)}(V)$ we must specify a **basis**, a **type** and a **dict_of_values**:
- The **basis** is a list of sympy symbols, which represent a basis for the generic vector space $V$.
- The **type** is a tuple $(p, q)$ of nonnegative integers.
- The **dict_of_vales** has for keys tuples of multiindices $(a, b)$ and values the actual value $\Gamma^a_b$.

For example, say we want to create the tensor

$$T = 3e_0\otimes e_0\otimes e_0^*\otimes e_1^* - 2 e_0\otimes e_1\otimes e_1^*\otimes e_3^* \in T^{(2,2)}(V)$$

we do so like this:

```python
e0, e1, e2, e3 = sympy.symbols('e_0 e_1 e_2 e_3')
basis = [e0, e1, e2, e3]
dict_of_values = {
    ((0,0), (0,1)): 3, # The first sumand
    ((0,1), (1,3)): -2 # The second sumand
}
T = gr.Tensor(basis, (2, 2), dict_of_values)
T
```

$(3)e_0 \otimes e_0 \otimes e_0^* \otimes e_1^* + (-2)e_0 \otimes e_1 \otimes e_1^* \otimes e_3^*$

The tensors object have a LaTeX printing function (but this depends obviously on the availability of IPython, LaTeX and on the running of `sympy.init_printing(use_latex=True)`). We could print it raw:

```python
print(T)
```

    (3)e_0 \otimes e_0 \otimes e_0* \otimes e_1* + (-2)e_0 \otimes e_1 \otimes e_1* \otimes e_3*

## More details about the `dict_of_values`

Say we want to construct a $(0,2)$ Tensor. Initially, the `dict_of_values` should be of the form

```python
dict_of_values_1 = {
    (None, (0,0)): -1,
    (None, (1,1)): 1,
    (None, (2,2)): 1,
    (None, (3,3)): 1,
}
```

That is, one should specify the $0$-multiindex `None`. In the long run, this becomes tedious, and it is because of this that the `Tensor` object can also accept, for this case, the following `dict_of_values`:

```python
dict_of_values_2 = {
    (0,0): -1,
    (1,1): 1,
    (2,2): 1,
    (3,3): 1,
}
```

and the two tensors generated would be equal (once we specify equal bases and equal types)

```python
T1 = gr.Tensor(basis, (0, 2), dict_of_values_1)
T2 = gr.Tensor(basis, (0, 2), dict_of_values_2)
print(T1 == T2)
```

    True

The same happens for $1$-multiindices: a priori a $1$-multiindex is of the form `(a_1, )`, so if we wanted to create for example a $(1,2)$ tensor one should create a `dict_of_values` of the form

```python
dict_of_values_1 = {
    ((0, ), (1, 1)): 3,
    ((1, ), (0, 1)): -1,
    ((0, ), (2, 3)): 2
}
```

We relax the conditions to `dict_of_values` like

```python
dict_of_values_2 = {
    (0, (1, 1)): 3,
    (1, (0, 1)): -1,
    (0, (2, 3)): 2
}
```

```python
T1 = gr.Tensor(basis, (1, 2), dict_of_values_1)
T2 = gr.Tensor(basis, (1, 2), dict_of_values_2)
print(T1 == T2)
```

    True

# Operations with `Tensor` objects

One can **add** tensor objects if they have the same basis and type, one can **multiply** a tensor object with `int`s, `float`s, $(0,0)$-tensors and even `sympy` objects and symbols. Moreover, one can contract indices and raise and lower indices according to a special $(0,2)$-tensor, but we will explain this on the second tutorial.

```python
dict_of_values_S = {
    ((0,0), (0,1)): 4,
    ((1,1), (2,1)): -1,
}
S = gr.Tensor(basis, (2, 2), dict_of_values_S)
```

```python
T
```

$(3)e_0 \otimes e_0 \otimes e_0^* \otimes e_1^* + (-2)e_0 \otimes e_1 \otimes e_1^* \otimes e_3^*$

```python
S
```

$(4)e_0 \otimes e_0 \otimes e_0^* \otimes e_1^* + (-1)e_1 \otimes e_1 \otimes e_2^* \otimes e_1^*$

```python
T + S
```

$(7)e_0 \otimes e_0 \otimes e_0^* \otimes e_1^* + (-1)e_1 \otimes e_1 \otimes e_2^* \otimes e_1^* + (-2)e_0 \otimes e_1 \otimes e_1^* \otimes e_3^*$

```python
3*T
```

$(9)e_0 \otimes e_0 \otimes e_0^* \otimes e_1^* + (-6)e_0 \otimes e_1 \otimes e_1^* \otimes e_3^*$

```python
sympy.pi*T
```

$(3*pi)e_0 \otimes e_0 \otimes e_0^* \otimes e_1^* + (-2*pi)e_0 \otimes e_1 \otimes e_1^* \otimes e_3^*$

## Substitution of `sympy.symbols` in tensors

Nothing impedes us of using `sympy.symbols` in our tensors. For example:

```python
t, r, theta, phi = sympy.symbols('t r \\theta \\phi')
dict_of_values = {
    (1, (0, 0)): t**2,
    (2, (1, 0)): r*t,
    (0, (1, 3)): sympy.sin(theta + phi)
}
basis = [t, r, theta, phi]
T = gr.Tensor(basis, (1, 2), dict_of_values)
T
```

$(t^2)r \otimes t^* \otimes t^* + (r*t)\theta \otimes r^* \otimes t^* + (sin(\phi + \theta))t \otimes r^* \otimes \phi^*$

we can substitute values with the usual `subs` function, which takes a list of tuples of the substitutions to be performed:

```python
T.subs([(t, 5), (r, 2.5), (theta, (1/2)*sympy.pi)])
```

$(25)r \otimes t^* \otimes t^* + (12.5000000000000)\theta \otimes r^* \otimes t^* + (sin(\phi + 0.5*pi))t \otimes r^* \otimes \phi^*$

This creates a new object, $T$ remains unmodified.

```python
T
```

$(t^2)r \otimes t^* \otimes t^* + (r*t)\theta \otimes r^* \otimes t^* + (sin(\phi + \theta))t \otimes r^* \otimes \phi^*$

.ipynb_checkpoints/Tutorial 1 - The basics-checkpoint.ipynb


```python
%matplotlib inline
%config InlineBackend.figure_format = 'retina'

%load_ext autoreload
%autoreload 2

import numpy as np
import matplotlib.pyplot as plt
import matplotlib.pylab as pylab
from simplified_monorotor import Monorotor
import plotting
import testing
import trajectories

pylab.rcParams['figure.figsize'] = 10,10
```

# PID controller

#### TODO - implement PID controller

Implement the PD Controller math in `thrust_control`:

$$
\begin{align}
e &= z_{\text{target}} - z_{\text{actual}} \\
\dot{e} &= \dot{z}_{\text{target}} - \dot{z}_{\text{actual}} \\
\bar{u}_1 &= k_p e + k_d \dot{e} + k_i\int_0^te(t')dt' +\ddot{z}_{\text{ff}} \\
u_1 &= m(g - \bar{u}_1)
\end{align}
$$

> **Note**: we've introduced `integrated_error` and set it to 0.0 at initialization. We also now need to know how much time `dt` has elapsed between successive calls to `thrust_control` as this influences the integrated error.

```python
class PIDController:

    def __init__(self, k_p, k_d, k_i, m):
        self.k_p = k_p
        self.k_d = k_d
        self.k_i = k_i
        self.vehicle_mass = m
        self.g = 9.81
        self.integrated_error = 0.0

    def thrust_control(self,
                z_target,
                z_actual,
                z_dot_target,
                z_dot_actual,
                dt=0.1,
                z_dot_dot_ff=0.0):
        #
        # TODO
        #   implement a PID controller according to the math
        #   shown above. Remember that the integral accumulates
        #   error by adding "rectangles" of error with area
        #   equal to err * dt
        #
        #   don't forget to return a thrust command
        #

        e = z_target - z_actual
        e_dot = z_dot_target - z_dot_actual
        self.integrated_error += e * dt

        u_bar = self.k_p * e \
              + self.k_d * e_dot \
              + self.k_i * self.integrated_error \
              + z_dot_dot_ff
        u = self.vehicle_mass * (self.g - u_bar)

        return u

testing.pid_controller_test(PIDController)
```

    Tests pass

#### TODO 2 - Adjust gains
Run the simulation code below with `K_I = 0.0` to remind yourself what a PD controller does when there's a large mass_error (remember, a PID controller with zero integral gain is just a PD controller).

Then slowly increase `K_I` and rerun the cell. Note what happens to the error.

```python
MASS_ERROR = 1.5
K_P = 20.0
K_D = 10.0
K_I = 10.0 # TODO - increase to 0.5, 1, 2, etc...

# preparation
drone = Monorotor()
perceived_mass = drone.m * MASS_ERROR
controller    = PIDController(K_P, K_D, K_I, perceived_mass)

t, z_path, z_dot_path = trajectories.step(duration=10.0)

dt = t[1] - t[0]

# run simulation
history = []
for z_target, z_dot_target in zip(z_path, z_dot_path):
    z_actual = drone.z
    z_dot_actual = drone.z_dot

    u = controller.thrust_control(z_target, z_actual,
                                  z_dot_target, z_dot_actual,
                                  dt)

    drone.thrust = u
    drone.advance_state(dt)
    history.append(drone.X)

# generate plots
z_actual = [h[0] for h in history]
plotting.compare_planned_to_actual(z_actual, z_path, t)
```

Hopefully you were able to adjust K_P, K_D, and K_I so that the drone gets to the target altitude (even with a large mass error) pretty quickly.

If so, congratulations! You now understand PID control!

[Solution](/notebooks/PID%20Controller%20Solution.ipynb)


# Problem Set #5 part 2: controllers that open cupboard doors

In this notebook, we will take a more careful look at controllers, which have been treated as a black box that generates torques ($\tau_{\text{cmd}}$) to drive the robot to desired configurations ($q_{\text{ref}}$).

## 1. Rigid body dynamics
Almost every controller starts with writing down the equations of motion ($F=ma$) of the robot:
\begin{equation}
M(q)\ddot{q} + C(q, \dot{q}) + g(q) = \tau_{m} + \tau_{external}   \quad\quad\quad (1)
\end{equation}
- $q \in \mathbb{R}^7$ is the configuration, or joint angles of the iiwa arm.
- $\dot{q}, \ddot{q} \in \mathbb{R}^7$ are the generalized velocities and accelerations.
- $M(q) \in \mathbb{R}^{7\times 7}$ is the mass matrix, which depends on $q$.
- $C(q, \dot{q}) \in \mathbb{R}^7$ is the Coriolis forces.
- $g(q) \in \mathbb{R}^7$ is the torque (generalized forces) generated by gravity.
- $\tau_{external}$ is the torque generated by external contact. It is zero if the robot is not making contact with the environment.
- $\tau_{m}$ is the torque applied to the robot by the robot's motors.

## 2. Robot controller and closed-loop dynamics
The controller of many industrial robotic arms can be decomposed into two layers: an inner, fast loop which directly controls motor torque (shown as "robot controller" in the figure below); and an outer, slower loop which accepts commands from users (shown as "your controller").

   Figure: Schematic diagram of frames and points in `ManipulationStation`. Frames are drawn in 2D for simplicity.

As a result of this decomposition, direct control of motor torque is usually not available through industrial robotic arms' API. As an example, the kuka iiwa robots used in lab sessions only accept a `iiwa_position_command`($q_{ref}$) and a `iiwa_feedforward_torque_command` ($\tau_{ff}$). The robot controller takes command from the user controller and computes the motor torque $\tau_m$ using
\begin{align}
\tau_{m} &= K_q(q_{ref} - q) - D_q\dot{q}  + g(q) + \tau_{\text{ff}}
\end{align}
where $K_q$ and $D_q$ are diagonal gain matrices. This inner loop controller gives the following closed-loop dynamics:
\begin{align}
M(q) \ddot{q} + \left(C(q, \dot{q}) + D_q\right)\dot{q} + K\left( q - q_{ref}\right) = \tau_{external} + \tau_{\text{ff}}
\end{align}

## 3. Tasks
Previously you have done various tasks by repeatedly solving inverse kinematics. This time around, we'll be comparing IK with Jacobian-based controllers . Specifically, you'll be asked to open the left door of the cupboard in `ManipulationStation` by mapping velocities or Cartesian forces into joint torques with the help of Jacobians. As usual, there is a **Youtube video** that shows what needs to be done. Click on the figure below to watch it.

   Video: opening the left door with impedance control.

## 4. Geometry of [`ManipulationStation`](https://drake.mit.edu/doxygen_cxx/classdrake_1_1examples_1_1manipulation__station_1_1_manipulation_station.html)

   Figure: Schematic diagram of frames and points in `ManipulationStation`. Frames are drawn in 2D for simplicity.

- W: world frame.
- L: frame fixed to the cupboard left door.
- C_left_hinge: the center of the hinge relative to frame L.
- C_handle: the center of the door handle relative to frame L.
- L7: frame fixed to link 7 (last link) of the robot.
- EE: frame fixed to the end effector of the robot, which is also fixed w.r.t. L7.
- Q: a point fixed in frame EE (and L7).

## 5. Geometric Jacobian
In [Drake's notation convention](https://drake.mit.edu/doxygen_cxx/group__multibody__notation__basics.html), geometric Jacobians are denoted by $J_v$. They map the robot's joint velocity $\dot{q}$ to the angular velocity of a frame, and the Cartesian velocity of a point fixed to that frame. The tasks in this notebook are centered around using geometric Jacobians to map quantities between joint and Cartesian spaces.

To open the door, the robot interacts with the door handle at a point Q fixed in frame L7. As $J_v$ is by definition a mapping from joint to Cartesian velocities, the following equality holds:
\begin{equation}
\begin{pmatrix}
[^W\omega^{L7}]_W\\
[^W v_Q]_W
\end{pmatrix}
\quad =\quad
[^W{J_v}^{L7}_Q]_W
\quad
\dot{q}
\end{equation}
- $[^W\omega^{L7}]_W$ is the angular velocity of frame L7 relative to frame W, expressed in frame W.
- $[^W v_Q]_W$ is the linear velocity of point Q (which is fixed in L7), expressed in frame W.
- $[^W{J_v}^{L7}_Q]_W \in \mathbb{R}^{6 \times 7}$ (written as `Jv_WL7q` in code) is the geometric Jacobian of point Q which is fixed in frame L7, relative to frame W and expressed in frame W.

In addition, the transpose of geometric Jacobians maps Cartesian forces/torques back to joint torque:
\begin{align}
[F^{L7}]_W
\quad &=\quad
\begin{pmatrix}
[\tau^{L7}]_W\\
[f^Q]_W
\end{pmatrix}\\
\tau
\quad &=\quad
{[^W{J_v}^{L7}_Q]_W}^{\intercal}
\quad
[F^{L7}]_W
\end{align}
- $[F^{L7}]_{W} \in \mathbb{R}^6$ is the [spatial force](https://drake.mit.edu/doxygen_cxx/group__multibody__quantities.html) on body(frame) L7.
- $[m^{L7}]_W \in \mathbb{R}^3$ is the moment on body L7.
- $[f^Q]_W \in \mathbb{R}^3$ is the force at point Q, which is fixed to body L7.

## Setup

```python
# Import stuff to be used in this notebook. Run this first!
%reload_ext autoreload
%matplotlib notebook
%autoreload 2

import matplotlib.pyplot as plt
from matplotlib import cm
import numpy as np
import meshcat
import meshcat.geometry as g
from manip_station_sim.plan_utils import PlotJointLog, PlotIiwaPositionLog

from pydrake.math import RollPitchYaw, RotationMatrix
from pydrake.trajectories import PiecewisePolynomial

# duration of the door-opening trajectory/plan.
duration_arc = 10
```

```python
# First start meshcat for visualization - this only has to be run once.

# If you interrupt the kernel of this notebook, you'll need to run this cell again to
# restart the meshcat server, and then refresh the visualization window.

# This will open a mesh-cat server in the background,
# click on the url to display visualization in a separate window.
vis = meshcat.Visualizer()
```

    You can open the visualizer by visiting the following URL:
    http://127.0.0.1:7000/static/

```python
def make_meshcat_color_array(N, r, g, b):
    '''
    Construct a color array to visualize a point cloud in meshcat

    Args:
        N: int. number of points to generate. Must be >= number of points in the
            point cloud to color
        r: float. The red value of the points, 0.0 <= r <= 1.0
        g: float. The green value of the points, 0.0 <= g <= 1.0
        b: float. The blue value of the points, 0.0 <= b <= 1.0

    Returns:
        3xN numpy array of the same color
    '''
    color = np.zeros((3, N))
    color[0, :] = r
    color[1, :] = g
    color[2, :] = b

    return color
```

```python
box_model = np.load("resources/point_cloud.npy")

M = box_model.shape[0]
red = make_meshcat_color_array(M, 0.5, 0, 0)
model_meshcat = g.PointCloud(box_model.T, red)
vis['model'].set_object(model_meshcat)
```

```python
# Run this if you feel the visualizer is messed up and want to start over.
vis.delete()
```

## Task 1. Arc tracking and door opening by IK
Perhaps the most straightforward solution to open the door is to solve IK at a few knot points along the arc of the door handle and interpolate between the solutions using a cubic polynomial.

**Your task** is to complete `InverseKinPointwise` in `./manip_station_sim/ik_utils.py`. This function is very similar to `MoveEndEffectorAlongStraightLine` in Pset 3a. The difference is that `InverseKinPointwise` supports interpolating along user-defined trajectories, including arcs. The functions which compute reference position for Q and reference orientation for L7 have been provided for you in `open_left_door.py`.

You can simulate with the doors open or closed by toggling the initial condition for door angles.

```python
from manip_station_sim.manipulation_station_simulator import ManipulationStationSimulator
from manip_station_sim.robot_plans import JointSpacePlan
from pydrake.trajectories import PiecewisePolynomial
from open_left_door import InterpolateArc, InterpolateYawAngle, p_L7Q, q_home, q0
from ik_utils import InverseKinPointwise
from pydrake.math import RigidTransform

X_WO = RigidTransform()
X_WO.set_translation([0.49, 0, 0.1])

rpy = RollPitchYaw([np.pi/2,0,0])
X_WO.set_rotation(rpy)

# Generate plan list
# follow arc
qtraj_open_door, q_knots = InverseKinPointwise(
                    InterpolatePosition=InterpolateArc,
                    InterpolateOrientation=InterpolateYawAngle,
                    p_L7Q=p_L7Q,
                    duration=duration_arc,
                    num_knot_points=10,
                    q_initial_guess=q_home,
                    position_tolerance=0.001)

# close gripper
q_knots_close_gripper = np.zeros((2, 7))
q_knots_close_gripper[0] = q_knots[0]
qtraj_close_gripper = PiecewisePolynomial.ZeroOrderHold(
                            [0, 1], q_knots_close_gripper.T)

plan_list = [JointSpacePlan(qtraj) for qtraj in [qtraj_close_gripper, qtraj_open_door]]
gripper_setpoint_list = [0.005, 0.005]

# Simulate with doors closed
# door_angles = np.array([-0.001, 0.001])

# Simulate with doors open
door_angle = np.pi/2 - 0.001
door_angles = np.array([-door_angle, door_angle])

# Construct simulator.
sim = ManipulationStationSimulator(X_WO)

# real_time_rate=0 means running the simulation as fast as possible.
# Setting real_time_rate 1 to run the simulation close to real time.
(iiwa_position_command_log, iiwa_position_measured_log,
    iiwa_external_torque_log, iiwa_velocity_estimated_log,
    plant_state_log, t_plan) = sim.RunSimulation(
    q_home, door_angles, plan_list, gripper_setpoint_list, extra_time=2.0, real_time_rate=0.0)
```

The cells below generate plots for tracking performance evaluation.

**(2 points) answer the following questions. Include your answers and relevant plots in a PDF file.**
1. **With the doors open**, generate tracking error plots for trajectories with 2, 5 and 10 knot points. How does the number of knot points affect tracking error?

2. Zoom in to the xy position plot of Q. Is the magnitude of tracking error consistent along the arc **with the doors open**? Why do you think the error is (in)consistent? What happens if the doors are closed?

```python
# Compute position of Q from simulation logs.
from manip_station_sim.plan_utils import CalcQPositionInWorldFrame, CalcL7PoseInWolrdFrame
from open_left_door import p_L7Q, p_WC_left_hinge, CalcHandleArcPoints, r_handle

# Get the section of the trajectory related to arc-tracking.
t_start = 11
t_end = 11 + duration_arc
n_start = int(t_start / 0.01)
n_end = int(t_end / 0.01) + 1

list_X_WL7, t = CalcL7PoseInWolrdFrame(iiwa_position_measured_log, n_start=n_start, n_end=n_end)
p_WQ = CalcQPositionInWorldFrame(list_X_WL7, p_L7Q)

# points on the arc
points_on_handle_arc = CalcHandleArcPoints()
```

```python
# Plot radial tracking error, which measures how much the trajectory of Q deviates from the door arc.
from open_left_door import CalcRadialTrackingError
r_error = CalcRadialTrackingError(p_WQ)

fig = plt.figure(dpi = 100)
plt.plot(t, r_error)
ax = plt.gca()
ax.grid(True)
ax.set_xlabel('time (s)')
ax.set_ylabel('r_tracking_error (m)')
plt.title("radial tracking error")
plt.show()
```

```python
# Plot orientation tracking error.
from open_left_door import CalcOrientationTrackingError
angle_error_list = CalcOrientationTrackingError(list_X_WL7, t, duration_arc)

fig = plt.figure(dpi=100)
plt.plot(t, angle_error_list)
ax = plt.gca()
ax.grid(True)
ax.set_xlabel('time (s)')
ax.set_ylabel('error angle (rad)')
plt.title("Orientation tracking error")
plt.show()
```

```python
# Plot position of Q in the xy-plane of world frame.
fig = plt.figure(figsize=(5, 5), dpi=120)
ax = plt.gca()
ax.plot(points_on_handle_arc[:, 0],
        points_on_handle_arc[:, 1],
        linewidth=2,
        label="handle arc")
ax.plot(p_WQ[:, 0],
        p_WQ[:, 1], linewidth=1.5,
        label="Q path")
ax.plot(p_WC_left_hinge[0], p_WC_left_hinge[1], "*",
           label="door hinge", markersize=20)

ax.set_aspect('equal', 'box')
ax.set_xlabel('x_world (m)')
ax.set_ylabel('y_world (m)')
ax.grid(True)
ax.set_title('position of Q in the xy-plane of world frame')
ax.legend()
plt.show()
```

## Task 2: Arc tracking and door opening by mapping velocities.
This controller uses the Jacobian to compute desired joint velocities:
\begin{align}
V_{\text{des}} &=
\begin{pmatrix}
K_{\omega} \theta \pmb{n}  \\
K_v e_{xyz}
\end{pmatrix}\\
\dot{q}_{des} &= {[^W{J_v}^{L7}_Q]_W}^{+} V_{\text{des}} \\
q_{ref} &= q_{current} + \dot{q}_{des} \Delta t \\
\tau_{\text{ff}} &= 0
\end{align}
- $e_{xyz} = [p_Q^W]_{W, \text{ref}} - [p_Q^W]_{W}$ is the position tracking error of point Q.
- $\theta$ and $\pmb{n}$ are the angle and axis of the orientation error $\mathbf{R}^{L7}_{L7_r}$. Computation of position and orientation errors have been implemented for you in `IiwaTaskSpacePlan.CalcPositionError` and `IiwaTaskSpacePlan.CalcOrientationError`.
- ${[^W{J_v}^{L7}_Q]_W}^{+}$ is the pseudo inverse of $[^W{J_v}^{L7}_Q]_W$
- $\Delta t$ is the controller time step, which is the `control_period` argument in the function you need to complete.
- $K_v$ and $K_\omega$ are 3x3 diagonal gain matrices. $K_v = \text{diag}([100, 100, 100])$ and $K_v = \text{diag}([50, 50, 50])$ have been working well in our tests.

Complete `calc_p_Q`, `calc_R_WL7_ref` and `IiwaTaskSpaceVelocityPlan.CalcPositionCommand` in `robot_plan.py` with the controller described above.

```python
from manip_station_sim.manipulation_station_simulator import ManipulationStationSimulator
from manip_station_sim.robot_plans import JointSpacePlan, IiwaTaskSpaceVelocityPlan
from ik_utils import SolveOneShotIk
from open_left_door import p_WC_handle, p_L7Q, q_home, theta0_hinge, calc_p_Q, calc_R_WL7_ref

# Generate plan list
R_WL7_0 = RollPitchYaw(0, np.pi/4*3, 0).ToRotationMatrix()
q_start = SolveOneShotIk(
            p_WQ_ref=p_WC_handle,
            R_WL7_ref=R_WL7_0,
            p_L7Q=p_L7Q,
            q_initial_guess=q_home,
            position_tolerance=0.0005)

# close gripper
q_knots = np.zeros((2, 7))
q_knots[0] = q_start
qtraj_close_gripper = PiecewisePolynomial.ZeroOrderHold([0, 1], q_knots.T)

# open door using task space plan
task_space_plan = IiwaTaskSpaceVelocityPlan(
    calc_p_Q=calc_p_Q,
    calc_R_WL7_ref=calc_R_WL7_ref,
    duration=duration_arc,
    p_L7Q=p_L7Q)

plan_list = [JointSpacePlan(qtraj_close_gripper), task_space_plan]
gripper_setpoint_list = [0.005, 0.005]

# Simulate with doors closed
# door_angles = np.array([-0.001, 0.001])

# Simulate with doors open
door_angle = np.pi/2 - 0.001
door_angles = np.array([-door_angle, door_angle])

# Construct simulator.
sim = ManipulationStationSimulator()

# real_time_rate=0 means running the simulation as fast as possible.
# Setting real_time_rate 1 to run the simulation close to real time.
(iiwa_position_command_log, iiwa_position_measured_log,
    iiwa_external_torque_log, iiwa_velocity_estimated_log,
    plant_state_log, t_plan) = sim.RunSimulation(
    q_home, door_angles, plan_list, gripper_setpoint_list, extra_time=2.0, real_time_rate=0.0)
```

**(2 points) answer the following questions. Include your answers and relevant plots in a PDF file.**
1. Generate tracking error plots **with the doors open**. How does the tracking error compare with the trajectory generated from IK? (Hint: tracking error should be smaller)

2. Zoom in to the xy position plot of Q. Is the magnitude of tracking error consistent along the arc **with the doors open**? Why do you think the error is (in)consistent? What happens if the doors are closed? (Hint: error along the arc should be smaller and more consistent).

```python
# Compute position of Q from simulation logs.
from manip_station_sim.plan_utils import CalcQPositionInWorldFrame, CalcL7PoseInWolrdFrame
from open_left_door import p_L7Q, p_WC_left_hinge, CalcHandleArcPoints, r_handle

# Get the section of the trajectory related to arc-tracking.
t_start = 11
t_end = 11 + duration_arc
n_start = int(t_start / 0.01)
n_end = int(t_end / 0.01) + 1

list_X_WL7, t = CalcL7PoseInWolrdFrame(iiwa_position_measured_log, n_start=n_start, n_end=n_end)
p_WQ = CalcQPositionInWorldFrame(list_X_WL7, p_L7Q)

# points on the arc
points_on_handle_arc = CalcHandleArcPoints()
```

```python
# Plot Radial tracking error.
from open_left_door import CalcRadialTrackingError
r_error = CalcRadialTrackingError(p_WQ)

fig = plt.figure(dpi = 100)
plt.plot(t, r_error)
ax = plt.gca()
ax.grid(True)
ax.set_xlabel('time (s)')
ax.set_ylabel('r_tracking_error (m)')
plt.title("radial tracking error")
plt.show()
```

```python
# Plot orientation tracking error.
from open_left_door import CalcOrientationTrackingError
angle_error_list = CalcOrientationTrackingError(list_X_WL7, t, duration_arc)

fig = plt.figure(dpi=100)
plt.plot(t, angle_error_list)
ax = plt.gca()
ax.grid(True)
ax.set_xlabel('time (s)')
ax.set_ylabel('error angle (rad)')
plt.title("Orientation tracking error")
plt.show()
```

```python
# Plot position of Q in the xy-plane of world frame.
fig = plt.figure(figsize=(5, 5), dpi=120)
ax = plt.gca()
ax.plot(points_on_handle_arc[:, 0],
        points_on_handle_arc[:, 1],
        linewidth=2,
        label="handle arc")
ax.plot(p_WQ[:, 0],
        p_WQ[:, 1], linewidth=1.5,
        label="Q path")
ax.plot(p_WC_left_hinge[0], p_WC_left_hinge[1], "*",
           label="door hinge", markersize=20)

ax.set_aspect('equal', 'box')
ax.set_xlabel('x_world (m)')
ax.set_ylabel('y_world (m)')
ax.grid(True)
ax.set_title('position of Q in the xy-plane of world frame')
ax.legend()
plt.show()
```

## Task 3: Arc tracking and door opening by mapping forces.
It is also possible to send feedforward torque command, and bypass the position command by sending the current position of the robot as position command:
\begin{align}
F &=
\begin{pmatrix}
K_{\omega} \theta \pmb{n}  \\
K_v e_{xyz}
\end{pmatrix}\\
\tau_{\text{ff}} &= {[^W{J_v}^{L7}_Q]_W}^{\intercal} F \\
q_{ref} &= q_{current}
\end{align}

Complete `IiwaTaskSpaceImpedancePlan.CalcTorqueCommand` in `robot_plan.py` with the controller described above.

```python
from manip_station_sim.manipulation_station_simulator import ManipulationStationSimulator
from manip_station_sim.robot_plans import JointSpacePlan, IiwaTaskSpaceImpedancePlan
from ik_utils import SolveOneShotIk
from open_left_door import p_WC_handle, p_L7Q, q_home, theta0_hinge, calc_p_Q, calc_R_WL7_ref

# Generate plan list
R_WL7_0 = RollPitchYaw(0, np.pi/4*3, 0).ToRotationMatrix()
q_start = SolveOneShotIk(
            p_WQ_ref=p_WC_handle,
            R_WL7_ref=R_WL7_0,
            p_L7Q=p_L7Q,
            q_initial_guess=q_home,
            position_tolerance=0.0005)

# close gripper
q_knots = np.zeros((2, 7))
q_knots[0] = q_start
qtraj_close_gripper = PiecewisePolynomial.ZeroOrderHold([0, 1], q_knots.T)

# open door using task space plan
task_space_plan = IiwaTaskSpaceImpedancePlan(
    calc_p_Q=calc_p_Q,
    calc_R_WL7_ref=calc_R_WL7_ref,
    duration=duration_arc,
    p_L7Q=p_L7Q)

plan_list = [JointSpacePlan(qtraj_close_gripper), task_space_plan]
gripper_setpoint_list = [0.005, 0.005]

# Simulate with doors closed
door_angles = np.array([-0.001, 0.001])

# Simulate with doors open
# door_angle = np.pi/2 - 0.001
# door_angles = np.array([-door_angle, door_angle])

# Construct simulator.
sim = ManipulationStationSimulator()

# real_time_rate=0 means running the simulation as fast as possible.
# Setting real_time_rate 1 to run the simulation close to real time.
(iiwa_position_command_log, iiwa_position_measured_log,
    iiwa_external_torque_log, iiwa_velocity_estimated_log,
    plant_state_log, t_plan) = sim.RunSimulation(
    q_home, door_angles, plan_list, gripper_setpoint_list, extra_time=2.0, real_time_rate=0.0)
```

**(2 points) answer the following questions. Include your answers and relevant plots in a PDF file.**
1. Generate tracking error plots **with the doors open**. How does the tracking error compare with the other approaches?

2. Generate tracking error plots **with the doors closed**. How does closing the door affect tracking error?

```python
# Compute position of Q from simulation logs.
from manip_station_sim.plan_utils import CalcQPositionInWorldFrame, CalcL7PoseInWolrdFrame
from open_left_door import p_L7Q, p_WC_left_hinge, CalcHandleArcPoints, r_handle

# Get the section of the trajectory related to arc-tracking.
t_start = 11
t_end = 11 + duration_arc
n_start = int(t_start / 0.01)
n_end = int(t_end / 0.01) + 1

list_X_WL7, t = CalcL7PoseInWolrdFrame(iiwa_position_measured_log, n_start=n_start, n_end=n_end)
p_WQ = CalcQPositionInWorldFrame(list_X_WL7, p_L7Q)

# points on the arc
points_on_handle_arc = CalcHandleArcPoints()
```

```python
# Plot Radial tracking error.
from open_left_door import CalcRadialTrackingError
r_error = CalcRadialTrackingError(p_WQ)

fig = plt.figure(dpi = 100)
plt.plot(t, r_error)
ax = plt.gca()
ax.grid(True)
ax.set_xlabel('time (s)')
ax.set_ylabel('r_tracking_error (m)')
plt.title("radial tracking error")
plt.show()
```

```python
# Plot orientation tracking error.
from open_left_door import CalcOrientationTrackingError
angle_error_list = CalcOrientationTrackingError(list_X_WL7, t, duration_arc)

fig = plt.figure(dpi=100)
plt.plot(t, angle_error_list)
ax = plt.gca()
ax.grid(True)
ax.set_xlabel('time (s)')
ax.set_ylabel('error angle (rad)')
plt.title("Orientation tracking error")
plt.show()
```

```python
# Plot position of Q in the xy-plane of world frame.
fig = plt.figure(figsize=(5, 5), dpi=120)
ax = plt.gca()
ax.plot(points_on_handle_arc[:, 0],
        points_on_handle_arc[:, 1],
        linewidth=2,
        label="handle arc")
ax.plot(p_WQ[:, 0],
        p_WQ[:, 1], linewidth=1.5,
        label="Q path")
ax.plot(p_WC_left_hinge[0], p_WC_left_hinge[1], "*",
           label="door hinge", markersize=20)

ax.set_aspect('equal', 'box')
ax.set_xlabel('x_world (m)')
ax.set_ylabel('y_world (m)')
ax.grid(True)
ax.set_title('position of Q in the xy-plane of world frame')
ax.legend()
plt.show()
```

## Utility functions

```python
# Plot joint velocity
PlotJointLog(iiwa_velocity_estimated_log, legend="qDt", y_label="rad/s")
```

```python
# Plot joint torque
PlotJointLog(iiwa_external_torque_log, legend="tau_ext", y_label="N*m")
```

```python
# Plot joint position logs.
PlotIiwaPositionLog(iiwa_position_command_log, iiwa_position_measured_log)
```

```python

```


# Stationarity of time-series stochastic process

Author: [Zeel B Patel](https://patel-zeel.github.io/)

Main resources used:
1. [Stochastic process charecteristics - MATLAB Help](https://in.mathworks.com/help/econ/stationary-stochastic-process.html)
1. [How to Check if Time Series Data is Stationary with Python - machinelearningmastery.com](https://machinelearningmastery.com/time-series-data-stationary-python/)
1. [Time Series Analysis in Python – A Comprehensive Guide with Examples](https://www.machinelearningplus.com/time-series/time-series-analysis-python/)

## What is a time-series?

Observations made at equally spaced time-stamps create a time-series

```python
import numpy as np
import matplotlib.pyplot as plt
from matplotlib import rc
from GPy.kern import RBF, Matern32, Matern52, Exponential, ExpQuad
```

```python
N = 20
l,h=-10,10
np.random.seed(0)
xs = np.linspace(l,h,N)
ys = np.sin(xs) + np.random.rand(N)
np.random.seed(0)
xns = np.sort(np.random.uniform(l,h,N))
yns = np.sin(xns) + np.random.rand(N)

rc('font', size=16)
# rc('text', usetex=True)
fig, ax = plt.subplots(1,2,sharey=True,sharex=True, figsize=(15,5))
ax[0].plot(xs, ys,'o-')
for i in range(N):
    ax[0].vlines(xs[i], -1, 2, alpha=0.4)
    ax[1].vlines(xns[i],-1, 2, alpha=0.4)
ax[1].plot(xns, yns, 'o-');
ax[0].set_xlabel('t');ax[1].set_xlabel('t');
ax[0].set_ylabel('Observations');

ax[0].set_title('Time-series')
ax[1].set_title('Not Time-series');
```

## What is a stochastic process?

A joint probability distribution for a collection of random variables

## Connection between time-series and stochastic process

A time-series can be treated as a realization (sample) of stochastic process.

| Stochastic process | Time-series |
| :-- | :-- |
| Variable | Time-stamp |
| Sample | Observations |

Example $\to$ A Time-series with 8 time-stamps is a sample taken from joint probability distribution of a stochastic process with 8 variables.

## Stationary time-series process
First moment ($\mathbb{E}$) and second moment ($V$) should be constant over time

For all $t$,
* $\mathbb{E}(y_t) = \mu $ Constant
* $V(y_t) = \sigma^2 $ Constant
* $Cov(y_t, y_{t+h}) = \gamma $ Constant

## Why stationarity is important?

* Autoregressive models for modeling and prediction works best with stationary time-series

## How to check for stationarity?

1. Visually
1. Global v/s local test
1. Statistical test (Augmented Dickey-Fuller test or Unit root test)

```python
import pandas as pd
from statsmodels.tsa.stattools import adfuller

fig, ax = plt.subplots(1,3,figsize=(15,5))
x = np.linspace(1,10,1000)
y1 = pd.read_csv('../data/international-airline-passengers.csv').values[:,1]
y2 = np.sin(x*5)+x-5
y3 = np.sin(x*5)
ax[0].plot(y1,'o-');
ax[1].plot(x,y2,'o-');
ax[2].plot(x,y3,'o-');
for each in ax[::-1]:
    each.set_xlabel('t')

ax[0].set_title(f'ADF Test p value= {np.round(adfuller(y1[:-1])[1],2)}')
ax[1].set_title(f'ADF Test p value= {np.round(adfuller(y2[:-1])[1],2)}')
ax[2].set_title(f'ADF Test p value= {np.round(adfuller(y3[:-1])[1],2)}')
each.set_ylabel('Y');
```

p-value close to $1$ means non-stationarity in adfuller test and vice versa.

## Linear time-series model
$$
X_t = \sum\limits_{i=1}^{p}\phi_iX_{t-i} + \epsilon_t
$$

### Characteristic equation and unit root

AutoRegressive(1) model
\begin{align}
X_t - \phi_1X_{t-1} &= a_t \\
(1-\phi_1B)X_t &= a_t \\
\text{Char. eq.}\\
1 - \phi_1Z &= 0
\end{align}

AutoRegressive(2) model
\begin{align}
X_t - \phi_1X_{t-1} - \phi_2X_{t-2}&= a_t \\
X_t- \phi_1 BX_t -\phi_2 B^2 X_t &=   a_t \\
X_t\left[ 1 - \phi_1 B - \phi_2 B^2  \right] &= a_t \\
\text{Char. eq.}\\
1 - \phi_1 Z - \phi_2 Z^2 &= 0
\end{align}

## Stationarity

if all roots are outside unit circle, we call the process as stationary

### How to convert non-stationary Time-series to stationary?

1. Remove known trends
1. Apply log transform
1. Take difference series

Below is an example of difference series.

```python
plt.plot(y1);
plt.title(f'adf test p value={np.round(adfuller(y1[:-1])[1], 2)}')
plt.xlabel('t');plt.ylabel('y');
plt.figure();
y11 = y1[:-1]
y11 = y11[1:] - y11[:-1]
plt.plot(y11);
plt.title(f'adf test p value={np.round(adfuller(y11)[1], 2)}');
plt.xlabel('t');plt.ylabel('y');
```