# Single Particle Model (SPM)

## Model Equations

The SPM consists of two spherically symmetric diffusion equations: one within a representative negative particle ($\text{k}=\text{n}$) and one within a representative positive particle ($\text{k}=\text{p}$). In the centre of the particle the classical no-flux condition is imposed. Since the SPM assumes that all particles in an electrode behave in exactly the same way, the flux on the surface of a particle is simply the current $I$ divided by the thickness of the electrode $L_{\text{k}}$. The concentration of lithium in electrode $\text{k}$ is denoted $c_{\text{k}}$ and the current is denoted by $I$. All parameters in the model stated here are dimensionless and are given in terms of dimensional parameters at the end of this notebook. The model equations for the SPM are then:
\begin{align}
\mathcal{C}_{\text{k}} \frac{\partial c_{\text{s,k}}}{\partial t} &= -\frac{1}{r_{\text{k}}^2} \frac{\partial}{\partial r_{\text{k}}} \left(r_{\text{k}}^2 N_{\text{s,k}}\right), \\
N_{\text{s,k}} &= -D_{\text{s,k}}(c_{\text{s,k}}) \frac{\partial c_{\text{s,k}}}{\partial r_{\text{k}}}, \quad \text{k} \in \text{n, p}, \end{align}
$$
N_{\text{s,k}}\big|_{r_{\text{k}}=0} = 0, \quad \text{k} \in \text{n, p}, \quad \ \ - \frac{a_{\text{k}}\gamma_{\text{k}}}{\mathcal{C}_{\text{k}}} N_{\text{s,k}}\big|_{r_{\text{k}}=1} =
\begin{cases}
		  \frac{I}{L_{\text{n}}}, \quad &\text{k}=\text{n}, \\
		  -\frac{I}{L_{\text{p}}}, \quad &\text{k}=\text{p},
\end{cases} \\
c_{\text{s,k}}(r_{\text{k}},0) = c_{\text{s,k,0}}, \quad \text{k} \in \text{n, p},$$
where $D_{\text{s,k}}$ is the diffusion coefficient in the solid, $N_{\text{s,k}}$ denotes the flux of lithium ions in the solid particle within the region $\text{k}$, and $r_{\text{k}} \in[0,1]$ is the radial coordinate of the particle in electrode $\text{k}$.

### Voltage Expression
The terminal voltage is obtained from the expression:
$$
V = U_{\text{p}}(c_{\text{p}})\big|_{r_{\text{p}}=1} - U_{\text{n}}(c_{\text{n}})\big|_{r_{\text{n}}=1} -2\sinh^{-1}\left(\frac{I}{j_{\text{0,p}} L_{\text{p}}}\right) - 2\sinh^{-1}\left(\frac{I}{j_{\text{0,n}} L_{\text{n}}}\right)
$$
with the exchange current densities given by
$$j_{\text{0,k}} =  \frac{\gamma_{\text{k}}}{\mathcal{C}_{\text{r,k}}}(c_{\text{k}})^{1/2}(1-c_{\text{k}})^{1/2}  $$

## Example solving SPM using PyBaMM

Below we show how to solve the SPM model, using the default geometry, mesh, paramters, discretisation and solver provided with PyBaMM.

First we need to import `pybamm`, and then change our working directory to the root of the pybamm folder.

```python
import pybamm
import numpy as np
import os
import matplotlib.pyplot as plt
os.chdir(pybamm.__path__[0]+'/..')
```

We then get the SPM model equations:

```python
model = pybamm.lithium_ion.SPM()
```

The model object is a subtype of [`pybamm.BaseModel`](https://pybamm.readthedocs.io/en/latest/source/models/base_model.html), and contains all the equations that define this particular model. For example, the `rhs` dict contained in `model` has a dictionary mapping variables such as $c_n$ to the equation representing its rate of change with time (i.e. $\partial{c_n}/\partial{t}$). We can see this explicitly by visualising this entry in the `rhs` dict:

```python
variable = next(iter(model.rhs.keys()))
equation = next(iter(model.rhs.values()))
print('rhs equation for variable \'',variable,'\' is:')
path = 'examples/notebooks/models/'
equation.visualise(path+'spm1.png')
```

    rhs equation for variable ' X-averaged negative particle concentration ' is:

We need a geometry to define our model equations over. In pybamm this is represented by the [`pybamm.Geometry`](https://pybamm.readthedocs.io/en/latest/source/geometry/geometry.html) class. In this case we use the default geometry object defined by the model

```python
geometry = model.default_geometry
```

This geometry object defines a number of geometry domains, each with its own name, spatial variables and min/max limits (the latter are represented as equations similar to the rhs equation shown above). For instance, the SPM model has the following domains:

```python
print('SPM domains:')
for i, (k, v) in enumerate(geometry.items()):
    print(str(i+1)+'.',k,'with variables:')
    for var, rng in v['primary'].items():
        if 'min' in rng:
            print('  -(',rng['min'],') <=',var,'<= (',rng['max'],')')
        else:
            print(var, '=', rng['position'])
```

    SPM domains:
    1. negative electrode with variables:
      -( 0.0 ) <= x_n <= ( Negative electrode thickness [m] / Negative electrode thickness [m] + Separator thickness [m] + Positive electrode thickness [m] )
    2. separator with variables:
      -( Negative electrode thickness [m] / Negative electrode thickness [m] + Separator thickness [m] + Positive electrode thickness [m] ) <= x_s <= ( Negative electrode thickness [m] / Negative electrode thickness [m] + Separator thickness [m] + Positive electrode thickness [m] + Separator thickness [m] / Negative electrode thickness [m] + Separator thickness [m] + Positive electrode thickness [m] )
    3. positive electrode with variables:
      -( Negative electrode thickness [m] / Negative electrode thickness [m] + Separator thickness [m] + Positive electrode thickness [m] + Separator thickness [m] / Negative electrode thickness [m] + Separator thickness [m] + Positive electrode thickness [m] ) <= x_p <= ( 1.0 )
    4. current collector with variables:
    z = 1.0
    5. negative particle with variables:
      -( 0.0 ) <= r_n <= ( 1.0 )
    6. positive particle with variables:
      -( 0.0 ) <= r_p <= ( 1.0 )

Both the model equations and the geometry are defined by a set of parameters, such as $\gamma_p$ or $L_p$. We can substitute these symbolic parameters in the model with values by using the [`pybamm.ParameterValues`](https://pybamm.readthedocs.io/en/latest/source/parameters/parameter_values.html) class, which takes either a python dictionary or CSV file with the mapping between parameter names and values. Rather than create our own instance of `pybamm.ParameterValues`, we will use the default parameter set included in the model

```python
param = model.default_parameter_values
```

We can then apply this parameter set to the model and geometry

```python
param.process_model(model)
param.process_geometry(geometry)
```

The next step is to mesh the input geometry. We can do this using the [`pybamm.Mesh`](https://pybamm.readthedocs.io/en/latest/source/meshes/meshes.html) class. This class takes in the geometry of the problem, and also two dictionaries containing the type of mesh to use within each domain of the geometry (i.e. within the positive or negative electrode domains), and the number of mesh points.

The default mesh types and the default number of points to use in each variable for the SPM model are:

```python
for k, t in model.default_submesh_types.items():
    print(k,'is of type',t.__repr__())
for var, npts in model.default_var_pts.items():
    print(var,'has',npts,'mesh points')
```

    negative electrode is of type Generator for Uniform1DSubMesh
    separator is of type Generator for Uniform1DSubMesh
    positive electrode is of type Generator for Uniform1DSubMesh
    negative particle is of type Generator for Uniform1DSubMesh
    positive particle is of type Generator for Uniform1DSubMesh
    current collector is of type Generator for SubMesh0D
    x_n has 20 mesh points
    x_s has 20 mesh points
    x_p has 20 mesh points
    r_n has 10 mesh points
    r_p has 10 mesh points
    y has 10 mesh points
    z has 10 mesh points

With these defaults, we can then create our mesh of the given geometry:

```python
mesh = pybamm.Mesh(geometry, model.default_submesh_types, model.default_var_pts)
```

The next step is to discretise the model equations over this mesh. We do this using the [`pybamm.Discretisation`](https://pybamm.readthedocs.io/en/latest/source/discretisations/discretisation.html) class, which takes both the mesh we have already created, and a dictionary of spatial methods to use for each geometry domain. For the case of the SPM model, we use the following defaults for the spatial discretisation methods:

```python
for k, method in model.default_spatial_methods.items():
    print(k,'is discretised using',method.__class__.__name__,'method')
```

    macroscale is discretised using FiniteVolume method
    negative particle is discretised using FiniteVolume method
    positive particle is discretised using FiniteVolume method
    current collector is discretised using ZeroDimensionalMethod method

We then create the `pybamm.Discretisation` object, and use this to discretise the model equations

```python
disc = pybamm.Discretisation(mesh, model.default_spatial_methods)
disc.process_model(model)
```

    <pybamm.models.full_battery_models.lithium_ion.spm.SPM at 0x7f7230ee96a0>

After this stage, all the equations in `model` have been discretised into purely linear algebra expressions that are ready to be evaluated within a time-stepping loop of a given solver. For example, the rhs expression for $\partial{c_n}/\partial{t}$ that we visualised above is now represented by:

```python
model.concatenated_rhs.children[0].visualise(path+'spm2.png')
```

Now we are ready to run the time-stepping loop to solve the model. Once again we use the default ODE solver like so:

```python
# Solve the model at the given time points
solver = model.default_solver
n = 250
t_eval = np.linspace(0, 1, n)
print('Solving using',type(solver).__name__,'solver...')
solution = solver.solve(model, t_eval)
print('Finished.')
```

    Solving using ScipySolver solver...
    Finished.

    /home/scott/Projects/PyBaMM/venv/lib/python3.7/site-packages/scipy/integrate/_ivp/ivp.py:146: RuntimeWarning: invalid value encountered in greater_equal
      up = (g <= 0) & (g_new >= 0)
    /home/scott/Projects/PyBaMM/venv/lib/python3.7/site-packages/scipy/integrate/_ivp/ivp.py:147: RuntimeWarning: invalid value encountered in less_equal
      down = (g >= 0) & (g_new <= 0)

Each model in pybamm has a list of relevent variables defined in the model, for use in visualising the model solution or for comparison with other models. The SPM model defines the following variables:

```python
print('SPM model variables:')
for v in model.variables.keys():
    print('\t-',v)
```

    SPM model variables:
    	- Total current density
    	- Total current density [A.m-2]
    	- Current [A]
    	- Time
    	- Time [s]
    	- Time [min]
    	- Time [h]
    	- Discharge capacity [A.h]
    	- x
    	- x [m]
    	- x_n
    	- x_n [m]
    	- x_s
    	- x_s [m]
    	- x_p
    	- x_p [m]
    	- Negative particle concentration
    	- Positive particle concentration
    	- Negative particle surface concentration
    	- Positive particle surface concentration
    	- Negative particle concentration [mol.m-3]
    	- Positive particle concentration [mol.m-3]
    	- Negative particle surface concentration [mol.m-3]
    	- Positive particle surface concentration [mol.m-3]
    	- r_n
    	- r_n [m]
    	- r_p
    	- r_p [m]
    	- Porosity
    	- Negative electrode porosity
    	- Separator porosity
    	- Positive electrode porosity
    	- X-averaged negative electrode porosity
    	- X-averaged separator porosity
    	- X-averaged positive electrode porosity
    	- Leading-order negative electrode porosity
    	- Leading-order separator porosity
    	- Leading-order positive electrode porosity
    	- Leading-order x-averaged negative electrode porosity
    	- Leading-order x-averaged separator porosity
    	- Leading-order x-averaged positive electrode porosity
    	- Porosity change
    	- Negative electrode porosity change
    	- Separator porosity change
    	- Positive electrode porosity change
    	- X-averaged porosity change
    	- X-averaged negative electrode porosity change
    	- X-averaged separator porosity change
    	- X-averaged positive electrode porosity change
    	- Leading-order x-averaged negative electrode porosity change
    	- Leading-order x-averaged separator porosity change
    	- Leading-order x-averaged positive electrode porosity change
    	- Volume-averaged velocity
    	- Volume-averaged velocity [m.s-1]
    	- Electrolyte pressure
    	- X-averaged negative particle concentration
    	- X-averaged negative particle concentration [mol.m-3]
    	- X-averaged negative particle surface concentration
    	- X-averaged negative particle surface concentration [mol.m-3]
    	- Negative electrode active volume fraction
    	- Negative electrode volume-averaged concentration
    	- Negative electrode volume-averaged concentration [mol.m-3]
    	- Negative electrode average extent of lithiation
    	- X-averaged positive particle concentration
    	- X-averaged positive particle concentration [mol.m-3]
    	- X-averaged positive particle surface concentration
    	- X-averaged positive particle surface concentration [mol.m-3]
    	- Positive electrode active volume fraction
    	- Positive electrode volume-averaged concentration
    	- Positive electrode volume-averaged concentration [mol.m-3]
    	- Positive electrode average extent of lithiation
    	- Electrolyte concentration
    	- Electrolyte concentration [mol.m-3]
    	- Electrolyte concentration [Molar]
    	- X-averaged electrolyte concentration
    	- X-averaged electrolyte concentration [mol.m-3]
    	- X-averaged electrolyte concentration [Molar]
    	- Negative electrolyte concentration
    	- Negative electrolyte concentration [mol.m-3]
    	- Negative electrolyte concentration [Molar]
    	- Separator electrolyte concentration
    	- Separator electrolyte concentration [mol.m-3]
    	- Separator electrolyte concentration [Molar]
    	- Positive electrolyte concentration
    	- Positive electrolyte concentration [mol.m-3]
    	- Positive electrolyte concentration [Molar]
    	- X-averaged negative electrolyte concentration
    	- X-averaged negative electrolyte concentration [mol.m-3]
    	- X-averaged separator electrolyte concentration
    	- X-averaged separator electrolyte concentration [mol.m-3]
    	- X-averaged positive electrolyte concentration
    	- X-averaged positive electrolyte concentration [mol.m-3]
    	- Electrolyte flux
    	- Electrolyte flux [mol.m-2.s-1]
    	- Negative current collector temperature
    	- Negative current collector temperature [K]
    	- X-averaged negative electrode temperature
    	- X-averaged negative electrode temperature [K]
    	- Negative electrode temperature
    	- Negative electrode temperature [K]
    	- X-averaged separator temperature
    	- X-averaged separator temperature [K]
    	- Separator temperature
    	- Separator temperature [K]
    	- X-averaged positive electrode temperature
    	- X-averaged positive electrode temperature [K]
    	- Positive electrode temperature
    	- Positive electrode temperature [K]
    	- Positive current collector temperature
    	- Positive current collector temperature [K]
    	- Cell temperature
    	- Cell temperature [K]
    	- X-averaged cell temperature
    	- X-averaged cell temperature [K]
    	- Volume-averaged cell temperature
    	- Volume-averaged cell temperature [K]
    	- Heat flux
    	- Heat flux [W.m-2]
    	- Negative current collector potential
    	- Negative current collector potential [V]
    	- Current collector current density
    	- Current collector current density [A.m-2]
    	- Leading-order current collector current density
    	- Negative electrode interfacial current density
    	- X-averaged negative electrode interfacial current density
    	- Negative electrode interfacial current density [A.m-2]
    	- X-averaged negative electrode interfacial current density [A.m-2]
    	- Negative electrode interfacial current density per volume [A.m-3]
    	- X-averaged negative electrode interfacial current density per volume [A.m-3]
    	- X-averaged negative electrode total interfacial current density
    	- X-averaged negative electrode total interfacial current density [A.m-2]
    	- X-averaged negative electrode total interfacial current density per volume [A.m-3]
    	- Negative electrode exchange current density
    	- X-averaged negative electrode exchange current density
    	- Negative electrode exchange current density [A.m-2]
    	- X-averaged negative electrode exchange current density [A.m-2]
    	- Negative electrode exchange current density per volume [A.m-3]
    	- X-averaged negative electrode exchange current density per volume [A.m-3]
    	- Negative electrode reaction overpotential
    	- X-averaged negative electrode reaction overpotential
    	- Negative electrode reaction overpotential [V]
    	- X-averaged negative electrode reaction overpotential [V]
    	- Negative electrode surface potential difference
    	- X-averaged negative electrode surface potential difference
    	- Negative electrode surface potential difference [V]
    	- X-averaged negative electrode surface potential difference [V]
    	- Negative electrode open circuit potential
    	- Negative electrode open circuit potential [V]
    	- X-averaged negative electrode open circuit potential
    	- X-averaged negative electrode open circuit potential [V]
    	- Negative electrode entropic change
    	- X-averaged negative electrode entropic change
    	- Positive electrode interfacial current density
    	- X-averaged positive electrode interfacial current density
    	- Positive electrode interfacial current density [A.m-2]
    	- X-averaged positive electrode interfacial current density [A.m-2]
    	- Positive electrode interfacial current density per volume [A.m-3]
    	- X-averaged positive electrode interfacial current density per volume [A.m-3]
    	- X-averaged positive electrode total interfacial current density
    	- X-averaged positive electrode total interfacial current density [A.m-2]
    	- X-averaged positive electrode total interfacial current density per volume [A.m-3]
    	- Positive electrode exchange current density
    	- X-averaged positive electrode exchange current density
    	- Positive electrode exchange current density [A.m-2]
    	- X-averaged positive electrode exchange current density [A.m-2]
    	- Positive electrode exchange current density per volume [A.m-3]
    	- X-averaged positive electrode exchange current density per volume [A.m-3]
    	- Positive electrode reaction overpotential
    	- X-averaged positive electrode reaction overpotential
    	- Positive electrode reaction overpotential [V]
    	- X-averaged positive electrode reaction overpotential [V]
    	- Positive electrode surface potential difference
    	- X-averaged positive electrode surface potential difference
    	- Positive electrode surface potential difference [V]
    	- X-averaged positive electrode surface potential difference [V]
    	- Positive electrode open circuit potential
    	- Positive electrode open circuit potential [V]
    	- X-averaged positive electrode open circuit potential
    	- X-averaged positive electrode open circuit potential [V]
    	- Positive electrode entropic change
    	- X-averaged positive electrode entropic change
    	- Interfacial current density
    	- Interfacial current density [A.m-2]
    	- Interfacial current density per volume [A.m-3]
    	- Exchange current density
    	- Exchange current density [A.m-2]
    	- Exchange current density per volume [A.m-3]
    	- Negative particle flux
    	- X-averaged negative particle flux
    	- Positive particle flux
    	- X-averaged positive particle flux
    	- Negative electrode potential
    	- Negative electrode potential [V]
    	- X-averaged negative electrode potential
    	- X-averaged negative electrode potential [V]
    	- Negative electrode ohmic losses
    	- Negative electrode ohmic losses [V]
    	- X-averaged negative electrode ohmic losses
    	- X-averaged negative electrode ohmic losses [V]
    	- Gradient of negative electrode potential
    	- Negative electrode current density
    	- Negative electrode current density [A.m-2]
    	- Negative electrolyte potential
    	- Negative electrolyte potential [V]
    	- Separator electrolyte potential
    	- Separator electrolyte potential [V]
    	- Positive electrolyte potential
    	- Positive electrolyte potential [V]
    	- Electrolyte potential
    	- Electrolyte potential [V]
    	- X-averaged electrolyte potential
    	- X-averaged electrolyte potential [V]
    	- X-averaged negative electrolyte potential
    	- X-averaged negative electrolyte potential [V]
    	- X-averaged separator electrolyte potential
    	- X-averaged separator electrolyte potential [V]
    	- X-averaged positive electrolyte potential
    	- X-averaged positive electrolyte potential [V]
    	- X-averaged electrolyte overpotential
    	- X-averaged electrolyte overpotential [V]
    	- Gradient of negative electrolyte potential
    	- Gradient of separator electrolyte potential
    	- Gradient of positive electrolyte potential
    	- Gradient of electrolyte potential
    	- Electrolyte current density
    	- Electrolyte current density [A.m-2]
    	- Negative electrolyte current density
    	- Negative electrolyte current density [A.m-2]
    	- Positive electrolyte current density
    	- Positive electrolyte current density [A.m-2]
    	- X-averaged concentration overpotential
    	- X-averaged electrolyte ohmic losses
    	- X-averaged concentration overpotential [V]
    	- X-averaged electrolyte ohmic losses [V]
    	- Positive electrode potential
    	- Positive electrode potential [V]
    	- X-averaged positive electrode potential
    	- X-averaged positive electrode potential [V]
    	- Positive electrode ohmic losses
    	- Positive electrode ohmic losses [V]
    	- X-averaged positive electrode ohmic losses
    	- X-averaged positive electrode ohmic losses [V]
    	- Gradient of positive electrode potential
    	- Positive electrode current density
    	- Positive electrode current density [A.m-2]
    	- Electrode current density
    	- Positive current collector potential
    	- Ohmic heating
    	- Ohmic heating [W.m-3]
    	- Irreversible electrochemical heating
    	- Irreversible electrochemical heating [W.m-3]
    	- Reversible heating
    	- Reversible heating [W.m-3]
    	- Total heating
    	- Total heating [W.m-3]
    	- X-averaged total heating
    	- X-averaged total heating [W.m-3]
    	- Volume-averaged total heating
    	- Volume-averaged total heating [W.m-3]
    	- Positive current collector potential [V]
    	- Local current collector potential difference
    	- Local current collector potential difference [V]
    	- X-averaged open circuit voltage
    	- Measured open circuit voltage
    	- X-averaged open circuit voltage [V]
    	- Measured open circuit voltage [V]
    	- X-averaged reaction overpotential
    	- X-averaged reaction overpotential [V]
    	- X-averaged solid phase ohmic losses
    	- X-averaged solid phase ohmic losses [V]
    	- Terminal voltage
    	- Terminal voltage [V]
    	- X-averaged battery open circuit voltage [V]
    	- Measured battery open circuit voltage [V]
    	- X-averaged battery reaction overpotential [V]
    	- X-averaged battery solid phase ohmic losses [V]
    	- X-averaged battery electrolyte ohmic losses [V]
    	- X-averaged battery concentration overpotential [V]
    	- Battery voltage [V]

To help visualise the results, pybamm provides the `pybamm.ProcessedVariable` class, which takes the output of a solver and a variable, and allows the user to evaluate the value of that variable at any given time or $x$ value. For example, we can create a `pybamm.ProcessedVariable` using the SPM voltage variable, and plot the voltage versus time, along with the surface particle concentration.

```python
voltage = pybamm.ProcessedVariable(model.variables['Terminal voltage [V]'], solution.t, solution.y, mesh=mesh)#
c_s_n_surf = pybamm.ProcessedVariable(
    model.variables['Negative particle surface concentration'], solution.t, solution.y, mesh=mesh
)
c_s_p_surf = pybamm.ProcessedVariable(
    model.variables['Positive particle surface concentration'], solution.t, solution.y, mesh=mesh
)
```

One we have these variables in hand, we can begin generating plots using a library such as Matplotlib. Below we plot the terminal voltage and surface particle concentrations versus time

```python
t = np.linspace(0,1,250)
f, (ax1, ax2, ax3) = plt.subplots(1, 3, figsize=(13,4))

ax1.plot(solution.t, voltage(solution.t))
ax1.set_xlabel(r'$t$')
ax1.set_ylabel('Terminal voltage')

ax2.plot(solution.t, c_s_n_surf(t=solution.t, x=0))
ax2.set_xlabel(r'$t$')
ax2.set_ylabel('Negative particle surface concentration')

ax3.plot(solution.t, c_s_p_surf(t=solution.t, x=1))
ax3.set_xlabel(r'$t$')
ax3.set_ylabel('Positive particle surface concentration')

plt.tight_layout()
plt.show()
```

Some of the output variables are defined over space as well as time. Once option to visualise these variables is to use the `interact` slider widget. Below we plot the negative/positive particle concentration over $r$, using a slider to change the current time point

```python
c_s_n = pybamm.ProcessedVariable(model.variables['Negative particle concentration'], solution.t, solution.y, mesh=mesh)
c_s_p = pybamm.ProcessedVariable(model.variables['Positive particle concentration'], solution.t, solution.y, mesh=mesh)
r = np.linspace(0,1,100)

def plot_concentrations(t):
    f, (ax1, ax2) = plt.subplots(1, 2 ,figsize=(10,5))
    plot_c_n, = ax1.plot(r, c_s_n_surf(x=r,t=t))
    plot_c_p, = ax2.plot(r, c_s_p_surf(x=r,t=t))
    ax1.set_ylabel('Negative particle concentration')
    ax2.set_ylabel('Positive particle concentration')
    ax1.set_xlabel(r'$r_n$')
    ax2.set_xlabel(r'$r_p$')
    ax1.set_ylim(0, 1)
    ax2.set_ylim(0, 1)
    plt.show()

import ipywidgets as widgets
widgets.interact(plot_concentrations, t=widgets.FloatSlider(min=0,max=0.17,step=0.01,value=0));

```

    interactive(children=(FloatSlider(value=0.0, description='t', max=0.17, step=0.01), Output()), _dom_classes=('…

The QuickPlot class can be used to plot the common set of useful outputs which should give you a good initial overview of the model. The method Quickplot.plot(t) is simply a function like plot_concentrations(t) above. We can therefore either use it statically for a particularl t or employ the slider widget.

```python
quick_plot = pybamm.QuickPlot(model, mesh, solution)
widgets.interact(quick_plot.plot, t=widgets.FloatSlider(min=0,max=quick_plot.max_t,step=0.05,value=0));
```

    interactive(children=(FloatSlider(value=0.0, description='t', max=1.0850295540089985, step=0.05), Output()), _…

## Dimensionless Parameters

In the table below, we provide the dimensionless parameters in the SPM in terms of the dimensional parameters in LCO.csv. We use a superscript * to indicate dimensional quanitities.

| Parameter                 | Expression                              |Interpretation                             |
|:--------------------------|:----------------------------------------|:------------------------------------------|
| $L_{\text{k}}$            | $L_{\text{k}}^*/L^*$                    | Ratio of region thickness to cell thickness|
|$\mathcal{C}_{\text{k}}$   | $\tau_{\text{k}}^*/\tau_{\text{d}}^*$   | Ratio of solid diffusion and discharge timescales |
|$\mathcal{C}_{\text{r,k}}$ |$\tau_{\text{r,k}}^*/\tau_{\text{d}}^*$  |Ratio of reaction and discharge timescales|
|$a_{\text{k}}$             |$a_{\text{k}}^* R_{\text{k}}^*$          | Product of particle radius and surface area density|
|$\gamma_{\text{k}}$        |$c_{\text{k,max}}^*/c_{\text{n,max}}^*$  |Ratio of maximum lithium concentrations in solid|

```python

```


# Transform to local coordinates

# Purpose
Ship model coordinates are given in world fix coordinates: x0,y0,psi.
Velocities will be transformed:

$$
    \begin{bmatrix}
    \frac{dX}{dt} \\
    \frac{dX}{dt} \\
    r \\
    \end{bmatrix}
    ->
    \begin{bmatrix}
    u \\
    v \\
    r \\
    \end{bmatrix}
$$

# Methodology
Transform with rotation of $\psi$

# Setup

```python
# %load imports.py
%matplotlib inline
%load_ext autoreload
%autoreload 2

## External packages:
import pandas as pd
pd.options.display.max_rows = 999
pd.options.display.max_columns = 999
pd.set_option("display.max_columns", None)

import numpy as np
import os
import matplotlib.pyplot as plt

import plotly.express as px
import plotly.graph_objects as go

import seaborn as sns
import sympy as sp
from sympy.physics.mechanics import (dynamicsymbols, ReferenceFrame,
                                      Particle, Point)
from sympy.physics.vector.printing import vpprint, vlatex
from IPython.display import display, Math, Latex
from src.substitute_dynamic_symbols import run, lambdify

import pyro

import sklearn
import pykalman
from statsmodels.sandbox.regression.predstd import wls_prediction_std
import statsmodels.api as sm

from scipy.integrate import solve_ivp
import seaborn as sns

## Local packages:
from src.data import mdl

```

    Duplicate key in file WindowsPath('C:/Users/maa/.matplotlib/stylelib/paper.mplstyle'), line 462 ('figure.figsize   : 5, 3   ## figure size in inches')
    Duplicate key in file WindowsPath('C:/Users/maa/.matplotlib/stylelib/paper.mplstyle'), line 463 ('figure.dpi       : 100        ## figure dots per inch')

```python
id=22773
df, units, meta_data = mdl.load(id=id)
```

```python
from src.visualization.plot import track_plot
fig,ax=plt.subplots()
fig.set_size_inches(10,10)
track_plot(df=df, lpp=meta_data.lpp, x_dataset='x0', y_dataset='y0',  psi_dataset='psi', beam=meta_data.beam, ax=ax);
```

```python
t = df.index.total_seconds()
df['x01d_gradient'] = np.gradient(df['x0'], t)
df['y01d_gradient'] = np.gradient(df['y0'], t)
df['z01d_gradient'] = np.gradient(df['z0'], t)
df['z02d_gradient'] = np.gradient(df['z01d_gradient'], t)

df['psi1d_gradient'] = np.gradient(df['psi'], t)
```

```python
from pykalman import KalmanFilter

def filter(df, key='x0', observation_covariance = 100000):

    df = df.copy()

    dt = t[1] - t[0]

    A = np.array([[1, dt, 0.5*(dt**2)],
                  [0,  1, dt         ],
                  [0,  0, 1          ],
                    ])

    kf = KalmanFilter(transition_matrices=A,
                      initial_state_mean = [df[key].iloc[0:100].median(),df[f'{key}1d_gradient'].iloc[0:100].median(),0],
                      random_state=np.random.RandomState(0),
                      transition_covariance=100 * np.eye(3),
                      observation_covariance=observation_covariance * np.eye(1),

                      em_vars=[
                          'transition_covariance',
                          'observation_covariance',
                          'initial_state_mean',
                      #    'initial_state_covariance'
                          ],

                      )

    observations = df[key]
    states_pred = kf.em(observations).smooth(observations)[0]

    df[f'{key}_filtered'] = states_pred[:,0]
    df[f'{key}1d'] = states_pred[:,1]
    df[f'{key}2d'] = states_pred[:,2]

    return df
```

```python
df = filter(df=df, key='x0', observation_covariance = 1000000)
df = filter(df=df, key='y0', observation_covariance = 100000)
df = filter(df=df, key='psi', observation_covariance = 100000)
```

```python
df.plot(y='x01d')
df.plot(y='y01d')
```

```python
from scipy.spatial.transform import Rotation as R
```

```python
rotation = R.from_euler('z', df['psi_filtered'], degrees=False)
```

```python
df[['u','v','w']] = rotation.inv().apply(df[['x01d','y01d','z01d_gradient']])
df[['u1d','v1d','w1d']] = rotation.inv().apply(df[['x02d','y02d','z02d_gradient']])

df['r'] = df['psi1d']
df['r1d'] = df['psi2d']
df['beta'] = -np.arctan2(df['v'],df['u'])

```

```python
df.plot(y='u')
df.plot(y='v')
df.plot(y='r')
df.plot(y='beta')

df.plot(y='u1d')
df.plot(y='v1d')
df.plot(y='r1d')

```

```python

```


## 引言
我一直认为没有必要在考试中求截面矩和质心一类玩意，因为现在的电脑太容易求出这些几何性质的量了。Ansys和AutoCAD都内置了相关函数，其实就算要求解析解也不困难，这里就做个演示。

```python
%pylab inline
from sympy import Polygon, Point, var, init_printing, simplify, series
import sympy
init_printing()
var('b,h,t')
from second_moments import second_moments
```

    Populating the interactive namespace from numpy and matplotlib

## 矩形截面：
假设宽为b，高为h。

```python
p = map(Point,((0,0),(b,0),(b,h),(0,h)))
poly = Polygon(*p)
poly.area
```

```python
poly.centroid
```

```python
Ix, Iy, Ixy = second_moments(poly)
Ix, Iy, Ixy
```

## L形截面
假设厚度都是t,宽为b，高为h

```python
p = map(Point,((0,0),(b,0),(b,t),(t,t),(t,h),(0,h)))
poly = Polygon(*p)
poly.area
```

```python
simplify(poly.centroid)
```

```python
Ix, Iy, Ixy = simplify(second_moments(poly))
Ix, Iy, Ixy
```

你实际上还可以得到$t$是小量时候的二阶近似

```python
simplify(sympy.series(Ix,t,n=2).removeO())
```

```python
simplify(sympy.series(Iy,t,n=2).removeO())
```

```python
simplify(sympy.series(Ixy,t,n=2).removeO())
```

## 六边形：
外接圆半径为R的吧！

```python
var('R')
p = [(sympy.cos(i)*R,sympy.sin(i)*R) for i in arange(6)/sympy.S.One/3*sympy.pi]
poly = Polygon(*p)
p
```

```python
poly.area
```

```python
poly.centroid
```

```python
second_moments(poly)
```

## 工字梁
等厚为$t$的情况。

```python
p = map(Point,((0,0),(b,0),(b,t),(b/2+t/2,t),(b/2+t/2,h-t),
(b,h-t),(b,h),(0,h),(0,h-t),(b/2-t/2,h-t),(b/2-t/2,t),(0,t))
)
poly = Polygon(*p)
poly.area,poly.centroid
```

```python
I = second_moments(poly)
Ix,Iy,Ixy=map(simplify,I)
Ix, Iy, Ixy
```

```python
series(Ix,t,0,2), series(Iy,t,0,2), series(Ixy,t,0,2)
```

翼缘和肋板不等厚的情况，翼缘为$t1$，肋板为$t2$的情况。

```python
var('t1,t2')
p = map(Point,((0,0),(b,0),(b,t1),(b/2+t2/2,t1),(b/2+t2/2,h-t1),
(b,h-t1),(b,h),(0,h),(0,h-t1),(b/2-t2/2,h-t1),(b/2-t2/2,t1),(0,t1))
)
poly = Polygon(*p)
poly.area,poly.centroid
```

```python
I = second_moments(poly)
Ix,Iy,Ixy=map(simplify,I)
Ix, Iy, Ixy
```

```python
series(series(series(Ix,t1,n=2),t2,n=2).removeO().expand(),t1*t2,n=1).removeO()
```

```python
series(series(series(Iy,t1,n=2),t2,n=2).removeO().expand(),t1*t2,n=1).removeO()
```

```python
series(series(series(Ixy,t1,n=2),t2,n=2).removeO().expand(),t1*t2,n=1).removeO()
```

## 槽钢的情况
等厚为$t$

```python
p = map(Point,((0,0),(b,0),(b,t),(t,t),(t,h-t),
(b,h-t),(b,h),(0,h)))
poly = Polygon(*p)
poly.area,poly.centroid
```

```python
I = second_moments(poly)
Ix,Iy,Ixy=map(simplify,I)
Ix, Iy, Ixy
```

```python
simplify(series(Ix,t,0,2).removeO()), simplify(series(Iy,t,0,2).removeO()), series(Ixy,t,0,2)
```

```python

```


# Radar FMCW and CSM algorithms

This notebook shows a naive (didactically intuitive) implementation of following algorithms:

- FMCW (Frequency Modulated Continuous Wave)
- CSM (Chirp Sequence Modulation)

Both algorithms are used for range and velocity measurements in automated/assisted driving domain.

## Introduction

### Doppler

An electromagnetic wave

\begin{align}
w(t) &= a_t cos(2\pi f_0t)
\end{align}

reflected on a moving object with velocity $v$ leads to a new wave:

\begin{align}
u_r(t) &= a_r \text{cos}(2\pi (f_0 - f_{d}) t + \phi_r + \phi_t) \\

\end{align}

In order to understand what signal the algorithms operate on we have to take a closer look at the inner workings of a continuous wave radar (simplified view):

[Source](https://en.wikipedia.org/wiki/Continuous-wave_radar#/media/File:Bsp2_CW-Radar.EN.png)

### Antennas

The CW-RADAR has one transmitting antenna and at least one receiving antenna. The former emits a continuous radio wave, that's different to pulse-based RADAR systems. The receiving antenna transforms the reflected waves from the object of interests and converts the signal into eletrical domain (voltage).

Automotive long range radars use a $77.7\text{GHz}$ base frequency.

### RF-Generator

The RF-generator is a module which dynamically generates a sine voltage with a particular frequency. Since both methods presented here modulate (alter) the frequency of the tramitted radio wave, the generator is an essential part in the system.

### Mixer

The mixer is the most important component in the system described here. It allows us to measure the frequency change introduced by the moving object we are going to detect. We apply to the antenna the voltage $u_t(t)$ and receive the voltage $u_r(t)$.

\begin{align}
u_t(t) &= A_t \text{cos}(2\pi f_0 t+\phi_t) \\
u_r(t) &= A_r \text{cos}(2\pi (f_0 - f_{d}) t + \phi_r)
\end{align}

With Doppler frequency

\begin{align}
f_{d} = \frac{2v}{\lambda} = \frac{2 \dot r}{\lambda} = \frac{2 f_0 \dot r}{c}
\end{align}

Given base wavelength $\lambda$ and target velocity $v=\dot r$.

Theoretically, if we could sample $f_0 - f_{d} \approx 77.7GHz$ and we were done. But high frequency AD converters (according to Nyquist $\geq 2f_0 \approx 145GHz$ is required) are techinically expensive and the change introduced by $f_d$ is very small to detect changes. That's why we need the mixer to get rid of the high frequency $f_0$ and work only on the $f_d$.

The mixer multiplies both signals, corresponding to trigonometry relations:

\begin{align}
\text{cos}(x)\text{cos}(y) = \frac{1}{2} [ \text{cos}(x-y) + \text{cos}(x+y)]
\end{align}

Applied to $u_t(t)$ and $u_r(t)$:

\begin{align}
u_t(t)u_r(t) &= \frac{A_t A_r}{2} [ \text{cos}(2\pi f_0 t+\phi_t-2\pi (f_0 - f_{d}) t - \phi_r) + \text{cos}(2\pi f_0 t+\phi_t+ 2\pi (f_0 - f_{d}) t + \phi_r)] \\
&= \frac{A_t A_r}{2} [ \text{cos}(2\pi f_{d} t + \phi_t - \phi_r) + \text{cos}(2\pi f_0 t+\phi_t + 2\pi (f_0- f_{d}) t + \phi_r)] \\
&= \frac{A_t A_r}{2} [ \text{cos}(2\pi f_{d} t + \phi_t - \phi_r) + \text{cos}(2\pi \underbrace{2 f_0}_{\text{2x 77GHz}} t - 2\pi f_{d}t + \phi_t + \phi_r)] \\
\end{align}

The product of both signals equals to a sum of two waves, the first, only dependent on the doppler frequency (which is velocity dependent) and the second a very high frequency signal (144 GHz) which we filter out with the low pass filter:

\begin{align}
u_{It,r}(t) = \frac{A_t A_r}{2} \text{cos}(2\pi f_{d} t + \phi_t - \phi_r)
\end{align}

After the AD converter we could sample $u_{It,r}(t)$ and measure the (unsigned) velocity. The index $I$ means in-phase. We later will introduce an other index in order to extract the sign of the velocity (approaching vs. departing).

```python
import numpy as np
import matplotlib.pyplot as plt
import pandas as pd
from scipy.fftpack import fft
```

Constant for velocity of light within the air:

```python
c = 299792458
```

## FCMW

Instead of sending with one frequency, we will alter $f_0$ over time:

\begin{align}
u_t(t) &= A_t \text{cos}(2\pi f_t t+\phi_t)
\end{align}

with

\begin{align}
f_t = f_t(t) = f_0 + m_w(t-t_0)
\end{align}

```python
f_0 = 77.7*1e9 # 77.7GHz

phi_t = 0
A_t = 1
f_ramp = 425*1e6 #Hz
T_ramp = 0.010
m_w = f_ramp/T_ramp
```

```python
def f_t(t):
    return f_0 + m_w*(t % T_ramp)
```

```python
def u_t(t):
    f_t = f_t(t)
    return A_t*np.cos(2*np.pi*f_b*t + phi_t)
```

The received signal depends on both the range $r$ and the velocity $v$:

\begin{align}
u_r(t) &= A_r \text{cos}(2\pi f_r t+\phi_r)
\end{align}

with

\begin{align}
f_r = f_r(t) = f_0 + m_w(t- \frac{2r}{c}-t_0) - \frac{2vf_0}{c}
\end{align}

```python
v = 50/3.6 # 50km/h
r = 100 # 100m distance
```

```python
def f_r(t):
    return f_0 + m_w*(t % T_ramp - 2*r/c) - 2*v*f_0/c
```

```python
t = np.arange(0, 2*T_ramp, 1e-8)
```

```python
plt.figure(figsize=(15,5))
plt.plot(t, f_t(t), label="$f_t$")
plt.plot(t, f_r(t), label="$f_r$")
plt.legend()
plt.xlabel("t [s]")
plt.ylabel("frequency [Hz]")
plt.title("Transmitted and received frequencies")
plt.grid();
```

Because we don't see any difference let's zoom in

```python
t_op = T_ramp
plt.figure(figsize=(10,10))
plt.plot(t, f_t(t), label="$f_t$")
plt.plot(t, f_r(t), label="$f_r$")
plt.legend()
plt.xlim([t_op-T_ramp/10000, t_op+T_ramp/10000])
plt.ylim([f_0-100e3, f_0+100e3])
plt.xlabel("t [s]")
plt.ylabel("frequency [Hz]")
plt.title("Transmitted and received frequencies")
plt.grid();
```

If we could measure $\Delta f$ directly we could derive $r$ and $v$:

\begin{align}
\Delta f = f_t-f_r = \frac{2m_w}{c}r + \frac{2 f_0}{c}\dot r
\end{align}

There is just one problem. We have one equation but two unknowns. The solution for that problem is to have a second ramp with a different $m_w$:

\begin{align}
f_t(t) = \{\begin{array}{lr}
        f_0 + m_{w,1}(t-t_0), & \text{for } 0 \leq t < T_{r,1}\\
        f_0 + m_w T_{r,1} + m_{w,2}(t-t_0), & \text{for } T_{r,1} \leq t < T_{r,1}+T_{r,2}\
        \end{array}
\end{align}

Define and implement both waveforms:

```python
T_ramp1 = T_ramp*0.7
T_ramp2 = T_ramp*0.3
m_w1 = f_ramp/T_ramp1
m_w2 = -f_ramp/T_ramp2
```

```python
def f_t(t):
    r1 = f_0 + m_w1*(t % (T_ramp1+T_ramp2))
    r1[(t%(T_ramp1+T_ramp2)) > T_ramp1] = 0

    r2 = f_0 + m_w1*T_ramp1 + m_w2*( (t-T_ramp1) % (T_ramp1+T_ramp2))
    r2[(t%(T_ramp1+T_ramp2)) <= T_ramp1] = 0

    return r1+r2
```

```python
def f_r(t):
    r1 = f_0 + m_w1*(t % (T_ramp1+T_ramp2) - 2*r/c) - 2*v*f_0/c
    r1[(t%(T_ramp1+T_ramp2)) > T_ramp1] = 0

    r2 = f_0 + m_w1*T_ramp1 + m_w2*( (t-T_ramp1) % (T_ramp1+T_ramp2) - 2*r/c) - 2*v*f_0/c
    r2[(t%(T_ramp1+T_ramp2)) <= T_ramp1] = 0
    return r1+r2
```

```python
t = np.arange(0, 2*T_ramp, 1e-7)
plt.figure(figsize=(15,5))
plt.plot(t, f_t(t), label="$f_t$")
plt.plot(t, f_r(t), label="$f_r$")
plt.legend()
plt.xlabel("t [s]")
plt.ylabel("frequency [Hz]")
plt.title("FMCW ramps with different $m_{w,1} \\neq m_{w,2}$")
plt.grid();
```

Zoom in again:

```python
t_op = T_ramp
plt.figure(figsize=(10,5))
plt.plot(t, f_t(t), label="$f_t$")
plt.plot(t, f_r(t), label="$f_r$")
plt.legend()
plt.xlim([t_op-T_ramp/5000, t_op+T_ramp/5000])
plt.ylim([f_0-100e3, f_0+600e3])
plt.xlabel("t [s]")
plt.ylabel("frequency [Hz]")
plt.title("FMCW ramps with different $m_{w,1} \\neq m_{w,2}$")
plt.grid();
```

Now we have two equations:

\begin{align}
\Delta f_1 = \frac{2m_{w,1}}{c}r + \frac{2 f_0}{c}\dot r \\
\Delta f_2 = \frac{2m_{w,2}}{c}r + \frac{2 f_0}{c}\dot r
\end{align}

Solving the equation system:

```python
t_f1 = np.asarray([T_ramp*0.1]) # beginning of rising ramp
t_f2 = np.asarray([T_ramp*0.8])  # beginning of falling ramp
delta_f_1 = f_t(t_f1) - f_r(t_f1)
delta_f_2 = f_t(t_f2) - f_r(t_f2)
```

```python
A = np.asarray([[2*m_w1/c, 2*f_0/c], [2*m_w2/c, 2*f_0/c]])
Y = np.asarray([delta_f_1, delta_f_2])
```

```python
x = np.linalg.solve(A, Y).flatten()
print("range r:", x[0])
print("velocity v:", x[1]*3.6)
```

    range r: 99.99999999693145
    velocity v: 49.99999988508472

As you could see we could successfully estimate the range and velocity.

**Note**: we get $\Delta f_i$ for free as a result of the FFT on the mixed and filtered signal

\begin{align}
u_{It,r}(t) = \frac{A_t A_r}{2} \text{cos}(2\pi (f_t - f_r) t + \phi_t - \phi_r) = \frac{A_t A_r}{2} \text{cos}(2\pi \Delta f t + \phi_t - \phi_r)
\end{align}

In the FMCW chapter we did not address various problems like:

- multi target estimation (what happens if the FFT returns 2, 3, 4, ... major peaks?)
- angle estimation

We will solve the first problem in our second method.

## Chirp Sequence Modulation

This method is an extension of FMCW. Instead of sending one singe rising and falling frequency chirp we will repeat that process $n_r$ times in a sequence with chirp frequency $f_{chirp}$:

```python
# chirp sequence frequency
f_chirp = 50*1e3 #Hz

# ramp frequency
f_r = 200*1e6 #Hz
T_r = 1/f_chirp # duration of one cycle
m_w = f_r/T_r

n_r = 150 # number of chirps
T_M = T_r*n_r
```

Because we perform analog sampling, we have to configure our AD-converter:

```python
# sample settings
f_s = 50e6 #50 MHz
n_s = int(T_r*f_s)
```

Base frequency setup

```python
f_0 = 77.7*1e9

# some helpful
w_0 = 2*np.pi*f_0
lambda_0 = c/f_0
```

```python
def f_transmitted(t):
    return f_0 + m_w*(t%T_r)
```

```python
def chirp(t):
    return np.cos(2*np.pi*(f_transmitted(t))*t)
```

Lets visualize the chirp sequence:

```python
t = np.linspace(0, 3*T_r, 1e6)
```

```python
plt.figure(figsize=(15,5))
plt.plot(t, f_transmitted(t))
plt.xlabel("t [s]")
plt.ylabel("frequency [Hz]")
plt.title("Chirp sequence Modulation, transmitted signal $f_t(t)$");
```

### Reflector setup

This is our target of interest

```python
r_0 = 50 # initial distance
v_veh = 36/3.6 # velocity
```

```python
def get_range(t):
    return r_0+v_veh*t
```

### Returned waveform

According to Eq. 17.46 in [1](https://www.springer.com/de/book/9783658057336) the returned waveform (after mixing and LF-filter) is:

\begin{align}
u_{It,r}(t) = \frac{A_t A_r}{2} \text{cos}\left(2\pi(\frac{2m_w}{c}r + \frac{2 f_0}{c}\dot r)t + \frac{4 \pi f_0 r}{c} + 2\pi(\frac{2 r}{c})^2m_w\right)
\end{align}

```python
4*np.pi*100*f_0/c, 2*np.pi*(2*100/c)**2*m_w
```

    (325694.31641129137, 27.963945936914552)

As you can see $2\pi(\frac{2 r}{c})^2m_w$ has not so much influence on the phase, so we ignore it:

\begin{align}
u_{It,r}(t) = \frac{A_t A_r}{2} \text{cos}\left(2\pi(\frac{2m_w}{c}r + \frac{2 f_0}{c}\dot r)t + \frac{4 \pi f_0 r}{c}\right)
\end{align}

```python
def itr(t):
    r = get_range(t)
    w_itr = 2*f_0*v_veh/c + 2*m_w*r/c
    # we do t%T_r because the eq. above only valid within the ramp
    v = np.cos(2*np.pi*w_itr*(t%T_r) +2*r*2*np.pi*f_0/c)
    return v
```

We build up a table of $n_r × n_s$ where $n_r$ is the number of chirps (ramps) and $n_s$ is the number of samples within a chirp.

```python
print(n_r, n_s)
```

    150 1000

```python
t_sample = np.linspace(0, T_M, n_r*n_s)
```

```python
v_sample = itr(t_sample)
```

```python
plt.figure(figsize=(15,5))
plt.plot(t_sample, v_sample, "+")
plt.xlim(0, 0.1*T_r)
plt.xlabel("t [s]")
plt.title("Samples visualized in time range [0, 10% $T_r$]");
```

We allocate memory for the sampling data

```python
table = np.zeros((n_r, n_s))
```

Fill in the values ...

```python
for chirp_nr in range(n_r):
    table[chirp_nr, :] = v_sample[(chirp_nr*n_s):(n_s*(chirp_nr+1))]
```

DF for pretty printing

```python
table_df = pd.DataFrame(data=table,
                        columns=["sample_%03d"%i for i in range(n_s)],
                        index=["chirp_%03d"%i for i in range(n_r)])
```

```python
table_df.head(10)
```

      sample_000
      sample_001
      sample_002
      sample_003
      sample_004
      sample_005
      sample_006
      sample_007
      sample_008
      sample_009
      ...
      sample_990
      sample_991
      sample_992
      sample_993
      sample_994
      sample_995
      sample_996
      sample_997
      sample_998
      sample_999

      chirp_000
      0.905353
      0.999836
      0.920138
      0.680144
      0.321662
      -0.092857
      -0.491199
      -0.803969
      -0.976681
      -0.979245
      ...
      0.415170
      0.007652
      -0.401199
      -0.740157
      -0.950170
      -0.994652
      -0.865854
      -0.586214
      -0.204448
      0.212936

      chirp_001
      0.976847
      0.804429
      0.491871
      0.093624
      -0.320933
      -0.679581
      -0.919838
      -0.999850
      -0.905677
      -0.653725
      ...
      -0.225759
      -0.603753
      -0.876566
      -0.996670
      -0.943140
      -0.725302
      -0.381107
      0.029482
      0.434935
      0.764616

      chirp_002
      0.644981
      0.276856
      -0.139501
      -0.531555
      -0.831006
      -0.985685
      -0.968646
      -0.782858
      -0.460686
      -0.058257
      ...
      -0.773011
      -0.964635
      -0.988205
      -0.839616
      -0.544753
      -0.154986
      0.261782
      0.632944
      0.893837
      0.999011

      chirp_003
      0.046790
      -0.365037
      -0.713270
      -0.937241
      -0.997932
      -0.884770
      -0.617469
      -0.242597
      0.174539
      0.561268
      ...
      -0.999508
      -0.925246
      -0.689792
      -0.334165
      0.079680
      0.479643
      0.796044
      0.973761
      0.981834
      0.818855

      chirp_004
      -0.570722
      -0.856199
      -0.992513
      -0.955916
      -0.752785
      -0.418507
      -0.011319
      0.397840
      0.737690
      0.949024
      ...
      -0.811267
      -0.501932
      -0.105152
      0.309948
      0.671049
      0.915242
      0.999983
      0.910510
      0.662410
      0.298907

      chirp_005
      -0.952571
      -0.993817
      -0.861923
      -0.579868
      -0.196791
      0.220570
      0.599505
      0.873996
      0.996222
      0.944891
      ...
      -0.286397
      0.129656
      0.523121
      0.825448
      0.983966
      0.971059
      0.788975
      0.469436
      0.068113
      -0.345076

      chirp_006
      -0.941085
      -0.721066
      -0.375424
      0.035623
      0.440464
      0.768568
      0.962774
      0.989248
      0.843378
      0.550576
      ...
      0.357312
      0.707445
      0.934326
      0.998429
      0.888586
      0.623932
      0.250578
      -0.166433
      -0.554448
      -0.845866

      chirp_007
      -0.541005
      -0.150571
      0.266096
      0.636404
      0.895838
      0.999200
      0.928483
      0.696006
      0.342272
      -0.071093
      ...
      0.852756
      0.991682
      0.957836
      0.757114
      0.424486
      0.017904
      -0.391798
      -0.733240
      -0.946936
      -0.995655

      chirp_008
      0.082467
      0.482098
      0.797739
      0.974397
      0.981296
      0.817232
      0.510791
      0.115359
      -0.300171
      -0.663405
      ...
      0.994356
      0.864427
      0.583895
      0.201635
      -0.215755
      -0.595555
      -0.871595
      -0.995784
      -0.946484
      -0.732285

      chirp_009
      0.671886
      0.915698
      0.999976
      0.910036
      0.661548
      0.297805
      -0.117823
      -0.512924
      -0.818662
      -0.981771
      ...
      0.723355
      0.378482
      -0.032331
      -0.437512
      -0.766468
      -0.961886
      -0.989720
      -0.845120
      -0.553281
      -0.165046

10 rows × 1000 columns

Now our table consists all sampled measurement during a cycle. We will use this matrix to calculate range and velocity.

### FFT over the first chirp

For didactic reasons we do the FFT procedure only for the first churp to make an example of the procedure.

```python
chirp0_samples = table_df.iloc[0].values
```

```python
chirp0_magnitude = fft(chirp0_samples)
```

```python
# frequencies found by FFT, will be used later
frequencies = np.arange(0, n_s//2)*f_s/n_s
```

```python
frequencies[:3]
```

    array([     0.,  50000., 100000.])

Each frequency is referred to a range

\begin{align}
f_* = \frac{2m_w}{c}r + \frac{2 f_0}{c}\dot r
\end{align}

Because $m_w$ is so large, the influence of the velocity (second term) is very low:

```python
f_star1 = 2*m_w/c*100
f_star2 = 2*f_0/c*33
print(f_star1, f_star2)
print(f_star2/f_star1*100, "%")
```

    6671281.9039630415 17105.833929951634
    0.25640999999999997 %

That means:

\begin{align}
f_* \approx \frac{2m_w}{c}r
\end{align}

We rearrange:

\begin{align}
r \approx \frac{f_* c}{2 m_w}
\end{align}

```python
def freq_to_range(f):
    return f*c/(2*m_w)
```

```python
ranges = freq_to_range(frequencies)
```

```python
plt.figure(figsize=(10,5))
plt.plot(ranges, 2.0/n_s*np.abs(chirp0_magnitude[0:n_s//2]))
plt.plot(ranges, 2.0/n_s*np.abs(chirp0_magnitude[0:n_s//2]), "k+")
plt.xlabel("range $r$ [m]")
plt.title("derived range (FFT of chirp_0)")
print(freq_to_range(frequencies)[np.argmax(2.0/n_s*np.abs(chirp0_magnitude[0:n_s//2]))])
```

Looks like we got the distance right: $\approx 50m$

# Calculate range bins for each chirp

Now we do the same for each of $n_r$ chirps and save the FFT results in a table of shape $n_r × n_s/2$.

```python
range_table = np.zeros((n_r, n_s//2), dtype=np.csingle)
```

```python
for chirp_nr in range(n_r):
    chirp_ad_values = table_df.iloc[chirp_nr].values
    chirp_fft = fft(chirp_ad_values) # FFT
    range_table[chirp_nr, :] = 2.0/n_s*chirp_fft[:n_s//2]
```

Let's visualize the table

```python
fig, ax = plt.subplots(nrows=2, ncols=2, figsize=(15,10), sharex=True, sharey=True)
abs_axes = ax[0, 0]
phi_axes = ax[0, 1]
real_axes = ax[1, 0]
imag_axes = ax[1, 1]

im_asb = abs_axes.imshow(np.abs(range_table), cmap = plt.get_cmap('RdYlBu'))
abs_axes.set_xticks(range(ranges.size)[::50])
abs_axes.set_xticklabels(ranges[::50], rotation=90)
fig.colorbar(im_asb, ax=abs_axes)
abs_axes.set_xlabel("range [m]")
abs_axes.set_ylabel("chirp number")
abs_axes.set_title("$|A(j\omega)|$")

im_phi = phi_axes.imshow(np.angle(range_table)*360/(2*np.pi), cmap = plt.get_cmap('RdYlBu'))
fig.colorbar(im_phi, ax=phi_axes)
phi_axes.set_xlabel("range [m]")
phi_axes.set_ylabel("chirp number")
phi_axes.set_title("$∠ A(j\omega)$")
phi_axes.set_xticks(range(ranges.size)[::50])
phi_axes.set_xticklabels(ranges[::50], rotation=90)

im_real = real_axes.imshow(np.real(range_table), cmap = plt.get_cmap('RdYlBu'))
fig.colorbar(im_real, ax=real_axes)
real_axes.set_xlabel("range [m]")
real_axes.set_ylabel("chirp number")
real_axes.set_title("Real{$A(j\omega)$}")
real_axes.set_xticks(range(ranges.size)[::50])
real_axes.set_xticklabels(ranges[::50], rotation=90)

im_imag = imag_axes.imshow(np.imag(range_table), cmap = plt.get_cmap('RdYlBu'))
fig.colorbar(im_imag, ax=imag_axes)
imag_axes.set_xlabel("range [m]")
imag_axes.set_ylabel("chirp number")
imag_axes.set_title("Imag{$A(j\omega)$}");
imag_axes.set_xticks(range(ranges.size)[::50])
imag_axes.set_xticklabels(ranges[::50], rotation=90);

fig.suptitle("Range FFT table visualized.");
```

## Velocity estimation

We make a second FFT over each range bin (column).

Again, we initialize an empty table. Now the rows will be different velocity ranges.

```python
velocity_table = np.zeros((n_r, range_table.shape[1]), dtype=np.csingle)
```

```python
for r in range(range_table.shape[1]):
    range_bin_magn = range_table[:, r]
    range_bin_fft = fft(range_bin_magn)
    velocity_table[:, r]= 2.0/n_r*range_bin_fft
```

Second FFT on columns returns the phase shift

\begin{align}
... + \frac{4 \pi f_0 }{c} r = ... + \frac{4 \pi f_0 }{c} vt \\
\Rightarrow \frac{4 \pi f_0 }{c}v = \omega_{\text{FFT, columns}}
\end{align}

After rearranging ...

```python
def angle_freq_to_velocity(w):
    return w*c/(4*np.pi*f_0)
```

```python
omega_second = 2*np.pi*np.concatenate((np.arange(0, n_r//2), np.arange(-n_r//2, 0)[::-1]))*f_chirp/n_r
```

```python
velocities = angle_freq_to_velocity(omega_second)
```

```python
plt.figure(figsize=(15,10))
plt.imshow(np.abs(velocity_table), cmap = plt.get_cmap('RdYlBu'))
plt.xticks(range(ranges.size)[::20], ranges[::20], rotation=90);
plt.yticks(range(velocities.size)[::10], velocities[::10]);
plt.xlim([0, 200])
plt.xlabel("range $r$ [m]")
plt.ylabel("velocity $\\dot r = v$ [m/s]");
plt.title("Chirp Sequence Modulation Result - $r, \\dot r$ map")
plt.colorbar();
```

As you can see, we could successfully recognize (blue dot) the target in 50m range with velocity of $10 m/s$.

```python

```


# Import Python modules

```python
import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
from sklearn.model_selection import train_test_split
```

# Generate data for Univariate Decision Tree Regression

```python
n = 100 #Number of observations in the training set

trueSplitPoint = 5

trueLowerLevel = 15
trueUpperLevel = 30

```

```python
x = np.random.uniform(0, 10, n)

def generateY(x, trueLowerLevel, trueUpperLevel, trueSplitPoint, sd):

    if x < trueSplitPoint:
        y = np.random.normal(trueLowerLevel, sd, 1)
    else:
        y = np.random.normal(trueUpperLevel, sd, 1)

    return y[0]

y = [generateY(i, trueLowerLevel, trueUpperLevel, trueSplitPoint, 3) for i in x]
```

```python
data = pd.DataFrame({'X':x, 'Y':y}) #Put the two arrays into a dataframe to make it easy to work with
data.head(10) #Inspect the first ten rows of our dataset
```

# Quickly plot the data so we know what it looks like

```python
plt.scatter(data['X'], data['Y'])
plt.show()
```

Data appears to be generated by two distinct processes (as indeed it is).

For the simplest version of decision tree we want to identify the split point. When predicting the (continuous) label for a new observation, if the feature is less than the split point we assign one value and if it's greater than the split point we assign another value. Thus the set of possible values we can assign has size two. Clearly this is a very simple regression technique and it's unlikely we'd use this in a real scenario, but we've generated the data in a very contrived way so that it should perform reasonably here and we'll see that the decision tree can approximate non-linear functions by using techniques such as Bagging.

Mathematically, we want to learn a function $f: \mathbb{R} \rightarrow \mathbb{R}$, such that:

\begin{equation}
  f(x)=\begin{cases}
    c_0, & \text{if $x<c_{split}$}.\\
    c_1, & \text{otherwise}.
  \end{cases}
\end{equation}

Our aim is to find the best combination of values for $c_0, c_1 \& c_{split}$

# How we're going to fit the model

We can't use a Maximum-Likelihood style approach because we don't make any probabilistic assumptions. We're going to test lots of split points, then for each split point we're calculate $c_0$ and $c_1$ by taking all of the points on the relevant side of the split point and calculating the mean of those points.

That is

$$c_0|c_{split} = \frac{\sum_{x_i < c_{split}} x_i}{\sum_{x_i < c_{split}} 1}$$

$$c_1|c_{split} = \frac{\sum_{x_i > c_{split}} x_i}{\sum_{x_i > c_{split}} 1}$$

And then we'll choose the values of $c_0, c_1 \text{and} c_{split}$ which minimise the squared error on the training set

```python
class univariateDecisionTree:

    def __init__(self, data, target, feature, trainTestRatio = 0.9):
        #data - a pandas dataset
        #target - the name of the pandas column which contains the true labels
        #feature - the name of the pandas column which we will use to do the regression
        #trainTestRatio - the proportion of the entire dataset which we'll use for training
                    #   - the rest will be used for testing

        self.target = target
        self.feature = feature

        #Split up data into a training and testing set
        self.train, self.test = train_test_split(data, test_size=1-trainTestRatio)

        #Generate mesh of split points to try. We want the mesh to be of a decent size (say 1000 points)
        #Think about what reasonable end points of the mesh would be - what would happen if we considered
        #a point that was smaller than the smallest point in the training set as a split point?

        #np.linspace() is a good function for constructing this 1-d mesh
        meshMin = np.min(self.train[self.feature])
        meshMax = np.max(self.train[self.feature])
        self.splitPointMesh = np.linspace(meshMin, meshMax, 1000)

    def computeMeansGivenSplitPoint(self, splitPoint):
        #Given a split point, we want to split the training set in two
        #One containing all the points below the split point and one containing all the points above the split point
        #The means are then the means of the targets in those datasets and they are the values we want to return

        pass

    def computeSquaredError(self, splitPoint):
        #Once we have a split point and a set of means, we need to have some way of identifying whether it's
        #a good split point or not

        #First apply compuuteMeansGivenSplitPoint to get the values for above and below the dataset
        #Then compute the sum of squared errors yielded by assigning the corresponding mean to each point in the training set
        #If we add these two sums of squares together then we have a single error number which indicates how good our split point is

        pass

    def fitDT(self):
        #Calculate the squared error for each split point in the mesh,
        #and save the values (splitPoint and two means) obtained by the best one

        pass

    def predict(self, X):

        pass

```

```python
myModel = univariateDecisionTree(data, 'Y', 'X')
```

```python
myModel.fitDT()
```

# Now lets see what the results look like

We'll plot the regression line we obtained against the values we observed in the training set

```python
regLine = myModel.predict(myModel.splitPointMesh)
```

```python
plt.scatter(myModel.train[myModel.feature], myModel.train[myModel.target], label = 'Training set')
plt.scatter(myModel.splitPointMesh, regLine, label = 'Regression Line')
plt.legend()
plt.show()

```

```python

```

Implementations/DecisionTree/.ipynb_checkpoints/Decision Tree - Regression - Single Variable - Empty-checkpoint.ipynb


## ZDT

The ZDT <cite data-cite="zdt"></cite> problem suite is based on the construction process

\begin{align}
\begin{split}
\min && \; f_1(x)\\[2mm]
\min && \; f_2(x) = g(x) \, h(f_1(x),g(x))
\end{split}
\end{align}

where two objective have to be minimized. The function $g(x)$ can be considered as the function for convergenece  and usually $g(x) = 1$ holds for pareto-optimal solutions (except for ZDT5).

### ZDT1

This is a 30-variable problem ($n=30$) with a convex Pareto-optimal set:

**Definition**

\begin{align}
\begin{split}
f_1(x) &= \, & x_1 \\
g(x) &=& 1 + \frac{9}{n-1} \; \sum_{i=2}^{n} x_i \\
h(f_1,g) &=& 1 - \sqrt{f_1/g} \\
\end{split}
\end{align}

$$0 \leq x_i \leq 1 \quad i=1,\ldots,n$$

**Optimum**

$$0 \leq x_1^* \leq 1  \quad \text{and} \quad x_i^*=0 \; \text{for} \; i=2,\ldots,n$$

**Plot**

```python
from pymoo.factory import get_problem
from pymoo.util.plotting import plot

problem = get_problem("zdt1")
plot(problem.pareto_front(), no_fill=True)
```

### ZDT2

This is also a 30-variable problem ($n=30$) with a non-convex Pareto-optimal set:

**Definition**

\begin{align}
\begin{split}
f_1(x) &= \, & x_1 \\
g(x) &=& 1 + \frac{9}{n-1} \; \sum_{i=2}^{n} x_i \\
h(f_1,g) &=& 1 - (f_1/g)^2 \\
\end{split}
\end{align}

$$0 \leq x_i \leq 1 \quad i=1,\ldots,n$$

**Optimum**

$$0 \leq x_1^* \leq 1  \quad \text{and} \quad x_i^*=0 \; \text{for} \; i=2,\ldots,n$$

**Plot**

```python
from pymoo.factory import get_problem
from pymoo.util.plotting import plot

problem = get_problem("zdt2")
plot(problem.pareto_front(), no_fill=True)
```

### ZDT3

This is also a 30-variable problem ($n=30$) with a number of disconnected Pareto-optimal fronts:

**Definition**

\begin{align}
\begin{split}
f_1(x) &= \, & x_1 \\
g(x) &=& 1 + \frac{9}{n-1} \; \sum_{i=2}^{n} x_i \\
h(f_1,g) &=& 1 - \sqrt{f_1/g} - (f_1/g) \; \sin(10\pi f_1)\\
\end{split}
\end{align}

$$0 \leq x_i \leq 1 \quad i=1,\ldots,n$$

**Optimum**

$$0 \leq x_1^* \leq 0.0830 \\
0.1822 \leq x_1^* \leq 0.2577\\
0.4093 \leq x_1^* \leq 0.4538 \\
0.6183 \leq x_1^* \leq 0.6525 \\
0.8233 \leq x_1^* \leq 0.8518 \\
x_i^*=0 \; \text{for} \; i=2,\ldots,n$$

**Plot**

```python
from pymoo.factory import get_problem
from pymoo.util.plotting import plot

problem = get_problem("zdt3")
plot(problem.pareto_front(), no_fill=True)
```

### ZDT4

This is a 10-variable ($n=10$) problem having a convex Pareto-optimal set. There exist many local Pareto-optimal solutions in this problem. Therefore, algorithms can easily get stuck in a local optimum.

**Definition**

\begin{align}
\begin{split}
f_1(x) &= \, & x_1 \\
g(x) &=& 1 + 10(n-1) + \sum_{i=2}^{n} (x_i^2 - 10 \cos(4\pi x_i))\\
h(f_1,g) &=& 1 - \sqrt{f_1/g}\\
\end{split}
\end{align}

$$0 \leq x_1 \leq 1 \\
-10 \leq x_i \leq 10 \quad i=2,\ldots,n$$

**Optimum**

$$0 \leq x_1^* \leq 1  \quad \text{and} \quad x_i^*=0 \; \text{for} \; i=2,\ldots,n$$

**Plot**

```python
from pymoo.factory import get_problem
from pymoo.util.plotting import plot

problem = get_problem("zdt4")
plot(problem.pareto_front(), no_fill=True)
```

### ZDT5

In ZDT5 in variables are decodec by bitsrings. At all 11 discrete variables are used, where $x_1$ is represented by 30 bits and the rest $x_2$ to $x_{11}$ by 5 bits each. The function $u(x)$ does nothing else than count the number of $1$ of the corresponding variable. Also, note that the objective function is deceptive, because the values of $v(u(x_i))$ are decreasing with the number of 1's, but has its minimum when all variables are indeed 1.

**Definition**

\begin{align}
\begin{split}
f_1(x) &= \, & 1+u(x_1)\\
g(x) &=& \sum_{i=2}^{n} v(u(x_i))\\[2mm]
v(u(x_i)) &=&
\begin{cases}
2 + u(x_i) \quad \text{if} \; u(x_i) < 5\\
1 \quad \quad \quad \; \; \; \text{if} \; u(x_i) = 5
\end{cases}\\[4mm]
h(f_1,g) &=& 1 /f_1(x)\\[2mm]
\end{split}
\end{align}

**Optimum**

$$0 \leq u(x_1^*) \leq 30  \quad \text{and} \quad u(x_i^*)=5 \; \text{for} \; i=2,\ldots,n$$

**Plot**

```python
from pymoo.factory import get_problem
from pymoo.util.plotting import plot

problem = get_problem("zdt5", normalize=False)
plot(problem.pareto_front(), no_fill=True)
```

Please note that by default here the Pareto-front is not normalized. However, this can be easily achived normalizing $f_1$ in the range of $(1,31)$ and $f_2$ in $(10/31, 10)$ which are the known bounds of the Pareto-set. By default the normalized problem is used.

```python
from pymoo.factory import get_problem
from pymoo.util.plotting import plot

problem = get_problem("zdt5", normalize=False)
plot(problem.pareto_front(), no_fill=True)
```

### ZDT6

This is a 10-variable ($n=10$) problem having a nonconvex Pareto-optimal set. The density of solutions across the Pareto-optimal region is non-uniform.

**Definition**

\begin{align}
\begin{split}
f_1(x) &= \, & 1 - \exp(-4 x_1) \sin^6 (6 \pi x_1) \\[2mm]
g(x) &=& 1 + 9 \bigg[\bigg( \sum_{i=2}^{n} x_i \bigg) / 9\bigg]^{0.25}\\[2mm]
h(f_1,g) &=& 1 - (f_1/g)^2\\
\end{split}
\end{align}

$$0 \leq x_i \leq 1 \quad i=1,\ldots,n$$

**Optimum**

$$0 \leq x_1^* \leq 1  \quad \text{and} \quad x_i^*=0 \; \text{for} \; i=2,\ldots,n$$

**Plot**

```python
from pymoo.factory import get_problem
from pymoo.util.plotting import plot

problem = get_problem("zdt6")
plot(problem.pareto_front(), no_fill=True)
```


## Solution for $\mathbb{u}_v$

In this notebook we validate our solution to the integral $\mathbb{u}$.

```python
import sympy
from sympy import *
from sympy.assumptions import assuming, Q
from sympy.assumptions.assume import global_assumptions
```

```python
# Initialize the session
init_session(quiet=True)
print("Using sympy version", sympy.__version__)

# Define our symbols
phi = symbols('phi')
v = symbols('v', integer=True)
global_assumptions.add(Q.positive(v))
```

    Using sympy version 1.4

```python
integrate(
    cos(phi) * sin(phi) ** (2 * v + 1),
    phi
)
```

We only need this integral for $v \ge 0$, so the indefinite integral is

$
\frac{\sin^{2v + 2}\phi}{2v + 2}
$

as expected.


# Mechanical Vibrations using RK method.

In this project, we will solve the second order differential equation of mechanical vibration using few Numerical Methods.

First we will solve the equation by **Euler's Method** and review it. Later we will solve it by **Runge-Kutta Method**.

## Solving Single Degree of Freedom vibration using State-Space form.

The second order differentail equation is as follows

$$m \ddot x + c \dot x + kx = f(t) \tag{1}$$

Writing the above equation in State-Space form
$$\begin{align} \vec Y =
\left[ \begin{array}{c}
\dot x\\x\end{array}\right]\end{align} \tag{2}$$

$$\begin{align} \dot{\vec Y} =
\left[ \begin{array}{c}
\ddot x\\ \dot x\end{array}\right]\end{align} \tag{3}$$

Writing the equation in Matrix form :-
$$\begin{align}
\left[ \begin{array}{cc}
m & 0\\
0 & 1
\end{array}\right]
\left[ \begin{array}{c}
\ddot x\\ \dot x\end{array}\right] +
\left[ \begin{array}{cc}
c & k\\
-1 & 0
\end{array}\right]
\left[ \begin{array}{c}
\dot x\\x\end{array}\right] =
\left[ \begin{array}{c}
f(t) \\  0\end{array}\right]\tag{4} \end{align}$$

Using Matrix A, B and F :-
$$\begin{align}
\left[ \begin{array}{cc}
A
\end{array}\right]
\left[ \begin{array}{c}
\ddot x\\ \dot x\end{array}\right] +
\left[ \begin{array}{cc}
B
\end{array}\right]
\left[ \begin{array}{c}
\dot x\\x\end{array}\right] =
\left[ \begin{array}{c}
\vec F\end{array}\right]\end{align}$$

From (2), (3) and (4) : -
$$\begin{align}
\left[ \begin{array}{cc}
A
\end{array}\right]
\left[ \begin{array}{c}
\dot{\vec Y}\end{array}\right] +
\left[ \begin{array}{cc}
B
\end{array}\right]
\left[ \begin{array}{c}
\vec Y\end{array}\right] =
\left[ \begin{array}{c}
\vec F\end{array}\right]\tag{5}\end{align}$$

$$\begin{align}
\left[ \begin{array}{c}
\dot{\vec Y}\end{array}\right] =
\left[ \begin{array}{cc}
A
\end{array}\right]^{-1}\cdot
\left( \left[ \begin{array}{c}
\vec F\end{array}\right] -
\left[ \begin{array}{cc}
B
\end{array}\right]\cdot
\left[ \begin{array}{c}
\vec Y\end{array}\right]\right)
\tag{6}\end{align}$$

Discretizing (6) :-
$$\dot{\vec Y} = \frac{\vec Y_{(t + h)} - \vec Y_{(t)}}{\Delta t}$$

$$\begin{align}
\frac{\vec Y_{(t + h)} - \vec Y_{(t)}}{\Delta t} =
\left[ \begin{array}{cc}
A
\end{array}\right]^{-1}\cdot
\left( \left[ \begin{array}{c}
\vec F\end{array}\right] -
\left[ \begin{array}{cc}
B
\end{array}\right]\cdot
\left[ \begin{array}{c}
\vec Y\end{array}\right]\right)
\end{align}$$

$$\begin{align}
\vec Y_{(t + h)} =
\vec Y_{(t)} + \Delta t \cdot
\left[ \begin{array}{cc}
A
\end{array}\right]^{-1}\cdot
\left( \left[ \begin{array}{c}
\vec F\end{array}\right] -
\left[ \begin{array}{cc}
B
\end{array}\right] \cdot
\left[ \begin{array}{c}
\vec Y\end{array}\right]\right)
\tag{7}\end{align}$$

The above equation is called an Explicit Numerical Integrator because the value of $\vec Y_{(t + h)}$ depends on $\vec Y_{(t)}$

Initial condition is :-
$$\vec Y_0 = \left[\begin{array}{c}
\dot x_0\\ x_0\end{array}\right]\tag{8}$$

We require 2 initial conditions because the D.E is of the second order.

-----
## Testing the Numerical Integrator.
To gauge the accuracy of the Integrator, we will use *'Law of conservation of energy'.*

**For free vibrations, the Kinetic and Potential Energy remains constant.** Like a pendulum which will keep on swinging if there is no loss of energy.

Kinectic energy: -$$T = \frac{1}{2}m\dot x^2$$
Potential energy: -$$V = \frac{1}{2}kx^2$$
For Free Vibrations: -$$T + V = constant$$

------
## Let's code!

```python
# Title : Numerical solution of SDOF vibration
# Author: Parmeshwar Prajapati
# Date  : 15/07/2020

# Importing Modules
import numpy as np
import sympy as sp
import matplotlib.pyplot as plt
sp.init_printing()
```

```python
# Time
dt = 0.1                         # Step
time_start = 0                   # start time(s)
time_end   = 50                  # end time(s)
time = np.arange(time_start, time_end + dt, dt)

# Variables
m = 2.0                          # mass(kg)
k = 2.0                          # stiffness(N/m)
c = 0.0                          # dampning(N-s/m)
F_excite_mag = 0.0               # exciting force(N)
freq_F = 0.159                   # frequency of exciting force(Hz)
omega = 2*np.pi*freq_F           # angular velocity(radians)
#omega = 1.0                     # angular velocity(radians)
velocity_init = 0.0              # Initial velocity(m/s)
displacement_init = 1.0          # Initial displacement(m)

# Initializing
A_matrix = np.array([[m, 0],     # 'A' matrix
                     [0, 1]])

B_matrix = np.array([[c, k],     # 'B' matrix
                    [-1, 0]])

Y_matrix = np.array([velocity_init, displacement_init])  # 'Y' matrix with initial velocity and displacement value.

velocity = np.empty_like(time)                           # velocity storage
displacement = np.empty_like(time)                       # displacement storage
Force_excite = np.empty_like(time)                       # Exciting force storage
Force_excite = F_excite_mag*np.cos((omega*time))         # Exciting force value appending
velocity[0] = velocity_init                              # Initializing the velocity value
displacement[0] = displacement_init                      # Initializing the displacement value

F_matrix = np.array([Force_excite, 0])                   # Force matrix

# Calculation using Euler's Method
for t in range(1, len(time)):
    Y_matrix = Y_matrix + dt*(np.matmul(np.linalg.inv(A_matrix), F_matrix[0][t] - np.matmul(B_matrix, Y_matrix))) # Calculation
    velocity[t] = Y_matrix[0]                      # velocity value appending
    displacement[t] = Y_matrix[1]                  # displacement value appending

# Plotting
plt.figure(figsize = (20, 5))
plt.plot(time, displacement, label = 'displacement')
#plt.plot(time, displacement, label = 'velocity', linestyle = '--')
plt.plot(time, Force_excite, label = 'Force excit')
plt.axis([0, time[-1], min(displacement), max(displacement)])
plt.title('Numerical Solution of SDOF vibration')
plt.xlabel('time[s]')
plt.legend()
plt.grid(True)
plt.show()
```

```python
# Energy conservation
T = 0.5*m*velocity**2            # Kinetic energy
V = 0.5*k*displacement**2        # Potential energy
Total = T + V                    # Total energy

plt.figure(figsize = (20, 5))
plt.plot(time, T, label = 'K.E')
plt.plot(time, V, label = 'P.E')
plt.plot(time, Total, label = 'T.E', linestyle = '--')
plt.xlabel('time[s]')
plt.ylabel('Energy')
plt.legend()
plt.grid(True)
plt.show()
```

## Validation
We were expecting a constant amplitude as the excitation force is zero. But from the figure above we can deduce that the Euler's Method is **unstable** as the **Amplitude and Total Energy keeps on increasing**.

## Conclusion
Euler's Method is unstable.

-----
## Runge-Kutta Integrator
Now we will use Runge-Kutta Integrator to solve the Differential Equation.

#### Problem with Euler's Method: -
1) The error keeps on adding up.

2) Euler's Method is a **Linear Method** which is not suitable for non-linear problems.

## Taylor's Series
$$\begin{align}
f(x) = f(0) + xf'(0) + \frac{x^2f''(0)}{2!} + \frac{x^3f'''(0)}{3!} + \frac{x^4f''''(0)}{4!} + \frac{x^5f'''''(0)}{5!} + ....\end{align}$$

Our function can be written as :-
$$\begin{align}
\vec Y(x + \delta t) &= \vec Y(x) + \delta t \dot{\vec Y(x)} + \frac{\delta t^2 \ddot{\vec Y(x)}}{2!}
                       + \frac{\delta t^3 \dddot{\vec Y(x)}}{3!}  + \frac{\delta t^4 \ddddot{\vec Y(x)}}{4!} +.... \tag{1}\\
                       \vec Y(t + 1) &= \vec Y(t) + \delta t \dot{\vec Y(t)} + \frac{\delta t^2 \ddot{\vec Y(t)}}{2!}
                       + \frac{\delta t^3 \dddot{\vec Y(t)}}{3!}  + \frac{\delta t^4 \ddddot{\vec Y(t)}}{4!} + ....\tag{2}\end{align}$$

Now $\dot{\vec Y}$ is,
$\begin{align}
\dot{\vec Y} =
\left[ \begin{array}{cc}
A
\end{array}\right]^{-1}\cdot
\left( \left[ \begin{array}{c}
\vec F\end{array}\right] -
\left[ \begin{array}{cc}
B
\end{array}\right]\cdot
\left[ \begin{array}{c}
\vec Y\end{array}\right]\right)
\end{align}$

Let
$\begin{align}
\dot{\vec Y} &= \vec G_t\\
\dot{\vec Y} &= \frac{\vec Y_{(t + 1)} - \vec Y_{(t)}}{\Delta t}\\
\therefore
\vec G_t &= \frac{\vec Y_{(t + 1)} - \vec Y_{(t)}}{\Delta t}\\
\vec Y_{(t + 1)} &= \vec Y_{(t)} + \vec G_t \Delta t\tag{3}\end{align}$

Using (2)
$\begin{align}\vec Y(t + 1) &= \vec Y(t) + \delta t \vec G_t + \frac{\delta t^2 \dot{\vec G_t}}{2!}
                       + \frac{\delta t^3 \ddot{\vec G_t}}{3!}  + \frac{\delta t^4 \dddot{\vec G_t)}}{4!} + ....\tag{4}\\
                       \vec Y(t + 1) &= \vec Y(t) + \delta t \left[\vec G_t + \frac{\delta t \dot{\vec G_t}}{2!}
                       + \frac{\delta t^2 \ddot{\vec G_t}}{3!}  + \frac{\delta t^3 \dddot{\vec G_t)}}{4!} + ....\right]\tag{5}
                       \end{align}$

The above equation is of the form of (3) and the term
$$\vec G_t + \frac{\delta t \dot{\vec G_t}}{2!}+ \frac{\delta t^2 \ddot{\vec G_t}}{3!}  + \frac{\delta t^3 \dddot{\vec G_t)}}{4!} + ....$$ is called **Average slope.**

## The Runge-Kutta Integrator can be written as :-
$$\vec Y_{(t + 1)} = \vec Y_{(t)} + (a_1k_1 + a_2k_2 + a_3k_3 + a_4k_4) \Delta t + ....$$

where, a = weights and k = slopes

## Using RK4 method to solve SDOF vibration.

Suppose you have a derivative

$$\begin{align}
\dot x &= f(x, t)\\
\end{align}$$

To find the average slope
$$\begin{align}
k1 &= f(x, t)\\
k2 &= f(x + \frac{h}{2}k1, t + \frac{h}{2})\\
k3 &= f(x + \frac{h}{2}k2, t + \frac{h}{2})\\
k4 &= f(x + hk3, t + h)\\
\end{align}$$

The average slope is
$$Avg \space slope = \frac{(k1 + 2k2 + 2k3 + k4)}{6}$$

---------

## Let's code again!

```python
# Title : Numerical solution of SDOF vibration using Runge-Kutta Method
# Author: Parmeshwar Prajapati
# Date  : 21/07/2020

# Time Series
start_time = 0                      # Start time
final_time = 50                     # Final time
dt = 0.1                            # Step
time = np.arange(start_time, final_time + dt, dt)

# Variables
m = 2.0                             # Mass(kg)
k = 2.0                             # Spring stiffness(N/m)
c = 0.0                             # Damping(N-s/m)
excite_Force_mag = 0.0              # Magnitude of external excitation force(N)
freq_excite_Force = 0.159           # Frequency of external excitation force(Hz)
omega = 2*np.pi*freq_excite_Force   # Angular velocity in (rad/s)

# Initialize
velocity_init = 0.0                 # Initial velocity
displacement_init = 1.0             # Initial displacement

# Functions
# Exciting Force Function
def excite_Force(excite_Force_mag, omega, time):
    return excite_Force_mag*np.cos(omega*time)

# Y_dot Function
def y_dot(Y_matrix, time):
    return np.matmul(np.linalg.inv(A_matrix), F_matrix - np.matmul(B_matrix, Y_matrix))

# Runge-Kutta Function
def RK4(Y_matrix, time):
    k1 = y_dot(Y_matrix, time)
    k2 = y_dot(Y_matrix + 0.5*dt*k1, time + 0.5*dt)
    k3 = y_dot(Y_matrix + 0.5*dt*k2, time + 0.5*dt)
    k4 = y_dot(Y_matrix + dt*k3, time + dt)
    return (k1 + 2*k2 + 2*k3 + k4)/6

# A Matrix
A_matrix = np.array([[m, 0],
                     [0, 1]])
# B Matrix
B_matrix = np.array([[c, k],
                    [-1, 0]])
# Y Matrix
Y_matrix = np.array([velocity_init, displacement_init])

# Storage for calculated values
velocity = np.empty_like(time)      # velocity storage
displacement = np.empty_like(time)  # displacement storage
velocity[0] = velocity_init         # Initializing the velocity value
displacement[0] = displacement_init # Initializing the displacement value

# Calculation
# **Note the initial time
for t in range(0, len(time) - 1):
    F_matrix = np.array([excite_Force(excite_Force_mag, omega, time[t]), 0]) # Force Matrix
    Y_matrix = Y_matrix + dt*RK4(Y_matrix, time[t])                          # Calculating Y
    velocity[t + 1] = Y_matrix[0]                                            # Velocity appending
    displacement[t + 1] = (Y_matrix[1])                                      # displacement appending
```

```python
# Plotting
plt.figure(figsize = (20, 5))
plt.plot(time, displacement, label = 'displacement')
#plt.plot(time, velocity, label = 'velocity', linestyle = '--')
#plt.plot(time, Force_excite, label = 'Force excit')
#plt.axis([0, time[-1], min(displacement), max(displacement)])
plt.title('Numerical Solution of SDOF vibration')
plt.xlabel('time[s]')
plt.legend()
plt.grid(True)
plt.show()
```

#### As you can see it is better than Euler's Method.

```python
# Energy conservation
T = 0.5*m*velocity**2            # Kinetic energy
V = 0.5*k*displacement**2        # Potential energy
Total = T + V                    # Total energy

plt.figure(figsize = (20, 5))
plt.plot(time, T, label = 'K.E')
plt.plot(time, V, label = 'P.E')
plt.plot(time, Total, label = 'T.E', linestyle = '--')
plt.xlabel('time[s]')
plt.ylabel('Energy')
#plt.axis([time[0], 3, min(T), max(T)])
plt.legend()
plt.grid(True)
plt.show()
```

* Putting dt = 0.1 in Euler's Method gives unstable solution.
* Putting dt = 0.1 in Runge-Kutta's Method gives stable solution.

## Validation
- The amplitude remains constant which is what we expect.
- The Total energy remains constant which is what we expect.

## Conclusion
Runge-Kutta Method is better than Euler's Method in solving the vibration equation.

Mechanical Engineering/Mechanical Vibrations using RK method/Mechanical Vibrations using RK method.ipynb


# EKF SLAM

```python
from IPython.display import Image
Image(filename="animation.png",width=600)
```

## Simulation

This is a simulation of EKF SLAM.

- Black stars: landmarks
- Green crosses: estimates of landmark positions
- Black line: dead reckoning
- Blue line: ground truth
- Red line: EKF SLAM position estimation

## Introduction

EKF SLAM models the SLAM problem in a single EKF where the modeled state is both the pose $(x, y, \theta)$ and
an array of landmarks $[(x_1, y_1), (x_2, x_y), ... , (x_n, y_n)]$ for $n$ landmarks. The covariance between each of the positions and landmarks are also tracked.

\begin{equation}
X = \begin{bmatrix} x \\ y \\ \theta \\ x_1 \\ y_1 \\ x_2 \\ y_2 \\ \dots \\ x_n \\ y_n \end{bmatrix}
\end{equation}

\begin{equation}
P = \begin{bmatrix}
\sigma_{xx} & \sigma_{xy} & \sigma_{x\theta} & \sigma_{xx_1} & \sigma_{xy_1} & \sigma_{xx_2} & \sigma_{xy_2} & \dots & \sigma_{xx_n} & \sigma_{xy_n} \\
\sigma_{yx} & \sigma_{yy} & \sigma_{y\theta} & \sigma_{yx_1} & \sigma_{yy_1} & \sigma_{yx_2} & \sigma_{yy_2} & \dots & \sigma_{yx_n} & \sigma_{yy_n} \\
 &  &  &  & \vdots &  &  &  &  &  \\
\sigma_{x_nx} & \sigma_{x_ny} & \sigma_{x_n\theta} & \sigma_{x_nx_1} & \sigma_{x_ny_1} & \sigma_{x_nx_2} & \sigma_{x_ny_2} & \dots & \sigma_{x_nx_n} & \sigma_{x_ny_n}
\end{bmatrix}
\end{equation}

A single estimate of the pose is tracked over time, while the confidence in the pose is tracked by the
covariance matrix $P$. $P$ is a symmetric square matrix whith each element in the matrix corresponding to the
covariance between two parts of the system. For example, $\sigma_{xy}$ represents the covariance between the
belief of $x$ and $y$ and is equal to $\sigma_{yx}$.

The state can be represented more concisely as follows.

\begin{equation}
X = \begin{bmatrix} x \\ m \end{bmatrix}
\end{equation}
\begin{equation}
P = \begin{bmatrix}
\Sigma_{xx} & \Sigma_{xm}\\
\Sigma_{mx} & \Sigma_{mm}\\
\end{bmatrix}
\end{equation}

Here the state simplifies to a combination of pose ($x$) and map ($m$). The covariance matrix becomes easier to
understand and simply reads as the uncertainty of the robots pose ($\Sigma_{xx}$), the uncertainty of the
map ($\Sigma_{mm}$), and the uncertainty of the robots pose with respect to the map and vice versa
($\Sigma_{xm}$, $\Sigma_{mx}$).

Take care to note the difference between $X$ (state) and $x$ (pose).

```python
"""
Extended Kalman Filter SLAM example
original author: Atsushi Sakai (@Atsushi_twi)
notebook author: Andrew Tu (drewtu2)
"""

import math
import numpy as np
%matplotlib notebook
import matplotlib.pyplot as plt

# EKF state covariance
Cx = np.diag([0.5, 0.5, np.deg2rad(30.0)])**2 # Change in covariance

#  Simulation parameter
Qsim = np.diag([0.2, np.deg2rad(1.0)])**2  # Sensor Noise
Rsim = np.diag([1.0, np.deg2rad(10.0)])**2 # Process Noise

DT = 0.1  # time tick [s]
SIM_TIME = 50.0  # simulation time [s]
MAX_RANGE = 20.0  # maximum observation range
M_DIST_TH = 2.0  # Threshold of Mahalanobis distance for data association.
STATE_SIZE = 3  # State size [x,y,yaw]
LM_SIZE = 2  # LM state size [x,y]

show_animation = True
```

## Algorithm Walkthrough

At each time step, the following is done.
- predict the new state using the control functions
- update the belief in landmark positions based on the estimated state and measurements

```python
def ekf_slam(xEst, PEst, u, z):
    """
    Performs an iteration of EKF SLAM from the available information.

    :param xEst: the belief in last position
    :param PEst: the uncertainty in last position
    :param u:    the control function applied to the last position
    :param z:    measurements at this step
    :returns:    the next estimated position and associated covariance
    """
    S = STATE_SIZE

    # Predict
    xEst, PEst, G, Fx = predict(xEst, PEst, u)
    initP = np.eye(2)

    # Update
    xEst, PEst = update(xEst, PEst, u, z, initP)

    return xEst, PEst

```

## 1- Predict
**Predict State update:** The following equations describe the predicted motion model of the robot in case we provide only the control $(v,w)$, which are the linear and angular velocity repsectively.

$\begin{equation*}
F=
\begin{bmatrix}
1 & 0 & 0 \\
0 & 1 & 0 \\
0 & 0 & 1
\end{bmatrix}
\end{equation*}$

$\begin{equation*}
B=
\begin{bmatrix}
\Delta t cos(\theta) & 0\\
\Delta t sin(\theta) & 0\\
0 & \Delta t
\end{bmatrix}
\end{equation*}$

$\begin{equation*}
U=
\begin{bmatrix}
v_t\\
w_t\\
\end{bmatrix}
\end{equation*}$

$\begin{equation*}
X = FX + BU
\end{equation*}$

$\begin{equation*}
\begin{bmatrix}
x_{t+1} \\
y_{t+1} \\
\theta_{t+1}
\end{bmatrix}=
\begin{bmatrix}
1 & 0 & 0 \\
0 & 1 & 0 \\
0 & 0 & 1
\end{bmatrix}\begin{bmatrix}
x_{t} \\
y_{t} \\
\theta_{t}
\end{bmatrix}+
\begin{bmatrix}
\Delta t cos(\theta) & 0\\
\Delta t sin(\theta) & 0\\
0 & \Delta t
\end{bmatrix}
\begin{bmatrix}
v_{t} + \sigma_v\\
w_{t} + \sigma_w\\
\end{bmatrix}
\end{equation*}$

Notice that while $U$ is only defined by $v_t$ and $w_t$, in the actual calcuations, a $+\sigma_v$ and
$+\sigma_w$ appear. These values represent the error bewteen the given control inputs and the actual control
inputs.

As a result, the simulation is set up as the following. $R$ represents the process noise which is added to the
control inputs to simulate noise experienced in the real world. A set of truth values are computed from the raw
control values while the values dead reckoning values incorporate the error into the estimation.

$\begin{equation*}
R=
\begin{bmatrix}
\sigma_v\\
\sigma_w\\
\end{bmatrix}
\end{equation*}$

$\begin{equation*}
X_{true} = FX + B(U)
\end{equation*}$

$\begin{equation*}
X_{DR} = FX + B(U + R)
\end{equation*}$

The implementation of the motion model prediciton code is shown in `motion_model`. The `observation` function
shows how the simulation uses (or doesn't use) the process noise `Rsim` to the find the ground truth and dead reckoning estimtates of the pose.

**Predict covariance:** Add the state covariance to the the current uncertainty of the EKF. At each time step, the uncertainty in the system grows by the covariance of the pose, $Cx$.

$
P = G^TPG + Cx
$

Notice this uncertainty is only growing with respect to the pose, not the landmarks.

```python
def predict(xEst, PEst, u):
    """
    Performs the prediction step of EKF SLAM

    :param xEst: nx1 state vector
    :param PEst: nxn covariacne matrix
    :param u:    2x1 control vector
    :returns:    predicted state vector, predicted covariance, jacobian of control vector, transition fx
    """
    S = STATE_SIZE
    G, Fx = jacob_motion(xEst[0:S], u)
    xEst[0:S] = motion_model(xEst[0:S], u)
    # Fx is an an identity matrix of size (STATE_SIZE)
    # sigma = G*sigma*G.T + Noise
    PEst[0:S, 0:S] = G.T @ PEst[0:S, 0:S] @ G + Fx.T @ Cx @ Fx
    return xEst, PEst, G, Fx
```

```python
def motion_model(x, u):
    """
    Computes the motion model based on current state and input function.

    :param x: 3x1 pose estimation
    :param u: 2x1 control input [v; w]
    :returns: the resutling state after the control function is applied
    """
    F = np.array([[1.0, 0, 0],
                  [0, 1.0, 0],
                  [0, 0, 1.0]])

    B = np.array([[DT * math.cos(x[2, 0]), 0],
                  [DT * math.sin(x[2, 0]), 0],
                  [0.0, DT]])

    x = (F @ x) + (B @ u)
    return x
```

## 2 - Update
In the update phase, the observations of nearby landmarks are used to correct the location estimate.

For every landmark observed, it is associated to a particular landmark in the known map. If no landmark exists
in the position surrounding the landmark, it is taken as a NEW landmark. The distance threshold for how far a
landmark must be from the next known landmark before its considered to be a new landmark is set by `M_DIST_TH`.

With an observation associated to the appropriate landmark, the **innovation** can be calculated. Innovation
($y$) is the difference between the observation and the observation that *should* have been made if the
observation were made from the pose predicted in the predict stage.

$
y = z_t - h(X)
$

With the innovation calculated, the question becomes which to trust more - the observations or the predictions?
To determine this, we calculate the Kalman Gain - a percent of how much of the innovation to add to the
prediction based on the uncertainty in the predict step and the update step.

$
K = \bar{P_t}H_t^T(H_t\bar{P_t}H_t^T + Q_t)^{-1}
$
In these equations, $H$ is the jacobian of the measurement function. The multiplications by $H^T$ and $H$
represent the application of the delta to the measurement covariance.
Intuitively, this equation is applying the following from the single variate Kalman equation but in the
multivariate form, i.e. finding the ratio of the uncertianty of the process compared the measurment.

$
K = \frac{\bar{P_t}}{\bar{P_t} + Q_t}
$

If $Q_t << \bar{P_t}$, (i.e. the measurement covariance is low relative to the current estimate), then the
Kalman gain will be $~1$. This results in adding all of the innovation to the estimate -- and therefore
completely believing the measurement.

However, if $Q_t >> \bar{P_t}$ then the Kalman gain will go to 0, signaling a high trust in the process
and little trust in the measurement.

The update is captured in the following.

$
xUpdate = xEst + (K * y)
$

Of course, the covariance must also be updated as well to account for the changing uncertainty.

$
P_{t} = (I-K_tH_t)\bar{P_t}
$

```python
def update(xEst, PEst, u, z, initP):
    """
    Performs the update step of EKF SLAM

    :param xEst:  nx1 the predicted pose of the system and the pose of the landmarks
    :param PEst:  nxn the predicted covariance
    :param u:     2x1 the control function
    :param z:     the measurements read at new position
    :param initP: 2x2 an identity matrix acting as the initial covariance
    :returns:     the updated state and covariance for the system
    """
    for iz in range(len(z[:, 0])):  # for each observation
        minid = search_correspond_LM_ID(xEst, PEst, z[iz, 0:2]) # associate to a known landmark

        nLM = calc_n_LM(xEst) # number of landmarks we currently know about

        if minid == nLM: # Landmark is a NEW landmark
            print("New LM")
            # Extend state and covariance matrix
            xAug = np.vstack((xEst, calc_LM_Pos(xEst, z[iz, :])))
            PAug = np.vstack((np.hstack((PEst, np.zeros((len(xEst), LM_SIZE)))),
                              np.hstack((np.zeros((LM_SIZE, len(xEst))), initP))))
            xEst = xAug
            PEst = PAug

        lm = get_LM_Pos_from_state(xEst, minid)
        y, S, H = calc_innovation(lm, xEst, PEst, z[iz, 0:2], minid)

        K = (PEst @ H.T) @ np.linalg.inv(S) # Calculate Kalman Gain
        xEst = xEst + (K @ y)
        PEst = (np.eye(len(xEst)) - (K @ H)) @ PEst

    xEst[2] = pi_2_pi(xEst[2])
    return xEst, PEst

```

```python
def calc_innovation(lm, xEst, PEst, z, LMid):
    """
    Calculates the innovation based on expected position and landmark position

    :param lm:   landmark position
    :param xEst: estimated position/state
    :param PEst: estimated covariance
    :param z:    read measurements
    :param LMid: landmark id
    :returns:    returns the innovation y, and the jacobian H, and S, used to calculate the Kalman Gain
    """
    delta = lm - xEst[0:2]
    q = (delta.T @ delta)[0, 0]
    zangle = math.atan2(delta[1, 0], delta[0, 0]) - xEst[2, 0]
    zp = np.array([[math.sqrt(q), pi_2_pi(zangle)]])
    # zp is the expected measurement based on xEst and the expected landmark position

    y = (z - zp).T # y = innovation
    y[1] = pi_2_pi(y[1])

    H = jacobH(q, delta, xEst, LMid + 1)
    S = H @ PEst @ H.T + Cx[0:2, 0:2]

    return y, S, H

def jacobH(q, delta, x, i):
    """
    Calculates the jacobian of the measurement function

    :param q:     the range from the system pose to the landmark
    :param delta: the difference between a landmark position and the estimated system position
    :param x:     the state, including the estimated system position
    :param i:     landmark id + 1
    :returns:     the jacobian H
    """
    sq = math.sqrt(q)
    G = np.array([[-sq * delta[0, 0], - sq * delta[1, 0], 0, sq * delta[0, 0], sq * delta[1, 0]],
                  [delta[1, 0], - delta[0, 0], - q, - delta[1, 0], delta[0, 0]]])

    G = G / q
    nLM = calc_n_LM(x)
    F1 = np.hstack((np.eye(3), np.zeros((3, 2 * nLM))))
    F2 = np.hstack((np.zeros((2, 3)), np.zeros((2, 2 * (i - 1))),
                    np.eye(2), np.zeros((2, 2 * nLM - 2 * i))))

    F = np.vstack((F1, F2))

    H = G @ F

    return H
```

## Observation Step
The observation step described here is outside the main EKF SLAM process and is primarily used as a method of
driving the simulation. The observations funciton is in charge of calcualting how the poses of the robots change
and accumulate error over time, and the theoretical measuremnts that are expected as a result of each
measurement.

Observations are based on the TRUE position of the robot. Error in dead reckoning and control functions are
passed along here as well.

```python
def observation(xTrue, xd, u, RFID):
    """
    :param xTrue: the true pose of the system
    :param xd:    the current noisy estimate of the system
    :param u:     the current control input
    :param RFID:  the true position of the landmarks

    :returns:     Computes the true position, observations, dead reckoning (noisy) position,
                  and noisy control function
    """
    xTrue = motion_model(xTrue, u)

    # add noise to gps x-y
    z = np.zeros((0, 3))

    for i in range(len(RFID[:, 0])): # Test all beacons, only add the ones we can see (within MAX_RANGE)

        dx = RFID[i, 0] - xTrue[0, 0]
        dy = RFID[i, 1] - xTrue[1, 0]
        d = math.sqrt(dx**2 + dy**2)
        angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0])
        if d <= MAX_RANGE:
            dn = d + np.random.randn() * Qsim[0, 0]  # add noise
            anglen = angle + np.random.randn() * Qsim[1, 1]  # add noise
            zi = np.array([dn, anglen, i])
            z = np.vstack((z, zi))

    # add noise to input
    ud = np.array([[
        u[0, 0] + np.random.randn() * Rsim[0, 0],
        u[1, 0] + np.random.randn() * Rsim[1, 1]]]).T

    xd = motion_model(xd, ud)
    return xTrue, z, xd, ud
```

```python
def calc_n_LM(x):
    """
    Calculates the number of landmarks currently tracked in the state
    :param x: the state
    :returns: the number of landmarks n
    """
    n = int((len(x) - STATE_SIZE) / LM_SIZE)
    return n

def jacob_motion(x, u):
    """
    Calculates the jacobian of motion model.

    :param x: The state, including the estimated position of the system
    :param u: The control function
    :returns: G:  Jacobian
              Fx: STATE_SIZE x (STATE_SIZE + 2 * num_landmarks) matrix where the left side is an identity matrix
    """

    # [eye(3) [0 x y; 0 x y; 0 x y]]
    Fx = np.hstack((np.eye(STATE_SIZE), np.zeros(
        (STATE_SIZE, LM_SIZE * calc_n_LM(x)))))

    jF = np.array([[0.0, 0.0, -DT * u[0] * math.sin(x[2, 0])],
                   [0.0, 0.0, DT * u[0] * math.cos(x[2, 0])],
                   [0.0, 0.0, 0.0]])

    G = np.eye(STATE_SIZE) + Fx.T @ jF @ Fx
    if calc_n_LM(x) > 0:
        print(Fx.shape)
    return G, Fx,

```

```python
def calc_LM_Pos(x, z):
    """
    Calcualtes the pose in the world coordinate frame of a landmark at the given measurement.

    :param x: [x; y; theta]
    :param z: [range; bearing]
    :returns: [x; y] for given measurement
    """
    zp = np.zeros((2, 1))

    zp[0, 0] = x[0, 0] + z[0] * math.cos(x[2, 0] + z[1])
    zp[1, 0] = x[1, 0] + z[0] * math.sin(x[2, 0] + z[1])
    #zp[0, 0] = x[0, 0] + z[0, 0] * math.cos(x[2, 0] + z[0, 1])
    #zp[1, 0] = x[1, 0] + z[0, 0] * math.sin(x[2, 0] + z[0, 1])

    return zp

def get_LM_Pos_from_state(x, ind):
    """
    Returns the position of a given landmark

    :param x:   The state containing all landmark positions
    :param ind: landmark id
    :returns:   The position of the landmark
    """
    lm = x[STATE_SIZE + LM_SIZE * ind: STATE_SIZE + LM_SIZE * (ind + 1), :]

    return lm

def search_correspond_LM_ID(xAug, PAug, zi):
    """
    Landmark association with Mahalanobis distance.

    If this landmark is at least M_DIST_TH units away from all known landmarks,
    it is a NEW landmark.

    :param xAug: The estimated state
    :param PAug: The estimated covariance
    :param zi:   the read measurements of specific landmark
    :returns:    landmark id
    """

    nLM = calc_n_LM(xAug)

    mdist = []

    for i in range(nLM):
        lm = get_LM_Pos_from_state(xAug, i)
        y, S, H = calc_innovation(lm, xAug, PAug, zi, i)
        mdist.append(y.T @ np.linalg.inv(S) @ y)

    mdist.append(M_DIST_TH)  # new landmark

    minid = mdist.index(min(mdist))

    return minid

def calc_input():
    v = 1.0  # [m/s]
    yawrate = 0.1  # [rad/s]
    u = np.array([[v, yawrate]]).T
    return u

def pi_2_pi(angle):
    return (angle + math.pi) % (2 * math.pi) - math.pi
```

```python
def main():
    print(" start!!")

    time = 0.0

    # RFID positions [x, y]
    RFID = np.array([[10.0, -2.0],
                     [15.0, 10.0],
                     [3.0, 15.0],
                     [-5.0, 20.0]])

    # State Vector [x y yaw v]'
    xEst = np.zeros((STATE_SIZE, 1))
    xTrue = np.zeros((STATE_SIZE, 1))
    PEst = np.eye(STATE_SIZE)

    xDR = np.zeros((STATE_SIZE, 1))  # Dead reckoning

    # history
    hxEst = xEst
    hxTrue = xTrue
    hxDR = xTrue

    while SIM_TIME >= time:
        time += DT
        u = calc_input()

        xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)

        xEst, PEst = ekf_slam(xEst, PEst, ud, z)

        x_state = xEst[0:STATE_SIZE]

        # store data history
        hxEst = np.hstack((hxEst, x_state))
        hxDR = np.hstack((hxDR, xDR))
        hxTrue = np.hstack((hxTrue, xTrue))

        if show_animation:  # pragma: no cover
            plt.cla()

            plt.plot(RFID[:, 0], RFID[:, 1], "*k")
            plt.plot(xEst[0], xEst[1], ".r")

            # plot landmark
            for i in range(calc_n_LM(xEst)):
                plt.plot(xEst[STATE_SIZE + i * 2],
                         xEst[STATE_SIZE + i * 2 + 1], "xg")

            plt.plot(hxTrue[0, :],
                     hxTrue[1, :], "-b")
            plt.plot(hxDR[0, :],
                     hxDR[1, :], "-k")
            plt.plot(hxEst[0, :],
                     hxEst[1, :], "-r")
            plt.axis("equal")
            plt.grid(True)
            plt.pause(0.001)
```

```python
%matplotlib notebook
%matplotlib notebook
main()
```

     start!!
    New LM
    New LM
    New LM

    <IPython.core.display.Javascript object>

    New LM