# Consistent Bayes: ODE Examples
---

Copyright 2018 Michael Pilosov

### Import Libraries

```python
# Mathematics and Plotting
from HelperFuns import * # pyplot wrapper functions useful for visualizations, numpy, scipy, etc.
%matplotlib inline
# %matplotlib notebook
# plt.xkcd(scale=1, length=1000, randomness=0)
from cbayes import sample, solve, distributions
# Interactivity
from ipywidgets import *
import matplotlib.colors as colors

# import numpy as np
from scipy.integrate import odeint
```

```python
plt.rcParams.update({'font.size': 14})
plt.rcParams['figure.figsize'] = 20, 10
```

```python
problem = 'ed'
```

# Example Summary

---

The second example is defined by the model for a harmonic oscillator given by the initial value problem

\begin{equation}
	\begin{cases}
		\frac{d^2u}{dt^2} &= -\lambda_2^2 u, \ t>0, \\
		u(0) &= \lambda_1.
	\end{cases}
\end{equation}

Choosing a particular parameter $\lambda=(\lambda_1,\lambda_2)\in\Lambda\subset[-1,1]\times\mathbb{R}^+$ corresponds to fixing an initial condition, $\lambda_1\in[-1,1]$, and frequency of oscillation, $\lambda_2$, where we again assume that $\lambda_2>0$.

The solution to the above is
\begin{equation}
	u(t) = \cos(\lambda_2 t + \arccos(\lambda_1)).
\end{equation}

---
## Define Functions

```python
def MSE_generator(model, obs_data, sigma=None):   # this generates a sum of squared residuals.
    def QoI_fun(inputs): # that conforms to our desired model input
        M = len(obs_data)
        predictions = model(inputs)
        assert predictions.shape[1] == M
        residuals = predictions - obs_data
        if sigma is not None:
            QoI = (1./M)*np.sum( (residuals/sigma)**2, axis=1 )   # MSE
        else:
            QoI = (1./M)*np.sum( (residuals/obs_data)**2, axis=1 )  # MRSE
        return QoI
    return QoI_fun

if problem == 'ed':
    # The model of the exponential decay
    def makemodel(t):
        def model(lam = np.array([[0.5, 0.5]]) ):
            QoI = lam[:,0].reshape(-1,1)*np.exp(np.outer(lam[:,1], -t))
            if QoI.shape[0] == 1:
                return QoI.ravel() # this allows support for simpler 1D plotting.
            else:
                return QoI
        return model
elif problem == 'ho':
    # The model of the harmonic oscillator
    def makemodel(t):
        def model(lam = np.array([[0, 1]]) ):
    #         return np.cos(lam[:,1]*t + np.arccos(lam[:,0]) )
            QoI = np.cos(np.outer(lam[:,1],t) + np.arccos(lam[:,0]).reshape(-1,1) )
            if QoI.shape[0] == 1:
                return QoI.ravel() # this allows support for simpler 1D plotting.
            else:
                return QoI
        return model

```

```python
##### FIXED PARAMETERS - DEFINE YOUR EXPERIMENT #####
num_observations = 50 # how many observations are you able to make?
start_time = 1
end_time = 5
####

t = np.linspace(start_time, end_time, num_observations)
model = makemodel(t)
u = model()
```

## Get a sense of the variation in the system you are studying

```python
n = 50
lam = np.random.random(size=(n,2))
if problem == 'ho':
    lam[:,0] = 0.5*lam[:,0] - 0.25
    lam[:,1] = lam[:,1] + 0.5
elif problem == 'ed':
#     lam[:,1] = 0.5
    pass # keep as is.

sd_test_n = 0.025
# sd_test_u = 0.1
# sd_test_c = 0.1
mintime = 0
maxtime = 5
tt = np.linspace(mintime, maxtime, 1000)
m = makemodel(tt)
ui = m(lam)

um = m()
plt.cla()
show_obs = False
# Plotting the default IR results from the epidemic_IR function.
for i in range(n):
    d = ui[i,:]
    if i == n-1:
        plt.plot(tt, d, c='k', alpha=5/n, label='Potential Signals')
    else:
        plt.plot(tt, d, c='k', alpha=5/n) # I

# plt.scatter(t, u + np.random.randn(num_observations)*sd_test_n, s=50, c='k', label='Normal', alpha=0.5)
# plt.scatter(t, u + (np.random.rand(num_observations)-0.5)*sd_test_u,c='b', s=5, label='Uniform')
# plt.scatter(t, u + (np.random.standard_cauchy(num_observations))*sd_test_c,c='g', s=50, marker='x',label='Cauchy')
plt.plot(tt, um, c='k', lw=3, ls='-', label='True Signal')
if show_obs:
    plt.scatter(t, u + np.random.randn(num_observations)*sd_test_n, marker='x', s=50, c='k', label='{} Observations'.format(num_observations), alpha=1)

plt.legend(fontsize=18,loc='upper left')
plt.ylabel('Magnitude', fontsize=18)
plt.xlabel('Time (s)', fontsize=18)
# plt.ylim([-.25,1.25])
plt.xlim([0,5])
ftype = 'png'
if show_obs:
    summary_save_str = "saveimgs/HO/HO_%d_r-%d_obs-%de-3_sd-t_%d_%d_st%d_%d.%s"%(n, num_observations, 1000*sd_test_n, mintime, maxtime, start_time, end_time, ftype)
else:
    summary_save_str = "saveimgs/HO/HO_signal-%d_t_%d_%d_st%d_%d.%s"%(n, mintime, maxtime, start_time, end_time, ftype)

print("saving " + summary_save_str)
# plt.savefig(summary_save_str)
plt.show()
```

---

# Define Prior Measure $P_\Lambda$

```python
input_dim = 2 # Specify input space dimension (n) -- this is a handle that the widgets depend on.
num_samples = int(1E4) # number of input samples (N)
s_set = sample.sample_set(size=(num_samples, input_dim))

if problem == 'ed':
    s_set.set_dist('beta', kwds={'a': 1, 'b': 1, 'loc': 0, 'scale': 1}, dim=0) # initial condition
    s_set.set_dist('beta', kwds={'a': 1, 'b': 1, 'loc': 0, 'scale': 1}, dim=1) # frequency
elif problem == 'ho':
    s_set.set_dist('beta', kwds={'a': 1, 'b': 1, 'loc': -0.25, 'scale': 0.5}, dim=0) # initial condition
    s_set.set_dist('beta', kwds={'a': 1, 'b': 1, 'loc': .5, 'scale': 1}, dim=1) # frequency

s_set.generate_samples()
lam = s_set.samples # create a pointer for ease of reference later with plotting.
```

### Visualize Prior

```python
# widgets.interactive(pltdata, data = fixed(lam), inds = fixed(None),
#                     N = widgets.IntSlider(value=500, min = 100, max=5000, step=100, continuous_update=False),
#                     eta_r = fixed(None), space=fixed(0.05), svd=widgets.Checkbox(value=False), color=widgets.Text(value="orange", continuous_update=False),
#                     view_dim_1 = widgets.IntSlider(value=0, min=0, max=input_dim-1, step=1, continuous_update=False),
#                     view_dim_2 = widgets.IntSlider(value=input_dim-1, min=0, max=input_dim-1, step=1, continuous_update=False))

```

---
# Compute Data Space $O(\Lambda) = \mathcal{D}$

Format: `(n_dims, n_samples)`

```python
u_true = model() # t as a subscript stands for "truth"
sd_true = sd_test_n
sd_guess = sd_true

# sd_true = 0.05
# sd_guess = 0.025

obs_data = u_true + np.random.randn(int(num_observations))*sd_true
# obs_data = u_true + np.random.standard_cauchy(int(num_observations))*sd_true
# obs_data = u_true + (np.random.rand(int(num_observations))-0.5)*sd_true

PtO_fun = MSE_generator(model, obs_data, sd_guess)
```

```python
p_set = sample.map_samples_and_create_problem(s_set, PtO_fun)
D = p_set.output.samples

# This is how we handle trying to infer the dimension based on what the map put out.
# You can delete this once you are certain your model is correctly defined.
try:
    output_dim = D.shape[1] # if your function was coded correctly, you should have an (n, d) data space.
except IndexError:
    print(Warning("Warning: Your map might be returning the wrong dimensional data."))
    try:
       output_dim = D.shape[0]
    except IndexError:
        print(Warning("Warning: Guessing it's 1-dimensional."))
        output_dim = 1
print('dimensions :  lambda = '+str(lam.shape)+'   D = '+str(D.shape) )
```

    dimensions :  lambda = (10000, 2)   D = (10000, 1)

# Compute Push-Forward of the Prior $P_{O(\Lambda)}$
_ ... i.e. Characterize the Data Space_

```python
# Interactive Marginal Visualization
# p_set.compute_pushforward_dist(mirror=False, kwds={'kernel': 'gaussian', 'bandwidth': .25})
# p_set.compute_pushforward_dist(mirror=False, kwds={'kernel': 'gaussian', 'bandwidth': 5})
p_set.compute_pushforward_dist()
pf_dist = p_set.pushforward_dist
```

```python
# widgets.interactive(pltdata, data = fixed(pf_dist), inds = fixed(None),
#         N = widgets.IntSlider(value=10000, min = 100, max=10000, step=100, continuous_update=False),
#         eta_r = fixed(None), space=fixed(0.05), svd=fixed(False), color=widgets.Text(value="brown", continuous_update=False),
#         view_dim_1 = widgets.IntSlider(value=0, min=0, max=output_dim-1, step=1, continuous_update=False),
#         view_dim_2 = widgets.IntSlider(value=output_dim-1, min=0, max=output_dim-1, step=1, continuous_update=False))
```

# Define Observed Probability Measure $P_\mathcal{D}$

```python
p_set.set_observed_dist('gamma', {'a':num_observations/2, 'scale':2/num_observations}, dim=0)
#
# p_set.set_observed_dist('chi2', {'df':num_observations}, dim=0)
# p_set.set_observed_dist('normal', {'loc': 0, 'scale': 0.05}, dim=0)
# p_set.set_observed_dist('uniform', {'loc': 0, 'scale': 0.0025}, dim=0)

obs_dist = p_set.observed_dist # this is define a pointer for ease of reference.

# widgets.interactive(pltdata, data = fixed(obs_dist), inds = fixed(None),
#         N = widgets.IntSlider(value=500, min = 100, max=5000, step=100, continuous_update=False),
#         eta_r = fixed(None), space=fixed(0.05), svd=fixed(False), color=widgets.Text(value="wine", continuous_update=False),
#         view_dim_1 = widgets.IntSlider(value=0, min=0, max=output_dim-1, step=1, continuous_update=False),
#         view_dim_2 = widgets.IntSlider(value=output_dim-1, min=0, max=output_dim-1, step=1, continuous_update=False))
xx = np.linspace(-1,10,500).reshape(-1,1)
obs_dist = p_set.observed_dist # this is define a pointer for ease of reference.
zz = pf_dist.pdf(xx)
yy = obs_dist.pdf(xx)
```

```python
plt.plot(xx, zz, c='blue',label='Pushforward of Prior')
plt.plot(xx, yy,c='orange',label='Observed Density')
# plt.ylim([0,0.05])
plt.legend()
plt.show()
```

---

At this point we have performed the computations we need to. We have evaluated the input points through our map and performed a KDE on them. It would be useful at this point to save this object and/or its evaluation at every point in the data space for later re-use. Doing so here would be an appropriate place.

---

# Accept/Reject Sampling of Posterior

Since we have already used the samples in our prior to compute the pushforward density, we can re-use these with an accept/reject algorithm to get a set of samples generated from the posterior according to the solution of the stochastic inverse problem as outlined in the Consistent Bayes formulation.

```python
p_set.set_ratio()
eta_r = p_set.ratio
s_set.seed = 11
solve.problem(p_set)
```

```python
accept_inds = p_set.accept_inds
lam_accept = p_set.input.samples[accept_inds,:]
if problem == 'ho':
    lam_true = np.array([0, 1])
elif problem == 'ed':
    lam_true = np.array([0.5, 0.5])
num_accept = len(accept_inds)
print('Number accepted: %d = %2.2f%%'%(num_accept, 100*np.float(num_accept)/num_samples))
```

    Number accepted: 266 = 2.66%

## Visualize Posterior Density
### (Visualize Accept/Reject Samples)

```python
# widgets.interactive(pltdata, data = fixed(lam), inds = fixed(accept_inds),
#         N = widgets.IntSlider(value=num_accept/2+1, min = 2, max=num_accept, step=1, continuous_update=False),
#         eta_r = fixed(None), space=fixed(0.05), svd=widgets.Checkbox(value=False), color=widgets.Text(value="orange", continuous_update=False),
#         view_dim_1 = widgets.IntSlider(value=0, min=0, max=input_dim-1, step=1, continuous_update=False),
#         view_dim_2 = widgets.IntSlider(value=input_dim-1, min=0, max=input_dim-1, step=1, continuous_update=False))

# You will visualize the accepted samples in a subset of size N of the input samples.
# This is mostly for faster plotting, but also so you can see the progression of accepted sampling in the algorithm.
plt.figure()
plt.title("Accept/Reject Results\n", fontsize=18)
plt.scatter(lam_accept[:,0], lam_accept[:,1], c='black', s=25)
plt.scatter(lam_true[0],lam_true[1], c='yellow', edgecolor='black', s=200, label='True Parameter')
# plt.scatter(lam[:,0], lam[:,1], c='gray', s=10)

plt.scatter(lam_accept[:,0].mean(), lam_accept[:,1].mean(),  c='red', edgecolor='black', s=100, label='Mean Estimate from\n{} Accepted Samples'.format(num_accept))
plt.legend(loc='upper center', fontsize=18)
if problem == 'ho':
    plt.xlim([-.25, .25])
    plt.ylim([0.5, 1.5])
if problem == 'ed':
    plt.xlim([0, 1])
    plt.ylim([0, 1])
plt.show()
```

# Alternative Visualization Approach - High Fidelity Mesh Plot (expensive)

Now that we have constructed the posterior distribution, we can evaluate it at whatever set of samples we want, so we plot it directly, rather than use Accept/Reject. This is done at the cost of a lot of model runs.

```python
nbins = 200
# mins = np.min(p_set.input.samples,axis=0)
# maxs = np.max(p_set.input.samples,axis=0)

if problem == 'ho':
    mins = np.array([-0.25, 0.5])
    maxs = np.array([0.5, 1.5])
elif problem == 'ed':
    mins = np.array([0.25, 0.25])
    maxs = np.array([0.75, 0.75])

X = ngrid(nbins, mins, maxs)
post_eval = p_set.evaluate_posterior(X)
# post_eval = p_set.compute_ratio(PtO_fun(X))*p_set.input.dist.pdf(X)
```

```python
# Make the plot
xi = X[:,0].reshape(nbins, nbins)
yi = X[:,1].reshape(nbins, nbins)
zi = post_eval.reshape(nbins, nbins)
plt.pcolormesh(xi, yi, zi)
plt.scatter(lam_true[0],lam_true[1], c='white', edgecolor='black', s=300, label='True Parameter')
plt.scatter(lam_accept[:,0].mean(), lam_accept[:,1].mean(),  c='red', edgecolor='black', s=200, label='Mean Accepted Sample')
# plt.scatter(X[inds_new,0].mean(), X[inds_new,1].mean(),  c='yellow', edgecolor='black', s=100, label='Mean Estimate') # can run after the cell below is executed
plt.legend(loc='upper right', fontsize=16)
if problem == 'ho':
    plt.xlim([-.25, 0.25])
    plt.ylim([0.5, 1.5])
if problem == 'ed':
    plt.xlim([0.25, 0.75])
    plt.ylim([0.25, 0.75])
plt.show()
# ?plt.pcolormesh
```

## Perform accept/reject with the samples we just used for the high-fidelity plot.
(because why not?)

```python
plot_new = True
XX = ngrid(nbins, mins, maxs)
# Perform accept/reject on that grid you see plotted above.
if plot_new:
    Y = PtO_fun(XX)
    inds_new = solve.perform_accept_reject(XX, p_set.compute_ratio(Y))
    print('Num accepted:', len(inds_new))
    new_accepted_in = XX[inds_new,:]
    new_accepted_out = Y[inds_new].reshape(-1,1)
    new_num_accept = len(inds_new)
```

```python
if plot_new:
    # Make the plot
    xi = XX[:,0].reshape(nbins, nbins)
    yi = XX[:,1].reshape(nbins, nbins)
    post_eval2 = p_set.evaluate_posterior(XX)
    zi = post_eval2.reshape(nbins, nbins)
    plt.pcolormesh(xi, yi, zi)
    plt.scatter(lam_true[0],lam_true[1], c='white', edgecolor='black', s=300, label='True Parameter')
    plt.scatter(lam_accept[:,0].mean(), lam_accept[:,1].mean(),  c='red', edgecolor='black', s=200, label='Mean Accepted Sample')
    plt.scatter(XX[inds_new,0].mean(), XX[inds_new,1].mean(),  c='yellow', edgecolor='black', s=100, label='Mean Estimate') # can run after the cell below is executed
    plt.legend(loc='lower right', fontsize=16)
    plt.show()
    print('Abs Error in Parameter and MAP:', [*np.abs( lam_true - np.mean(X[inds_new,:],axis=0) )] )
    print('Param. Test Stats for Gamma Dist:', [*np.abs( p_set.observed_dist.mean()-np.mean(Y[inds_new],axis=0))/p_set.observed_dist.mean() , *np.abs( p_set.observed_dist.std()-np.std(Y[inds_new],axis=0))/p_set.observed_dist.std()])

```

---
# Now what?

Well, we can...

## _Visualize the Quality of our SIP Solution by Comparing it to the Observed_
_We compare the push-forward of the posterior using accepted samples against the observed density_
_(SIP = Stochastic Inverse Problem)_
### Observed:

```python
xxi = np.linspace(0,4,100)
plt.plot(xxi, p_set.observed_dist.pdf(xxi), label='Observed', ls=':')
dy = distributions.skde(data=p_set.output.samples[accept_inds,:], **{'kernel': 'gaussian', 'bandwidth': 0.25})
plt.plot(xxi, dy.pdf(xxi), label='Pushforward of Posterior - {} AS Originally'.format(len(accept_inds)));
if plot_new:
    dyy = distributions.skde(data=new_accepted_out, **{'kernel': 'gaussian', 'bandwidth': 0.25})
    plt.plot(xxi, dyy.pdf(xxi), label='Pushforward of Posterior - {} AS from Refined Eyeball Run'.format(len(inds_new)))

plt.legend(loc='best')
plt.xlabel('Response')
plt.ylabel('Relative Likelihood')
plt.title('Pushforward of Posterior')
plt.show()
```

```python
show_prior = False # show the potential signals?
u_acc = m(lam_accept)

plt.scatter(t, obs_data, marker='x', c='k', s=100, label='Observed Data')

dd = np.mean(u_acc, axis=0)
plt.plot(tt, dd, c='k', alpha=1, lw=3, label='Mean Predicted Signal')

for i in range(u_acc.shape[0]):
    d = u_acc[i,:]
    if i==1:
        plt.plot(tt, d, c='b', alpha=0.5, lw=1, label='Accepted Samples')
    else:
        plt.plot(tt, d, c='b', alpha=0.025, lw=1)

if show_prior:
    for i in range(n):
        d = ui[i,:]
        if i == n-1:
            plt.plot(tt, d, c='k', alpha=5/n, label='Potential Signals')
        else:
            plt.plot(tt, d, c='k', alpha=5/n) # I

plt.ylabel('Height', fontsize=18)
plt.xlabel('Time (s)', fontsize=18)
plt.title('Recovered Signal based on Accepted Samples', fontsize=28)
plt.legend(fontsize=18,loc='upper left')
plt.xlim([0,5])
plt.savefig('recovered{}.png'.format(problem.upper()))
plt.legend()
plt.show()
```

```python
# widgets.interactive(pltdata, data = fixed(obs_dist), inds = fixed(None),
#         N = widgets.IntSlider(value=500, min = 100, max=5000, step=100, continuous_update=False),
#         eta_r = fixed(None), space=fixed(0.05), svd=fixed(False), color=widgets.Text(value="wine", continuous_update=False),
#         view_dim_1 = widgets.IntSlider(value=0, min=0, max=output_dim-1, step=1, continuous_update=False),
#         view_dim_2 = widgets.IntSlider(value=output_dim-1, min=0, max=output_dim-1, step=1, continuous_update=False))
```

## Pushforward of Posterior:

```python
# widgets.interactive(pltdata, data = fixed(D), inds = fixed(accept_inds),
#         N = widgets.IntSlider(value=num_accept/2, min = 2, max=num_accept-1, step=1, continuous_update=False),
#         eta_r = fixed(None), space=fixed(0.05), svd=fixed(False), color=widgets.Text(value="eggplant", continuous_update=False),
#         view_dim_1 = widgets.IntSlider(value=0, min=0, max=output_dim-1, step=1, continuous_update=False),
#         view_dim_2 = widgets.IntSlider(value=output_dim-1, min=0, max=output_dim-1, step=1, continuous_update=False))

```

## Verify our parametric test statistics.
Let's see if the pushforward of the posterior results in a sample mean and standard deviation that are close to the observed.

```python
# [np.mean(D[accept_inds,:]), np.std(D[accept_inds,:])]
```

```python
# [p_set.observed_dist.mean(), p_set.observed_dist.std()]
```

Now go back to the [Define Functions](#Define-Functions) and change the distribution on the prior or choose another example problem.

```python

```

```python

```


# Quadruped Forward/Inverse Kinematics
---
Kevin Walchko
Created: 24 Oct 2016

# Denavit-Hartenberg (DH)

Development of the DH forward kinematics for a quadruped robot. We will use the DH method to develop the symbolic equations and use python to simplify them.

```python
%matplotlib inline
%reload_ext version_information
%version_information numpy, matplotlib
```

Software	Version

Python	2.7.14 64bit [GCC 4.2.1 Compatible Apple LLVM 9.0.0 (clang-900.0.37)]

IPython	5.5.0

OS	Darwin 17.3.0 x86_64 i386 64bit

numpy	1.13.3

matplotlib	2.1.0

Sun Feb 11 00:35:36 2018 MST

```python
from __future__ import print_function
from __future__ import division
from matplotlib import pyplot as plt
import numpy as np
from sympy import symbols, sin, cos, pi, simplify
from math import radians as d2r
from math import degrees as r2d
from math import atan2, sqrt, acos, fabs
```

```python
t1, t2, t3 = symbols('t1 t2 t3')
Lc, Lf, Lt = symbols('Lc Lf Lt')
```

The following class follows the traditional [DH convention](https://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters). Where:

| Parameter | Definition|
|-----------|-----------|
| $\alpha_i$   | angle about common normal, from old z axis to new z axis (twist)|
| $d_i$        | offset along previous z to the common normal |
| $\theta_i$   | angle about previous z, from old x to new x |
| $a_i$        | length of the common normal. Assuming a revolute joint, this is the radius about previous z. |

```python
class DH(object):
    def __init__(self):
        pass

    def fk(self, params):
        t = np.eye(4)
        for p in params:
            t = t.dot(self.makeT(*p))
        return t

    def makeT(self, a, alpha, d, theta):
        # create a modified DH homogenious matrix
        # this is the same matrix as above
        return np.array([
            [           cos(theta),           -sin(theta),           0,             a],
            [sin(theta)*cos(alpha), cos(theta)*cos(alpha), -sin(alpha), -d*sin(alpha)],
            [sin(theta)*sin(alpha), cos(theta)*sin(alpha),  cos(alpha),  d*cos(alpha)],
            [                    0,                     0,           0,             1]
        ])
    @staticmethod
    def eom(f, inputs):
        h = []
        for i in range(0, 3):
            tmp = (f[i,3]).subs(inputs)
            h.append(simplify(tmp.evalf()))
        return h
```

```python
# def eval(f, inputs):
#     h = []
#     for i in range(0, 3):
#         tmp = (f[i,3]).subs(inputs)
#         h.append(tmp.evalf())
#     return h
```

The parameters are:

| i |$a_i$        | $\alpha_i$   | $d_i$   | $theta_i$  |
|---|-------------|--------------|---------|------------|
| 1 | $L_{coxa}$  | 0            | 0       | $\theta_1$ |
| 2 | 0           | 90           | 0       | $\theta_2$ |
| 3 | $L_{femur}$ | 0            | 0       | $\theta_3$ |
| 4 | $L_{tibia}$ | 0            | 0       |            |

```python
# a, alpha, d, theta
# params = [
#     [Lc,    0, 0, t1],
#     [ 0, pi/2, 0, t2],
#     [Lf,    0, 0, t3],
#     [Lt,    0, 0,  0]
# ]
params = [
    [ 0,    0, 0, t1],
    [Lc, pi/2, 0, t2],
    [Lf,    0, 0, t3],
    [Lt,    0, 0,  0]
]
dh = DH()
t = dh.fk(params)
t = dh.eom(t,[])
```

```python
for i in t:
    print(i)
```

    1.0*(Lc + Lf*cos(t2) + Lt*cos(t2 + t3))*cos(t1)
    1.0*(Lc + Lf*cos(t2) + Lt*cos(t2 + t3))*sin(t1)
    1.0*Lf*sin(t2) + 1.0*Lt*sin(t2 + t3)

```python
import math
def fk(t1, t2, t3, Lc, Lf, Lt):
    t1 = d2r(t1)
    t2 = d2r(t2)
    t3 = d2r(t3)
#     return np.array([
#         Lc + Lf*math.cos(t1)*math.cos(t2) + Lt*math.cos(t1)*math.cos(t2 + t3),
#         (Lf*math.cos(t2) + Lt*math.cos(t2 + t3))*math.sin(t1),
#         Lf*math.sin(t2) + Lt*math.sin(t2 + t3)
#     ])
    return np.array([
        (Lc + Lf*math.cos(t2) + Lt*math.cos(t2 + t3))*math.cos(t1),
        (Lc + Lf*math.cos(t2) + Lt*math.cos(t2 + t3))*math.sin(t1),
        Lf*math.sin(t2) + Lt*math.sin(t2 + t3)
    ])
```

```python
print(fk(0,0,0, 1,2,3))
print(fk(0,90,0, 1,2,3))
print(fk(0,0,-90, 1,2,3))
print(fk(90,0,0, 1,2,3))
print(fk(-90,0,0, 1,2,3))
print(fk(90,90,0, 1,2,3))
print(fk(90,45,-90, 1,2,2))
print(fk(90,45,45, 1,2,2))
```

    [ 6.  0.  0.]
    [ 1.  0.  5.]
    [ 3.  0. -3.]
    [  3.67394040e-16   6.00000000e+00   0.00000000e+00]
    [  3.67394040e-16  -6.00000000e+00   0.00000000e+00]
    [  6.12323400e-17   1.00000000e+00   5.00000000e+00]
    [  2.34423551e-16   3.82842712e+00   0.00000000e+00]
    [  1.47827946e-16   2.41421356e+00   3.41421356e+00]

# Inverse Kinematics

Often, we need to find the angles of the leg joints given a location (x,y,z) of the foot. To calculate this, remember the leg is composed of 3 revolute joints. The shoulder is in one plane and both the femur joint and tibia joint are in another plane.

**insert picture here**

## Useful Trigonometry

The **law of cosines** for calculating one side of a triangle when the angle
opposite and the other two sides are known. Can be used in conjunction with
the law of sines to find all sides and angles.

## Derivation

So let's start with the shoulder, look down from the top, we see the x-y plane. We can calculate the it's rotation $\theta_1$ by using x and y of the foot location.

$$\theta_1 = atan2(y,x)$$

Next, let's look at the plane which the next 2 joints lie in. Now we will calculate the $\theta_2$ and $\theta_3$ angles.

$$ m = \sqrt{x^2 + y^2} $$

$$ f = m - L_{coxa} $$

$$ \beta_1 = atan2(f,z) $$

$$ d = \sqrt{f^2 + z^2} $$

$$ \beta_2 = \arccos( \frac{L_{femur}^2+d^2-L_{tibia}^2}{2 d L_{femur}} ) $$

$$ \theta_2 = \beta_1 + \beta_2 $$

$$\theta_3 = \arccos( \frac{L_{femur}^2+L_{tibia}^2-d^2}{2 L_{femur} L_{tibia}} ) $$

Now finally, you should notice the definition is different between forward and reverse kinematics. So let's fix that ...

$$ \theta_{2fk} = \theta_{2ik} - \pi $$
$$ \theta_{3fk} = \theta_{3ik} - \pi/2 $$

```python
def ik(x, y, z, Lc, Lf, Lt):
    t1 = atan2(y, x)
    f = sqrt(x**2 + y**2) - Lc

    # depending on z, you have to do a different atan
    if z < 0.0:
        b1 = atan2(f, fabs(z))
    else:
        b1 = atan2(z, f)
    d = sqrt(f**2 + z**2)
    b2 = acos((Lf**2 + d**2 - Lt**2) / (2.0 * Lf * d))
    t2 = b1 + b2
    t3 = acos((Lf**2 + Lt**2 - d**2) / (2.0 * Lf * Lt))

    t3 -= pi  # fix to align fk and ik frames
    if z < 0.0:
        t3 -= pi/2

    return [r2d(t1), r2d(t2),r2d(t3)]

import math

def cl(a,b,c):
    return acos((c**2-(a**2+b**2))/(-2*a*b))

def ik2(x, y, z, L1, L2, L3):
    t1 = atan2(y, x)
    m = sqrt((x)**2 + y**2)-L1
    e = atan2(z,m)
    print('e',e)
    r = sqrt(m**2+z**2)
    aa = cl(L2,r,L3)
    t2 = aa+e
    gg = cl(L2,L3,r)
    t3 = (math.pi + gg)

#     print('alt', r2d(-aa+e), r2d(-(math.pi + gg)))

    return [r2d(t1), r2d(t2),r2d(t3)]
```

```python
pts = fk(0.0,0.,270., 10, 40, 10)
print('pts: {:.3f} {:.3f} {:.3f}'.format(*pts))

angles = ik2(pts[0], pts[1], pts[2], 10, 40, 10)
print('angles: {:.1f} {:.1f} {:.1f}'.format(*angles))
```

    pts: 50.000 0.000 -10.000
    e -0.244978663127
    angles: 0.0 0.0 270.0

```python
sa = []
sb = []
sc = []
try:
    for a in range(-45,45,5):
        for b in range(-45,45,5):
            for c in range(200,360,5):
                pts = fk(a,b,c, 10, 40, 100)
                angles = ik2(pts[0], pts[1], pts[2], 10, 40, 100)
                sa.append((a-angles[0]))
                sb.append((b-angles[1]))
                sc.append((c-angles[2]))
#                 if (a-angles[0]) > 10:
#                     print(a,b,c)
except Exception as e:
    print('fuck:', e)
    print(pts)
    print(a,b,c)
```

    fuck: math domain error
    [-37.01457039  37.01457039  13.97755493]
    -45 -45 200

```python
plt.subplot(3,1,1)
plt.plot(sa,'r-')
plt.ylabel('Error a')
plt.grid(True);
plt.subplot(3,1,2)
plt.plot(sb,'g-')
plt.ylabel('Error b')
plt.grid(True);
plt.subplot(3,1,3)
plt.plot(sc,'b-')
plt.ylabel('Error c')
plt.grid(True);
```

```python
plt.plot(sa,'r-')
plt.ylabel('Error a')
plt.grid(True);
```

```python
plt.plot(sb,'g-')
plt.ylabel('Error b')
```

```python
plt.plot(sc,'b-')
plt.ylabel('Error c')
plt.grid(True);
```

## Loading

```python
def calcTorque(foot, Lc, Lf, Lt):
    """
    s     s
    +-----+--------+ CM
    |              |
    |              v
    |
        torque balance:  s + s = r W/3
    the servos are at location s, the center mass is at CM, the weight is W
    and r is the distance from the foot to CM. The shoulder servo works perpendicular
    to this plane and doesn't help to lift.
    """
    # servo = 0.15  # TG9e servo torque in Nm
    servo = 0.39  # XL-320 servo torque in Nm

    x = 0.001*sqrt(foot[0]**2 + foot[1]**2)  # convert to mm
    r = .144/2 + x

    # 2*servo = r F
    F = 2.0*servo/r  # 2 servos per leg lifting
    return 3.0*F  # 3 legs are lifting at all times

Lc = 40
Lf = 50
Lt = 100
robot_weight = 560
foot = fk(0,0,-45, Lc, Lf, Lt)
print('Foot location {:.3f} {} {:.3f}'.format(*foot))
N = calcTorque(foot, Lc, Lf, Lt)
print('This can lift {:.3f} N'.format(N))

# why the fuck do people confuse mass and weight!!! weight is N and mass is kg
# the the stupid scale I used was in grams for weight ... idiots!!!
G = 1000*N/9.81
print('This can lift {:.3f} gram force'.format(G))  # convert N to grams force
print('The robot is: {} gram force'.format(robot_weight))
print('Factor of safety: {:.2f}'.format(G/robot_weight))
```

    Foot location 160.711 0 -70.711
    This can lift 10.055 N
    This can lift 1025.016 gram force
    The robot is: 560 gram force
    Factor of safety: 1.83

```python
from mpl_toolkits.mplot3d import Axes3D
from matplotlib import cm
from matplotlib.ticker import LinearLocator, FormatStrFormatter
import matplotlib.pyplot as plt
import numpy as np

# fig = plt.figure()
# ax = fig.gca(projection='3d')
x = np.arange(0, 45, 1.0)
y = np.arange(-135, -90, 1.0)
# X, Y = np.meshgrid(X, Y)
# x = range(0, 25)
# y = range(-115, -90)
Z = []
for femur in x:
    z=[]
    for tibia in y:
#         print('{} {}'.format(femur, tibia))
        pos = ik(0, femur, tibia, Lc, Lf, Lt)
        N = calcTorque(pos, Lc, Lf, Lt)
        G = 1000*N/9.81
        z.append(G/robot_weight)
    Z.append(z)

X, Y = np.meshgrid(x, y)
# ax.plot_surface(X, Y, Z, rstride=1, cstride=1, cmap=cm.coolwarm,
#                        linewidth=0, antialiased=False)
# ax.plot_surface(X, Y, Z);
plt.figure()
plt.contour(X,Y,Z)
```

```python
import matplotlib.pyplot as plt

plt.subplot(2,2,1);
plt.plot(px)
plt.ylabel('x')

plt.subplot(2,2,2)
plt.plot(py)
plt.ylabel('y')

plt.subplot(2,2,3);
plt.plot(pz)
plt.ylabel('z')

plt.subplot(2,2,4);
plt.plot(px,py)
plt.ylabel('y')
plt.xlabel('x');
```

```python

```

```python
atan2(-3,4)
```

    -0.6435011087932844

-----------

This work is licensed under a Creative Commons Attribution-ShareAlike 4.0 International License.


# Battery and ESC selection
*Written by Marc Budinger (INSA Toulouse) and Scott Delbecq (ISAE-SUPAERO), Toulouse, France.*

**Sympy** package permits us to work with symbolic calculation.

```python
import math

from sympy import Symbol
from sympy import *
```

## Design graph

The following diagram represents the design graph of the battery and ESC selection.

> **Questions:**
* Recall the main equations to take into account during ESC and battery selection
* Orientate the arrows
* Give equations order, inputs/outputs at each step of this part of sizing procedure

### Sizing code and optimization

> Exercice: propose a sizing code for the selection of a battery and ESC.

```python
# Specifications
N_pro=4.0#[-] Number of propellers
M_pay=10.0#[kg] Payload mass

# Reference parameters for scaling laws
# Ref : MK-quadro
M_bat_ref = .329 # [kg] mass
E_bat_ref = 220.*3600.*.329 # [J]

# Ref : Turnigy K_Force 70HV
P_esc_ref = 3108. # [W] Power
M_esc_ref = .115 # [kg] Mass

# Assumption
U_bat_est=4.0#[V] Battery voltage estimation
P_el_mot_hov=10.0#[W] Electrical power consumption for one motor during hover
P_el_mot_to=10.0#[W] Electrical power consumption for one motor during takeoff
U_mot_to=4.0#[V] Motor voltage during takeoff
```

Define the design variables as a symbol under `variableExample= Symbol('variableExample')`

```python
#Design variables
k_mb=Symbol('k_mb')#[-]sizing coefficient on the battery load mass (0.1,100)
```

```python
# -----------------------
# equations
# -----------------------
N_s_bat = math.ceil(U_bat_est/3.7) # [-] Cell number, round (up value)
U_bat = 3.7 * N_s_bat # [V] Battery voltage

M_bat = k_mb * M_pay # [kg] Battery mass

# Hover --> autonomy
E_bat = E_bat_ref * M_bat/M_bat_ref*.8 # [J] Energy  of the battery (.8 coefficient because 80% use only of the total capacity)
C_bat = E_bat/U_bat # [A.s] Capacity  of the battery
I_bat = (P_el_mot_hov * N_pro) /.95/U_bat # [A] Current of the battery
t_hov = C_bat/I_bat/60. # [min] Hover time

# ESC
P_esc = P_el_mot_to * U_bat_est / U_mot_to # [W] power electronic power (corner power or apparent power)
M_esc = M_esc_ref * (P_esc/P_esc_ref) # [kg] Mass ESC
V_esc = 1.84 * P_esc**0.36 # [V] ESC voltage
```


**Pregunta 1**

```python
#PREGUNTA 1
import numpy as np

def caida_libre(x):
    v0 = float(input("Ingrese la velocidad inicial:"))
    g = float(input("Ingrese la gravedad:"))
    theta = float(input("Ingrese el ángulo:"))
    y0 = float(input("Ingrese la altura:"))

    y = x*np.tan(theta) - (1/(2*v0))*(g*x**2)/(np.cos(theta))**2 + y0

    return y
```

```python
caida_libre(15)
```

    Ingrese la velocidad inicial:60
    Ingrese la gravedad:10.6
    Ingrese el ángulo:45
    Ingrese la altura:600

    552.2761534586175

**Pregunta 2**

```python
#PREGUNTA 2
import sympy

exp = sympy.sqrt(2)
exp.evalf(100)
```

$\displaystyle 1.414213562373095048801688724209698078569671875376948073176679737990732478462107038850387534327641573$

```python
x, y = sympy.symbols('x y')
expr = (x + y)**6
sympy.expand(expr)
```

$\displaystyle x^{6} + 6 x^{5} y + 15 x^{4} y^{2} + 20 x^{3} y^{3} + 15 x^{2} y^{4} + 6 x y^{5} + y^{6}$

```python
x = sympy.symbols('x')
expr = sympy.sin(x)/x**2
sympy.limit(expr,x,0)
```

$\displaystyle \infty$

```python
expr = sympy.log(x)/x
sympy.diff(expr,x)
```

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

```python
expr = x**4 - 3*x + 1
a = sympy.Poly(expr,x)
```

```python
sum(a.coeffs())
```

$\displaystyle -1$

**Pregunta 3**

```python
#PREGUNTA 3
sun_hours = [
[43.8, 60.5, 190.2, 144.7, 240.9, 210.3, 219.7, 176.3, 199.1, 109.2, 78.7, 67.0],
[49.9, 54.3, 109.7, 102.0, 134.5, 211.2, 174.1, 207.5, 108.2, 113.5, 68.7, 23.3],
[63.7, 72.0, 142.3, 93.5, 150.1, 158.7, 127.9, 135.5, 92.3, 102.5, 62.4, 38.5],
[51.0, 57.9, 133.4, 110.9, 112.4, 199.3, 124.0, 178.3, 102.1, 100.7, 55.7, 58.0],
[69.5, 94.3, 187.6, 152.5, 170.2, 226.9, 237.6, 242.7, 177.3, 101.3, 53.9, 59.0],
[65.9, 96.6, 122.5, 124.9, 216.3, 192.7, 269.3, 184.9, 149.1, 81.5, 48.7, 31.3],
[48.1, 62.0, 121.5, 127.3, 188.5, 196.3, 274.3, 199.9, 144.7, 102.6, 65.4, 48.9],
[43.4, 89.2, 71.4, 133.2, 179.5, 166.2, 119.2, 184.7, 79.3, 103.1, 48.9, 62.3],
[50.9, 66.6, 99.7, 103.1, 185.0, 181.3, 140.1, 202.3, 143.0, 79.1, 65.9, 41.2],
[41.2, 66.9, 172.3, 180.9, 144.9, 190.6, 133.5, 151.3, 110.9, 118.1, 70.0, 52.4],
[46.4, 104.9, 86.2, 171.7, 184.9, 227.9, 139.7, 153.7, 147.0, 94.3, 41.1, 46.0],
[83.1, 22.8, 128.3, 118.1, 215.4, 273.4, 165.1, 199.5, 179.5, 95.5, 76.8, 46.5],
[41.7, 67.9, 118.7, 106.9, 141.9, 210.3, 227.5, 163.7, 123.7, 120.2, 47.1, 46.9],
[45.1, 53.9, 69.4, 202.5, 209.4, 234.0, 150.1, 132.7, 124.5, 84.6, 57.8, 51.0],
[54.7, 79.3, 132.9, 166.6, 244.1, 192.9, 196.7, 178.3, 142.5, 84.9, 72.3, 49.5],
[41.2, 62.4, 142.7, 147.0, 235.6, 170.3, 97.5, 185.2, 143.8, 102.0, 49.3, 64.1],
[51.5, 65.7, 152.6, 209.1, 156.1, 182.4, 159.0, 144.8, 64.9, 111.7, 31.0, 46.6],
[49.9, 78.7, 107.2, 203.3, 162.9, 149.8, 197.6, 134.8, 98.5, 79.3, 42.9, 74.7],
[59.5, 26.3, 70.9, 150.5, 147.3, 185.9, 144.5, 274.9, 159.9, 107.3, 75.4, 37.9],
[45.7, 92.9, 160.2, 205.2, 237.1, 124.2, 174.7, 133.7, 146.4, 93.7, 68.6, 65.4],
[51.0, 115.1, 112.5, 182.5, 233.3, 242.1, 262.5, 210.3, 151.1, 125.0, 76.2, 65.4],
[40.6, 67.5, 138.8, 163.7, 174.1, 244.5, 174.0, 171.1, 112.7, 96.6, 56.9, 55.3],
[48.9, 58.6, 92.6, 200.4, 152.1, 251.9, 216.7, 174.7, 110.8, 105.6, 75.1, 69.8],
[94.1, 96.7, 105.0, 178.2, 207.0, 217.6, 194.0, 180.5, 140.3, 105.0, 72.1, 77.7],
[42.5, 75.9, 140.7, 183.3, 223.0, 139.7, 203.4, 237.4, 151.7, 84.1, 54.4, 28.4],
[75.7, 79.7, 107.9, 202.4, 145.9, 157.1, 157.1, 123.5, 168.8, 94.5, 60.1, 54.5],
[40.1, 86.3, 161.4, 173.7, 217.5, 155.3, 268.3, 188.0, 153.1, 119.7, 71.5, 47.3],
[50.3, 78.9, 149.7, 158.7, 246.6, 145.0, 168.0, 161.4, 94.3, 116.5, 77.9, 18.2],
[50.8, 83.1, 110.2, 168.0, 205.6, 297.1, 157.9, 170.5, 102.6, 92.9, 76.4, 62.3],
[54.6, 55.4, 110.7, 145.2, 196.0, 145.7, 188.1, 119.6, 118.0, 93.7, 51.8, 29.5],
[85.8, 65.5, 102.0, 153.8, 228.0, 226.3, 272.7, 245.6, 213.9, 144.2, 70.6, 45.0],
[37.8, 82.3, 78.0, 164.9, 182.3, 274.9, 129.7, 147.1, 122.8, 60.9, 73.4, 54.5],
[43.6, 65.1, 173.2, 86.9, 225.2, 231.2, 196.5, 185.7, 135.8, 118.2, 63.4, 76.5],
[70.0, 70.6, 126.3, 143.3, 177.5, 280.3, 137.3, 154.5, 142.3, 108.8, 32.7, 72.6],
[58.9, 66.4, 85.8, 119.1, 193.4, 199.4, 188.2, 142.6, 129.7, 78.8, 60.4, 49.8],
[37.2, 57.3, 65.9, 128.5, 190.8, 156.1, 214.7, 217.7, 210.4, 134.5, 55.0, 51.1],
[83.7, 31.1, 137.7, 141.6, 179.6, 188.7, 122.8, 181.2, 122.9, 109.9, 77.4, 71.9],
[42.5, 41.5, 121.5, 81.5, 234.9, 199.0, 149.7, 188.6, 168.0, 90.4, 61.0, 41.7],
[64.2, 88.2, 174.6, 130.8, 184.2, 232.0, 234.4, 167.1, 116.5, 95.1, 69.2, 70.6],
[50.0, 54.0, 148.5, 184.5, 155.0, 206.6, 136.2, 124.0, 114.9, 66.5, 47.9, 35.9],
[40.0, 78.1, 70.5, 221.3, 161.9, 276.9, 243.8, 157.5, 97.4, 112.0, 84.6, 35.6],
[34.5, 115.9, 120.8, 132.7, 224.8, 270.9, 192.4, 185.6, 157.3, 106.2, 64.7, 43.8],
[42.1, 69.5, 106.0, 122.9, 228.9, 143.5, 259.3, 134.2, 166.5, 135.2, 102.0, 29.8],
[41.8, 27.3, 144.0, 117.6, 141.9, 150.4, 168.7, 160.9, 129.1, 91.6, 80.6, 47.6],
[38.8, 74.1, 150.7, 167.7, 168.0, 249.5, 171.1, 192.0, 153.9, 95.1, 89.1, 62.9],
[56.3, 58.3, 101.7, 142.1, 191.4, 206.2, 187.8, 198.7, 146.5, 105.4, 52.9, 58.8],
[44.7, 57.8, 72.7, 131.4, 159.1, 301.0, 242.4, 218.6, 147.0, 120.7, 85.1, 34.4],
[70.3, 42.6, 107.8, 148.7, 172.0, 261.4, 254.2, 257.4, 118.2, 43.6, 54.1, 58.6],
[35.4, 74.4, 87.2, 157.9, 217.5, 123.2, 193.6, 123.4, 101.8, 107.3, 102.4, 45.2],
[53.6, 58.9, 128.1, 113.6, 202.8, 171.7, 146.4, 157.4, 159.3, 87.5, 77.3, 34.3],
[72.0, 67.3, 92.9, 126.4, 190.9, 166.6, 192.2, 167.4, 171.0, 117.0, 70.0, 59.7],
[84.0, 58.8, 86.7, 165.4, 228.7, 186.7, 168.9, 169.0, 136.4, 111.8, 61.4, 64.4],
[50.9, 75.3, 57.8, 110.4, 98.3, 122.8, 129.0, 199.8, 157.3, 101.9, 43.7, 57.5],
[55.0, 33.8, 144.7, 164.4, 187.2, 148.4, 151.4, 159.8, 141.0, 66.3, 68.5, 60.4],
[54.9, 74.0, 89.5, 150.5, 126.8, 180.3, 257.5, 214.5, 92.4, 119.7, 44.3, 62.9],
[86.1, 66.0, 48.8, 236.8, 143.4, 244.3, 249.4, 199.8, 99.6, 88.6, 53.8, 57.6],
[51.1, 78.0, 112.4, 138.3, 178.3, 165.0, 216.0, 164.9, 143.3, 100.8, 86.1, 44.2],
[76.4, 71.2, 127.3, 139.6, 205.6, 222.7, 201.2, 147.0, 171.6, 119.7, 77.9, 64.8],
[68.8, 67.8, 111.6, 158.7, 168.7, 129.3, 179.4, 158.2, 132.3, 109.5, 43.9, 42.9],
[47.7, 103.7, 85.2, 132.0, 178.1, 142.3, 138.8, 178.8, 136.9, 120.0, 91.4, 46.7],
[68.2, 107.0, 100.9, 133.9, 300.8, 244.4, 280.4, 269.5, 141.6, 90.8, 104.1, 26.5],
[58.3, 95.7, 144.1, 234.4, 285.0, 121.1, 268.5, 236.6, 164.7, 124.4, 83.7, 58.8],
[66.9, 60.0, 87.2, 156.6, 142.9, 150.0, 217.3, 241.4, 165.4, 79.4, 57.4, 58.4],
[46.8, 67.3, 73.3, 139.2, 262.7, 212.2, 164.0, 173.6, 120.2, 101.3, 61.5, 47.2],
[38.0, 54.7, 135.0, 111.9, 196.7, 231.4, 190.0, 238.3, 107.7, 120.0, 76.3, 55.0],
[87.0, 77.6, 127.6, 177.7, 162.1, 254.9, 248.3, 191.8, 113.1, 137.5, 46.7, 68.2],
[61.8, 74.9, 198.7, 190.1, 233.5, 194.4, 247.6, 285.1, 135.3, 139.9, 78.1, 40.9],
[29.3, 103.4, 76.4, 148.3, 185.7, 290.7, 256.6, 211.6, 125.3, 130.8, 101.0, 55.5],
[51.4, 64.2, 150.2, 189.9, 261.4, 137.1, 231.7, 172.0, 169.7, 153.8, 47.1, 55.7],
[64.0, 113.0, 77.5, 105.8, 199.8, 114.0, 157.0, 225.0, 133.8, 94.5, 66.3, 38.1],
[51.1, 81.3, 97.4, 147.6, 153.6, 202.1, 235.4, 159.2, 155.2, 144.8, 81.1, 60.9],
[82.1, 104.9, 112.6, 143.4, 189.8, 164.6, 161.2, 209.4, 126.1, 83.9, 69.2, 51.9],
[83.3, 85.0, 74.1, 148.2, 198.3, 226.8, 206.1, 184.1, 123.0, 100.9, 86.9, 79.2],
[44.4, 80.5, 101.1, 210.0, 177.5, 163.3, 178.8, 166.2, 167.1, 104.8, 52.3, 41.3],
[87.7, 94.4, 154.8, 169.8, 191.2, 213.6, 192.0, 228.4, 175.3, 134.8, 78.9, 53.6],
[62.7, 79.1, 101.5, 150.3, 195.5, 223.6, 169.5, 194.1, 174.4, 102.4, 52.4, 58.3],
[65.4, 66.3, 79.3, 136.3, 226.4, 177.6, 192.0, 235.7, 155.4, 92.0, 88.0, 55.7],
[54.9, 73.1, 95.5, 152.5, 165.7, 246.2, 303.7, 167.2, 156.5, 109.0, 101.2, 42.7],
[79.8, 67.6, 165.4, 210.7, 165.5, 149.0, 195.1, 209.2, 142.6, 102.5, 86.9, 57.2],
[62.4, 124.1, 115.2, 161.2, 173.2, 223.8, 198.5, 141.8, 113.5, 132.2, 67.0, 73.5],
[69.3, 64.5, 161.4, 168.4, 226.1, 203.3, 212.3, 190.6, 163.7, 109.7, 73.5, 61.5],
]
```

```python
import pandas as pd
df = pd.DataFrame(sun_hours)
df.columns = ['ene','feb','mar','abr','may','jun','jul','ago','sep','oct','nov','dic']
df.index = range(1929,2010)
```

```python
df
```

      ene
      feb
      mar
      abr
      may
      jun
      jul
      ago
      sep
      oct
      nov
      dic

      1929
      43.8
      60.5
      190.2
      144.7
      240.9
      210.3
      219.7
      176.3
      199.1
      109.2
      78.7
      67.0

      1930
      49.9
      54.3
      109.7
      102.0
      134.5
      211.2
      174.1
      207.5
      108.2
      113.5
      68.7
      23.3

      1931
      63.7
      72.0
      142.3
      93.5
      150.1
      158.7
      127.9
      135.5
      92.3
      102.5
      62.4
      38.5

      1932
      51.0
      57.9
      133.4
      110.9
      112.4
      199.3
      124.0
      178.3
      102.1
      100.7
      55.7
      58.0

      1933
      69.5
      94.3
      187.6
      152.5
      170.2
      226.9
      237.6
      242.7
      177.3
      101.3
      53.9
      59.0

      ...
      ...
      ...
      ...
      ...
      ...
      ...
      ...
      ...
      ...
      ...
      ...
      ...

      2005
      65.4
      66.3
      79.3
      136.3
      226.4
      177.6
      192.0
      235.7
      155.4
      92.0
      88.0
      55.7

      2006
      54.9
      73.1
      95.5
      152.5
      165.7
      246.2
      303.7
      167.2
      156.5
      109.0
      101.2
      42.7

      2007
      79.8
      67.6
      165.4
      210.7
      165.5
      149.0
      195.1
      209.2
      142.6
      102.5
      86.9
      57.2

      2008
      62.4
      124.1
      115.2
      161.2
      173.2
      223.8
      198.5
      141.8
      113.5
      132.2
      67.0
      73.5

      2009
      69.3
      64.5
      161.4
      168.4
      226.1
      203.3
      212.3
      190.6
      163.7
      109.7
      73.5
      61.5

81 rows × 12 columns

```python
df.apply('mean',axis=0)
```

    ene     56.638272
    feb     72.662963
    mar    116.522222
    abr    153.203704
    may    191.070370
    jun    198.546914
    jul    193.750617
    ago    184.339506
    sep    138.341975
    oct    104.633333
    nov     67.375309
    dic     52.354321
    dtype: float64

```python
#Junio tiene mas horas sol
import matplotlib.pyplot as plt

for i in df.columns:
    plt.hist(df[i])
    plt.title(i)
    plt.show()
```

**Pregunta 4**

```python
import sympy as sym

def simpsons(expr,a,b,n):
    i=sym.symbols("i")
    h = (b - a)/n
    cons = (b-a)/(3*n)
    par1 = expr.subs(x,a)
    par2 = expr.subs(x,b)
    a1 = expr.subs(x,a + (2*i-1)*(h))
    par3 = sym.Sum(a1,(i,1,n//2)).doit()
    a2 = expr.subs(x,a + 2*i*h)
    par4 = sym.Sum(a2,(i,1,n//2 - 1)).doit()
    res = (cons)*( par1 + par2 + 4 * par3 + 2 * par4)

    return res.evalf(5)

```

```python
x = sym.symbols('x')
expr = 3/2 * sym.sin(x)**3
a = 0
b = sym.pi
n = 500
h = (b - a)/n
```

```python
h = (b - a)/n
cons = (b-a)/(3*n)
```

```python
par1 = expr.subs(x,a)
par2 = expr.subs(x,b)
```

```python
x = sym.symbols('x')
expr = 3/2 * sym.sin(x)**3
a = 0
b = sym.pi
n = 500
simpsons(expr,a,b,n)
```

$\displaystyle 2.0$

```python
x = sym.symbols('x')
expr = 3/2 * sym.sin(x)**3
a = 0
b = sym.pi
n = 100
simpsons(expr,a,b,n)
```

$\displaystyle 2.0$

```python
x = sym.symbols('x')
expr = 3/2 * sym.sin(x)**3
a = 0
b = sym.pi
n = 12
simpsons(expr,a,b,n)
```

$\displaystyle 1.9995$

```python
x = sym.symbols('x')
expr = 3/2 * sym.sin(x)**3
a = 0
b = sym.pi
n = 6
simpsons(expr,a,b,n)
```

$\displaystyle 1.9892$

```python

```

```python
!pip install sympy
```

    Requirement already satisfied: sympy in c:\users\cesar aaron\anaconda3\lib\site-packages (1.7.1)
    Requirement already satisfied: mpmath>=0.19 in c:\users\cesar aaron\anaconda3\lib\site-packages (from sympy) (1.1.0)

```python

```

**PREGUNTA 5**

```python
import math as m
import random as rnd
import sympy as sp
import numpy as np

import matplotlib.pyplot as plt

import pandas as pd

```

```python
data=pd.read_csv("https://raw.githubusercontent.com/robintux/Datasets4StackOverFlowQuestions/master/AmericaSur.csv",
                          index_col = 0,
                          parse_dates = True)

```

```python
type(data)
```

    pandas.core.frame.DataFrame

```python
data.columns
```

    Index(['Argentina', 'Bolivia', 'Brazil', 'Chile', 'Colombia', 'Ecuador',
           'Guyana', 'Peru', 'Paraguay', 'Suriname', 'Uruguay', 'Venezuela, RB'],
          dtype='object')

```python
data.index
```

    DatetimeIndex(['1960-01-01', '1961-01-01', '1962-01-01', '1963-01-01',
                   '1964-01-01', '1965-01-01', '1966-01-01', '1967-01-01',
                   '1968-01-01', '1969-01-01', '1970-01-01', '1971-01-01',
                   '1972-01-01', '1973-01-01', '1974-01-01', '1975-01-01',
                   '1976-01-01', '1977-01-01', '1978-01-01', '1979-01-01',
                   '1980-01-01', '1981-01-01', '1982-01-01', '1983-01-01',
                   '1984-01-01', '1985-01-01', '1986-01-01', '1987-01-01',
                   '1988-01-01', '1989-01-01', '1990-01-01', '1991-01-01',
                   '1992-01-01', '1993-01-01', '1994-01-01', '1995-01-01',
                   '1996-01-01', '1997-01-01', '1998-01-01', '1999-01-01',
                   '2000-01-01', '2001-01-01', '2002-01-01', '2003-01-01',
                   '2004-01-01', '2005-01-01', '2006-01-01', '2007-01-01',
                   '2008-01-01', '2009-01-01', '2010-01-01', '2011-01-01',
                   '2012-01-01', '2013-01-01', '2014-01-01', '2015-01-01',
                   '2016-01-01', '2017-01-01', '2018-01-01', '2019-01-01'],
                  dtype='datetime64[ns]', freq=None)

```python
data.head()
```

      Argentina
      Bolivia
      Brazil
      Chile
      Colombia
      Ecuador
      Guyana
      Peru
      Paraguay
      Suriname
      Uruguay
      Venezuela, RB

      1960-01-01
      1.55
      NaN
      2.679135
      3.118406
      1.884323
      2.376237
      NaN
      2.508001
      2.528964
      NaN
      NaN
      3.058044

      1961-01-01
      2.14
      1.203464
      2.364066
      3.527447
      2.143256
      2.046767
      NaN
      2.644757
      2.279150
      NaN
      NaN
      1.422222

      1962-01-01
      2.66
      1.158700
      2.398683
      3.043709
      3.099415
      2.045454
      NaN
      2.425068
      2.297392
      NaN
      NaN
      2.312902

      1963-01-01
      1.68
      1.326412
      2.260839
      2.865396
      3.515140
      1.765212
      NaN
      3.316391
      2.398854
      NaN
      NaN
      2.553588

      1964-01-01
      1.15
      NaN
      2.014498
      2.635187
      3.180804
      1.909954
      NaN
      3.340228
      2.403766
      NaN
      NaN
      2.446689

# https://stackoverflow.com/questions/43745394/python-scatterplot-column-names

```python

```


```python
import numpy
from sympy import *

```

# System Equations

## Declare System Variables

```python

# Body angle
phi, theta, psi = symbols('phi theta psi')

# Rotation rates in body frame
dPhi, dTheta, dPsi = symbols('dPhi, dTheta, dPsi')

# Linear velocity in inertial frame
p, q, r = symbols('p q r')

```

## Derive Newton-Euler Equations

### Rotation Matrix from Body Frame to Inertial Frame

```python
# Rotate around the X-axis
Rx = Matrix([[1, 0, 0], [0, cos(phi), -1*sin(phi)], [0, sin(phi), cos(phi)]])

# Rotate around the Y-axis
Ry = Matrix([[cos(theta), 0, sin(theta)], [0, 1, 0], [-1*sin(theta), 0, cos(theta)]])

# Rotate around the Z-axis
Rz = Matrix([[cos(psi), -1*sin(psi), 0], [sin(psi), cos(psi), 0], [0, 0, 1]])

# Rotation matrix from the body frame to the inertial frame
R = Rz*Ry*Rx
R

```

$\displaystyle \left[\begin{matrix}\cos{\left(\psi \right)} \cos{\left(\theta \right)} & \sin{\left(\phi \right)} \sin{\left(\theta \right)} \cos{\left(\psi \right)} - \sin{\left(\psi \right)} \cos{\left(\phi \right)} & \sin{\left(\phi \right)} \sin{\left(\psi \right)} + \sin{\left(\theta \right)} \cos{\left(\phi \right)} \cos{\left(\psi \right)}\\\sin{\left(\psi \right)} \cos{\left(\theta \right)} & \sin{\left(\phi \right)} \sin{\left(\psi \right)} \sin{\left(\theta \right)} + \cos{\left(\phi \right)} \cos{\left(\psi \right)} & - \sin{\left(\phi \right)} \cos{\left(\psi \right)} + \sin{\left(\psi \right)} \sin{\left(\theta \right)} \cos{\left(\phi \right)}\\- \sin{\left(\theta \right)} & \sin{\left(\phi \right)} \cos{\left(\theta \right)} & \cos{\left(\phi \right)} \cos{\left(\theta \right)}\end{matrix}\right]$

### Transformation Matrix for Angular Velocity

```python
# Inertial frame to body frame
dEta = Matrix([[1, sin(phi)*tan(theta), cos(phi)*tan(theta)], [0, cos(phi), -1*sin(phi)], [0, sin(phi)/cos(theta), cos(phi)/cos(theta)]]) * Matrix([p, q, r])
V = Matrix([[1, 0, -1*sin(theta)], [0, cos(phi), cos(theta)*sin(phi)], [0, -1*sin(phi), cos(theta)*cos(phi)]])*Matrix([dPhi, dTheta, dPsi])

```

$\displaystyle \left[\begin{matrix}dPhi - dPsi \sin{\left(\theta \right)}\\dPsi \sin{\left(\phi \right)} \cos{\left(\theta \right)} + dTheta \cos{\left(\phi \right)}\\dPsi \cos{\left(\phi \right)} \cos{\left(\theta \right)} - dTheta \sin{\left(\phi \right)}\end{matrix}\right]$

```python
print(theta)
print(Rx)
Rx
```

    theta
    Matrix([[1, 0, 0], [0, cos(phi), -sin(phi)], [0, sin(phi), cos(phi)]])

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

```python

```


# Lynx Motion AL5D

Here is the serial manipulator we are using for our lab. Note all of the dimension at the bottom center.

The toy RC servos for the robot arm work off of a pulse width modulation (PWM) where a pulse emmited with the following width will move the servo to a desired position.

| Dir    | PWM  |
|--------|------|
|CCW_max | 1200 |
|Center  | 1500 |
|CW_max  | 1800 |

Since this interface was developed around the 1970's, and toy RC servos are built for price not performance, different servos behave differently to the same signal. Thus, the user typically has to find the true limits of each servo to know how to command it. But the times shown above are approximate (close enough for hand grenades and horseshoes).

```python
%matplotlib inline
```

```python
from __future__ import print_function
from __future__ import division
import numpy as np
from matplotlib import pyplot as plt
from sympy import symbols, sin, cos, simplify, trigsimp, pi
from math import radians as d2r
from math import degrees as r2d
from math import atan2, sqrt, acos, fabs
```

```python
class mDH(object):
    """
    This uses the modified DH parameters
    see Craig, eqn (3.6)
    """
    def __init__(self):
        pass

    def fk(self, params):
        t = np.eye(4)
        for p in params:
            t = t.dot(self.makeT(*p))
        return t

    def makeT(self, a, alpha, d, theta):
        return np.array([  # classic DH
            [           cos(theta),           -sin(theta),           0,             a],
            [sin(theta)*cos(alpha), cos(theta)*cos(alpha), -sin(alpha), -d*sin(alpha)],
            [sin(theta)*sin(alpha), cos(theta)*sin(alpha),  cos(alpha),  d*cos(alpha)],
            [                    0,                     0,           0,             1]
        ])

def eval(f):
    """
    This allows you to simplify the trigonomic mess that kinematics can
    create and also substitute in some inputs in the process
    """
    c = []
    for row in f:
        r = []
        for col in row:
            # use python symbolic toolbox to simplify the trig mess above
            r.append(simplify(col))
        c.append(r)
    return np.array(c)

def subs(f, m):
    """
    This allows you to simplify the trigonomic mess that kinematics can
    create and also substitute in some inputs in the process
    """
    c = []
    for row in f:
        r = []
        for col in row:
            r.append(col.subs(m))
        c.append(r)
    return np.array(c)

def rplot(t1, t2, t3, t4):
    """Given the 4 joint angles (in rads), plot the arm in the x-y and w-z planes"""
    l1 = 5.75
    l2 = 7.375
    l3 = 3.375
    ptsx = [0]
    ptsy = [0]

    # our definition is reverse or these joints
    t3 = -t3
    t4 = -t4

    # link 1
    x1 = l1*cos(t2)
    y1 = l1*sin(t2)
    ptsx.append(x1)
    ptsy.append(y1)

    # link 2
    x2 = x1 + l2*cos(t2 + t3)
    y2 = y1 + l2*sin(t2 + t3)
    ptsx.append(x2)
    ptsy.append(y2)

    # link 3
    x3 = x2 + l3*cos(t2 + t3 + t4)
    y3 = y2 + l3*sin(t2 + t3 + t4)
    ptsx.append(x3)
    ptsy.append(y3)

    plt.subplot(1,2,1,projection='polar')
    plt.plot([0, t1], [0, 1.0])
    plt.grid(True)
    plt.title('Azimuth Angle (x-y plane)')

    plt.subplot(1,2,2)
    plt.plot(ptsx, ptsy, 'b-', marker='o')
    plt.axis('equal')
    plt.grid(True)
    plt.title('w-z Plane')
```

```python
def printT(tt):
    """Print out the entire T matrix"""
    R = tt[0:3,0:3]
    D = tt[0:3, 3]
    print('-'*30)
    print('Position:')
    print('  x:', D[0])
    print('  y:', D[1])
    print('  z:', D[2])
    # R(n, o, a)
    print('-'*30)
    print('Orientation')
    print('  nx:', R[0,0])
    print('  ny:', R[0,1])
    print('  nz:', R[0,2])
    print('')
    print('  ox:', R[1,0])
    print('  oy:', R[1,1])
    print('  oz:', R[1,2])
    print('')
    print('  ax:', R[2,0])
    print('  ay:', R[2,1])
    print('  az:', R[2,2])

def printP(dh):
    """dh is the forward kinematics equations matrix"""
    pos = [float(x) for x in dh[0:3, 3]]
    print('Pos (x,y,z): {:5.2f} {:5.2f} {:5.2f}'.format(*pos))
    return pos

def printDegrees(angles):
    """angles are in radians"""
    a = [r2d(x) for x in angles]
    print('Angles: {:6.1f} {:6.1f} {:6.1f} {:6.1f}'.format(*a))
```

The DH parameters are:

| i |$a_i$        | $\alpha_i$   | $d_i$   | $theta_i$  |
|---|-------------|--------------|---------|------------|
| 1 | 0           | 0            | $d_2$   | $\theta_1$ |
| 2 | 0           |$\frac{\pi}{2}$| 0       | $\theta_2$ |
| 3 | $a_3$       | 0            | 0       | $\theta_3$ |
| 4 | $a_4$       | 0            | 0       | $\theta_4$ |
| 5 | $a_5$       | 0            | 0       |          0 |

```python
# from sympy import pi
from sympy import symbols, sin, cos, simplify, trigsimp, pi
t1, t2, t3, t4 = symbols('t1 t2 t3 t4')

# t1 - base
# t2 - shoulder
# t3 - elbow
# t4 - wrist

a2 = 0.0
a3 = 5.75   # shoulder to elbow
a4 = 7.375  # elbow to wrist
a5 = 3.375  # wrist to end effector

# put all angles in rads
# sybolic can't handle converting a symbol using d2r()
# a, alpha, d, theta
params = [
    [    0,    0,    0, t1],
    [    0, pi/2,   0, t2],
    [   a3,    0,    0, t3],
    [   a4,    0,    0, t4],
    [   a5,    0,    0,  0]
]
# params = [
#     [       0,    0, 2.75, t1],
#     [       0, pi/2,    0, t2],
#     [    5.75,    0,    0, t3],
#     [   7.375,    0,    0, t4],
#     [   3.375,    0,    0,  0]
# ]
dh = mDH()
al5d = dh.fk(params)
al5d = eval(al5d)
```

```python
# the symbolic forward kinematics of our robot arm
printT(al5d)
```

    ------------------------------
    Position:
      x: (5.75*cos(t2) + 7.375*cos(t2 + t3) + 3.375*cos(t2 + t3 + t4))*cos(t1)
      y: (5.75*cos(t2) + 7.375*cos(t2 + t3) + 3.375*cos(t2 + t3 + t4))*sin(t1)
      z: 5.75*sin(t2) + 7.375*sin(t2 + t3) + 3.375*sin(t2 + t3 + t4)
    ------------------------------
    Orientation
      nx: 1.0*cos(t1)*cos(t2 + t3 + t4)
      ny: -1.0*sin(t2 + t3 + t4)*cos(t1)
      nz: 1.0*sin(t1)

      ox: 1.0*sin(t1)*cos(t2 + t3 + t4)
      oy: -1.0*sin(t1)*sin(t2 + t3 + t4)
      oz: -1.0*cos(t1)

      ax: 1.0*sin(t2 + t3 + t4)
      ay: 1.0*cos(t2 + t3 + t4)
      az: 0

```python
# Let's set some angles
simp=subs(al5d, [(t1,0.0), (t2,pi/2), (t3, -pi/2), (t4, 0.0)])
printT(simp)
```

    ------------------------------
    Position:
      x: 10.7500000000000
      y: 0
      z: 5.75000000000000
    ------------------------------
    Orientation
      nx: 1.00000000000000
      ny: 0
      nz: 0

      ox: 0
      oy: 0
      oz: -1.00000000000000

      ax: 0
      ay: 1.00000000000000
      az: 0

```python
# simp=subs(al5d, [(t1,0.0),(t2,d2r(111.5)), (t3, d2r(-127)), (t4, d2r(-74.5))])
# printP(simp)
```

# Inverse Kinematics

## Law of Cosines

$$
a^2 = b^2 + c^2 - 2bc \cos(A) \rightarrow \cos(A)=\frac{-a^2+b^2+c^2}{2bc}\\
b^2 = a^2 + c^2 - 2ac \cos(B) \rightarrow \cos(B)=\frac{a^2-b^2+c^2}{2ac}\\
c^2 = a^2 + b^2 - 2ab \cos(C) \rightarrow \cos(C)=\frac{a^2+b^2-c^2}{2ab}
$$

- [Wolfram: law of cosines](http://mathworld.wolfram.com/LawofCosines.html)

## Law of Sines

$$
\frac{a}{\sin(A)} = \frac{b}{\sin(B)} = \frac{c}{\sin(C)}
$$

- [Wolfram: law of sines](http://mathworld.wolfram.com/LawofSines.html)

## Arm

We are going to solve this using geometry! So, given a point (w,z) and the
orientation of the end effector, we will calculate the joint angles ($\theta_2$,
$\theta_3$, $\theta_4$).

You will also want to make some checks:

- $z \geq 0$
- $-1 \geq \arctan(\frac{y}{x}) \leq 1$
- $\|(x,y,z) \| \leq \|(l_1, l_2, l_3) \|$

```python
from math import atan2, acos, sqrt, pi, cos, sin
```

```python
def cosine_law(a, b, c, phase=False):
    if phase:
        angle = ((c**2 - (a**2 + b**2))/(2*a*b))
    else:
        angle = ((c**2 - (a**2 + b**2))/(-2*a*b))

#     print('cosine_law', angle)

    if angle > 1 or angle < -1:
        raise Exception('angle outside range')
    return acos(angle)

def line(x1, y1, x2, y2):
    return sqrt((x2-x1)**2 + (y2-y1)**2)

def mag(a, b):
    return sqrt(a**2 + b**2)

def mag3(a, b, c):
    return sqrt(a**2 + b**2 + c**2)
```

```python
def inverse(x, y, z, orient):
    """
    Azimuth angle is between x and w and lies in the x-y plane

               ^ x
         w     |
           \   |
            \  |
             \ |
              \|
    <----------+ (z is out of the page - right hand rule)
    y

    Most of the robot arm move in the plane defined by w-z

    ^ z
    |   o-----o
    |  /       \
    | /         E
    |/
    +----------------> w

    All joint angles returned are in radians: (t1, t2, t3, t4)

    """
    l1 = 5.75
    l2 = 7.375
    l3 = 3.375

    # check workspace constraints
    if z < 0:
        raise Exception('z in ground')
    elif mag3(x,y,z) > (l1 + l2 + l3):
        raise Exception('out of reach')

    # get x-y plane azimuth
    t1 = atan2(y, x)

    # Now, most of the arm operates in the w-z frame
    w = mag(x, y)         # new frame axis
    gamma = atan2(z, w)
    r = mag(z, w)

    c = mag(w-l3*cos(orient), z-l3*sin(orient))

    t3 = cosine_law(l1, l2, c, True)

    d = cosine_law(l2, c, l1)
    e = cosine_law(c, l3, r)
    t4 = pi - d - e

    alpha = cosine_law(l1, c, l2)
    beta = cosine_law(c,r,l3)

    t2 = alpha + beta + gamma

    return (t1, t2, t3, t4)
```

```python
def checkPts(x, y, z, orient):
    """Given a point (in inches) and orientation (in rads), this calculates
    the joint angles, then uses those angles to calculate the forward solution
    and prints out the error. It also plots the arm.
    """
    angles = inverse(x, y, z, orient)

    a,b,c,d = angles

    simp=subs(al5d, [(t1, a), (t2, b), (t3, -c), (t4, -d)])
    pts = printP(simp)

    printDegrees(angles)
    rplot(*angles)

    error = [a-b for a,b in zip((x,y,z), pts)]
    print('Error: {:6.3f} {:6.3f} {:6.3f}'.format(*error))
```

```python
checkPts(10.75, 0, 5.75, 0.0)
```

```python
# 0 90 -90 0
checkPts(7.385, 0, 5.75-3.375, -pi/2)
```

```python
x,y,z = (7.385*cos(pi/4), 7.385*sin(pi/4), 5.75-3.375)
checkPts(x,y,z, -pi/2)
```

```python
x,y,z = (7.385*cos(-pi/4), 7.385*sin(-pi/4), 5.75-3.375)
checkPts(x,y,z, -pi/2)
```

```python
x,y,z = (7.385*cos(pi/2), 7.385*sin(pi/2), 5.75-3.375)
checkPts(x,y,z, -pi/2)
```

```python
checkPts(5,0,0, -pi/2)
```

```python
checkPts(7,-3,0, -pi/2)
```

```python
checkPts(7,6,4, -pi/2)
```


# An interactive introduction to polyphase filterbanks

**Author:** Danny Price, UC Berkeley

**License:** [CC-BY 4.0](https://creativecommons.org/licenses/by/4.0/)

```python
%matplotlib inline
```

```python
# Import required modules
import numpy as np
import scipy
from scipy.signal import firwin, freqz, lfilter
import matplotlib.pyplot as plt

import seaborn as sns
sns.set_style("white")

def db(x):
    """ Convert linear value to dB value """
    return 10*np.log10(x)
```

## Introduction

If you've opened up this notebook, you're probably trying to learn about polyphase filterbanks and/or spectrometers and found it all a bit confusing.

This notebook is here to help.

To get the most out of this notebook, you should supplement it with a more rigorous overview of the PFB and spectrometers. I've written up a [chapter on spectrometers in radio astronomy](http://arxiv.org/abs/1607.03579) which can serve as your noble steed. There is quite a bit of background knowledge about digital signal processing (DSP) that I'm not going to present -- head on over to the free [DSP Guide](http://www.dspguide.com/ch1.htm) by Stephen Smith if you need a refresher.

## What is a PFB?

A polyphase filterbank (PFB) is simply an efficient computational structure used to form a bank of filters. All that is required to form a PFB is to place a "prototype polyphase filter structure" in front of an FFT. The frontend enhances the filter response of the FFT, making it better by using time samples and filter coefficients.

That's it. For more information, have a read of [this chapter](http://arxiv.org/abs/1607.03579). As a first call though, let's look at polyphase decomposition, and how to do it using `Numpy`.

## Polyphase decomposition

Polyphase decomposition is at the heart of the PFB technique, and is just decomposing a signal $x(n)$ into multiple 'phases' or 'branches'. For example, even and odd decomposition is just:

$$\begin{eqnarray}
x_{even}(n') & = & \left\{ x(0),x(2),x(4),...\right\} \\
x_{odd}(n') & = & \left\{ x(1),x(3),x(5),...\right\} .
\end{eqnarray}$$

More generally, we can decompose $x(n)$ into $P$ phases, denoted $x_p(n')$. Below is a simple example of polyphase decomposition using numpy:

```python
x = np.array([1,2,3,4,5,6,7,8,9,10])
P = 5

x_p = x.reshape((len(x)//P, P)).T
print (x_p)
```

    [[ 1  6]
     [ 2  7]
     [ 3  8]
     [ 4  9]
     [ 5 10]]

### The PFB frontend

Next, let's have a look at the polyphase frontend. This sounds fancy but isn't all that complicated. The purpose of the PFB frontend is to convert your set of $P$ polyphase branches $x_p(n')$ into a set of subfiltered signals, $y_p(n')$

$$
\begin{equation}
y_{p}(n')=\sum_{m=0}^{M-1}h_{p}(m)x_{p}(n'-m),
\end{equation}
$$

where $h_p$ are filter coefficients that have been divided between the $P$ branches.

Here is a diagram showing the operations performed by the frontend, for $M=3$ taps:

The diagram shows an input signal being divided into $M$ taps, each with $P$ points. Within each tap, the signal is  multiplied by the filter coefficients, then a sum across taps is performed. After this, another $P$ points are read, and the signals propagate left-to-right into the next tap (following the arrows).

Not 100% sure you really understand that diagram? Well, let's try and code it up, and hopefully get a better handle on what's happening. Here's a simple implementation:

```python
def pfb_fir_frontend(x, win_coeffs, M, P):
    W = int(x.shape[0] / M / P)
    x_p = x.reshape((W*M, P)).T
    h_p = win_coeffs.reshape((M, P)).T
    x_summed = np.zeros((P, M * W - M))
    for t in range(0, M*W-M):
        x_weighted = x_p[:, t:t+M] * h_p
        x_summed[:, t] = x_weighted.sum(axis=1)
    return x_summed.T
```

Wow. Only 9 lines required! This is short enough for us to go through line by line:

1. Function declaration. The frontend reads in:
  * an input signal x (a numpy array). For this simple code, x has to be a multiple of $M*P$
  * some window coefficients,
  * an integer M representing the number of taps
  * an integer P representing the number of branches
2. Compute the number of windows of length $P$ there are in the data.
3. We apply polyphase decomposition on $x(n)$ to get a set of branches $x_p(n')$.
4. We also divide the window coefficients into branches.
6. Instantiate an empty array to store the signal $y_p(n')$. This is a little shorter than the original $x_p(n')$ as it takes a few cycles for the taps to fill up with data.
7. Now we start a loop, so we can multiply through each time step by the filter coefficients.
8. This is the magic line. we take $M$ samples from each branch, $x_p(n')$, and multiply it through by the filter coefficients. We need to march through the entire `x_p` array, hence the loop.
9. Now we sum over taps.
10. Return the data, with a transpose so that axes are returned as (time, branch).

Let's apply this to some example data. To do that, we'll need a function to generate window coefficients. Fortunately, this is built in to `scipy`. We can make a simple function to generate a `sinc` of the right length and multiply it through by the window of our choice:

```python
def generate_win_coeffs(M, P, window_fn="hamming"):
    win_coeffs = scipy.signal.get_window(window_fn, M*P)
    sinc       = scipy.signal.firwin(M * P, cutoff=1.0/P, window="rectangular")
    win_coeffs *= sinc
    return win_coeffs
```

```python
M = 8
P = 32

x = np.sin(np.arange(0, M*P*10) / np.pi)
win_coeffs = generate_win_coeffs(M, P, window_fn="hamming")

plt.subplot(2,1,1)
plt.title("Time samples")
plt.plot(x)
plt.xlim(0, M*P*3)

plt.subplot(2,1,2)
plt.title("Window function")
plt.plot(win_coeffs)
plt.xlim(0, M*P)

plt.tight_layout(pad=1.0)
plt.show()
```

Now we are ready to try applying `pfb_fir_frontend` to our data:

```python
y_p = pfb_fir_frontend(x, win_coeffs, M, P)

print("n_taps: %i" % M)
print("n_branches: %i" % P)
print("Input signal shape: %i" % x.shape)
print("Window shape:       %i" % win_coeffs.shape)
print("Output data shape: %s" % str(y_p.shape))
```

    n_taps: 8
    n_branches: 32
    Input signal shape: 2560
    Window shape:       256
    Output data shape: (72, 32)

And we can plot the output `y_p` using `imshow`:

```python
plt.figure()
plt.imshow(y_p)
plt.xlabel("Branch")
plt.ylabel("Time")

plt.figure()
plt.plot(y_p[0], label="p=0")
plt.plot(y_p[1], label="p=1")
plt.plot(y_p[2], label="p=2")
plt.xlabel("Time sample, $n'$")
plt.legend()
plt.show()
```

Don't spend too much time trying to interpret this! The frontend only becomes interesting when you follow it up with an FFT.

## Polyphase filterbank

now we have an PFB frontend, all we need is to add on an FFT. Here is the code to implement a simple PFB in python:

```python
def fft(x_p, P, axis=1):
    return np.fft.rfft(x_p, P, axis=axis)

def pfb_filterbank(x, win_coeffs, M, P):
    x_fir = pfb_fir_frontend(x, win_coeffs, M, P)
    x_pfb = fft(x_fir, P)
    return x_pfb
```

The first function is just a helper, and uses the in-built `numpy.fft` library. We apply the FFT over a given axis, which in this case is branches (the number of branches == length of FFT).

The actual `pfb_filterbank` function is now just two lines long: apply a `pfb_fir_frontend` to the data, and then apply an `fft` to the output. The final step is taking the output of the `pfb_filterbank`, squaring it, and taking an average over time.

Finally, here's a function that implements a spectrometer:

```python
def pfb_spectrometer(x, n_taps, n_chan, n_int, window_fn="hamming"):
    M = n_taps
    P = n_chan

    # Generate window coefficients
    win_coeffs = generate_win_coeffs(M, P, window_fn)

    # Apply frontend, take FFT, then take power (i.e. square)
    x_fir = pfb_fir_frontend(x, win_coeffs, M, P)
    x_pfb = fft(x_fir, P)
    x_psd = np.abs(x_pfb)**2

    # Trim array so we can do time integration
    x_psd = x_psd[:np.round(x_psd.shape[0]//n_int)*n_int]

    # Integrate over time, by reshaping and summing over axis (efficient)
    x_psd = x_psd.reshape(x_psd.shape[0]//n_int, n_int, x_psd.shape[1])
    x_psd = x_psd.mean(axis=1)

    return x_psd
```

Let's try it out by generating some data

```python
M     = 4          # Number of taps
P     = 1024       # Number of 'branches', also fft length
W     = 1000       # Number of windows of length M*P in input time stream
n_int = 2          # Number of time integrations on output data

# Generate a test data steam
samples = np.arange(M*P*W)
noise   = np.random.normal(loc=0.5, scale=0.1, size=M*P*W)
freq = 1
amp  = 0.02
cw_signal = amp * np.sin(samples * freq)
data = noise + cw_signal
```

Which we can have a quick look at first:

```python
plt.subplot(3,1,1)
plt.title("Noise")
plt.plot(noise[:250])
plt.subplot(3,1,2)
plt.title("Sin wave")
plt.plot(cw_signal[:250])
plt.subplot(3,1,3)
plt.title("Noise + sin")
plt.plot(data[:250])
plt.xlabel("Time samples")
plt.tight_layout()
plt.show()
```

Now, let's compute the spectrum and plot it over frequency vs. time using `imshow`

```python
X_psd = pfb_spectrometer(data, n_taps=M, n_chan=P, n_int=2, window_fn="hamming")

plt.imshow(db(X_psd), cmap='viridis', aspect='auto')
plt.colorbar()
plt.xlabel("Channel")
plt.ylabel("Time")
plt.show()
```

This plot over frequency vs. time is known as a *waterfall plot*. At the moment, we can't see the sin wave we put in there. If we integrate longer, the noise integrates down as $\sqrt{t}$ (see the radiometer equation), whereas the sin wave is coherent. Using a longer time integration:

```python
X_psd2 = pfb_spectrometer(data, n_taps=M, n_chan=P, n_int=1000, window_fn="hamming")

plt.plot(db(X_psd[0]), c='#cccccc', label='short integration')
plt.plot(db(X_psd2[1]), c='#cc0000', label='long integration')
plt.ylim(-50, -30)
plt.xlim(0, P/2)
plt.xlabel("Channel")
plt.ylabel("Power [dB]")
plt.legend()
plt.show()
```

### Testing leakage with sin waves

Is the PFB's spectral leakage as good as people claim? We can test this out by sweeping a sine wave input and looking at the response of a few channels as a function of sine wave period.

```python
M, P, W = 6, 512, 256                   # taps, channels, windows
period = np.linspace(0, 0.025, 101)

chan0_val = []
chan1_val = []
chan2_val = []
for p in period:
    t = np.arange(0, M*P*W)
    x = np.sin(t * p) + 0.001
    X_psd = pfb_spectrometer(x, n_taps=M, n_chan=P, n_int=256, window_fn="hamming")
    chan0_val.append(X_psd[0, 0])
    chan1_val.append(X_psd[0, 1])
    chan2_val.append(X_psd[0, 2])

plt.plot(period, db(chan0_val))
plt.plot(period, db(chan1_val))
plt.plot(period, db(chan2_val))
plt.xlim(period[0], period[-1])
plt.ylabel("Power [dB]")
plt.xlabel("Input sine wave period")
plt.show()
```

## Where to go from here

The PFB code in this notebook is quite simple, with no bells and whistles. As an exercise, you could:
* add some error handling (e.g. what happens when the time stream isn't a multiple of $M\times P$?),
* make it read from a file and output to another file
* make it work on datasets larger than your computer's memory
* Implement some more fancy features like oversampling
* Implement an inverse PFB
* port it to Julia, Cythonize it, put it in a docker container, print out a figure and stick it on your macbook.
* etcetera.

If you do something that you think would make a great example, please push it to this github repository!

### Open source codes
Are you about to build a new instrument that needs a PFB spectrometer? The good news is that you probably don't have to write your own highly efficient PFB implementation, because people have done it for you. Here's a selection of codes:

* The [CASPER](https://casper.berkeley.edu/wiki/Getting_Started) collaboration provide a FPGA-based PFB and a design environment for making FPGA-based instruments for radio astronomy.
* Karel Adámek, Jan Novotný and Wes Armour wrote very efficient PFB codes for CPU, GPU and Intel Phi, available on [github](https://github.com/wesarmour/astro-accelerate) and detailed in [arXiv](http://arxiv.org/abs/1511.03599)
* Jayanth Chennamangalam created a PFB GPU code, which is used in the [VEGAS spectrometer](http://www.gb.nrao.edu/vegas/). It is available on [github](https://github.com/jayanthc/grating/) and detailed on [arXiv](http://arxiv.org/abs/1411.0436).

### Citing

If you find this notebook useful, please consider referencing the accompanying chapter in your thesis / paper / postcard / sticky note:

Danny C. Price, *Spectrometers and Polyphase Filterbanks in Radio Astronomy*, 2016. Available online at: http://arxiv.org/abs/1607.03579


```python
# ############################################################## #
# skip slide: imports, functions and code used in further slides #
# ############################################################## #

# imports

import datetime as dt
import sys
sys.path.append('../../')

import numpy as np
np.random.seed(42)
import pandas as pd
import matplotlib.pyplot as plt
import matplotlib.dates as mdates

DATA_DIR = '../../data'

confirmed_cases = np.loadtxt(f'{DATA_DIR}/confirmed_cases.dat')
recovered_cases = np.loadtxt(f'{DATA_DIR}/recovered_cases.dat')
death_cases = np.loadtxt(f'{DATA_DIR}/death_cases.dat')
daily_tests = np.loadtxt(f'{DATA_DIR}/tests.dat')
epidemics_start_date = dt.datetime(2020, 2, 26)

date_list = [epidemics_start_date + dt.timedelta(days=i) for i in range(confirmed_cases.size)]
dataframe = pd.DataFrame({
    'date': date_list,
    'confirmed_cases': confirmed_cases,
    'recovered_cases': recovered_cases,
    'death_cases': death_cases,
    'daily_tests': daily_tests})
dataframe = dataframe.reset_index(drop=True)
```

Modeling the Epidemic Outbreak and Dynamics of COVID-19 in Croatia

    Ante Lojic Kapetanovic1, Dragan Poljak2

    1alojic00@fesb.hr, 2dpoljak@fesb.hr

Department of Electronics and Computer Engineering, University of Split

Paper (submission date 11/4/2020): on ArXiv.

Paper code (Python package): on GitHub.

---

## Content

* Abstract
* Results
* Introduction
* Data
* Initial outbreak modeling
    * Growth modeling
        * Exponential growth
        * Sigmoidal growth
* Dynamics modeling
    * Modified SEIR model
    * SEIRD model
    * Multiwave simulation
    * Interesting read
* Effective reproduction number
* Conclusion
* Supplementary material

## Abstract (from the paper written during the lockdown in March, 2020)

"The paper deals with a modeling of the ongoing epidemic caused by Coronavirus disease 2019 (COVID-19) on the closed territory of the Republic of Croatia. Using the official public information on the number of confirmed infected, recovered and deceased individuals, the modified SEIR compartmental model is developed to describe the underlying dynamics of the epidemic. Fitted modified SEIR model provides the prediction of the disease progression in the near future, considering strict control interventions by means of social distancing and quarantine for infected and at-risk individuals introduced at the beginning of COVID-19 spread on February, 25th by Croatian Ministry of Health. Assuming the accuracy of provided data and satisfactory representativeness of the model used, the basic reproduction number is derived. Obtained results portray potential positive developments and justify the stringent precautionary measures introduced by the Ministry of Health."

## Results (from the paper written during the lockdown in March, 2020)

"Fitting the data provides optimal values for epidemiological parameters $\alpha$, $\beta$, $\gamma$ and $\delta$. The basic reproduction number is then calculated for different phases of the epidemic and the  resulting  values  are $1.43$, $1.33$ and $1.25$ for  $80\%$  of  the data, $88\%$ of the data and the complete data set, respectively.

These results imply the effectiveness of the control measures implemented to combat the  epidemic as $R0$ decreases  with each increase of the data set. In case there is no change in control measures, one could infer that the positive downward trend will continue up until the late  April when the number of confirmed active infected cases will reach its maximum. The maximum point is also the inflection point indicating the moment at which $R0<1$ and after which, with the retention of the control measures, the number of total confirmed cases stops increasing."

```python
from covid_19.plotting import plot_data

plot_data(
    epidemics_start_date, confirmed_cases, recovered_cases,
    death_cases, daily_tests)
```

## Introduction

The epidemic of coronavirus disase 2019 (COVID-19), caused by severe acute respiratory syndrome coronavirus2 (SARS-CoV-2), began in Wuhan, China, in late December 2019 [1](#fn1).

A lot of effort has been invested to develop the best possible models that would predict the behavior and dynamics of the epidemic from the day one.

In order to determine epidemic parameters, stochastic models are well adopted and preferred in the current research, but during the ongoing epidemic process, the data are sparse and the epidemic dynamics are better described using deterministic data driven modeling [2](#fn2) [3](#fn3) [4](#fn4) [5](#fn5) [6](#fn6).

1World Health Organization. (2020) Coronavirus disease (COVID-19) outbreak

2Liangrong, P. et al. (2020)  Epidemic analysis of covid-19 in china by dynamical modeling

3Zhao, S. et al. (2020) Modeling the epidemic dynamics and control ofCOVID-19 outbreak in China

4Lopez, L. R. et al. (2020) A modified SEIR model to predict the COVID-19 outbreak in Spain: simulating control scenarios and multi-scale epidemics

5Cereda, D. et al. (2020) The early phase of the COVID-19 outbreak  in  Lombardy, Italy

6Calafiore, G. C. et al. (2020) A Modified SIR Model for the COVID-19 Contagion in Italy

In order to determine parameters of any epidemiological model for epidemics, all clincal features of the pathogen have to be known. Even though coronavirus-based diseases are well known and documented, there are novel important features [7](#fn7):
* a prolonged incubation period, which cause the time delay between real dynamics and the actual status;
* asymptomatic individuals are capable of being infectious carriers of the pathogen;
* the disease transmission is achieved via respiratory droplets and is extremely  difficult to prevent due the well resilient pathogen hardly affected by external atmospheric conditions.

7Guan, W. et al. (2020) Clinical Characteristics of Coronavirus Disease 2019 in China

Here, we introduce the modified version of SEIR(D) model, based on the early work of Kermack and McKendrick [8](#fn8) [9](#fn9) [10](#fn10), with a single additional parameter that enables asymptomatic individuals to be active infectious pathogen carriers to fit and implicitly include additional compartment for quarantined and self-isolated individuals:

8Kermack, W. and McKendrick, A. (1991) Contributions to the mathematical theory of epidemics – I

9Kermack, W. and McKendrick, A. (1991) Contributions to the mathematical theory of epidemics – II. The problem of endemicity

10Kermack, W. and McKendrick, A. (1991) Contributions to the mathematical theory of epidemics – III. Further studies of the problem of endemicity

\begin{align}
    \label{eqn.s}
    S' &= - \beta \cdot \frac{I}{N} \cdot S - \delta \cdot E \cdot S \\
    \label{eqn.e}
    E' &= \beta \cdot \frac{I}{N} \cdot S - \alpha \cdot E + \delta \cdot E \cdot S \\
    \label{eqn.i}
    I' &= \alpha \cdot E - \gamma \cdot I - \mu \cdot I \\
    \label{eqn.r}
    R' &= \gamma \cdot I \\
    \label{eqn.d}
    \big(D' &= \mu \cdot I\big)
\end{align}

where

* $S$ is the susceptibles compartment;
* $E$ is the exposed compartment;
* $I$ is the infected compartment;
* $R$ is the recovered compartment and
* $D$ is the deceased compartment.

and

* $\beta$ - transition or infectious rate; controls the rate of spread which represents the probability of transmitting disease between a susceptible and an infected individual per contact per unit time;
* $\gamma$ - recovery rate;
* $\mu$ - mortality rate;
* $\alpha$ - incubation rate, the reciprocal value of the incubation period;
* $\delta$ - direct transition rate between susceptible and exposed individual;
* $q$ - quarantine or self-isolation rate.

## Data

Daily data on the number of confirmed infected, recovered, deceased individuals, as well as the number of daily PCR tests performed were collected from the official website [11](#fn11) and stored locally for further analysis and modeling.

11Croatian institue of public health. (2020) Official government website for accurate and verified infromation on Coronavirus.

```python
dataframe
```

      date
      confirmed_cases
      recovered_cases
      death_cases
      daily_tests

      0
      2020-02-26
      1.0
      0.0
      0.0
      8.0

      1
      2020-02-27
      3.0
      0.0
      0.0
      8.0

      2
      2020-02-28
      3.0
      0.0
      0.0
      8.0

      3
      2020-02-29
      5.0
      0.0
      0.0
      8.0

      4
      2020-03-01
      6.0
      0.0
      0.0
      8.0

      ...
      ...
      ...
      ...
      ...
      ...

      194
      2020-09-07
      11964.0
      9008.0
      198.0
      3509.0

      195
      2020-09-08
      12081.0
      9266.0
      201.0
      1793.0

      196
      2020-09-09
      12285.0
      9553.0
      203.0
      3813.0

      197
      2020-09-10
      12626.0
      9833.0
      206.0
      4172.0

      198
      2020-09-11
      12917.0
      10142.0
      208.0
      4118.0

199 rows × 5 columns

## Initial outbreak modeling

### Growth modeling

#### Exponential growth

```python
from covid_19 import simulate

eff_date = dt.datetime(2020, 8, 1)
cases = dataframe[dataframe.date > eff_date].confirmed_cases

simulate.initial_growth(
    'exponential', eff_date, cases,
    normalize_data=False, n_days=7)
```

```python
eff_date = dt.datetime(2020, 8, 1)
cases = dataframe[dataframe.date > eff_date].confirmed_cases

simulate.initial_growth(
    'exponential', eff_date, cases,
    normalize_data=False, n_days=7,
    plot_confidence_intervals=True)
```

Unfortunatelly, there is no confirmed value of sensitivity and specificity for tests in Croatia (different hospitals) use different tests. Ideal case is when both sensitivity and specificity are 1 (no false classifications). Realistic case is to expect high value of specificity and sensitivity between 72% and 98% [12](#fn12). Since this simulator takes worse case scenario into an account (95% CI lower bound for sensitivity and upper bound for specificity), the simulation performed here uses sensitivity with expected value of 85% (for 95% CI range between 80.75% and 89.25%, where lower value is taken into an account) and specificity with expected value of 95% (for 95% CI range between 90.25% and 99.75%, where upper value is taken into an account).

12Watson J. et al. (2020) Interpreting a COVID-19 test result

```python
eff_date = dt.datetime(2020, 8, 1)
cases = dataframe[dataframe.date > eff_date].confirmed_cases
tests = dataframe[dataframe.date > eff_date].daily_tests.values

simulate.initial_growth(
    'exponential', eff_date, cases,
    normalize_data=False, n_days=7,
    plot_confidence_intervals=True,
    sensitivity=0.85, specificity=0.95, ci_level=95, daily_tests=tests)
```

```python
from covid_19 import simulate

simulate.averaged_new_cases_v_total_cases(confirmed_cases)
```

#### Sigmoidal growth

```python
from covid_19 import simulate

eff_date = dt.datetime(2020, 8, 1)
cases = dataframe[dataframe.date > eff_date].confirmed_cases
tests = dataframe[dataframe.date > eff_date].daily_tests.values

simulate.initial_growth(
    'logistic', eff_date, cases,
    normalize_data=True, n_days=7,
    plot_confidence_intervals=True,
    sensitivity=0.85, specificity=0.95, ci_level=95, daily_tests=tests)
```

## Dynamics modeling

### Modified SEIR model

```python
removed_cases = recovered_cases + death_cases
active_cases = confirmed_cases - removed_cases

duration = 101

S0 = 2200
E0 = 3 * active_cases[0]
I0 = active_cases[0]
R0 = removed_cases[0]
```

```python
from covid_19 import simulate

(S, E, I, R), seir_model, loss = simulate.seir_dynamics(
    active_cases=active_cases[:duration],
    removed_cases=removed_cases[:duration],
    initial_conditions=(S0, E0, I0, R0),
    epidemics_start_date=epidemics_start_date,
    plot_sim=True,
    plot_l=False)
```

### SEIRD model

```python
duration = 45

S0 = 2200
E0 = 3 * active_cases[0]
I0 = active_cases[0]
R0 = recovered_cases[0]
D0 = death_cases[0]
```

```python
from covid_19 import simulate

(S, E, I, R, D), seird_model, loss = simulate.seird_dynamics(
    active_cases=active_cases[:duration],
    recovered_cases=recovered_cases[:duration],
    death_cases=death_cases[:duration],
    initial_conditions=(S0, E0, I0, R0, D0),
    epidemics_start_date=epidemics_start_date,
    plot_sim=True,
    plot_l=False,
    sensitivity=0.90,
    specificity=0.96,
    new_positives=np.diff(np.concatenate((np.array([0]), confirmed_cases[:duration]))),
    total_tests=daily_tests[:duration])
```

```python
from covid_19.plotting import plot_compartmental_model_forecast

S_pred, E_pred, I_pred, R_pred, D_pred = seird_model.forecast(30)
plot_compartmental_model_forecast(
    epidemics_start_date,
    active_cases[:duration], I, I_pred,
    recovered_cases[:duration], R, R_pred,
    death_cases[:duration], D, D_pred)
```

## Multiwave simulation

```python
from covid_19 import simulate

(S, E, I, R, D) = simulate.seird_multiple_waves(
    active_cases=active_cases,
    recovered_cases=recovered_cases,
    death_cases=death_cases,
    first_wave_eff_population=2200,
    eff_dates=[dt.datetime(2020, 2, 26),
               dt.datetime(2020, 6, 9),
               dt.datetime(2020, 8, 8)],
    plot_sim=True)
```

### Interesting read on the topic

1. [Extended SEIRS model for studying population structure, social distancing, testing, tracing, and quarantining—including stochastic implementations of these models on dynamic networks](https://twitter.com/RS_McGee/status/1242949797247508480) by Ryan McGee;

2. [COVID-19 Projections Using Machine Learning](https://covid19-projections.com/) by Youyang Gu;

3. [Answering the Initial 20 Questions on COVID-19](https://medium.com/@irudan/answering-the-initial-20-questions-on-covid-19-83f40b0486d1) and [Answering 20 More Questions on COVID-19](https://medium.com/@irudan/answering-20-more-questions-on-covid-19-26f179e0c354) by Igor Rudan.

## Reproduction number

**Basic reproduction number $(R_0)$**

The  expected  number  of  secondary infections in a sufficiently large population without prior immunity to a disease. The non-immunity assumption is well aligned with the COVID-19 disease outbreak, since there is no maternal immunity nor there is a functional vaccine yet.

\begin{align}
    \label{eqn.R0}
    R_0 &= \frac{\beta}{\gamma + \alpha}
\end{align}

**Effective reproduction number $(R_t)$**

The expected number of secondary infections caused by a single infected individual at time $t$ in the partially susceptible population. Importance here lies in the varying proportions of the population that become immune for a variety of reasons at any time $t$.

\begin{align}
    \label{eqn.Rt}
    R_t &= S(t) \cdot R_0
\end{align}

```python
from covid_19 import R0

R0.run(
    epidemics_start_date,
    confirmed_cases,
    averaging_period=16,
    symptoms_delay=3,
    ci_plot=True, sensitivity=0.8, specificity=0.95, daily_tests=daily_tests)
```

## Conclusion

* Conclusion of the paper

* Python package for solving, fitting and forecasting the epidemiological situation based on the deterministic modified SEIR(D) compartmental model  is presented

* Capabilities:
    * estimation of transmission epidemiological parameters using the Runge-Kutta initial value problem solver and iterative (limited memory) BFGS optimizer;
    * initial growth modeling and short-term forecasting using predefined fitted growth functions;
    * dynamics modeling and forecasting with uncertainty quantification;
    * estimation of the basic reproduction number and effective reproduction number.

## Supplementary material

Code repository: https://github.com/antelk/covid-19

For all questions: alojic00@fesb.hr