# vector quiver(cupid)
## plt.figure.axes.quiver([sx],[sy],[sz],[ex],[ey],[ez])
        sx[] = starting compoent of x cordinate
        sy[] = starting compoent of y cordinate
        sz[] = starting compoent of z cordinate
            sx0 = starting point of x[0]
            sy0 = starting point of y[0]
            sz0 = starting point of z[0]
        ex[] = end compoent of x cordinate
        ey[] = end compoent of y cordinate
        ez[] = end compoent of z cordinate
            ex0 = ending point of x[0]
            ey0 = ending point of y[0]
            ez0 = ending point of z[0]

```python
import matplotlib as mpl
mpl.rcParams['mathtext.fontset']='stix'
mpl.rcParams['font.size']=9.0
mpl.rcParams['text.usetex']=True
#print(mpl.rcParams.keys())
import matplotlib.pyplot as plt
import numpy as np
fig = plt.figure()
ax  = fig.add_subplot(projection='3d')
x = np.linspace(1,5,50)
y = np.linspace(1,5,50)
x,y = np.meshgrid(x,y)
ax.plot_surface(x,y,x+y)
ax.axvline(x=0)
ax.axhline(y=0)
```

# Electric dipole
## potencial voltage:

### the Electric Force by two charge
위치 $\vec{r_{1}}$에 있는
    source chage $q_{1}$의 전기장에서
    위치$\vec{r_{2}}$에 있는 전하 $q_{2}$가 받는 힘

$$ \vec{F}\,(\,\vec{r_{2}}\,) =
    \frac{1}{4\pi\epsilon_{0}}\,
    \frac{q_{2}\,q_{1}}{|\vec{r_{2}} - \vec{r_{1}}|^2}\,
    \frac{\vec{r_{2}} - \vec{r_{1}}}{|\vec{r_{2}} - \vec{r_{1}}|}
$$
### the electric energy by a point charge
$$ \vec{E} = \frac{\vec{F}}{q_{2}}$$

$$ \vec{E}\,(\,\vec{r}\,) =
    \frac{1}{4\pi\epsilon_{0}}\,
    \frac{q_{1}}{|\vec{r} - \vec{r_{1}}|^2}\,
    \frac{\vec{r} - \vec{r_{1}}}{|\vec{r} - \vec{r_{1}}|}\\
$$
$$
\because \,
\vec{E}\,(\,\vec{r}\,) =
    \frac{1}{4\pi\epsilon_{0}}\,
    \frac{Q}{|\vec{r} - \vec{r_{1}}|^2}\,
    \frac{\vec{r} - \vec{r_{1}}}{|\vec{r} - \vec{r_{1}}|}
$$

### 전하가 연속으로 분포된 전기장 에서의 전위차(V)
$q_{i}=1,\cdots,n$

$\vec{r_{i}}= 1,\cdots,n$

$$ \vec{V}\,(\,\vec{r}\,)
    =
    \frac{1}{4\pi\epsilon_{0}}\, \sum_{i=1}^{n}\,
    \frac{q_i}{|\vec{r} - \vec{r_i}|}\,
    \frac{\vec r - \vec r_i}{|\vec r - \vec r_i|}\\
$$

#### 전하밀도 $\rho$
$$ \rho\,(\,\vec r\,) = \frac{\Delta q}{\Delta v} $$

#### $\vec {r'}$에 놓인 전하 $\Delta q$ 가 $\vec{r}$에 놓인 시험전하 q에 작용하는 전기력 $\vec F(\vec r)$

$$ \Delta \vec F(\vec r) = \frac{1}{4\pi\epsilon_0}\,\frac{q\,\Delta q}{|\vec r - \vec {r'}|^2}\,\frac{\vec r - \vec {r'}}{|\vec r - \vec {r'}|}
$$

#### 전기장 potencial energy
$\Delta \vec E(\vec r)$
$$ \Delta \vec E(\vec r) = \frac{\Delta \vec F(\vec r)}{q} \\
\because \,
\Delta \vec E(\vec r) = \frac{1}{4\pi\epsilon_0}\,\frac{\Delta q}{|\vec r - \vec {r'}|^2}\,\frac{\vec r - \vec {r'}}{|\vec r - \vec {r'}|}
$$
$\vec E(\vec r)$
$$
\vec E(\vec r)  = \sum \Delta E(\vec r) \\
\vec E(\vec r)  =
    \frac{1}{4\pi\epsilon_0}\,
    \sum \frac{\Delta q}{|\vec r - \vec {r'}|^2}\,
    \frac{\vec r - \vec {r'}}{|\vec r - \vec {r'}|}
$$
$$ \rho\,(\,\vec r\,) = \frac{\Delta q}{\Delta v} $$
$$
\vec E(\vec r) = \int d \vec{E} =
\frac{1}{4\pi\epsilon_0}\,\int_v \frac{dq}{|\vec r - \vec{r'}|^2}\,
\frac{\vec r - \vec{r'}}{|\vec r - \vec{r'}|} \\
=
\frac{1}{4\pi\epsilon_0}\,
\int_v \frac{\rho(\vec{r'})}{|\vec r - \vec{r'}|^2}\,
\frac{\vec r - \vec {r'}}{|\vec r - \vec {r'}|}\,dv
$$
### in the line charge

$$ V(\,\vec{r}\,) = \frac{1}{4\pi\epsilon_{0}}\, \int_{l}\, \frac{\rho_{l}\,dl'}{|\vec{r} - \vec{r'}|}$$

### in the surface charge
$$ V(\,\vec{r}\,) = \frac{1}{4\pi\epsilon_{0}}\, \int_{s}\, \frac{\rho_{s}\,ds'}{|\vec{r} - \vec{r'}|}$$

### in the volume charge
$$ V(\,\vec{r}\,) = \frac{1}{4\pi\epsilon_{0}}\, \int_{v}\, \frac{\rho_{v}\,dv'}{|\vec{r} - \vec{r'}|}$$
## potencial voltage:
### the Electric Potensial Energy at the p(a point)
$$ V(\,\vec{r}\,) = \frac{0}{4\pi\epsilon_{0}}\, \sum\, \frac{Q\vec{r'}}{|\vec{r} - \vec{r'}|} \\
V(\,\vec{r}\,) = \frac{1}{4\pi\epsilon_{0}}\, \sum_{k}\, \frac{Q_{k}}{|\vec{r} - \vec{r_{k}}|}
$$

```python
import matplotlib.pyplot as plt
import numpy as np
%matplotlib notebook
fig = plt.figure(figsize=(9,9))
ax = fig.add_subplot(projection='3d')
ax.set_xlim(xmin=-.1, xmax=7)
ax.set_ylim(ymin=-.1, ymax=7)
ax.set_zlim(zmin=-.1, zmax=7)
ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_zlabel("Z")

# Q1(0,1,3) Q2(2,0,2)
Qx = np.array([2,0,0])
Qy = np.array([0,2,0])
Qz = np.array([0,0,2])
O = np.zeros_like(Qx)
ax.plot([0],[0],[0],'ro')
ax.text(0.1,0.1,0.1,'O')
ax.quiver(O,O,O,Qx,Qy,Qz, arrow_length_ratio=0.1)

ax.text(Qx[0],Qy[0],Qz[0],'$Q_{1}$')
ax.text(Qx[1],Qy[1],Qz[1],'$Q_{2}$')
ax.text(Qx[2],Qy[2],Qz[2],'$Q_{i}$')

#ax.plot(np.hstack([Qx,[Qx[0]]]),np.hstack([Qy,[Qy[0]]]),np.hstack([Qz,[Qz[0]]]),'r:')
#ax.plot(np.hstack([Qx,[Qx[0]]]),np.hstack([Qy,[Qy[0]]]),np.hstack([Qz,[Qz[0]]]),'r:')

## q
q1 = 6
q = np.array([q1])
v_q = q/np.sqrt(q1**2 + q1**2)
qx = np.hstack([[Qx[0]],q])
qy = np.hstack([[Qy[0]],q])
qz = np.hstack([[Qz[0]],q])
ax.plot(np.hstack([[Qx[0]],q]),np.hstack([[Qy[0]],q]),np.hstack([[Qz[0]],q]),'r:')
ax.plot(np.hstack([[Qx[1]],q]),np.hstack([[Qy[1]],q]),np.hstack([[Qz[1]],q]),'r:')
ax.plot(np.hstack([[Qx[2]],q]),np.hstack([[Qy[2]],q]),np.hstack([[Qz[2]],q]),'r:')
ax.text(q[0],q[0],q[0],'q')
#ax.quiver([0],[0],[0],q,q,q, normalize=True)
ax.quiver([0],[0],[0],q,q,q, arrow_length_ratio=0.05)

plt.show()

```

    <IPython.core.display.Javascript object>

# Electric dipole
## potencial voltage:
### the Electric Potensial Energy at the p(a point)
$$ V(\,\vec{r}\,) = \frac{0}{4\pi\epsilon_{0}}\, \sum\, \frac{Q\vec{r'}}{|\vec{r} - \vec{r'}|} \\
V(\,\vec{r}\,) = \frac{1}{4\pi\epsilon_{0}}\, \sum_{k}\, \frac{Q_{k}}{|\vec{r} - \vec{r_{k}}|}
$$

### in the line charge
$$ V(\,\vec{r}\,) = \frac{1}{4\pi\epsilon_{0}}\, \int_{l}\, \frac{\rho_{l}\,dl'}{|\vec{r} - \vec{r'}|}$$

### in the surface charge
$$ V(\,\vec{r}\,) = \frac{1}{4\pi\epsilon_{0}}\, \int_{s}\, \frac{\rho_{s}\,ds'}{|\vec{r} - \vec{r'}|}$$

### in the volume charge
$$ V(\,\vec{r}\,) = \frac{1}{4\pi\epsilon_{0}}\, \int_{v}\, \frac{\rho_{v}\,dv'}{|\vec{r} - \vec{r'}|}$$
# https://www.numbercrunch.de/blog/2013/05/visualizing-vector-fields/
A vector field assigns to every point in space some vector. Vector fields are used in many branches of physics, e.g., to describe force fields, electromagnetic fields or velocity fields. Two-dimensional vector fields can be easily visualized using Python and the popular matplotlib package. As an example let us visualize the vector field
\begin{equation}
\vec E(\vec r) =
\begin{pmatrix}
E_x(x, y) \\ E_y(x, y)
\end{pmatrix} =
\begin{pmatrix}
-y\frac{\exp(-(x^2+y^2))}{\sqrt{x^2+y^2}} \\
x\frac{\exp(-(x^2+y^2))}{\sqrt{x^2+y^2}}
\end{pmatrix}\,.
\end{equation}Using the quiver function this is not difficult as the following example shows.

```python
#!/usr/bin/env python

# import useful modules
import matplotlib.pyplot as plt
import numpy as np

fig = plt.figure(figsize=(6, 4.5))
ax = fig.add_subplot()

# generate grid
x=np.linspace(-2, 2, 32)
y=np.linspace(-1.5, 1.5, 24)
x, y=np.meshgrid(x, y)
# calculate vector field
vx=-y/np.sqrt(x**2+y**2)*np.exp(-(x**2+y**2))
vy= x/np.sqrt(x**2+y**2)*np.exp(-(x**2+y**2))
# plot vecor field
ax.quiver(x, y, vx, vy, pivot='middle', headwidth=4, headlength=6)
ax.set_xlabel('$x$')
ax.set_ylabel('$y$')
plt.show()
```

# a pointer charge
## Electric Field ( Potencial Energy )
$$ \vec {E}(\vec{r})  = \frac{\vec{F}}{q} =
\frac{1}{4\pi\epsilon_{0}} \frac{q_{1}} {|\vec{r} - \vec{r_{1}}|^2}\,
\frac{\vec{r} - \vec{r_{1}}}{|\vec{r} - \vec{r_{1}}|}
= \frac{1}{4\pi\epsilon_{0}}\,
\frac{q_1\,(\vec{r} - \vec{r_1})}
{\sqrt{
        (r_{x} - r_{1x})^2 +
        (r_{y} - r_{1y})^2 +
        (r_{z} - r_{1z})^2
       }^3
}
$$

## vector components
$$
E_x =
\frac{1}{4\pi\epsilon_{0}}\,
\frac{q_1\,(\vec{r_x} - \vec{r_1x})}
{\sqrt{
        (r_{x} - r_{1x})^2 +
        (r_{y} - r_{1y})^2 +
        (r_{z} - r_{1z})^2
       }^3
}\\
E_y =
\frac{1}{4\pi\epsilon_{0}}\,
\frac{q_1\,(\vec{r_y} - \vec{r_1y})}
{\sqrt{
        (r_{x} - r_{1x})^2 +
        (r_{y} - r_{1y})^2 +
        (r_{z} - r_{1z})^2
       }^3
}\\
E_z =
\frac{1}{4\pi\epsilon_{0}}\,
\frac{q_1\,(\vec{r_z} - \vec{r_1z})}
{\sqrt{
        (r_{x} - r_{1x})^2 +
        (r_{y} - r_{1y})^2 +
        (r_{z} - r_{1z})^2
       }^3
}
$$

# dipole (쌍극자)
In a second example we will try to visualize the vector field of an electric dipole consisting of a positive charge $q$ at $\vec r_+$ and a negative charge $-q$ at $\vec r_-$. Its electromagnetic field is given by

\begin{equation}
\vec E(\vec r) = \frac{q}{4\pi\varepsilon_0}\left(
\frac{\vec r-\vec r_+}{|\vec r-\vec r_+|^3} – \frac{\vec r-\vec r_-}{|\vec r-\vec r_-|^3}
\right)
\end{equation}and we will plot its vector field in the $x$-$y$-plane. Similarly to our first example the Python code is given as follows.

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

fig = plt.figure()
ax = fig.add_subplot(projection='3d')

x = np.linspace(1,6,10)
y = np.linspace(1,6,10)
z = np.linspace(1,6,10)
x,y,z = np.meshgrid(x,y,z)
#r = np.zeros(3)
r=np.array([0,0,0])
q = 1

def E(q,r,x,y,z):
    return q*(x-r[0])/((x-r[0])**2 + (y-r[1])**2 + (z-r[2])**2)**(3/2),\
           q*(y-r[1])/((x-r[0])**2 + (y-r[1])**2 + (z-r[2])**2)**(3/2),\
           q*(z-r[2])/((x-r[0])**2 + (y-r[1])**2 + (z-r[2])**2)**(3/2)

vx,vy,vz = E(q,r,x,y,z)
ax.quiver(x,y,z,vx,vy,vz)
plt.show()

```

```python
#!/usr/bin/env python

# import useful modules
import matplotlib.pyplot as plt
import numpy as np

fig = plt.figure(figsize=(6, 4.5))
ax = fig.add_subplot()
# generate grid
x = np.linspace(-2, 2, 32)
y = np.linspace(-1.5, 1.5, 24)
x, y = np.meshgrid(x, y)

def E(q, a, x, y):
    return q*(x-a[0])/((x-a[0])**2+(y-a[1])**2)**(1.5), q*(y-a[1])/((x-a[0])**2+(y-a[1])**2)**(1.5)

# calculate vector field
Ex1, Ey1=E(1, [-1, 0], x, y)
Ex2, Ey2=E(-1, [1, 0], x, y)
Ex=Ex1+Ex2
Ey=Ey1+Ey2
# plot vecor field
ax.quiver(x, y, Ex, Ey, pivot='middle', headwidth=4, headlength=6)
ax.set_xlabel('$x$')
ax.set_ylabel('$y$')
plt.show()
```

```python

# import usefull modules
import matplotlib
from numpy import *
from pylab import *

# use LaTeX, choose nice some looking fonts and tweak some settings
matplotlib.rc('font', family='serif')
matplotlib.rc('font', size=16)
matplotlib.rc('legend', fontsize=16)
matplotlib.rc('legend', numpoints=1)
matplotlib.rc('legend', handlelength=1.5)
matplotlib.rc('legend', frameon=False)
matplotlib.rc('xtick.major', pad=7)
matplotlib.rc('xtick.minor', pad=7)
matplotlib.rc('text', usetex=True)
matplotlib.rc('text.latex',
              preamble=[r'\usepackage[T1]{fontenc}',
                        r'\usepackage{amsmath}',
                        r'\usepackage{txfonts}',
                        r'\usepackage{textcomp}'])

close('all')
figure(figsize=(6, 4.5))

# generate grid
x=linspace(-2, 2, 32)
y=linspace(-1.5, 1.5, 24)
x, y=meshgrid(x, y)

def E(q, a, x, y):
    return q*(x-a[0])/((x-a[0])**2+(y-a[1])**2)**(1.5), \
        q*(y-a[1])/((x-a[0])**2+(y-a[1])**2)**(1.5)

# calculate vector field
Ex1, Ey1=E(1, [-1, 0], x, y)
Ex2, Ey2=E(-1, [1, 0], x, y)
Ex=Ex1+Ex2
Ey=Ey1+Ey2
# plot normalized vecor field
quiver(x, y, Ex/sqrt(Ex**2+Ey**2), Ey/sqrt(Ex**2+Ey**2), pivot='middle', headwidth=4, headlength=6)
xlabel('$x$')
ylabel('$y$')
axis('image')
show()
```

The vector field of an electric dipole in the $x$-$y$-plane with $r_+=(-1,0,0)$ and $r_-=(1,0,0)$. All vectors normalized to unity. Thus, the plot visualizes the direction of the electric dipole field, but not the field strength.

# 4th
Normalizing the vector field to unity has the drawback of loosing information about the field strength. An alternative approach to get rid of extremely long vectors is to plot only those vectors whose lengths is below some given threshold $E_{\mathrm{max}}$. This can be archived by replacing vectors which are too long by some invalid value, that is by the value nan (not a number).

```python
#!/usr/bin/env python

# import useful modules
import matplotlib
from numpy import *
from pylab import *

# use LaTeX, choose nice some looking fonts and tweak some settings
matplotlib.rc('font', family='serif')
matplotlib.rc('font', size=16)
matplotlib.rc('legend', fontsize=16)
matplotlib.rc('legend', numpoints=1)
matplotlib.rc('legend', handlelength=1.5)
matplotlib.rc('legend', frameon=False)
matplotlib.rc('xtick.major', pad=7)
matplotlib.rc('xtick.minor', pad=7)
matplotlib.rc('text', usetex=True)
matplotlib.rc('text.latex',
              preamble=[r'\usepackage[T1]{fontenc}',
                        r'\usepackage{amsmath}',
                        r'\usepackage{txfonts}',
                        r'\usepackage{textcomp}'])

close('all')
figure(figsize=(6, 4.5))

# generate grid
x=linspace(-2, 2, 32)
y=linspace(-1.5, 1.5, 24)
x, y=meshgrid(x, y)

def E(q, a, x, y):
    return q*(x-a[0])/((x-a[0])**2+(y-a[1])**2)**(1.5), \
        q*(y-a[1])/((x-a[0])**2+(y-a[1])**2)**(1.5)

# calculate vector field
Ex1, Ey1=E(1, [-1, 0], x, y)
Ex2, Ey2=E(-1, [1, 0], x, y)
Ex=Ex1+Ex2
Ey=Ey1+Ey2
# remove vector with length larger than E_max
E_max=10
E=sqrt(Ex**2+Ey**2)
k=find(E.flat[:]>E_max)
Ex.flat[k]=nan
Ey.flat[k]=nan
# plot vecor field
quiver(x, y, Ex, Ey, pivot='middle', headwidth=4, headlength=6)
xlabel('$x$')
ylabel('$y$')
axis('image')
show()
```

The vector field of an electric dipole in the $x$-$y$-plane with $r_+=(-1,0,0)$ and $r_-=(1,0,0)$. All vectors longer than some threshold are removed from the plot. Thus, the plot visualizes both the direction and the strength of the electric dipole field while avoiding vectors of extreme length.

```python

```


```python
from IPython.display import HTML
tag = HTML('''
Toggle cell visibility <a href="javascript:code_toggle()">here</a>.''')
display(tag)
```

Toggle cell visibility here.

```python
# Erasmus+ ICCT project (2018-1-SI01-KA203-047081)

%matplotlib notebook
import numpy as np
import math
import matplotlib.pyplot as plt
from scipy import signal
import ipywidgets as widgets
import control as c
import sympy as sym
from IPython.display import Latex, display, Markdown # For displaying Markdown and LaTeX code
from fractions import Fraction
import matplotlib.patches as patches
```

## Sistemi del primo ordine senza zeri

### Introduzione

I sistemi del primo ordine senza zeri sono caratterizzati dalla seguente funzione di trasferimento:

\begin{equation}
    G(s)=\frac{k}{s+k}.
\end{equation}

Il valore $k$ è importante poiché definisce i seguenti parametri:
- $1/k$ indica la *costante di tempo* della risposta, che definisce il tempo necessario affinché la risposta al gradino raggiunga $\approx$ il 63% del suo valore finale.
- $t_r$ indica il *tempo di salita*, ovvero il tempo necessario affinché la risposta del sistema passi dal 10 \% al 90 \% del valore di regime.
- $t_s$ indica il *tempo di assestamento*, ovvero l'istante per cui la risposta del sistema entra all'interno della banda di errore (es.2 \% come impostato nell'esempio sotto) senza più uscirne.

La risposta al gradino di questi sistemi è data da:

\begin{equation}
    c(t)=1-e^{-at},
\end{equation}

dove la risposta forzata è uguale a $1$ e la risposta libera a $-e^{-at}$.

---

### Come usare questo notebook?

Sposta lo slider per definire il valore $k$ nella funzione di trasferimento del sistema del primo ordine $G(s)=\frac{k}{s+k}$ e osserva la risposta al gradino del sistema definito.

```python
# set up plot
fig, ax = plt.subplots(figsize=[9.8,4],num='Sistema del primo ordine')
ax.set_ylim([-1, 2])
ax.set_xlim([0, 5])
ax.grid(True)
ax.set_title ('Risposta')
ax.set_xlabel('$t$ [s]')
ax.set_ylabel('Input, output')
xaxis = ax.axhline(y=0,color='k',lw=1)

response, = ax.plot([], [])
slope, = ax.plot([], [])
x1a, = ax.plot([], [])
y1a, = ax.plot([], [])
tr11, = ax.plot([], [])
trv1, = ax.plot([], [])
trv2, = ax.plot([], [])
trh1, = ax.plot([], [])
trh2, = ax.plot([], [])
ts11, = ax.plot([], [])
ts1, = ax.plot([], [])
ts2, = ax.plot([], [])
texttr=ax.text(0,0,'')
textts=ax.text(0,0,'')

ax.step([0,5],[0,1],color='C0',label='input')

# generate x values
t = np.linspace(0, 2 * np.pi, 10000)

def response_func(t, k):
    """"Return response function"""
    return 1-np.exp(-k*t)

@widgets.interact(k=(1, 5, 1))

def update(k=1):
    """Remove old lines from plot and plot new one"""
    global response,slope,x1a,y1a,tr11,trv1,trv2,trh1,trh2,ts11,ts1,ts2,texttr,textts
    ax.lines.remove(response)
    ax.lines.remove(slope)
    ax.lines.remove(x1a)
    ax.lines.remove(y1a)
    ax.lines.remove(tr11)
    ax.lines.remove(trv1)
    ax.lines.remove(trv2)
    ax.lines.remove(trh1)
    ax.lines.remove(trh2)
    ax.lines.remove(ts11)
    ax.lines.remove(ts1)
    ax.lines.remove(ts2)
    texttr.remove()
    textts.remove()
    response, = ax.plot(t, response_func(t,k), color='C1',lw=2)
    response.set_label('output')
    slope, = ax.plot([0,1/k], [0,1], color='C2',lw=2)
    slope.set_label('pendenza iniziale')
    x1a, = ax.plot([1/k,1/k],[0,1-np.exp(-1)],'--',color='k',lw=.8)
    y1a, = ax.plot([0,1/k],[1-np.exp(-1),1-np.exp(-1)],'--',color='k',lw=.8)
# rise time
    tr11, = ax.plot([-np.log(0.9)/k,-np.log(0.1)/k],[-0.5,-0.5],color='k',lw=.8)
    trv1, = ax.plot([-np.log(0.9)/k,-np.log(0.9)/k],[-0.5,0.1],'--',color='k',lw=.8)
    trv2, = ax.plot([-np.log(0.1)/k,-np.log(0.1)/k],[-0.5,0.9],'--',color='k',lw=.8)
    trh1, = ax.plot([0,-np.log(0.9)/k],[0.1,0.1],'--',color='k',lw=.8)
    trh2, = ax.plot([0,-np.log(0.1)/k],[0.9,0.9],'--',color='k',lw=.8)
# settling time
    ts11, = ax.plot([0,-np.log(0.02)/k],[-0.7,-0.7],color='k',lw=.8)
    ts1, = ax.plot([0,0],[-0.7,0],'--',color='k',lw=.8)
    ts2, = ax.plot([-np.log(0.02)/k,-np.log(0.02)/k],[-0.7,0.98],'--',color='k',lw=.8)
    ax.legend()
    texttr=ax.text((-np.log(0.1)/k-(-np.log(0.9)/k))/2,-0.45, '$t_r$',fontsize=13)
    textts=ax.text((-np.log(0.02)/k)/2-0.1,-0.65, '$t_s$',fontsize=13)

    plt.xticks([0,1/k,2,4], [0,'${1}/{%s}$'%k,2,4],fontsize=8)
    plt.yticks([0.1,0.5,0.63,0.9,1,1.5,2], [0.1,0.5,0.63,0.9,1,1.5,2],fontsize=8)

    num1=[k]
    den1=[1,k]
    display(Markdown('La funzione di trasferimento del sistema $G(s)$ è uguale a:'))
    tf_sys1=c.TransferFunction(num1,den1)
    s=sym.Symbol('s')
    eq=(k/(s+k))
    display(eq)
```

    <IPython.core.display.Javascript object>

    interactive(children=(IntSlider(value=1, description='k', max=5, min=1), Output()), _dom_classes=('widget-inte…

## Sistemi del secondo ordine

### Introduzione

Differentemente dai sistemi del primo ordine presentati sopra, in cui il parametro $k$ influenza solo la velocità della risposta, il cambiamento dei parametri analoghi nei sistemi del secondo ordine possono influenzare la forma effettiva della risposta. In questi sistemi sono possibili le seguenti quattro risposte:
- risposta *sovrasmorzata*,
- risposta *sottosmorzata*,
- risposta *non smorzata* e
- risposta con *smorzamento critico*.

### Come usare questo notebook?

Sposta gli sliders per definire i valori di $a$ e $b$ nella funzione di trasferimento del sistema del secondo ordine dalla forma $G(s)=\frac{b}{s^2+as+b}$ e osserva la mappa poli-zeri e la risposta al gradino del sistema definito.

```python
# set up plot
fig1, ax1 = plt.subplots(1,2,figsize=[9.8,4],num='Sistema del secondo ordine')
ax1[0].set_ylim([-3.5, 3])
ax1[1].set_ylim([0, 2.5])
# ax1.set_xlim([0, 5])
ax1[0].grid(True)
ax1[1].grid(True)
ax1[0].axhline(y=0,color='k',lw=.8)
ax1[1].axhline(y=0,color='k',lw=.8)
ax1[0].axvline(x=0,color='k',lw=.8)
ax1[1].axvline(x=0,color='k',lw=.8)
ax1[0].set_xlabel('Re')
ax1[0].set_ylabel('Im')
ax1[1].set_xlabel('$t$ [s]')
ax1[1].set_ylabel('Input, output')
ax1[0].set_title('Mappa poli-zeri')
ax1[1].set_title('Risposta')

t = np.linspace(0, 20, 10000)

textGs = ax1[0].text(0,0,'')

ax1[1].step([0,20],[0,1],color='C0',label='input')

plotzero, = ax1[0].plot([], [])
response2, = ax1[1].plot([], [])

def response_func2(t, a, b):
    num_sys=np.array([b])
    den_sys=np.array([1,a,b])
    tf_sys=c.TransferFunction(num_sys,den_sys)
    poles_sys,zeros_sys=c.pzmap(tf_sys, Plot=False)
    T, yout = c.step_response(tf_sys,t)
    return T, yout, poles_sys, tf_sys

@widgets.interact(a=(0, 10, 1),b=(1,10,1))

def update(a=7,b=9):
    """ Update plots """
    global response2, plotzero, textGs
    ax1[0].lines.remove(plotzero)
    ax1[1].lines.remove(response2)
#     textGs.remove()
    T, yout, poles_sys, tf_sys = response_func2(t, a, b)
    plotzero, = ax1[0].plot(np.real(poles_sys), np.imag(poles_sys), 'xg', markersize=10, label = 'Poles')
#     textGs = ax1[0].text(-7,1,tf_sys)
    response2, = ax1[1].plot(T,yout,color='C1',label='output')
    s=sym.Symbol('s')
    eq=b/(s**2+a*s+b)
    coeff = [1,a,b]
    rootsdenom=np.roots(coeff)
    eq2=b/((s-rootsdenom[0])*(s-rootsdenom[1]))
    display(Markdown('La funzione di trasferimento del sistema $G(s)$ è uguale a:'))
    display(eq),display(Markdown('o')),display(eq2)

    if np.imag(poles_sys)[0] == 0 and np.imag(poles_sys)[1] == 0 and np.real(poles_sys)[0] < 0 and np.real(poles_sys)[1] < 0 and np.real(poles_sys)[0]!=np.real(poles_sys)[1]:
        display(Markdown('Il sistema è **sovrasmorzato** in quanto entrambi i poli hanno solamente la parte reale negativa.'))
    elif math.isclose(0, np.imag(poles_sys)[0], abs_tol=10**-6) and math.isclose(0, np.imag(poles_sys)[1], abs_tol=10**-6) and np.real(poles_sys)[1] < 0 and np.real(poles_sys)[0]==np.real(poles_sys)[1]:
        display(Markdown('Il sistema presenta **smorzamento critico** in quanto presenta un polo con molteplicità algebrica 2 con solamente la parte reale negativa.'))
    elif np.real(poles_sys)[0] == 0 and np.real(poles_sys)[1] == 0:
        display(Markdown('Il sistema è **non smorzato** in quanto i poli hanno solamente la parte immaginaria.'))
    elif np.imag(poles_sys)[0] != 0 and np.imag(poles_sys)[1] != 0 and np.real(poles_sys)[0] != 0 and np.real(poles_sys)[1] != 0:
        display(Markdown('Il sistema è **sottosmorzato**.'))
    ax1[0].legend()
    ax1[1].legend()
```

    <IPython.core.display.Javascript object>

    interactive(children=(IntSlider(value=7, description='a', max=10), IntSlider(value=9, description='b', max=10,…


# メモ

チャート式数学III , 極限のところを読む。

興味の対象は無限級数

# 極限 p.28

**数列の極限**

数列 $\quad \{a_n\} \quad (n=1, 2, \cdots \cdots)$ は無限数列とする

収束 $\quad  \displaystyle \lim_{n \rightarrow \infty} a_n = \alpha$ (極限値)

発散 $\quad  \displaystyle \lim_{n \rightarrow \infty} a_n = \infty$ 極限がある

発散 $\quad  \displaystyle \lim_{n \rightarrow \infty} a_n = - \infty$ 極限がある

発散 $\quad$ 数列は振動する $\quad$ 極限がない

$\pm \infty$ は極限値とは言わない

**数列の極限の性質**

$\{a_n\}, \{b_n\}$ が収束して $\displaystyle \lim_{n \rightarrow \infty} a_n = \alpha, \lim_{n \rightarrow \infty} b_n = \beta$ のとき

定数倍 $\quad \displaystyle \lim_{n \rightarrow \infty} ka_n = k \alpha \quad$ ただし $k$ は定数

和  $\quad \displaystyle \lim_{n \rightarrow \infty} (a_n+b_n) = \alpha + \beta$

差  $\quad \displaystyle \lim_{n \rightarrow \infty} (a_n-b_n) = \alpha - \beta$

$\displaystyle \lim_{n \rightarrow \infty} (ka_n+lb_n) = k \alpha+l\beta$ ただし $k,l$ は定数

積  $\quad \displaystyle \lim_{n \rightarrow \infty} a_n b_n = \alpha  \beta$

商  $\quad \displaystyle \lim_{n \rightarrow \infty} \frac{a_n}{b_n} = \frac{\alpha}{\beta} \quad$  ただし $\beta \neq 0$

$a_n \leq b_n \quad (n=1,2,3,\cdots \cdots)$ で $\displaystyle \lim_{n \rightarrow \infty} a_n = \infty$ ならば  $\displaystyle \lim_{n \rightarrow \infty} b_n = \infty$ である

```python
# 基本例題 14 p.30
# (1)
from sympy.abc import *
from sympy import*
init_printing()
display([1/Rational(n+1)**2 for n in range(5)])
```

```python
# 基本例題 14 p.30
# (1)
from sympy.abc import *
from sympy import*
from fractions import Fraction
init_printing()
display([1/Fraction(n+1)**2 for n in range(5)])
```

    [Fraction(1, 1),
     Fraction(1, 4),
     Fraction(1, 9),
     Fraction(1, 16),
     Fraction(1, 25)]

```python
from sympy.abc import *
limit(1/x**2, x, oo) #=> 0 よって 0 に収束
```

$\displaystyle 0$

メモ

`[]` の中に expression for i in interator [optional if] を書くのが python の内包表記 list comprehension。

分数は fractions.Fraction を使うか、sympy.Rational を使う。

`/` が割り算を float にしようとするのでそれを阻止しなければならないので変なところに Fraction や Rational を入れる必要がある。

```python
# 基本例題 14 p.30
# (2), (3)
from sympy import *
display([200-n*5 for n in range(5)]) #=> -oo に発散
display([3**(n%2) for n in range(5)]) #=> 振動
```

    [200, 195, 190, 185, 180]

    [1, 3, 1, 3, 1]

```python
# 基本例題 14 p.30
# (4)
from sympy.abc import *
from sympy import *
init_printing()
display([(7+3*n)/Rational(n+1)**2 for n in range(5)])
print()
limit((7*3*n)/(n+1)**2, n, oo) #=> 0 に収束
```

```python
# (5)
from sympy import *
init_printing()
display([(4*n+3)/(Rational(n+1)*2) for n in range(5)])
display([sqrt((4*n+3)/(Rational(n+1)*2)) for n in range(5)])
limit(sqrt((4*n+3)/((n+1)*2)), n, oo) #=> sqrt(2) に収束
```

メモ
上のセルで 分数の sqrt をうまく表示できなかった。

たとえば最初の項は

$\quad \quad \displaystyle \sqrt{\frac{3}{2}}$

となって欲しい。

```python
# 練習 p.30
# (1)
from sympy import *
init_printing()
display([n*2+1 for n in range(5)])
display([sqrt(n*2+1) for n in range(5)])
limit(sqrt(n*2+1), n, oo) #=> oo に発散
```

```python
# 練習 p.30
# (2)
from sympy import *
init_printing()
display([n+1 for n in range(5)])
display([n+2 for n in range(5)])
display([sqrt((n+1)/Rational(n+2))for n in range(5)])
limit(sqrt((n+1)/(n+2)),n,oo) #=> 1 に収束
```

```python
# 練習 p.30
# (3)
from sympy import *
init_printing()
display([n*2+1 for n in range(5)])
display([(-1)**n for n in range(5)])
display([(n*2+1)*(-1)**n for n in range(5)]) #=> 発散、振動
```

```python
# 練習 p.30
# (4)
from sympy import *
display([(n+1)**3 for n in range(5)])
display([(-1)**n for n in range(5)])
display([((n+1)**3)*(-1)**n for n in range(5)]) #=> 発散、振動
```

    [1, 8, 27, 64, 125]

    [1, -1, 1, -1, 1]

    [1, -8, 27, -64, 125]

```python
# 練習 p.30
# (5)
# [2/3, -1/2, -8/9, -13/12]
# [2/3, -3/6, -8/9, -13/12]
from sympy.abc import *
from sympy import*
init_printing()
display([2-n*5 for n in range(5)])
display([n*3+3 for n in range(5)])
display([(2-n*5)/Rational(n*3+3 ) for n in range(5)])
limit((2-n*5)/(n*3+3),n,oo) #=> -5/3に収束
```

```python
# 基本例題 15 p.31
from sympy.abc import *
from sympy import*
init_printing()
# (1)
display(limit(3*n-n**3,n,oo)) #=> -oo
# (2)
display(limit((2*n**2-3)/(n**2+1),n,oo)) #=> 2 に収束
```

```python
# 基本例題 15 p.31
from sympy.abc import *
from sympy import*
init_printing()
# (3)
expr=sqrt(n**3+1)/(sqrt(n**2+1)+sqrt(n))
display(expr)
limit(expr, n, oo) #=> oo

```

```python
(4)
expr=sqrt(n+2)-sqrt(n)
display(expr)
limit(expr, n, oo) #=> oo
```

```python
(5)
display(log(x,2))
display(x**(1/n))
expr=log(3**(1/n),2)
display(expr)
limit(expr, n, oo) #=> 0

```

```python
(6)
expr=cos(n*pi)
display(expr)
display(limit(expr, n, oo)) #=> <-1,1>
display(type(limit(expr, n, oo)))

```

```python
# 練習 p.31
(1)
expr=(3*n-4)/(n+1)
display(expr)
display(limit(expr, n, oo)) #=> 3
```

```python
(2)
expr=(3*n**2+n+1)/(n+1)-3*n
display(expr)
display(factor(expr))
display(limit(expr, n, oo)) #=> -2
```

```python
(3)
expr=sqrt(n**2+1)-n
display(expr)
display(limit(expr, n, oo)) #=> 0
```

```python
(4)
expr=4*n/(sqrt(n**2+2*n)+n)
display(expr)
display(limit(expr, n, oo)) #=> 2
```

```python
(5)
expr=sin(n*pi/2)
display(expr)
display(limit(expr, n, oo)) #=> <-1,1>
```

```python
expr=tan(n*pi)
display(expr)
display(limit(expr, n, oo)) #=> <-oo,oo>
```

基本例題 16 p.32

(1) 次の極限を求める

$
\lim \frac{3+7+11+\cdots\cdots+(4n-1)}{3+5+7+\cdots\cdots+(2n+1)}
$

```python
# 基本例題 16 p.32
# (1) 次の極限を求める
# \lim \frac{3+7+11+\cdots\cdots+(4n-1)}{3+5+7+\cdots\cdots+(2n+1)}
from sympy.abc import *
from sympy import *
A=Sum(4*n-1,(n,1,x))
display(A)
A=summation(4*n-1,(n,1,x))
display(A)
B=Sum(2*n+1,(n,1,x))
display(B)
B=summation(2*n+1,(n,1,x))
display(B)
display(A/B)
display(limit(A/B,x,oo))
```

$\displaystyle \sum_{n=1}^{x} \left(4 n - 1\right)$

$\displaystyle 2 x^{2} + x$

$\displaystyle \sum_{n=1}^{x} \left(2 n + 1\right)$

$\displaystyle x^{2} + 2 x$

$\displaystyle \frac{2 x^{2} + x}{x^{2} + 2 x}$

$\displaystyle 2$

```python
# 基本例題 16 p.32
# (2) 実数 k に対し k > 0 のとき数列 {n^k} の極限を求める
from sympy.abc import *
from sympy import *
expr = n**k
display(expr)
# display(limit(expr, n, oo)) #=> error
# k>0 なのだから k=0.5, 1, 2 でやってみる
expr= n**0.5
display(limit(expr, n, oo)) #=> oo
expr= n**1
display(limit(expr, n, oo)) #=> oo
expr= n**2
display(limit(expr, n, oo)) #=> oo
# kが無理数のとき、0とkの間の適当な有理数が存在するので、証明終わり
```

$\displaystyle n^{k}$

$\displaystyle \infty$

$\displaystyle \infty$

$\displaystyle \infty$

```python
# 練習 p.32 (1)
from sympy.abc import *
from sympy import *
expr=(2*n)**2
A=Sum(expr,(n,1,x))
display(A)
A=summation(expr,(n,1,x))
A=simplify(A)
display(A)
expr=n**2
B=Sum(expr,(n,1,x))
display(B)
B=simplify(summation(expr,(n,1,x)))
display(B)
display(A/B)
display(limit(A/B,x,oo))
```

$\displaystyle \sum_{n=1}^{x} 4 n^{2}$

$\displaystyle \frac{2 x \left(2 x^{2} + 3 x + 1\right)}{3}$

$\displaystyle \sum_{n=1}^{x} n^{2}$

$\displaystyle \frac{x \left(2 x^{2} + 3 x + 1\right)}{6}$

$\displaystyle 4$

$\displaystyle 4$

```python
# 練習 p.32 (2)
# 実数 k に対し k < 0 のとき 数列 {n^k} の極限は 0 であることを示す。
# k > 0 のとき無限大 oo であることを利用してもよい
# 解答
# k < 0 のとき k = -k とおくと、 {n^k} の極限は {1/(n^k)} の極限になる
# これで分母が 無限大 oo なので証明終わり
```

# いまここ

**極限値の大小関係**


[目录](./table_of_contents.ipynb)

# 扩展卡尔曼滤波器

```python
%matplotlib inline
```

```python
#format the book
import book_format
book_format.set_style()
```

<style>
.output_wrapper, .output {
    height:auto !important;
    max-height:100000px;
}
.output_scroll {
    box-shadow:none !important;
    webkit-box-shadow:none !important;
}
</style>

我们发展了线性卡尔曼滤波器的理论。然后，在最后两章，我们提出了使用卡尔曼滤波器的非线性问题的主题。本章我们将学习扩展卡尔曼滤波器(EKF)。EKF通过线性化当前估计点的系统来处理非线性，然后使用线性卡尔曼滤波器对线性化的系统进行滤波。它是用于非线性问题的最早的技术之一，并且仍然是最常用的技术。

EKF为滤波器的设计提供了重要的数学挑战;这是书中最具挑战性的一章。我尽我所能地避免使用EKF，而采用其他已经开发出来的用于过滤非线性问题的技术。然而，这个话题是不可避免的;所有经典论文和该领域的大多数当前论文都使用EKF。即使你在自己的工作中没有使用EKF，你也需要熟悉这个主题，以便能够阅读相关文献。

## 线性化卡尔曼滤波器

卡尔曼滤波器使用线性方程，所以它对非线性问题不起作用。问题的非线性表现在两方面。首先，流程模型可能是非线性的。物体在大气中下落时遇到阻力，使其加速度减小。阻力系数根据物体的速度而变化。由此产生的行为是非线性的——它不能用线性方程来建模。第二，测量可能是非线性的。例如，雷达给出目标的距离和方位。我们使用非线性的三角函数来计算目标的位置。

对于线性滤波器，我们有这些过程和测量模型的方程:

$$\begin{aligned}\dot{\mathbf x} &= \mathbf{Ax} + w_x\\
\mathbf z &= \mathbf{Hx} + w_z
\end{aligned}$$

其中$\mathbf A$为系统动态矩阵。使用在**卡尔曼滤波数学**一章中涉及的状态空间方法，这些方程可以被转换成
$$\begin{aligned}\bar{\mathbf x} &= \mathbf{Fx} \\
\mathbf z &= \mathbf{Hx}
\end{aligned}$$

其中$\mathbf F$是基本矩阵。噪声项$w_x$和$w_z$被合并到矩阵$\mathbf R$和$\mathbf Q$中。这种形式的方程允许我们计算步骤$k$的状态给定步骤$k$的测量和步骤$k-1$的状态估计。在前面的章节中，我通过使用牛顿方程描述的问题，建立了你的直觉并最小化了数学。我们知道如何根据高中物理设计$\mathbf F$。

对于非线性模型，将线性表达式$\mathbf{Fx} + \mathbf{Bu}$替换为非线性函数$f(\mathbf x， \mathbf u)$，将线性表达式$\mathbf{Hx}$替换为非线性函数$h(\mathbf x)$:

$$\begin{aligned}\dot{\mathbf x} &= f(\mathbf x, \mathbf u) + w_x\\
\mathbf z &= h(\mathbf x) + w_z
\end{aligned}$$

你可以想象我们可以通过寻找一组新的卡尔曼滤波方程来最优地解决这些方程。但如果你还记得**非线性滤波**一章中的图表，你就会回想起，将高斯函数传递给非线性函数会导致概率分布不再是高斯分布。所以这行不通。

EKF不改变卡尔曼滤波器的线性方程。相反，它将当前估计点的非线性方程*线性化*，并在线性卡尔曼滤波器中使用这种线性化。

*线性化*的意思是它听起来像什么。我们找到一条与曲线在某一点上最接近的直线。下图线性化了$x=1.5$时的抛物线$f(x)=x^2-2x$。

```python
import kf_book.ekf_internal as ekf_internal
ekf_internal.show_linearization()
```

如果上面的曲线是过程模型，那么虚线显示了估计$x=1.5$时该曲线的线性化。

我们通过求导来线性化系统，求出曲线的斜率:

$$\begin{aligned}
f(x) &= x^2 -2x \\
\frac{df}{dx} &= 2x - 2
\end{aligned}$$

然后将其计算为$x$:

$$\begin{aligned}m &= f'(x=1.5) \\&= 2(1.5) - 2 \\&= 1\end{aligned}$$

微分方程组的线性化也是类似的。我们线性化$f(\mathbf x, \mathbf u)$和$h(\mathbf x)$，方法是对 $\mathbf F$和$\mathbf H$在$\mathbf x_t$和$\mathbf u_t$处求偏导数。我们称矩阵的偏导数为[*雅可比矩阵*](https://en.wikipedia.org/wiki/Jacobian_matrix_and_determinant)。这给了我们离散状态转移矩阵和测量模型矩阵:

$$
\begin{aligned}
\mathbf F
&= {\frac{\partial{f(\mathbf x_t, \mathbf u_t)}}{\partial{\mathbf x}}}\biggr|_{{\mathbf x_t},{\mathbf u_t}} \\
\mathbf H &= \frac{\partial{h(\bar{\mathbf x}_t)}}{\partial{\bar{\mathbf x}}}\biggr|_{\bar{\mathbf x}_t}
\end{aligned}
$$

这就引出了下面的EKF方程。我把与线性过滤器的区别放在框框里:
$$\begin{array}{l|l}
\text{linear Kalman filter} & \text{EKF} \\
\hline
& \boxed{\mathbf F = {\frac{\partial{f(\mathbf x_t, \mathbf u_t)}}{\partial{\mathbf x}}}\biggr|_{{\mathbf x_t},{\mathbf u_t}}} \\
\mathbf{\bar x} = \mathbf{Fx} + \mathbf{Bu} & \boxed{\mathbf{\bar x} = f(\mathbf x, \mathbf u)}  \\
\mathbf{\bar P} = \mathbf{FPF}^\mathsf{T}+\mathbf Q  & \mathbf{\bar P} = \mathbf{FPF}^\mathsf{T}+\mathbf Q \\
\hline
& \boxed{\mathbf H = \frac{\partial{h(\bar{\mathbf x}_t)}}{\partial{\bar{\mathbf x}}}\biggr|_{\bar{\mathbf x}_t}} \\
\textbf{y} = \mathbf z - \mathbf{H \bar{x}} & \textbf{y} = \mathbf z - \boxed{h(\bar{x})}\\
\mathbf{K} = \mathbf{\bar{P}H}^\mathsf{T} (\mathbf{H\bar{P}H}^\mathsf{T} + \mathbf R)^{-1} & \mathbf{K} = \mathbf{\bar{P}H}^\mathsf{T} (\mathbf{H\bar{P}H}^\mathsf{T} + \mathbf R)^{-1} \\
\mathbf x=\mathbf{\bar{x}} +\mathbf{K\textbf{y}} & \mathbf x=\mathbf{\bar{x}} +\mathbf{K\textbf{y}} \\
\mathbf P= (\mathbf{I}-\mathbf{KH})\mathbf{\bar{P}} & \mathbf P= (\mathbf{I}-\mathbf{KH})\mathbf{\bar{P}}
\end{array}$$

我们通常不使用$\mathbf{Fx}$来传播EKF的状态，因为线性化会导致不准确。通常使用合适的数值积分技术如欧拉或龙格库塔计算$\bar{\mathbf x}$。因此，我写$ mathbf{\bar x} = f(\mathbf x， \mathbf u)$。出于同样的原因，我们在计算残差时不使用$\mathbf{H\bar{x} $，而选择更精确的$ H (\bar{\mathbf x})$。

我认为理解EKF最简单的方法是从一个例子开始。稍后你可能会想回来重读这一节。

## 示例:追踪一架飞机

这个例子使用地面雷达跟踪一架飞机。我们在上一章为这个问题实现了一个UKF。现在我们将为相同的问题实现一个EKF，这样我们就可以比较滤波器性能和实现滤波器所需的工作水平。

雷达的工作原理是发射一束无线电波并扫描回波。光束路径上的任何东西都会将部分信号反射回雷达。通过计时反射信号返回雷达所需的时间，系统可以计算出*倾斜距离*——从雷达装置到物体的直线距离。

雷达的倾斜距离$r$、仰角$\epsilon$与飞机的水平位置$x$、高度$y$的关系如下图所示:

```python
ekf_internal.show_radar_chart()
```

这就得到了等式:

$$\begin{aligned}
\epsilon &= \tan^{-1} \frac y x\\
r^2 &= x^2 + y^2
\end{aligned}$$

### 设计状态变量

我们想要跟踪飞机的位置假设匀速和高度，并测量到飞机的倾斜距离。这意味着我们需要3个状态变量——水平距离、水平速度和高度:

$$\mathbf x = \begin{bmatrix}\mathtt{distance} \\\mathtt{velocity}\\ \mathtt{altitude}\end{bmatrix}=    \begin{bmatrix}x \\ \dot x\\ y\end{bmatrix}$$

### 设计过程模型

我们假设这架飞机采用牛顿力学的运动学系统。我们已经在前几章中使用了这个模型，所以通过检查你可能会意识到我们想要

$$\mathbf F = \left[\begin{array}{cc|c} 1 & \Delta t & 0\\
0 & 1 & 0 \\ \hline
0 & 0 & 1\end{array}\right]$$

我将矩阵分割成块，以表明左上方块是一个恒定速度模型，对于$x$，右下方块是一个恒定位置模型，对于$y$。

但是，让我们练习一下如何找到这些矩阵。我们用一组微分方程来建立系统的模型。我们需要这种形式的方程

$$\dot{\mathbf x} = \mathbf{Ax} + \mathbf{w}$$
where $\mathbf{w}$ is the system noise.

变量$x$和$y$是独立的，所以我们可以分别计算它们。一维运动微分方程为:

$$\begin{aligned}v &= \dot x \\
a &= \ddot{x} = 0\end{aligned}$$

现在我们把微分方程化成状态空间形式。如果这是一个二阶或更大阶的微分系统我们必须首先把它们化简成等价的一阶方程组。方程是一阶的，所以我们把它们写成状态空间矩阵的形式

$$\begin{aligned}\begin{bmatrix}\dot x \\ \ddot{x}\end{bmatrix} &= \begin{bmatrix}0&1\\0&0\end{bmatrix} \begin{bmatrix}x \\
\dot x\end{bmatrix} \\ \dot{\mathbf x} &= \mathbf{Ax}\end{aligned}$$
where $\mathbf A=\begin{bmatrix}0&1\\0&0\end{bmatrix}$.

回想一下$\mathbf A$是*系统动力学矩阵*。它描述了一组线性微分方程。从它我们必须计算状态转移矩阵$\mathbf F$。$\mathbf F$描述了一个离散时间步长$\Delta t$的计算$\mathbf x$的线性方程组。

计算$\mathbf F$的常用方法是利用矩阵指数的幂级数展开:

$$\mathbf F(\Delta t) = e^{\mathbf A\Delta t} = \mathbf{I} + \mathbf A\Delta t  + \frac{(\mathbf A\Delta t)^2}{2!} + \frac{(\mathbf A \Delta t)^3}{3!} + ... $$

$\mathbf A^2 = \begin{bmatrix}0&0\\0&0\end{bmatrix}$，所以$\mathbf A$ 的所有高次幂都是$\mathbf{0}$。因此幂级数展开为:

$$
\begin{aligned}
\mathbf F &=\mathbf{I} + \mathbf At + \mathbf{0} \\
&= \begin{bmatrix}1&0\\0&1\end{bmatrix} + \begin{bmatrix}0&1\\0&0\end{bmatrix}\Delta t\\
\mathbf F &= \begin{bmatrix}1&\Delta t\\0&1\end{bmatrix}
\end{aligned}$$

这和运动学方程的结果是一样的!这个练习是不必要的，除了说明如何从线性微分方程找到状态转移矩阵。我们将以一个需要使用这种技术的示例来结束本章。

### 设计测量模型

测量函数取之前$\bar{\mathbf x}$的状态估计，并将其转换为倾斜距离的测量。我们用勾股定理来推导:

$$h(\bar{\mathbf x}) = \sqrt{x^2 + y^2}$$

由于平方根的关系，倾斜距离和地面位置之间的关系是非线性的。我们通过求它在$\mathbf x_t$处的偏导数来线性化它:

$$
\mathbf H = \frac{\partial{h(\bar{\mathbf x})}}{\partial{\bar{\mathbf x}}}\biggr|_{\bar{\mathbf x}_t}
$$

一个矩阵的偏导数叫做雅可比矩阵，它是这样的

$$\frac{\partial \mathbf H}{\partial \bar{\mathbf x}} =
\begin{bmatrix}
\frac{\partial h_1}{\partial x_1} & \frac{\partial h_1}{\partial x_2} &\dots \\
\frac{\partial h_2}{\partial x_1} & \frac{\partial h_2}{\partial x_2} &\dots \\
\vdots & \vdots
\end{bmatrix}
$$

换句话说，矩阵中的每个元素都是函数$h$对$x$变量的偏导数。解决我们的问题

$$\mathbf H = \begin{bmatrix}{\partial h}/{\partial x} & {\partial h}/{\partial \dot{x}} & {\partial h}/{\partial y}\end{bmatrix}$$

依次解决每个问题:

$$\begin{aligned}
\frac{\partial h}{\partial x} &= \frac{\partial}{\partial x} \sqrt{x^2 + y^2} \\
&= \frac{x}{\sqrt{x^2 + y^2}}
\end{aligned}$$

和

$$\begin{aligned}
\frac{\partial h}{\partial \dot{x}} &=
\frac{\partial}{\partial \dot{x}} \sqrt{x^2 + y^2} \\
&= 0
\end{aligned}$$

和

$$\begin{aligned}
\frac{\partial h}{\partial y} &= \frac{\partial}{\partial y} \sqrt{x^2 + y^2} \\
&= \frac{y}{\sqrt{x^2 + y^2}}
\end{aligned}$$

推导

$$\mathbf H =
\begin{bmatrix}
\frac{x}{\sqrt{x^2 + y^2}} &
0 &
&
\frac{y}{\sqrt{x^2 + y^2}}
\end{bmatrix}$$

这可能看起来令人生畏，所以后退一步，认识到所有这些数学运算都是非常简单的。我们有一个关于飞机倾斜距离的方程，它是非线性的。卡尔曼滤波器只适用于线性方程，所以我们需要找到一个近似$\mathbf H$的线性方程。正如我们上面所讨论的，求非线性方程在某一点的斜率是一个很好的近似。对于卡尔曼滤波器，“给定点”是状态变量$\mathbf x$，因此我们需要对倾斜范围对$\mathbf x$求导。对于线性卡尔曼滤波器，$\mathbf H$是我们在运行滤波器之前计算的常数。对于EKF， $\mathbf H$在每一步都更新，因为计算点$\bar{\mathbf x}$在每个epoch都发生了变化。

为了让这个更具体，我们现在写一个Python函数来计算这个问题的雅可比矩阵$h$。

```python
from math import sqrt
def HJacobian_at(x):
    """ compute Jacobian of H matrix at x """

    horiz_dist = x[0]
    altitude   = x[2]
    denom = sqrt(horiz_dist**2 + altitude**2)
    return array ([[horiz_dist/denom, 0., altitude/denom]])
```

最后，让我们提供$h(\bar{\mathbf x})$的代码:

```python
def hx(x):
    """ compute measurement for slant range that
    would correspond to state x.
    """

    return (x[0]**2 + x[2]**2) ** 0.5
```

现在让我们为我们的雷达写一个模拟。

```python
from numpy.random import randn
import math

class RadarSim:
    """ Simulates the radar signal returns from an object
    flying at a constant altityude and velocity in 1D.
    """

    def __init__(self, dt, pos, vel, alt):
        self.pos = pos
        self.vel = vel
        self.alt = alt
        self.dt = dt

    def get_range(self):
        """ Returns slant range to the object. Call once
        for each new measurement at dt time from last call.
        """

        # add some process noise to the system
        self.vel = self.vel  + .1*randn()
        self.alt = self.alt + .1*randn()
        self.pos = self.pos + self.vel*self.dt

        # add measurement noise
        err = self.pos * 0.05*randn()
        slant_dist = math.sqrt(self.pos**2 + self.alt**2)

        return slant_dist + err
```

### 设计过程和测量噪声

雷达测量到目标的距离。我们将使用$\sigma_{range}= 5$ meters来表示噪声。这给了我们

$$\mathbf R = \begin{bmatrix}\sigma_{range}^2\end{bmatrix} = \begin{bmatrix}25\end{bmatrix}$$

$\mathbf Q$的设计需要一些讨论。状态 $\mathbf x= \begin{bmatrix}x & \dot x & y\end{bmatrix}^\mathtt{T}$。前两个元素是位置(向下距离)和速度，所以我们可以使用`Q_discrete_white_noise`噪声来计算$\mathbf Q$的左上角的值。$\mathbf x$的第三个元素是高度，我们假设它与下降距离无关。这将我们引向$\mathbf Q$的块设计:

$$\mathbf Q = \begin{bmatrix}\mathbf Q_\mathtt{x} & 0 \\ 0 & \mathbf Q_\mathtt{y}\end{bmatrix}$$

### 实现

`FilterPy`提供类`ExtendedKalmanFilter`。它的工作原理类似于我们一直使用的`KalmanFilter`类，除了它允许你提供一个函数，计算雅可比矩阵$\mathbf H$和函数$h(\mathbf x)$。

我们首先导入过滤器并创建它。`x` 的尺寸是3,`z`的尺寸是1。

```python
from filterpy.kalman import ExtendedKalmanFilter

rk = ExtendedKalmanFilter(dim_x=3, dim_z=1)
```
我们创建雷达模拟器:
```python
radar = RadarSim(dt, pos=0., vel=100., alt=1000.)
```
我们将在飞机的实际位置附近初始化过滤器:
```python
rk.x = array([radar.pos, radar.vel-10, radar.alt+100])
```

我们使用上面计算的泰勒级数展开的第一项来指定系统矩阵:
```python
dt = 0.05
rk.F = eye(3) + array([[0, 1, 0],
                       [0, 0, 0],
                       [0, 0, 0]])*dt
```

在给$\mathbf R$， $\mathbf Q$和$\mathbf P$赋了合理的值之后，我们可以用一个简单的循环运行过滤器。我们将计算雅可比矩阵$\mathbf H$和$h(x)$的函数传递给 `update`方法。

```python
for i in range(int(20/dt)):
    z = radar.get_range()
    rk.update(array([z]), HJacobian_at, hx)
    rk.predict()
```

添加一些样板代码来保存和绘制我们得到的结果:

```python
from filterpy.common import Q_discrete_white_noise
from filterpy.kalman import ExtendedKalmanFilter
from numpy import eye, array, asarray
import numpy as np

dt = 0.05
rk = ExtendedKalmanFilter(dim_x=3, dim_z=1)
radar = RadarSim(dt, pos=0., vel=100., alt=1000.)

# make an imperfect starting guess
rk.x = array([radar.pos-100, radar.vel+100, radar.alt+1000])

rk.F = eye(3) + array([[0, 1, 0],
                       [0, 0, 0],
                       [0, 0, 0]]) * dt

range_std = 5. # meters
rk.R = np.diag([range_std**2])
rk.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=dt, var=0.1)
rk.Q[2,2] = 0.1
rk.P *= 50

xs, track = [], []
for i in range(int(20/dt)):
    z = radar.get_range()
    track.append((radar.pos, radar.vel, radar.alt))

    rk.update(array([z]), HJacobian_at, hx)
    xs.append(rk.x)
    rk.predict()

xs = asarray(xs)
track = asarray(track)
time = np.arange(0, len(xs)*dt, dt)
ekf_internal.plot_radar(xs, track, time)
```

## 使用symy计算雅可比矩阵

根据你对导数的经验你可能会发现雅可比矩阵的计算很困难。即使你发现它很简单，稍微困难一点的问题很容易导致非常困难的计算。

如附录A所述，我们可以使用symy包为我们计算雅可比矩阵。

```python
import sympy
from IPython.display import display
sympy.init_printing(use_latex='mathjax')

x, x_vel, y = sympy.symbols('x, x_vel y')

H = sympy.Matrix([sympy.sqrt(x**2 + y**2)])

state = sympy.Matrix([x, x_vel, y])
J = H.jacobian(state)

display(state)
display(J)
```

$\displaystyle \left[\begin{matrix}x\\x_{vel}\\y\end{matrix}\right]$

$\displaystyle \left[\begin{matrix}\frac{x}{\sqrt{x^{2} + y^{2}}} & 0 & \frac{y}{\sqrt{x^{2} + y^{2}}}\end{matrix}\right]$

这个结果和我们上面计算的结果是一样的，而且我们的努力要少得多!

## 机器人定位程序

是时候试试真正的问题了。我警告你这部分很难。然而，大多数书选择简单的教科书问题和简单的答案，你留下疑惑如何解决一个现实世界的问题。

我们将考虑机器人的定位问题。我们已经在**Unscented卡尔曼滤波器**一章中实现了这一点，如果你还没有读过，我建议你现在就阅读它。在这个场景中，我们有一个机器人，它正在通过一个传感器来检测地标。这可能是一辆使用计算机视觉识别树木、建筑和其他地标的自动驾驶汽车。它可能是那些为你的房子吸尘的小型机器人，或者是仓库里的机器人。

机器人有4个轮子，配置与汽车相同。它通过转动前轮来操纵。这使得机器人在前进时绕后轴旋转。这是我们需要建模的非线性行为。

机器人有一个传感器，可以测量已知目标的距离和方位。这是非线性的，因为从一个范围和方位计算一个位置需要平方根和三角。

过程模型和度量模型都是非线性的。EKF可以容纳这两种情况，因此我们暂时认为EKF是解决这一问题的可行选择。

### 机器人运动模型

近似地说，汽车是通过向前移动时转动前轮来控制方向的。汽车的前部在以后轮为中心转动时，会朝着车轮指向的方向移动。这种简单的描述由于摩擦引起的滑移、橡胶轮胎在不同速度下的不同行为，以及外胎行驶半径与内胎不同的需求等问题而变得复杂。精确的转向建模需要一套复杂的微分方程。

对于低速机器人应用，一种更简单的“自行车模型”表现良好。这是模型的描述:

```python
ekf_internal.plot_bicycle()
```

在**Unscented卡尔曼滤波器**一章中，我们推导了以下方程:

$$\begin{aligned}
\beta &= \frac d w \tan(\alpha) \\
x &= x - R\sin(\theta) + R\sin(\theta + \beta) \\
y &= y + R\cos(\theta) - R\cos(\theta + \beta) \\
\theta &= \theta + \beta
\end{aligned}
$$

其中$\theta$是机器人的方向。

如果你对转向模型不感兴趣，你不需要了解这个模型的细节。重要的是要认识到我们的运动模型是非线性的，我们需要用卡尔曼滤波器来处理它。

### 设计状态变量

对于我们的过滤器，我们将保持机器人的位置$x,y$和方向$\theta$:

$$\mathbf x = \begin{bmatrix}x \\ y \\ \theta\end{bmatrix}$$

我们的控制输入$\mathbf u$是速度$v$和转向角度$\alpha$:

$$\mathbf u = \begin{bmatrix}v \\ \alpha\end{bmatrix}$$

### 设计系统模型

我们将系统建模为非线性运动模型加上噪声。

$$\bar x = f(x, u) + \mathcal{N}(0, Q)$$

使用我们上面创建的机器人的运动模型，我们可以将其扩展为

$$\bar{\begin{bmatrix}x\\y\\\theta\end{bmatrix}} = \begin{bmatrix}x\\y\\\theta\end{bmatrix} +
\begin{bmatrix}- R\sin(\theta) + R\sin(\theta + \beta) \\
R\cos(\theta) - R\cos(\theta + \beta) \\
\beta\end{bmatrix}$$

我们通过求$f(x,u)$的雅可比矩阵来求$\mathbf F$。

$$\mathbf F = \frac{\partial f(x, u)}{\partial x} =\begin{bmatrix}
\frac{\partial f_1}{\partial x} &
\frac{\partial f_1}{\partial y} &
\frac{\partial f_1}{\partial \theta}\\
\frac{\partial f_2}{\partial x} &
\frac{\partial f_2}{\partial y} &
\frac{\partial f_2}{\partial \theta} \\
\frac{\partial f_3}{\partial x} &
\frac{\partial f_3}{\partial y} &
\frac{\partial f_3}{\partial \theta}
\end{bmatrix}
$$

当我们计算这些时，我们得到

$$\mathbf F = \begin{bmatrix}
1 & 0 & -R\cos(\theta) + R\cos(\theta+\beta) \\
0 & 1 & -R\sin(\theta) + R\sin(\theta+\beta) \\
0 & 0 & 1
\end{bmatrix}$$

我们可以和symy一起检查一下。

```python
import sympy
from sympy.abc import alpha, x, y, v, w, R, theta
from sympy import symbols, Matrix
sympy.init_printing(use_latex="mathjax", fontsize='16pt')
time = symbols('t')
d = v*time
beta = (d/w)*sympy.tan(alpha)
r = w/sympy.tan(alpha)

fxu = Matrix([[x-r*sympy.sin(theta) + r*sympy.sin(theta+beta)],
              [y+r*sympy.cos(theta)- r*sympy.cos(theta+beta)],
              [theta+beta]])
F = fxu.jacobian(Matrix([x, y, theta]))
F
```

$\displaystyle \left[\begin{matrix}1 & 0 & - \frac{w \cos{\left(\theta \right)}}{\tan{\left(\alpha \right)}} + \frac{w \cos{\left(\frac{t v \tan{\left(\alpha \right)}}{w} + \theta \right)}}{\tan{\left(\alpha \right)}}\\0 & 1 & - \frac{w \sin{\left(\theta \right)}}{\tan{\left(\alpha \right)}} + \frac{w \sin{\left(\frac{t v \tan{\left(\alpha \right)}}{w} + \theta \right)}}{\tan{\left(\alpha \right)}}\\0 & 0 & 1\end{matrix}\right]$

这看起来有点复杂。我们可以使用symy来替换术语:

```python
# reduce common expressions
B, R = symbols('beta, R')
F = F.subs((d/w)*sympy.tan(alpha), B)
F.subs(w/sympy.tan(alpha), R)
```

$\displaystyle \left[\begin{matrix}1 & 0 & - R \cos{\left(\theta \right)} + R \cos{\left(\beta + \theta \right)}\\0 & 1 & - R \sin{\left(\theta \right)} + R \sin{\left(\beta + \theta \right)}\\0 & 0 & 1\end{matrix}\right]$

这种形式验证了雅可比矩阵的计算是正确的。

现在我们可以把注意力转移到噪音上。在这里，噪声是在我们的控制输入，所以它是在*控制空间*。换句话说，我们命令一个特定的速度和转向角度，但我们需要将其转换为误差$x, y， \theta$。在真实的系统中，这可能会随着速度而变化，因此需要为每一个预测重新计算。我将选择这个作为噪声模型;对于一个真正的机器人，你需要选择一个能准确描述系统错误的模型。

$$\mathbf{M} = \begin{bmatrix}\sigma_{vel}^2 & 0 \\ 0 & \sigma_\alpha^2\end{bmatrix}$$

如果这是一个线性问题，我们将使用现在熟悉的$\mathbf{FMF}^\mathsf T$形式从控制空间转换到状态空间。由于我们的运动模型是非线性的，我们不试图找到一个封闭形式的解，而是用雅可比矩阵线性化它，我们将其命名为$\mathbf{V}$。

$$\mathbf{V} = \frac{\partial f(x, u)}{\partial u} \begin{bmatrix}
\frac{\partial f_1}{\partial v} & \frac{\partial f_1}{\partial \alpha} \\
\frac{\partial f_2}{\partial v} & \frac{\partial f_2}{\partial \alpha} \\
\frac{\partial f_3}{\partial v} & \frac{\partial f_3}{\partial \alpha}
\end{bmatrix}$$

这些偏导数变得非常难处理。让我们用symy来计算。

```python
V = fxu.jacobian(Matrix([v, alpha]))
V = V.subs(sympy.tan(alpha)/w, 1/R)
V = V.subs(time*v/R, B)
V = V.subs(time*v, 'd')
V
```

$\displaystyle \left[\begin{matrix}t \cos{\left(\beta + \theta \right)} & \frac{d \left(\tan^{2}{\left(\alpha \right)} + 1\right) \cos{\left(\beta + \theta \right)}}{\tan{\left(\alpha \right)}} - \frac{w \left(- \tan^{2}{\left(\alpha \right)} - 1\right) \sin{\left(\theta \right)}}{\tan^{2}{\left(\alpha \right)}} + \frac{w \left(- \tan^{2}{\left(\alpha \right)} - 1\right) \sin{\left(\beta + \theta \right)}}{\tan^{2}{\left(\alpha \right)}}\\t \sin{\left(\beta + \theta \right)} & \frac{d \left(\tan^{2}{\left(\alpha \right)} + 1\right) \sin{\left(\beta + \theta \right)}}{\tan{\left(\alpha \right)}} + \frac{w \left(- \tan^{2}{\left(\alpha \right)} - 1\right) \cos{\left(\theta \right)}}{\tan^{2}{\left(\alpha \right)}} - \frac{w \left(- \tan^{2}{\left(\alpha \right)} - 1\right) \cos{\left(\beta + \theta \right)}}{\tan^{2}{\left(\alpha \right)}}\\\frac{t}{R} & \frac{d \left(\tan^{2}{\left(\alpha \right)} + 1\right)}{w}\end{matrix}\right]$

这将使您了解到EKF在数学上变得多么棘手。

这给了我们预测方程的最终形式:

$$\begin{aligned}
\mathbf{\bar x} &= \mathbf x +
\begin{bmatrix}- R\sin(\theta) + R\sin(\theta + \beta) \\
R\cos(\theta) - R\cos(\theta + \beta) \\
\beta\end{bmatrix}\\
\mathbf{\bar P} &=\mathbf{FPF}^{\mathsf T} + \mathbf{VMV}^{\mathsf T}
\end{aligned}$$

这种形式的线性化并不是预测$\mathbf x$的唯一方法。例如，我们可以使用一种数值积分技术，如*龙格库塔*来计算机器人的运动。如果时间步长比较大，就需要这样做。EKF不像卡尔曼滤波器那样是固定不变的。对于一个真正的问题，你必须小心地用微分方程建模你的系统，然后确定最合适的方法来解决这个系统。正确的方法取决于您所需的精度、方程的非线性程度、处理器预算和数值稳定性问题。

### 设计测量模型

机器人的传感器提供了对景观中多个已知位置的噪声轴承和距离测量。测量模型必须将状态$\begin{bmatrix}x & y&\theta\end{bmatrix}^\mathsf T$转换为一个距离和地标的方位。如果$\mathbf p$是一个地标的位置，则范围$r$是

$$r = \sqrt{(p_x - x)^2 + (p_y - y)^2}$$

传感器提供了相对于机器人方向的轴承，所以我们必须从轴承中减去机器人的方向来获得传感器读数，如下所示:

$$\phi = \arctan(\frac{p_y - y}{p_x - x}) - \theta$$

因此我们的度量模型$h$为

$$\begin{aligned}
\mathbf z& = h(\bar{\mathbf x}, \mathbf p) &+ \mathcal{N}(0, R)\\
&= \begin{bmatrix}
\sqrt{(p_x - x)^2 + (p_y - y)^2} \\
\arctan(\frac{p_y - y}{p_x - x}) - \theta
\end{bmatrix} &+ \mathcal{N}(0, R)
\end{aligned}$$

这显然是非线性的，所以我们需要线性化$h$在$\mathbf x$处，通过取它的雅可比矩阵。我们用下面的symy来计算。

```python
px, py = symbols('p_x, p_y')
z = Matrix([[sympy.sqrt((px-x)**2 + (py-y)**2)],
            [sympy.atan2(py-y, px-x) - theta]])
z.jacobian(Matrix([x, y, theta]))
```

$\displaystyle \left[\begin{matrix}\frac{- p_{x} + x}{\sqrt{\left(p_{x} - x\right)^{2} + \left(p_{y} - y\right)^{2}}} & \frac{- p_{y} + y}{\sqrt{\left(p_{x} - x\right)^{2} + \left(p_{y} - y\right)^{2}}} & 0\\- \frac{- p_{y} + y}{\left(p_{x} - x\right)^{2} + \left(p_{y} - y\right)^{2}} & - \frac{p_{x} - x}{\left(p_{x} - x\right)^{2} + \left(p_{y} - y\right)^{2}} & -1\end{matrix}\right]$

现在我们需要把它写成一个Python函数。例如，我们可以这样写:

```python
from math import sqrt

def H_of(x, landmark_pos):
    """ compute Jacobian of H matrix where h(x) computes
    the range and bearing to a landmark for state x """

    px = landmark_pos[0]
    py = landmark_pos[1]
    hyp = (px - x[0, 0])**2 + (py - x[1, 0])**2
    dist = sqrt(hyp)

    H = array(
        [[-(px - x[0, 0]) / dist, -(py - x[1, 0]) / dist, 0],
         [ (py - x[1, 0]) / hyp,  -(px - x[0, 0]) / hyp, -1]])
    return H
```

我们还需要定义一个将系统状态转换为度量值的函数。

```python
from math import atan2

def Hx(x, landmark_pos):
    """ takes a state variable and returns the measurement
    that would correspond to that state.
    """
    px = landmark_pos[0]
    py = landmark_pos[1]
    dist = sqrt((px - x[0, 0])**2 + (py - x[1, 0])**2)

    Hx = array([[dist],
                [atan2(py - x[1, 0], px - x[0, 0]) - x[2, 0]]])
    return Hx
```

### 设计测量噪声

因此，可以合理地假设距离和方位测量的噪声是独立的

$$\mathbf R=\begin{bmatrix}\sigma_{range}^2 & 0 \\ 0 & \sigma_{bearing}^2\end{bmatrix}$$

### 实现

我们将使用`FilterPy`的`ExtendedKalmanFilter`类来实现过滤器。它的`predict()`方法使用流程模型的标准线性方程。我们的是非线性的，所以我们必须用自己的实现覆盖`predict()`。我还想用这个类来模拟机器人，所以我将添加一个方法`move()`来计算机器人的位置，`predict()`和我的模拟都可以调用这个方法。

预测步骤的矩阵是相当大的。在编写这段代码时，我犯了几个错误，直到我最终使它工作。我是通过使用symy的`evalf`函数才发现错误的。`evalf`使用变量的特定值计算symy `Matrix`。我决定向您演示这一技术，并在卡尔曼滤波代码中使用了`evalf`。你需要理解以下几点。

首先，`evalf`使用字典来指定值。例如，如果你的矩阵包含`x`和`y`，你可以写

```python
    M.evalf(subs={x:3, y:17})
```

求`x=3`和`y=17`的矩阵值。

其次，`evalf`返回`sympy.Matrix` 矩阵的对象。使用 `numpy.array(M).astype(float)` 将其转换为NumPy数组。`numpy.array(M)`创建一个类型为`object`的数组，这不是你想要的。

这里是EKF的代码:

```python
from filterpy.kalman import ExtendedKalmanFilter as EKF
from numpy import array, sqrt
class RobotEKF(EKF):
    def __init__(self, dt, wheelbase, std_vel, std_steer):
        EKF.__init__(self, 3, 2, 2)
        self.dt = dt
        self.wheelbase = wheelbase
        self.std_vel = std_vel
        self.std_steer = std_steer

        a, x, y, v, w, theta, time = symbols(
            'a, x, y, v, w, theta, t')
        d = v*time
        beta = (d/w)*sympy.tan(a)
        r = w/sympy.tan(a)

        self.fxu = Matrix(
            [[x-r*sympy.sin(theta)+r*sympy.sin(theta+beta)],
             [y+r*sympy.cos(theta)-r*sympy.cos(theta+beta)],
             [theta+beta]])

        self.F_j = self.fxu.jacobian(Matrix([x, y, theta]))
        self.V_j = self.fxu.jacobian(Matrix([v, a]))

        # save dictionary and it's variables for later use
        self.subs = {x: 0, y: 0, v:0, a:0,
                     time:dt, w:wheelbase, theta:0}
        self.x_x, self.x_y, = x, y
        self.v, self.a, self.theta = v, a, theta

    def predict(self, u):
        self.x = self.move(self.x, u, self.dt)

        self.subs[self.theta] = self.x[2, 0]
        self.subs[self.v] = u[0]
        self.subs[self.a] = u[1]

        F = array(self.F_j.evalf(subs=self.subs)).astype(float)
        V = array(self.V_j.evalf(subs=self.subs)).astype(float)

        # covariance of motion noise in control space
        M = array([[self.std_vel*u[0]**2, 0],
                   [0, self.std_steer**2]])

        self.P = F @ self.P @ F.T + V @ M @ V.T

    def move(self, x, u, dt):
        hdg = x[2, 0]
        vel = u[0]
        steering_angle = u[1]
        dist = vel * dt

        if abs(steering_angle) > 0.001: # is robot turning?
            beta = (dist / self.wheelbase) * tan(steering_angle)
            r = self.wheelbase / tan(steering_angle) # radius

            dx = np.array([[-r*sin(hdg) + r*sin(hdg + beta)],
                           [r*cos(hdg) - r*cos(hdg + beta)],
                           [beta]])
        else: # moving in straight line
            dx = np.array([[dist*cos(hdg)],
                           [dist*sin(hdg)],
                           [0]])
        return x + dx
```

现在我们有另一个问题要处理。理论上剩余的值被计算为$y = z - h(x)$，但这是行不通的，因为我们的测量包含了一个角。假设z的方位是$1^\circ$，而$h(x)$的方位是$359^\circ$。简单地减去它们将得到角差$-358^\circ$，而正确的值是$2^\circ$。我们必须编写代码来正确计算位置残差。

```python
def residual(a, b):
    """ compute residual (a-b) between measurements containing
    [range, bearing]. Bearing is normalized to [-pi, pi)"""
    y = a - b
    y[1] = y[1] % (2 * np.pi)    # force in range [0, 2 pi)
    if y[1] > np.pi:             # move to [-pi, pi)
        y[1] -= 2 * np.pi
    return y
```

其余代码运行模拟并绘制结果，到目前为止不需要太多注释。我创建了一个变量`landmarks`，它包含地标坐标。我每秒更新模拟机器人的位置10次，但每秒只运行一次EKF。这有两个原因。首先，我们没有使用龙格-库塔来整合运动微分方程，所以较窄的时间步长可以让我们的模拟更精确。其次，在嵌入式系统中，有限的处理速度是相当正常的。这迫使您只在绝对需要的时候频繁地运行卡尔曼滤波器。

```python
from filterpy.stats import plot_covariance_ellipse
from math import sqrt, tan, cos, sin, atan2
import matplotlib.pyplot as plt

dt = 1.0

def z_landmark(lmark, sim_pos, std_rng, std_brg):
    x, y = sim_pos[0, 0], sim_pos[1, 0]
    d = np.sqrt((lmark[0] - x)**2 + (lmark[1] - y)**2)
    a = atan2(lmark[1] - y, lmark[0] - x) - sim_pos[2, 0]
    z = np.array([[d + randn()*std_rng],
                  [a + randn()*std_brg]])
    return z

def ekf_update(ekf, z, landmark):
    ekf.update(z, HJacobian=H_of, Hx=Hx,
               residual=residual,
               args=(landmark), hx_args=(landmark))

def run_localization(landmarks, std_vel, std_steer,
                     std_range, std_bearing,
                     step=10, ellipse_step=20, ylim=None):
    ekf = RobotEKF(dt, wheelbase=0.5, std_vel=std_vel,
                   std_steer=std_steer)
    ekf.x = array([[2, 6, .3]]).T # x, y, steer angle
    ekf.P = np.diag([.1, .1, .1])
    ekf.R = np.diag([std_range**2, std_bearing**2])

    sim_pos = ekf.x.copy() # simulated position
    # steering command (vel, steering angle radians)
    u = array([1.1, .01])

    plt.figure()
    plt.scatter(landmarks[:, 0], landmarks[:, 1],
                marker='s', s=60)

    track = []
    for i in range(200):
        sim_pos = ekf.move(sim_pos, u, dt/10.) # simulate robot
        track.append(sim_pos)

        if i % step == 0:
            ekf.predict(u=u)

            if i % ellipse_step == 0:
                plot_covariance_ellipse(
                    (ekf.x[0,0], ekf.x[1,0]), ekf.P[0:2, 0:2],
                     std=6, facecolor='k', alpha=0.3)

            x, y = sim_pos[0, 0], sim_pos[1, 0]
            for lmark in landmarks:
                z = z_landmark(lmark, sim_pos,
                               std_range, std_bearing)
                ekf_update(ekf, z, lmark)

            if i % ellipse_step == 0:
                plot_covariance_ellipse(
                    (ekf.x[0,0], ekf.x[1,0]), ekf.P[0:2, 0:2],
                    std=6, facecolor='g', alpha=0.8)
    track = np.array(track)
    plt.plot(track[:, 0], track[:,1], color='k', lw=2)
    plt.axis('equal')
    plt.title("EKF Robot localization")
    if ylim is not None: plt.ylim(*ylim)
    plt.show()
    return ekf
```

```python
landmarks = array([[5, 10], [10, 5], [15, 15]])

ekf = run_localization(
    landmarks, std_vel=0.1, std_steer=np.radians(1),
    std_range=0.3, std_bearing=0.1)
print('Final P:', ekf.P.diagonal())
```

我把这些地标画成立体的正方形。机器人的路径用黑线画出来。预测步长的协方差椭圆为浅灰色，更新的协方差为绿色。为了使它们在这个比例下可见，我将椭圆边界设置为6$\sigma$。

我们可以看到，我们的运动模型增加了很多不确定性，而且大多数误差都在运动方向上。我们从蓝色椭圆的形状来确定。经过几个步骤，我们可以看到滤波器包含地标测量和误差改善。

我在UKF章节中使用了相同的初始条件和地标位置。UKF在误差椭圆方面取得了更好的精度。这两种方法的表现都和它们对$\mathbf x$的估计差不多。

现在让我们添加另一个地标。

```python
landmarks = array([[5, 10], [10, 5], [15, 15], [20, 5]])

ekf = run_localization(
    landmarks, std_vel=0.1, std_steer=np.radians(1),
    std_range=0.3, std_bearing=0.1)
plt.show()
print('Final P:', ekf.P.diagonal())
```

在接近轨道末端的估计中，不确定性较小。我们可以通过使用前两个路标看到多个路标对不确定性的影响。

```python
ekf = run_localization(
    landmarks[0:2], std_vel=1.e-10, std_steer=1.e-10,
    std_range=1.4, std_bearing=.05)
print('Final P:', ekf.P.diagonal())
```

经过地标后，估计值很快就偏离了机器人的路径。协方差也快速增长。让我们看看只有一个里程碑会发生什么:

```python
ekf = run_localization(
    landmarks[0:1], std_vel=1.e-10, std_steer=1.e-10,
    std_range=1.4, std_bearing=.05)
print('Final P:', ekf.P.diagonal())
```

正如你可能怀疑的那样，一个里程碑会产生一个非常糟糕的结果。相反，大量的地标让我们能够做出非常准确的估计。

```python
landmarks = array([[5, 10], [10,  5], [15, 15], [20,  5], [15, 10],
                   [10,14], [23, 14], [25, 20], [10, 20]])

ekf = run_localization(
    landmarks, std_vel=0.1, std_steer=np.radians(1),
    std_range=0.3, std_bearing=0.1, ylim=(0, 21))
print('Final P:', ekf.P.diagonal())
```

### 讨论

我说这是一个真实的问题，在某种程度上确实是。我看过一些替代的演示用机器人运动模型得出了更简单的雅可比矩阵。另一方面，我的运动模型在许多方面也很简单。首先，它使用了自行车模型。一辆真正的汽车有两组轮胎，每组轮胎的行驶半径不同。轮子不能很好地抓住地面。我还假设机器人对控制输入立即做出反应。Sebastian Thrun在《概率机器人》中写道，这种简化模型是合理的，因为用于跟踪真实车辆时，过滤器表现良好。这里的教训是，虽然你必须有一个合理准确的非线性模型，但它不需要完美地运行良好。作为设计师，你需要平衡模型的保真度、数学难度和执行线性代数所需的CPU时间。

这个问题的另一种简化方式是我们假设我们知道地标和测量值之间的对应关系。但假设我们使用雷达，我们如何知道特定的信号返回对应于当地场景中的特定建筑?这个问题暗示了SLAM算法——同时定位和绘图。SLAM不是本书的重点，所以我不会详细阐述这个主题。

## UKF vs EKF

在上一章中，我使用UKF来解决这个问题。执行方面的差异应该非常清楚。尽管是一个基本的运动模型，但状态和测量模型的雅可比矩阵的计算并不简单。另一个问题可能会导致难于或不可能通过解析推导的雅可比矩阵。相反，UKF只需要您提供一个计算系统运动模型的函数和另一个用于测量模型的函数。

在很多情况下，雅可比矩阵不能通过解析找到。细节超出了这本书的范围，但是你必须使用数值方法来计算雅可比矩阵。这项任务不是微不足道的，你将花费大量的硕士学位在STEM学校学习处理这种情况的技术。即使这样，你可能也只能解决与你的领域相关的问题——航空工程师对Navier Stokes方程了解很多，但对化学反应速率建模了解不多。

所以，ukf很简单。他们是准确的吗?在实践中，他们往往比EKF表现得更好。你可以找到大量的研究论文，证明UKF在不同的问题领域优于EKF。不难理解为什么这是真的。EKF通过在单个点上线性化系统模型和测量模型来工作，UKF使用$2n+1$点。

让我们看一个具体的例子。取$f(x) = x^3$并通过它传递一个高斯分布。我将用蒙特卡罗模拟计算出一个准确的答案。我根据高斯分布随机生成50,000个点，将每个点传递到$f(x)$，然后计算结果的均值和方差。

EKF将函数线性化，通过求导求出在估值点$x$处的斜率。这个斜率变成了我们用来变换高斯函数的线性函数。这是一个情节。

```python
import kf_book.nonlinear_plots as nonlinear_plots
nonlinear_plots.plot_ekf_vs_mc()
```

EKF计算相当不准确。相比之下，UKF的性能如下:

```python
nonlinear_plots.plot_ukf_vs_mc(alpha=0.001, beta=3., kappa=1.)
```

这里我们可以看到UKF的计算精确到小数点后2位。标准差略有偏差，但您也可以通过使用$\alpha$、$\beta$和$\gamma$参数来微调UKF计算分布的方式，以生成sigma点。这里我使用$\alpha=0.001$， $\beta=3$， $\gamma=1$。您可以随意修改它们以查看结果。你应该能得到比我更好的结果。但是，要避免针对特定测试过度调优UKF。对于您的测试用例，它可能表现得更好，但一般来说更差。

```python

```


# Project for [Nuclear Talent](http://www.nucleartalent.org) course on *Many-body methods for nuclear physics, from Structure to Reactions* at [Henan Normal University, P.R. China](http://www.htu.cn/english/), July 16-August 5 2018

**Kevin Fossez**, [National Superconducting Cyclotron Laboratory](http://www.nscl.msu.edu/), [Michigan State University](http://www.msu.edu/), East Lansing, MI 48824, USA

 **Morten Hjorth-Jensen**, [National Superconducting Cyclotron Laboratory](http://www.nscl.msu.edu/) and [Department of Physics and Astronomy](https://www.pa.msu.edu/), [Michigan State University](http://www.msu.edu/), East Lansing, MI 48824, USA

 **Baishan Hu**, [School of Physics, Peking University, Beijing 100871, P.R. China](http://english.pku.edu.cn/)

 **Weiguang Jiang**, [Department of Physics and Astronomy](https://www.phys.utk.edu/), [University of Tennessee](http://www.utk.edu/), Knoxville, TN 37996-1200, USA and [Oak Ridge National Laboratory](http://www.ornl.gov/), Oak Ridge, TN, USA

 **Thomas Papenbrock**, [Department of Physics and Astronomy](https://www.phys.utk.edu/), [University of Tennessee](http://www.utk.edu/), Knoxville, TN 37996-1200, USA and [Oak Ridge National Laboratory](http://www.ornl.gov/), Oak Ridge, TN, USA

 **Ragnar Stroberg**, [Departmentof Physics, Reed College](http://www.reed.edu/), Portland, OR, 97202 and [Department of Physics, University of Washington](https://sharepoint.washington.edu/phys/Pages/default.aspx), Seattle, WA 98195-1560, USA

 **Zhonghao Sun**, [Department of Physics and Astronomy](https://www.phys.utk.edu/), [University of Tennessee](http://www.utk.edu/), Knoxville, TN 37996-1200, USA and [Oak Ridge National Laboratory](http://www.ornl.gov/), Oak Ridge, TN, USA

 **Yu-Min Zhao**, [School of Physics and Astronomy, Shanghai Jiao Tong University](http://www.physics.sjtu.edu.cn/en/about/news/3), Shanghai 200240, P.R. China

Date: **Jul 17, 2018**

## Introduction

An essential element of the Talent courses is to develop a large
project(s) which allows you to study and understand theoretical
concepts in nuclear physics.  These concepts will in turn allow you to
interpret results from experiments and understand the pertinent
physics in terms of the underlying forces and laws of motion.

Together with the regular lectures in the morning, the hope is that
during these three weeks you will be able to write and run a program
which implements at least one of the methods discussed during the
lectures.  The lectures will also cover additional material which aims
at giving you a broader view on what can be achieved with the methods
to be discussed. Combined with the 'hands-on' afternoon sessions, the
hope is that the lectures and the computational projects will together
allow you to achieve these goals.

The project is divided in four main parts. The first part deals with
a simple pairing model and the development of a shell-model program
related to this model.  This program can then serve as a benchmark
program for the Coupled Cluster theory and in-medium SRG codes to be
developed. The latter form the remaining parts of the project.

If you have not used version control before now, it is time to do so.
Proper version control is central to a good ethical scientific conduct.
We do require that you use some kind of version control software when working on the projects. We recommend strongly [github](https://github.com/). All lectures and additional material are available at the github [address of the course](https://github.com/NuclearTalent/ManyBody2018).

Furthermore, before coming to the course, we recommend that you refresh your knowledge on second quantization.

## Some basic ingredients for a successful numerical project

When building up a numerical project there are several elements you should think of, amongst these we take the liberty of mentioning the following:
  1. How to structure a code in terms of functions

  2. How to make a module

  3. How to read input data flexibly from the command line

  4. How to create graphical/web user interfaces

  5. How to write unit tests (test functions)

  6. How to refactor code in terms of classes (instead of functions only), in our case you think of a system and a solver class

  7. How to conduct and automate large-scale numerical experiments

  8. How to write scientific reports in various formats (LaTeX, HTML)

The conventions and techniques outlined here will save you a lot of time when you incrementally extend software over time from simpler to more complicated problems. In particular, you will benefit from many good habits:
1. New code is added in a modular fashion to a library (modules)

2. Programs are run through convenient user interfaces

3. It takes one quick command to let all your code undergo heavy testing

4. Tedious manual work with running programs is automated,

5. Your scientific investigations are reproducible, scientific reports with top quality typesetting are produced both for paper and electronic devices.

We expect you to form working
groups consisting of typically three (or more) participants. Every
group should establish its own Github or Gitlab repository for the
project.

## Part 1, pairing problem

In the first part of the  project we will thus work with a simplified Hamiltonian consisting of a one-body operator and a so-called
 pairing interaction term. It is a model which to a large extent mimicks some central features of
atomic nuclei, certain atoms and systems which exhibit superfluiditity or superconductivity.
Pairing plays a central role in nuclear physics, in particular, for identical particles it makes up large fractions of the correlations among particles. The partial wave $^{1}S_0$ of the nucleon-nucleon force plays a central role in setting up pairing correlations in nuclei. Without this particular partial wave, the $J=0$ ground state spin assignment for many nuclei with even numbers of particles would not be possible.

We define first the Hamiltonian, with a definition of the model space and
the single-particle basis. Thereafter, we present the various steps which are needed to develop a shell-model program for studying the pairing problem.

The Hamiltonian acting in the complete Hilbert space (usually infinite
dimensional) consists of an unperturbed one-body part, $\hat{H}_0$,
and a perturbation $\hat{H}_I$.

We limit ourselves to at most two-body interactions, our Hamiltonian  is
then represented by the following operators

$$
\begin{equation}
\hat{H} = \hat{H}_0 +\hat{H}_I=\sum_{pq}\langle p |h_0|q\rangle a_{p}^{\dagger}a_{q} +\frac{1}{4}\sum_{pqrs}\langle pq| V|rs\rangle a_{p}^{\dagger}a_{q}^{\dagger}a_{s}a_{r},
\label{eq:hamiltonian} \tag{1}
\end{equation}
$$

where $a_{p}^{\dagger}$ and $a_{q}$ etc are standard fermion creation and annihilation operators, respectively,
and $pqrs$ represent all possible single-particle quantum numbers.
The full single-particle space is defined by the completeness relation
$\hat{1} = \sum_{p =1}^{\infty}|p \rangle \langle p|$.
In our calculations  we will let  the single-particle states $|p\rangle$
be eigenfunctions of  the one-particle operator $\hat{h}_0$.

The above Hamiltonian
acts in turn on various many-body Slater determinants constructed from the single-basis defined by the one-body
operator $\hat{h}_0$.

Our specific model consists of only $2$ doubly-degenerate and equally spaced
single-particle levels labeled by $p=1,2,\dots$ and spin $\sigma=\pm
1$.
In Eq. ([eq:hamiltonian](#eq:hamiltonian)) the labels $pqrs$ could also include spin $\sigma$. From now and for the rest of this project, labels like $pqrs$ represent the states without spin. The spin quantum numbers need to be accounted for explicitely.

We write
the Hamiltonian as

$$
\hat{H} = \hat{H}_0 +\hat{H}_I=\hat{H}_0 + \hat{V} ,
$$

where

$$
\hat{H}_0=\xi\sum_{p\sigma}(p-1)a_{p\sigma}^{\dagger}a_{p\sigma}.
$$

Here, $H_0$ is the unperturbed Hamiltonian with a spacing between
successive single-particle states given by $\xi$, which we will set to
a constant value $\xi=1$ without loss of generality.

The two-body
operator $\hat{V}$ has one term only. It represents the
pairing contribution and carries a constant strength $g$
and is  given by

$$
\langle q+q-| V|s+s-\rangle  = -g
$$

where $g$ is a constant. The above labeling means that for a  general matrix elements
$\langle pq| V|rs\rangle$ we require that the states $p$ and $q$ (and $r$ and $s$) have the same number
quantum number $q$ but opposite spins. The two spins values are
$\sigma = \pm 1$.
When setting up the Hamiltonian matrix you need to figure out how to make the two-body interaction antisymmetric.
The variables $\sigma=\pm$ represent the two possible spin values. The
interaction can only couple pairs and excites therefore only two
particles at the time.

In our model we have kept both the interaction strength and the single-particle level as constants.
In a realistic system like the atomic  nucleus this is not the case.

The   unperturbed Hamiltonian  $\hat{H}_0$ and $\hat{V}$  commute
with  the spin projection $\hat{S}_z$ and the total spin
$\hat{S}^2$.
This is an important feature of our system that allows us to block-diagonalize
the full Hamiltonian. In this project we will focus only on total spin $S=0$, this case is normally called the no-broken pair case.

### Part 1a: Paper and pencil gym while we wait for the more serious stuff

Show that the
unperturbed Hamiltonian  $\hat{H}_0$ and $\hat{V}$ commute
with both the spin projection $\hat{S}_z$ and the total spin
$\hat{S}^2$, given by

$$
\hat{S}_z := \frac{1}{2}\sum_{p\sigma} \sigma a^{\dagger}_{p\sigma}a_{p\sigma}
$$

and

$$
\hat{S}^2 := \hat{S}_z^2 + \frac{1}{2}(\hat{S}_+\hat{S}_- +
  \hat{S}_-\hat{S}_+),
$$

where

$$
\hat{S}_\pm := \sum_{p} a^{\dagger}_{p\pm} a_{p\mp}.
$$

This is an important feature of our system that allows us to block-diagonalize
the full Hamiltonian. We will focus on total spin $S=0$.
In this case, it is convenient to define the so-called pair creation and pair
annihilation operators

$$
\hat{P}^{+}_p = a^{\dagger}_{p+}a^{\dagger}_{p-},
$$

and

$$
\hat{P}^{-}_p = a_{p-}a_{p+},
$$

respectively.

The Hamiltonian (with $\xi=1$) we will use can be written as

$$
\hat{H}=\sum_{p\sigma}(p-1)a_{p\sigma}^{\dagger}a_{p\sigma}
-g\sum_{pq}\hat{P}^{+}_p\hat{P}^{-}_q.
$$

Show  that Hamiltonian commutes with the product of the pair creation and annihilation operators.
This model corresponds to a system with no broken pairs. This means that the Hamiltonian can only link two-particle states in so-called spin-reversed states.

### Part 1b: Simpler case

Assume now that the effective Hilbert space consists only of the two lowest single-particle states and that we have two particles only.
Set up the possible two-particle configurations when we have only two single-particle states, that is $p=1$ and $p=2$.
Construct thereafter the Hamiltonian matrix using second quantization and for example Wick's theorem
for a system with no broken pairs and spin $S=0$ (with projection $S_z=0$) for the case of the two lowest single-particle levels  and two particles only.  This gives you a
 $2\times 2$ matrix to be diagonalized.

Find the eigenvalues by diagonalizing the Hamiltonian matrix.
Vary your results for selected values of $g\in [-1,1]$ and comment your results.

### Part 1c: Setting up the Hamiltonian matrix

Construct thereafter the Hamiltonian matrix for a system with no broken pairs and spin $S=0$ for the case of the four lowest single-particle levels. Our system consists of four particles only.
Our single-particle space consists of only the four lowest levels
$p=1,2,3,4$.  You need to set up all possible Slater determinants and the Hamiltonian matrix using second quantization and
find all eigenvalues by diagonalizing the Hamiltonian matrix.
Vary your results for values of $g\in [-1,1]$.  Your Hamiltonian matrix is a $6\times 6$ matrix.
These results will serve as a benchmark for the construction  of our shell-model program and the CC and IMSRG programs.
We  refer to this as the exact results. Comment the behavior of the ground state as function of $g$.

To help, your final Hamiltonian matrix reads

$$
H = \left (
\begin{array}{cccccc}
2\delta -2g & -g & -g & -g & -g & 0 \\
 -g & 4\delta -2g & -g & -g & -0 & -g \\
-g & -g & 6\delta -2g & 0 & -g & -g \\
 -g & -g & 0 & 6\delta-2g & -g & -g \\
 -g & 0 & -g & -g & 8\delta-2g & -g \\
0 & -g & -g & -g & -g & 10\delta -2g
\end{array} \right )
$$

### Part 1d: Diagonalizing the Hamiltonian matrix

Our next step is to develop a code which sets up the above Hamiltonian matrices for two and four particles in 2 and 4 single-particles states (the same as what you did in exercises b) and c) and obtain the eigenvalues.
To achieve this you should

* Decide whether you want to read from file the single-particle data and the matrix elements in $m$-scheme, or set them up internally in your code. The latter is the simplest possibility for the pairing model, whereas the first option gives you a more general code which can be extended to the more realistic cases discussed in the second part.

* Based on the single-particle basis, write a function which sets up all possible Slater determinants which have total $M=0$.  Test that this function  reproduces the cases in b) and c). If you make this function more general, it can then be reused for say a shell-model calculation of $sd$-shell nuclei in the second part.

* Use the Slater determinant basis from the previous step to set up the Hamiltonian matrix.

* With the Hamiltonian matrix, you can finally diagonalize the matrix and obtain the final eigenvalues and test against the results of b) and c).

Codes to diagonalize in C++ or Fortran can be provided. For Python, numpy contains eigenvalue solvers based on for example Householder's and Givens' algorithms.   These are topics which can we discuss separately. The lecture slides contain a rather detailed recipe
on how to construct a Slater determinant basis and how to set up the Hamiltonian matrix to diagonalize.

### Part 1e: Further benchmarks and optional part

In developing the code it also useful to test against cases which have closed-form solutions. One obvious case is that of removing the
two-body interaction. Then we have only the single-particle energies.
For the case of degenerate single-particle orbits, that is one value of total single-particle angular momentum only $j$, with degeneracy $\Omega=2j+1$, one can show that the ground state energy $E_0$ is with $n$ particles

$$
E_0= -\frac{g}{4}n\left(\Omega-n+2\right).
$$

Challenge: Enlarge now your system to six and eight fermions and to $p=6$ and $p=8$ single-particle states, respectively. Run your program for a degenerate single-particle state with degeneracy $\Omega$ and test
against the exact result for the ground state. Introduce thereafter a finite single-particle spacing and study the results as you vary $g$,  as done in b) and c). Comment your results.

## Part 2: Coupled cluster calculations with doubles excitations only for the pairing model

This project serves as a continuation
of the pairing model with the aim being to solve the same problem but
now by developing a program that implements the coupled cluster method
with double excitations only. In doing so you will find it convenient
to write classes which define the single-particle basis and the
Hamiltonian. Your functions that solve the coupled cluster equations
will then just need to set up variables which point to interaction
elements and single-particle states with their pertinent quantum
numbers. Use for example the setup discussed in the FCI lectures for
the pairing model.

1. Explain why no single excitations are involved in this model.

2. Set up the coupled cluster equations for doubles excitations and convince yourself about their meaning and correctness.

3. Write a class which holds single-particle data like specific quantum numbers, single-particle Hamiltonian etc. Write also a class which sets up and stores two-body matrix elements defined by the single-particle states.  Write thereafter functions/classes which implement the coupled cluster method with doubles only.

4. Compare your results with tose from the exact diagonalization with and without the $4p4h$ excitation. Compare also your results to perturbation theory at different orders, in particular to second order. Discuss your results.

## Part 3: Coupled cluster calculations with doubles excitations only for infinite nuclear matter

This project forms one possible final path for the remaining two weeks. It can also be extended in order to define the final project.  You should be able to use the program you developed in connection with the
solution of the pairing model.

1. Explain why we don't have single excitations in infinite matter.

2. Set up the relavent quantum numbers for a cartesian basis with plane waves in three dimensions. Make the according changes to the code you developed in connection with the pairing model. Implement periodic boundary conditions.

3. Replace the two-body interaction from the pairing model with the Minnesota potential model discussed during the [lectures](https://github.com/NuclearTalent/Course2ManyBodyMethods/blob/master/doc/pub/cc/pdf/Lectures1-2_TALENT_NuclearMatter_GH.pdf).

4. Use the program you developed in connection with the pairing model to perform coupled cluster calculations in infinite matter with doubles excitations. Perform coupled cluster calculations for infinite neutron matter with the Minnesota interaction for $N=14$ neutrons. Limit yourself to two-particle and two-hole intermediate excitations only.

## Part 4: IMSRG code for the pairing model and infinite matter

Our final step consists in modifying the above program in order to include the IMSRG method, applying it to both the pairing model and infinite neutron matter.
Compare your results with those obtained with Coupled Cluster theory and comment your results obtained with the pairing model as well as those for  infinite neutron matter.


# Modeling Volumes and Multiple Compartments

This notebook example provides a basic demonstration on how to create and dynamically simulate multi-compartment models.

Illustrated below is the multi-compartment model utilized in this notebook:

In the above example:
* For metabolite **x**:
    * The biochemical pathway for the conversion of **x** occurs in the `large` compartment outlined by the  dotted black line.
* For metabolite **y**:
    * Cofactor **y** is necessary for the conversion of metabolite **x** in the biochemical pathway.
    * The synthesis of **y** occurs in the `medium` compartment outlined by the blue line.
    * `R_Ytr` is an antiporter, coupling the import of **y2** into the `large` compartment with the export of **y3** to the `medium` compartment.
* For metabolite **z**:
    * Protein **z** is synthesized in the `small` compartment outlined by the red line.
    * Protein **z** is also to facilitate the conversion of **x5** back into **x4** and for metabolic functions outside of the model's scope.

The reaction converting **x5** back to **x4** converted into **x5** The pair of irreversible reactions `R3_X` and `R_XZ` form a cycle that is used to  The synthesis and degradation of metabolite **y** occurs in the **medium compartment** outlined in blue.

COBRApy is currently in the process of developing improved compartment handling. These changes are outlined in the following COBRApy issues:

* https://github.com/opencobra/cobrapy/pull/725
* https://github.com/opencobra/cobrapy/projects/5

MASSpy is awaiting these COBRApy changes in order to improve how compartments are handled in dynamic simulations, SBML compatibity, etc. Once these changes have been implemented in COBRApy, a new version of MASSpy will be developed and released with improved functionality around compartments and their handling.

## Models with Multiple Compartments

```python
import sympy as sym

from mass import (
    MassConfiguration, MassMetabolite, MassModel, MassReaction, Simulation)
from mass.example_data import create_example_model
from mass.visualization import plot_time_profile
model = create_example_model("MultiCompartment")
```

    Set parameter Username

### Viewing compartments in a model
The `MassModel.compartments` attribute is used to get `dict` with compartment identifiers and their corresponding names.

```python
model.compartments
```

    {'l': 'Large', 'm': 'Medium', 's': 'Small'}

The names for the compartments can be reset or changed by using the `MassModel.compartments` attribute setter method. To reset compartment names, pass an empty dict:

```python
model.compartments = {}
```

To set a new name for a compartment, set a `dict` using the `MassModel.compartments` method with the compartment identifer as the key and the compartment name as the value. Compartments can be set one at a time, or multiple at once:

```python
model.compartments = {"l": "the large compartment"}
print(model.compartments)

model.compartments = {"m": "the medium compartment", "s": "the small compartment"}
print(model.compartments)
```

    {'l': 'the large compartment', 'm': '', 's': ''}
    {'l': 'the large compartment', 'm': 'the medium compartment', 's': 'the small compartment'}

#### Volume units

To get a list of all UnitDefinition(s) that contain a volume base unit, an modified filter that scans the base units can be applied:

```python
def volumes_filter(udef):
    if list(filter(lambda u: u.kind in ["liter","litre"], udef.list_of_units)):
        return True
    return False
print(model.units.query(volumes_filter))
```

    [<UnitDefinition Milliliters "mL" at 0x7f7af86e95e0>, <UnitDefinition Concentration "mol_per_mL" at 0x7f7af86e9730>]

### Enabling compartment volumes in rate laws
By default, the ``MassConfiguration.exclude_compartment_volumes_in_rates`` is set as ``True``.

```python
mass_config = MassConfiguration()
print(mass_config.exclude_compartment_volumes_in_rates)
```

    True

Therefore, all automatically generated mass action rate laws do not include the compartment volume:

```python
print(model.reactions.get_by_id("R2_X").rate)
```

    kf_R2_X*x3_l(t)

To enable compartment volumes in rate laws, the ``MassConfiguration.exclude_compartment_volumes_in_rates`` attribute must be set to ``False``.

```python
mass_config.exclude_compartment_volumes_in_rates = False
print(model.reactions.get_by_id("R2_X").rate)
```

    kf_R2_X*volume_l*x3_l(t)

As seen above, volume parameters are added into the rate laws to represent compartment volumes. The volume parameters
have identifiers of format `volume_CID` , with `CID` referring to the compartment identifier (e.g., "l" for large compartment).
For a reaction that crosses compartments, more than one "volume" parameter will appear as a variable in the rate:

```python
for param in model.reactions.get_by_id("R_Ytr").rate.atoms(sym.Symbol):
    if str(param).find("volume") != -1:
        print(param)
```

    volume_m
    volume_l

See the section on [Excluding compartments from rates](../tutorials/global_configuration.ipynb#excluding-compartments-from-rates) in the [Global Configuration](../tutorials/global_configuration.ipynb) tutorial for more information about the ``exclude_compartment_volumes_in_rates`` attribute.

### The "boundary" compartment

In boundary reactions (e.g., pseudeoreactions such as sinks, demands, and exchanges), metabolites that exist in the `boundary` a.k,a. the boundary conditions, are given a default "boundary" compartment with the identifier "b". This compartment is treated as a pseudo-compartment, and therefore the 'boundary' metabolites are treated as pseudo-metabolites, meaning no corresponding object is created for them.

Boundary metabolites can be accessed either through the `MassReaction.boundary_metabolite` method.

```python
x1_b = model.reactions.get_by_id("SK_x1_l").boundary_metabolite
x1_b
```

    'x1_b'

If a reaction is not a boundary reaction (i.e., `MassReaction.boundary==False`) then `None` will be returned:

```python
print(model.reactions.get_by_id("R_Ytr").boundary_metabolite)
```

    None

The `boundary_metabolite` attribute is useful for getting and setting values in the `MassModel.boundary_conditions` attribute.

```python
model.boundary_conditions[x1_b] = 2
model.boundary_conditions
```

    {'x1_b': 2}

To change the 'boundary' compartment identifier and name, a `dict` is passed to the `MassConfiguration.boundary_compartment` attribute setter:

```python
print("Before: {0}\n{1}".format(mass_config.boundary_compartment, model.boundary_metabolites))
mass_config.boundary_compartment = {"xt": "External compartment"}
print("\nAfter: {0}\n{1}".format(mass_config.boundary_compartment, model.boundary_metabolites))
```

    Before: {'b': 'boundary'}
    ['x1_b', 'x5_b', 'y1_b', 'y4_b', 'z1_b', 'z2_b']

    After: {'xt': 'External compartment'}
    ['x1_xt', 'x5_xt', 'y1_xt', 'y4_xt', 'z1_xt', 'z2_xt']

The "boundary" compartment is automatically assumed to have a volume of 1, and therefore is not factored in the rate laws. It is also ignored by the `MassModel.compartments` attribute, even when explicitly set:

```python
for r in model.sinks:
    print("{0}: {1}".format(r.id, r.get_mass_action_rate()))
model.compartments = {"xt": "External compartment"}
model.compartments
```

    SK_y1_m: kf_SK_y1_m*y1_xt
    SK_z1_s: kf_SK_z1_s*z1_xt

    {'l': 'the large compartment',
     'm': 'the medium compartment',
     's': 'the small compartment'}

See the section on [For compartments and SBML](../tutorials/global_configuration.ipynb#for-compartments-and-SBML) in the [Global Configuration](../tutorials/global_configuration.ipynb) tutorial for more information about the ``boundary_compartment`` attribute.

The 'boundary' pseudo-compartment and 'boundary' pseudo-metabolites are designed to make working with boundary conditions convenient at the cost of having finer user control. This primarily useful for
* Setting functions as boundary conditions (e.g., an oscillating function for external oxygen concentration)
* Using custom rates to set fixed inputs, causing irrelevant boundary conditions to be ignored altogether.

However, for finer control over external compartment and boundary conditions (and general best practices for SBML compatibility in MASSpy), it is recommended to (1) create new `MassMetabolite` objects, define their `compartment` and `initial_condition` attributes, (2) set the  `fixed` attribute as `True`, and (3) add the metabolites to the appropriate reactions. This ensures the concentration of the metabolite is fixed at a constant value, and that its initial condition value is treated as a boundary condition.

#### Fixed inputs
To bypass using the 'boundary' pseudo-compartment, it is recommended to set a fixed input using a custom rate law:

```python
for r in model.reactions.get_by_any(["SK_x1_l", "SK_y1_m", "SK_z1_s"]):
    model.add_custom_rate(r, custom_rate=r.kf_str)
    print("{0}: {1}".format(r.id, r.rate))
```

    SK_x1_l: kf_SK_x1_l
    SK_y1_m: kf_SK_y1_m
    SK_z1_s: kf_SK_z1_s

## Getting and setting compartment volumes
Support for compartment volumes is currently through the `MassModel.custom_parameters` attribute. To view what compartment volumes are set:

```python
def volume_filter(parameter):
    if str(parameter).startswith("volume"):
        return True
    return False

for vol_id in filter(volume_filter, model.custom_parameters):
    print("{0}: {1}".format(vol_id, model.custom_parameters[vol_id]))
```

    volume_l: 10.0
    volume_m: 5.0
    volume_s: 1.0

To set or change a compartment volume, the value in the `MassModel.custom_parameters` dict is set using the volume parameter ID as the key:

```python
# Set the large compartment volume to 15
model.custom_parameters["volume_l"] = 15

# Double current medium compartment volume
model.custom_parameters["volume_m"] = model.custom_parameters["volume_m"] * 2

# 10% decrease to current small compartment volume
model.custom_parameters["volume_s"] = model.custom_parameters["volume_s"] * (1 + (-10/100))

for vol_id in filter(volume_filter, model.custom_parameters):
    print("{0}: {1}".format(vol_id, model.custom_parameters[vol_id]))
```

    volume_l: 15
    volume_m: 10.0
    volume_s: 0.9

## Simulating with Volumes and Multiple Compartments
Using a newly loaded model, the following section provides guidance on dynamic simulations for models with multiple compartments and includes examples on perturbing compartment volume.

```python
# Ensure compartments are active and boundary compartment is reset
mass_config.exclude_compartment_volumes_in_rates = False
mass_config.boundary_compartment = {'b': 'boundary'}

# Start with a fresh model, checking to ensure compartment volumes are reset
model = create_example_model("MultiCompartment")
for vol_id in filter(volume_filter, model.custom_parameters):
    print("{0}: {1}".format(vol_id, model.custom_parameters[vol_id]))
```

    volume_l: 10.0
    volume_m: 5.0
    volume_s: 1.0

As always, a model must first must be loaded into the `mass.Simulation`. object in order to run a simulation. A quick simulation shows that the model is already at a steady state:

```python
simulation = Simulation(model, verbose=True)
conc_sol = simulation.simulate(model, time=(0, 1000))[0]
plot_time_profile(conc_sol, plot_function="loglog", legend="right outside")
```

A volume parameter can be perturbed just like any other parameter using a `dict`. For example, suppose volume of the large compartment `volume_m` lost 40% of its volume:

```python
conc_sol = simulation.simulate(model, time=(0, 1000), perturbations={
    "volume_m": "volume_m * (1 - 0.4)",
})[0]
plot_time_profile(conc_sol, plot_function="loglog", legend="right outside")
```

Note that in the above simulation, several of the metabolite concentrations in the large compartment changed. The `observable` argument can be used with the `MassMetabolite.compartment` attribute to look at metabolites in a specific compartment for futher examination

```python
plot_time_profile(conc_sol, observable=list(model.metabolites.query(lambda m: m.compartment == "m")),
                  plot_function="semilogx", legend="right outside")
```

Multiple volumes also can be perturbed simultaneously. For example, suppose 1 mL of fluid from the `large` compartment was transfered to the `small` compartment, while 1.5 mL was transfered to the `medium` compartment:

```python
conc_sol = simulation.simulate(model, time=(0, 1000), perturbations={
    "volume_l": "volume_l - 2.5",
    "volume_m": "volume_m + 1.5",
    "volume_s": "volume_s + 1.0"
})[0]
plot_time_profile(conc_sol, plot_function="loglog", legend="right outside")
```

Helpful tips: When enabling compartment volumes, it is up to the user to track their units to ensure that no numerical consistency issues arise. To make this a bit easier, be aware of the following MASSpy expectations and behaviors:

* When compartment volumes are disabled, MASSpy expects that volumes are already factored into initial condition values, and therefore considers values to be initial concentrations. Consequently, metabolite solutions returned by solutions will be for metabolite concentrations (e.g., mol/L, g/cDW)

* When compartment volumes are enabled, MASSpy expects that volumes have not been factored factored into initial condition values, and therefore considers values to be initial amounts.
    Consequently, metabolite solutions returned by solutions will be for metabolite amounts (e.g., mol, grams)


```python
import gluoncv as gcv
import mxnet as mx
# mxnet NDarray
from mxnet import nd

# MXNet Gluon Neural network layers.
from mxnet.gluon import nn

# optional for displaying the image
import ipyplot  # Need to remove this and quit using it
import matplotlib.pyplot as plt
import numpy as np
import os
import math
# pip install pillow but import PIL
from PIL import Image
```

# Loading images via  PIL and numpy

```python
# setup file system locations
# yes, this should use os file path TODO: fix it
TOP_DIR='../'
print('TOP_DIR = ', TOP_DIR)
ICY_BRIDGE_DIR = TOP_DIR + 'icy-bridge/'
print('TOP_DIR = ', ICY_BRIDGE_DIR)
MODEL_NAME = 'c6'
MODEL_TOP_DIR = ICY_BRIDGE_DIR + MODEL_NAME + '/'
print('MODEL_TOP_DIR = ', MODEL_TOP_DIR)
MODEL_RAW_IMAGE_DIR = MODEL_TOP_DIR + 'raw/'
MODEL_POST_IMAGE_DIR = MODEL_TOP_DIR + 'post_rotate_crop/'
print('MODEL_RAW_IMAGE_DIR = ', MODEL_RAW_IMAGE_DIR)
print('MODEL_POST_IMAGE_DIR = ', MODEL_POST_IMAGE_DIR)
```

    TOP_DIR =  ../
    TOP_DIR =  ../icy-bridge/
    MODEL_TOP_DIR =  ../icy-bridge/c6/
    MODEL_RAW_IMAGE_DIR =  ../icy-bridge/c6/raw/
    MODEL_POST_IMAGE_DIR =  ../icy-bridge/c6/post_rotate_crop/

```python
# test to make sure directory is correct
my_limit = 3
for a_obj_name in os.listdir(MODEL_RAW_IMAGE_DIR):
    my_limit = my_limit - 1
    if my_limit < 0:
        break
    print(a_obj_name)

```

    BUS40_SALEM_20220115Z1655.jpg
    BUS40_SALEM_20220117Z0940.jpg
    BUS40_SALEM_20220114Z1255.jpg

```python
# load an image using numpy and display it.
def show_it(an_nparray, viewit=False):
    if viewit:
        print("array is ", an_nparray)
    print("shape is ", an_nparray.shape)
    print("dtype is ", an_nparray.dtype)
    print("ndim is ", an_nparray.ndim)
    print("itemsize is ", an_nparray.itemsize) # size in bytes of each array element
    print("nbytes is ", an_nparray.nbytes) # size in bytes of each array element
    if viewit:
        print("view is ", an_nparray.view)
    #print("order is ", an_array.order)
    #print("type is ", img_filename_array.type)
    #print("descr is ", img_filename_array.descr)
    if viewit:
        ipyplot.plot_images(an_nparray)
```

```python
# a grayscale png
stimpyG_filepath = 'imgs/stimpyG.png'
stimpyG_nparray = np.array([stimpyG_filepath])
# array(['imgs/stimpyG.png'], dtype='<U16')
show_it(stimpyG_nparray, True)
# show it says this is a scalar.  Its just a filename.
# The filename is passed to ipyplot and it dumps the image
# by reading the filename
print(stimpyG_nparray[0]) # there is not index 1
# you can also tell if you try to plot it as an array
# It will fail
#plt.imshow(stimpyG_nparray, cmap='gray')

# you can
png_pil_img = Image.open(stimpyG_filepath)
print(png_pil_img.format, png_pil_img.size, png_pil_img.mode)
png_np_img = np.asarray(png_pil_img)
plt.imshow(png_np_img, cmap='gray')
show_it(png_np_img)
```

```python
# a color jpeg
jpegC_filepath = MODEL_RAW_IMAGE_DIR + 'BUS40_SALEM_20220114Z1255.jpg'
# This is a jpeg. Its compressed images
jpegC_nparray = np.array([jpegC_filepath])
print("show the jpeg as an array")
show_it(jpegC_nparray, True)
# this doesn't work because its not a bitmap.

# you can work around it using python image library to perfrom decompression

jpegC_pil_img = Image.open(jpegC_filepath)
# this will show its JPEG 704x480 and RGB (color)
print(jpegC_pil_img.format, jpegC_pil_img.size, jpegC_pil_img.mode)
# sadly this does not work on my box since it has this bug
# https://github.com/python-pillow/Pillow/issues/5945
# jpegC_pil_img.sho

# convert to nparray
jpegC_np_img = np.asarray(jpegC_pil_img)
# note this will fail with ipyplot
#show_it(jpegC_np_img)
# But if I try to print it using just the array, which it is now it will
plt.imshow(jpegC_np_img)
show_it(jpegC_np_img)

```

All of this is immaterial because I want to use mxnet to do my transforms, but it helps to have example code
around if I suspect something is wrong.  I can use alternative means to manipulate it.

# Loading images via Apache MXNet

```python
def show_it_mxnet(a_mxnet_image):
    plt.imshow(a_mxnet_image.asnumpy())
    print("type: ", type(a_mxnet_image))
    print("shape: ", a_mxnet_image.shape)

```

```python
# a color jpeg
jpegC_filepath = MODEL_RAW_IMAGE_DIR + 'BUS40_SALEM_20220114Z1255.jpg'

jpeg_30255 = mx.image.imread(jpegC_filepath, flag=1) # flag=1 for color flag=0 for grayscale
show_it_mxnet(jpeg_30255)
```

```python
# convert to float - matplotlib can print floats but only [0--1]
jpeg_30255f = jpeg_30255.astype('float32')
jpeg_301f = jpeg_30255f/255
show_it_mxnet(jpeg_301f)

```

# convert to grayscale

```python
# grayscale augmentation
# https://mxnet.apache.org/versions/1.6/api/python/docs/api/mxnet/image/index.html#mxnet.image.RandomGrayAug

# general getting started tutorials
# https://mxnet.apache.org/versions/1.6/api/python/docs/tutorials/getting-started/index.html

# how to use image augmentation
# https://mxnet.apache.org/versions/1.2.1/tutorials/python/types_of_data_augmentation.html

```

```python
!wget https://raw.githubusercontent.com/dmlc/web-data/master/mxnet/doc/tutorials/data_aug/inputs/0.jpg
```

    --2022-02-14 11:48:49--  https://raw.githubusercontent.com/dmlc/web-data/master/mxnet/doc/tutorials/data_aug/inputs/0.jpg
    Resolving raw.githubusercontent.com (raw.githubusercontent.com)... 185.199.109.133, 185.199.108.133, 185.199.111.133, ...
    Connecting to raw.githubusercontent.com (raw.githubusercontent.com)|185.199.109.133|:443... connected.
    HTTP request sent, awaiting response... 200 OK
    Length: 194713 (190K) [image/jpeg]
    Saving to: ‘0.jpg.1’

    0.jpg.1             100%[===================>] 190.15K  --.-KB/s    in 0.03s

    2022-02-14 11:48:49 (5.57 MB/s) - ‘0.jpg.1’ saved [194713/194713]

```python
def plot_mx_array(array):
    """
    Array expected to be height x width x 3 (channels), and values are floats between 0 and 255.
    """
    assert array.shape[2] == 3, "RGB Channel should be last"
    plt.imshow((array.clip(0, 255)/255).asnumpy())
```

```python
example_image = mx.image.imread("./0.jpg")
example_image = example_image.astype("float32")
plot_mx_array(example_image)
```

```python
example_image_copy = example_image.copy()
aug = mx.image.RandomGrayAug(p=1)
aug_image = aug(example_image_copy)
plot_mx_array(aug_image)
```

```python
aug = mx.image.RandomGrayAug(p=1)
jpeg_101f = aug(jpeg_301f)
show_it_mxnet(jpeg_101f)
```

# Rotate

From this source

https://en.wikipedia.org/wiki/Rotation_matrix

\begin{vmatrix}
\cos(\theta) & -\sin(\theta) \\
\sin(\theta) & \cos(\theta)
\end{vmatrix}

rotates points in the xy plane counterclockwise through an angle θ with respect to the positive x axis about the origin of a two-dimensional Cartesian coordinate system. To perform the rotation on a plane point with standard coordinates v = (x, y), it should be written as a column vector, and multiplied by the matrix R:

\begin{align}
\begin{vmatrix}
\cos(\theta) & -\sin(\theta) \\
\sin(\theta) & \cos(\theta)
\end{vmatrix}
\begin{vmatrix} x \\ y \end{vmatrix} =
\begin{vmatrix}
x \cos(\theta) - y \sin(\theta) \\
x \sin(\theta) + y \cos(\theta)
\end{vmatrix}
\end{align}

```python
numpy_ex_int_array = np.array([
                                  [[1,2]],
                                  [[4,5]]
                              ], dtype=np.int32)
```

```python
numpy_ex_int_array
```

    array([[[1, 2]],

           [[4, 5]]], dtype=int32)

```python
mx_ex_int_array = mx.nd.array(numpy_ex_int_array)
mx_ex_int_array
```

    [[[1. 2.]]

     [[4. 5.]]]
    <NDArray 2x1x2 @cpu(0)>

```python
def build_rot_matrix(angle):
    #  a  b
    #  c  d

    # calculate terms
    a = math.cos(x)
    b = -math.sin(x)
    c = math.sin(x)
    d = math.cos(x)

    # print("a = ", a)
    # print("b = ", b)
    # print("c = ", c)
    # print("d = ", d)

    # build np array
    np_array = np.array([
        [[a,b]],
        [[c,d]]
    ])

    # build mx.nd array
    nd_array = mx.nd.array(np_array)

    return nd_array
```

```python
x=0.5*math.pi

rotate_90 = build_rot_matrix(x)
```

```python
# create a weight constant
#w = mx.init.Constant(rotate_90)
# create a nn. You need to do this.  Calling the layer.initialize() function below
# will not update the filter without it?
#layer = nn.Conv2D(channels=1, kernel_size=(3,3), in_channels=1, strides=(1,1), padding=(0,0), activation='relu', prefix='conv_')

# init layer with this constant
#layer.initialize(w)

image_HWC1 = mx.image.imread('imgs/testy-grid.png', 0)
image_HWC1 = mx.image.imread('imgs/stimpyG.png', 0)
image_HWC1f = image_HWC1.astype('float32')

# So that its channel first.
#image_CHWf = jpeg_101f.transpose((2,0,1))
#image_CHWf = image_HWC1f.transpose((2,0,1))
# Convert 3D to 4D with leading batch channel.
#image_NCHWf = image_CHWf.expand_dims(axis=0)
# Now its in NCHW format
#output = layer(image_NCHWf)
# output is a 4D tensor in NCHW format
# Use squeeze to remove any 1-D dimension
#result_numpy_HWC = output.squeeze().asnumpy()
# show result
#plt.imshow(result_numpy_HWC, cmap='gray')
```

```python
jpeg_101f.shape
```

    (480, 704, 3)

```python
image_H
```

https://mxnet.apache.org/versions/1.5.0/tutorials/basic/reshape_transpose.html

```python

```