# The Kalman Filter

This filter is the optimal estimator for linear functions and Gaussian distributions.
The Kalman filter can be used as a solution to the online SLAM problem, defined as follows:

**Given:**

   - The robot's controls $u_{1:T} = \{u_1,u_2,u_3...,u_T\}$
   - Observations $z_{1:T} = \{z_1,z_2,z_3...,z_T\}$

**Wanted:**

   - Map of the environment $m$
   - Path of the robot $x_{0:T} = \{x_0,x_1,x_2...,x_T\}$

We will make a probabilistic estimation of the robot's path and the map according to the motion and observation models.

**Motion Model**

The motion model describes the relative motion of the robot. Some examples of motion model are the Odometry-based model (wheeled robots) or the velocity-based model (flying robots).

The motion model is represented by matrices and thus is a linear function.
    \begin{equation}
        x_t = A_tx_{t-1} + B_tu_t + \epsilon_t
    \end{equation}

- $A_t (n\times n)$ maps the state at $t$, given the previous state at $t-1$.
- $B_t (n\times l)$ describes state change from $t-1$ to $t$, given control command.

**Observation Model**

The observation or sensor model relates measurements with the robot's pose: what I am going to observe given that I know the pose or the map. The Beam-Endpoint model is characterized by a gaussian blur around the obstables. The Ray-cast model experiments an exponential decay and covers dynamic obstacles.

The linear mapping between the state and the observation space:
    \begin{equation}
        z_t = C_tx_{t} + \delta_t
    \end{equation}

- $C_t (k\times n)$ maps state $x_t$ to an observation $z_t$.

The Kalman filter is a recursive algorithm and it is computed as follows:

Where $Q_t$ and $R_t$ describe measurement and motion noise, $\mathbf{\mu}_{t}$ is the mean, $\textstyle\sum_t$ is the covariance (uncertainty), $K_t$ is the Kalman gain. The Kalman gain computes how certain I am about the prediction with respect to the motion.

We introduce to the algorithm our current estimate of where we have been in tearms of mean estimate and covariance matrix, as well as the new control command and the observations. We want to update the mean and the covariance matrix so we transit from t-1 to t.
We are computing a weighted sum with the Kalman gain weighting the prediction and the correction.

# The Extended Kalman Filter

However, in most real scenarios we do not have linear functions to describe the movements or the sensor model.
Non-linear functions lead to non-Gaussian distributions and we cannot use the Kalman filter anymore. This is resolved by the Extended Kalman Filter by local linearization. However, we're still assuming Gaussian noise and Gaussian uncertainties.

The algorithm for the Extended Kalman Filter is:

The terms highlighted in blue are those in which the EKF perform the local linearization. Hereafter, I will introduce how these terms are computed.

## Using the EKF to solve the SLAM problem

First we need to define our state vector, and the assumptions for the example that we are using in this notebook:

- Assumption: known correspondences. That is, when I get an observation, I know which landmark it is in my map.
- State space (for 2D plane) contains the robot pose (3 dimensions) and the landmark locations (2 dimensions each), and it's defined as:

\begin{equation}
    x_t = ( \underbrace{x,y,\theta}_{\text{robot's pose}},\underbrace{m_{1,x},m_{1,y}}_{\text{landmark 1}},...,\underbrace{m_{n,x},m_{n,y}}_{\text{landmark n}})^T
\end{equation}

- State representation (very compactly, with $x_R\rightarrow x)$:

\begin{equation}
    \underbrace{\begin{pmatrix}x\\m\end{pmatrix}}_{\mu}\underbrace{\begin{pmatrix}\textstyle\sum_{xx} \textstyle\sum_{xm}\\\textstyle\sum_{mx} \textstyle\sum_{mm}\end{pmatrix}}_{\textstyle\sum}
\end{equation}

With $\sum_{xx}$ representing the uncertainty around the pose, $\sum_{mm}$ the uncertainty about the landmark location, and $\sum_{xm}$ the link between the landmark locations and the position of the robot within the platform.

# Let's code!

```python
import pandas as pd
import matplotlib.pyplot as plt
from matplotlib.patches import Ellipse
from celluloid import Camera
from IPython.display import HTML
import numpy as np
import os
import math
import seaborn as sns
%matplotlib inline
```

### Auxiliary functions

```python
def normalize_angle(phi):
    # Normalize phi to be between -pi and pi
    while(phi>np.pi):
        phi -= 2*np.pi;

    while(phi<-np.pi):
        phi += 2*np.pi

    phiNorm = phi

    return phiNorm

```

```python
def normalize_all_bearings(z):
    # Go over the observations vector and normalize the bearings
    # The expected format of z is [range; bearing; range; bearing; ...]

    for i in range (1,z.shape[0],2):
        z[i] = normalize_angle(z[i])

    zNorm = z
    return zNorm
```

```python
def plot_state(mu,sigma,landmarks,observedLandmarks,fig,ax):

    # Visualizes the state of the EKF SLAM algorithm.
    #
    # The resulting plot displays the following information:
    # - map ground truth (black +'s)
    # - current robot pose estimate (red)
    # - current landmark pose estimates (blue)
    # - visualization of the observations made at this time step (line between robot and landmark)

    # using seaborn, set background grid to gray
    sns.set_style("dark")
    #fig,ax = plt.subplots()

    ax.set_xticks([x for x in range(-2,12)],minor=True )
    ax.set_yticks([y for y in range(-2,12)],minor=True)

    # Plot grid on minor axes in gray (width = 1)
    plt.grid(which='minor',ls='-',lw=1, color='white')

    # Plot grid on major axes in larger width
    plt.grid(which='major',ls='-',lw=2, color='white')

    # Draw the robot
    ax.text(mu[0], mu[1], 'o', ha='center', va='center', color='black', fontsize=20)
    e = plot_conf_ellipse(mu[0:3],sigma[0:3,0:3], 0.6, 'red')
    ax.add_patch(e)

    # Draw the ground truth of the landmarks
    for i,l in enumerate(landmarks):
        ax.text(l[0], l[1], 'x', ha='center', va='center', color='black', fontsize=20)
        if (observedLandmarks[0][i] == 1.0):
            # plot landmark ellipse
            e = plot_conf_ellipse(mu[2*i+3:2*i+5],sigma[2*i+3:2*i+5,2*i+3:2*i+5], 0.6, 'blue')
            ax.add_patch(e)

    return fig

```

```python
from scipy.stats.distributions import chi2
def plot_conf_ellipse(x,C,alpha,color):
    # Calculate unscaled half axes
    sxx = C[0,0]
    syy = C[1,1]
    sxy = C[0,1]
    # Remove imaginary parts in case of neg. definite C
    a = np.sqrt(0.5*(sxx+syy+np.sqrt((sxx-syy)**2+4*sxy**2))).real   # always greater
    b = np.sqrt(0.5*(sxx+syy-np.sqrt((sxx-syy)**2+4*sxy**2))).real   # always smaller

    # Scaling in order to reflect specified probability

    a = a*np.sqrt(chi2.ppf(alpha, df=2))
    b = b*np.sqrt(chi2.ppf(alpha, df=2))

    # Calculate inclination (numerically stable)
    if math.isclose(sxx, syy, rel_tol=0.1): # this function launches a warning
        angle = 0.5*np.arctan(2*sxy/(sxx-syy))
    elif (sxy==0):
        angle = 0
    elif (sxy>0):
        angle = np.pi/4
    elif (sxy<0):
        angle = -np.pi/4

    return Ellipse((x[0],x[1]), a, b, angle, edgecolor = color, facecolor = color, alpha = 0.5)

```

## Prediction step: defining $g(u_t,\mu_{t-1})$

In this step we only update the pose of the robot $x,y,\theta$ and its covariance $\sum_{xx}$ according to the motion model $g$. Note that since we are just considering the robot's movement, without taking into account the sensor's measurements yet, the landmarks locations are not updated in this step.

### The motion model

Here the motion model considered is the Odometry Model:
- The robot moves from $[\overline{x},\overline{y},\overline{\theta}]$ to $[\overline{x}',\overline{y}',\overline{\theta}']$
- We have odometry information $u = [\delta_{rot1},\delta_{rot2},\delta_{trans}]$

\begin{equation}
\delta_{trans} = \sqrt{(\overline{x}'-\overline{x})^2+(\overline{y}'-\overline{y})^2}
\end{equation}

\begin{equation}
\delta_{rot1} = atan2(\overline{y}'-\overline{y},\overline{x}'-\overline{x})-\overline{\theta}
\end{equation}

\begin{equation}
\delta_{rot2} = \overline{\theta}'-\overline{\theta}-\delta_{rot1}
\end{equation}

Thus, our odometry motion model (without the noise model) that we will use to update the robot pose in the state vector $\mu$ is:

\begin{equation}
x' = x + \delta_{trans}\cos{(\theta+\delta_{rot1})}
\end{equation}

\begin{equation}
y' = y + \delta_{trans}\sin{(\theta+\delta_{rot1})}
\end{equation}

\begin{equation}
\theta' = \theta + \delta_{rot1} + \delta_{rot2}
\end{equation}

Next, we update the elements in the covariance matrix associated to the robot pose by performing a local linearization of the function. This is achieved with the partial derivatives of the previous functions, that is, with the Jacobian matrix $G$.
\begin{equation}
G_t^x = \frac{\delta g(u_t,\mu')}{\delta (x,y,\theta)}
\end{equation}
Remember that we're only updating the robot values in the state space. That is, our Jacobian will have the structure:
\begin{equation}
G_t = \begin{bmatrix}
\frac{\delta g(u_t,\mu')}{\delta (x,y,\theta)} & \textbf{0}\\
\textbf{0} & \textbf{I}
\end{bmatrix}
\end{equation}

```python
def prediction_step( mu, sigma, u):
    ''' Updates the belief concerning the robot pose according to the motion model,
    args:
    - mu :: 2N+3 x 1 vector representing the state mean.
    - sigma :: 2N+3 x 2N+3 covariance matrix.
    - u: odometry reading (r1, t, r2).

    '''
    m,n = sigma.shape

    # Compute new mu based on the noise-free (odometry-based) motion model
    mu[0,0] = mu[0,0] + u['t'] * np.cos(mu[2,0] + u['r1']) # x
    mu[1,0] = mu[1,0] + u['t'] * np.sin(mu[2,0] + u['r1']) # y
    mu[2,0] = mu[2,0] + u['r1'] + u['r2'] # y
    mu[2,0] = normalize_angle(mu[2,0])

    # Compute the 3x3 Jacobian Gx of the motion model
    Gx = np.identity(3)
    Gx[0,2] = - u['t'] * np.sin(mu[2,0] + u['r1'])
    Gx[1,2] =   u['t'] * np.cos(mu[2,0] + u['r1'])

    # Construct the full Jacobian G
    G = np.concatenate((Gx,np.zeros((3,n-3))),axis = 1)
    aux = np.concatenate((np.zeros((n-3,3)), np.identity(n-3)),axis = 1)
    G = np.concatenate((G,aux), axis = 0)

    # Motion noise R
    motionNoise = 0.1
    R3 = np.identity(3)*motionNoise
    R3[2,2] = motionNoise/10

    R = np.zeros((sigma.shape))

    R[0:3, 0:3] = R3

    # Compute predicted sigma
    sigma = np.matmul(G,np.matmul(sigma,G.T)) +R # i.e.  sigma = G*sigma*G.T + R

    return mu,sigma

```

# Correction step

Once the pose has been update, it's time for the correction.

- Predicted measurement $h(x)$ : we need to predict what the robot sees. For that, we take the current position of the robot, and the position of the landmarks in the map, and with that we compute our predicted measurement.
- Obtained measurement $z$: we take the real observations of the landmarks according to the sensor.
- Data association: we compute the discrepancy between $h(x)$ and $z$.
- Update step: finally, all the elements in $\mu$ and $\sum$ are updated

```python
def correction_step(mu,sigma,z,observedLandmarks):
    ''' Updates the belief, i. e., mu and sigma after observing landmarks, according to the sensor model.
        The employed sensor model measures the range and bearing of a landmark.
        mu: 2N+3 x 1 vector representing the state mean.
        The first 3 components of mu correspond to the current estimate of the robot pose [x; y; theta]
        The current pose estimate of the landmark with id = j is: [mu(2*j+2); mu(2*j+3)]
        sigma: 2N+3 x 2N+3 is the covariance matrix
        z: struct array containing the landmark observations.
        Each observation z(i) has an id z(i).id, a range z(i).range, and a bearing z(i).bearing
        The vector observedLandmarks indicates which landmarks have been observed at some point by the robot.
        observedLandmarks(j) is false if the landmark with id = j has never been observed before.
    '''
    # Number of measurements in this time step
    m = z.shape[0]
    # Number of dimensions to mu
    dim = mu.shape[0]
    # Z: vectorized form of all measurements made in this time step: [range_1; bearing_1; range_2; bearing_2; ...; range_m; bearing_m]
    # ExpectedZ: vectorized form of all expected measurements in the same form.
    # They are initialized here and should be filled out in the for loop below
    Z = np.zeros([m*2, 1],float)
    expectedZ = np.zeros([m*2, 1],float)

    # Iterate over the measurements and compute the H matrix
    # (stacked Jacobian blocks of the measurement function)
    # H will be 2m x 2N+3
    H = []

    j = 0
    for i,row in z.iterrows():
        # Get the id of the landmark corresponding to the i-th observation
        landmarkId = int(row['r1']) # r1 == ID here
        #landmarkId = landmarkId -1 # adapt the 1-9 range to 0-8 range of the array

        # If the landmark is obeserved for the first time:
        if (observedLandmarks[0][landmarkId-1] == 0):
            # Initialize its pose in mu based on the measurement and the current robot pose:
            a = float(row['t']*np.cos(row['r2']+mu[2]))
            b = float(row['t']*np.sin(row['r2']+mu[2]))
            mu[2*landmarkId+1 : 2*landmarkId+3] = mu[0:2] + np.array([[a], [b]])

            # Indicate in the observedLandmarks vector that this landmark has been observed
            observedLandmarks[0][landmarkId-1] = 1

        # Add the landmark measurement to the Z vector
        Z[2*j] = row['t']
        Z[2*j+1] = row['r2']

        # Use the current estimate of the landmark pose
        # to compute the corresponding expected measurement in expectedZ:
        delta = mu[2 * landmarkId + 1 : 2 * landmarkId + 2+1] - mu[0:2]
        q = np.matmul(delta.T,delta)

        expectedZ[2*j] = math.sqrt(q)
        expectedZ[2*j+1] = normalize_angle(np.arctan2(delta[1],delta[0]) - mu[2])

        delta0 = float(delta[0])
        delta1 = float(delta[1])

        # Compute the Jacobian Hi of the measurement function h for this observation
        Hi = 1/q * np.array([[float(-math.sqrt(q)*delta0),-math.sqrt(q)*delta1, 0, math.sqrt(q)*delta0, math.sqrt(q)*delta1], [ delta1, -delta0, float(-q), -delta1, delta0] ])

        # Map Jacobian Hi to high dimensional space by a mapping matrix Fxj
        Fxj = np.zeros([5,dim])
        Fxj[0:3,0:3] = np.identity(3)
        Fxj[3,2*landmarkId+1] = 1
        Fxj[4,2*landmarkId+2] = 1

        Hi = np.matmul(Hi, Fxj)

        # Augment H with the new Hi

        H.append(Hi)

        j+=1

    # Construct the sensor noise matrix Q
    Q = 0.01*np.identity(2*m)

    # Compute the Kalman gain
    # K = sigma * H.T * inv(H * sigma * H.T + Q)
    Hnp = np.asarray(H).reshape((2*m,dim))
    sigmaxHt = np.matmul(sigma,Hnp.T)
    inverse = np.linalg.inv(np.matmul(Hnp,sigmaxHt)+Q)
    K = np.matmul(sigmaxHt,inverse)

    # Compute the difference between the expected and recorded measurements.
    # Remember to normalize the bearings after subtracting!

    diffZ = normalize_all_bearings(Z-expectedZ)

    # Finish the correction step by computing the new mu and sigma.
    # Normalize theta in the robot pose.
    mu = mu + np.matmul(K, diffZ)
    # sigma = (eye(dim) - K * H) * sigma
    sigma = np.matmul((np.identity(dim) - np.matmul(K, Hnp)), sigma)

    return mu, sigma, observedLandmarks

```

## Initialization

### Data preprocessing

First, we need to read the world data and the sensor measurements.
The world data contains the landmark positions. The sensor data includes the odometry measurements and the range-bearing sensor measurements.

```python
def read_world(filename,path):
    landmarks = pd.read_csv(path+filename,delimiter = ' ',header=None, names = ['x','y'])
    return (np.asarray(landmarks))
def read_data(filename,path):
    data = pd.read_csv(path + filename,delimiter = ' ',header=None, names = ['sensor','r1','t','r2']) # or id, range and bearing for sensor
    return (data)

world_ldmrks = read_world('/data/ekf_world.dat',os.path.abspath(os.getcwd()))
data = read_data('/data/ekf_sensor_data.dat',os.path.abspath(os.getcwd()))
```

We read the sensor data file and assign a time step for each

```python
data = read_data('/data/ekf_sensor_data.dat',os.path.abspath(os.getcwd()))
indexodometry = data[ (data['sensor'] == 'ODOMETRY')].index

timestepindex = []
timestep = 0
for i in range (0,data.shape[0]):
    if(timestep+1 < indexodometry.shape[0]):
        if (i < indexodometry[timestep+1]) :
            timestepindex.append(timestep)
        else:
            timestep +=1
            timestepindex.append(timestep)
    else:
        timestepindex.append(timestep)

data.insert(0, "timestep", timestepindex, True)
data
```

      timestep
      sensor
      r1
      t
      r2

      0
      0
      ODOMETRY
      0.100692
      0.100073
      0.000171

      1
      0
      SENSOR
      1.000000
      1.896454
      0.374032

      2
      0
      SENSOR
      2.000000
      3.853678
      1.519510

      3
      1
      ODOMETRY
      0.099366
      0.099968
      -0.000241

      4
      1
      SENSOR
      1.000000
      1.839227
      0.248026

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

      1538
      330
      SENSOR
      5.000000
      4.951031
      -1.512977

      1539
      330
      SENSOR
      6.000000
      4.917189
      -0.862938

      1540
      330
      SENSOR
      7.000000
      -0.035066
      0.975887

      1541
      330
      SENSOR
      8.000000
      1.900675
      3.144946

      1542
      330
      SENSOR
      9.000000
      4.171552
      0.072790

1543 rows × 5 columns

Once the data is stored in a pandas dataframe, we can for example read the odometry measurements for the time step 0 as:

```python
data.loc[(data['timestep'] == 0) & (data['sensor'] == 'ODOMETRY')]
```

      timestep
      sensor
      r1
      t
      r2

      0
      0
      ODOMETRY
      0.100692
      0.100073
      0.000171

Or the range bearing measurement for the time step 330 as:

```python
data.loc[(data['timestep'] == 330) & (data['sensor'] == 'SENSOR')]
```

      timestep
      sensor
      r1
      t
      r2

      1536
      330
      SENSOR
      3.0
      3.510814
      0.993067

      1537
      330
      SENSOR
      4.0
      4.925102
      -2.218075

      1538
      330
      SENSOR
      5.0
      4.951031
      -1.512977

      1539
      330
      SENSOR
      6.0
      4.917189
      -0.862938

      1540
      330
      SENSOR
      7.0
      -0.035066
      0.975887

      1541
      330
      SENSOR
      8.0
      1.900675
      3.144946

      1542
      330
      SENSOR
      9.0
      4.171552
      0.072790

Get the number of landmarks in the map

```python
N = world_ldmrks.shape[0]
N
```

    9

observedLandmarks is a vector that keeps track of which landmarks have been observed so far.
observedLandmarks(i) will be true if the landmark i has been observed at some point

```python
observedLandmarks = np.zeros((1,N))
```

### Initializing the belief

Given that we know the number of landmarks in our map, we can define the shape of the mean and the covariance matrix:

- mu: 2N+3x1 vector representing the mean of the normal distribution.
The first 3 components of mu correspond to the pose of the robot, and the landmark poses (xi, yi) are stacked in ascending id order.

- sigma: (2N+3)x(2N+3) covariance matrix of the normal distribution.

Everything is completely unknown in the beginnig, so we define the starting point as our coordinate system. That is, $\mu = \textbf{0}$. Since we are completely certain about that (because we've defined it), the corresponding values in sigma are also zero $\sum_{xx} = \textbf{0}$. However, we don't know anything about the landmarks, because we haven't seen anything yet, so they have an infinite uncertainty.

```python
# Initialize mu
mu = np.zeros((2*N+3,1),dtype = 'float')
# Initialize sigma
robSigma = np.zeros((3,3))
robMapSigma = np.zeros((3,2*N))
mapSigma = np.identity((2*N))*1000 # 1000 as a "infinite" or just "high" value

aux1 = np.concatenate((robSigma,robMapSigma),axis = 1)
aux2 = np.concatenate((robMapSigma.T,mapSigma), axis = 1)

sigma = np.concatenate((aux1,aux2),axis = 0)
```

The sigma values corresponding to the robot pose, as mentioned, have 0. value (no uncertainty)

```python
sigma[0:3, 0:3]
```

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

However, the covariance associated to the landmark pose has infinite value(here 1000, just a high value), for instance, for the first landmark:

```python
sigma[3,3]
```

    1000.0

```python
def ekf_loop(mu,sigma,observedLandmarks,fig,ax,camera):

    for t in range (0,timestepindex[-1]):
        # Perform the prediction step of the EKF
        mu,sigma = prediction_step(mu, sigma, data.loc[(data['timestep'] == t) & (data['sensor'] == 'ODOMETRY')])

        #Perform the correction step of the EKF
        mu, sigma, observedLandmarks = correction_step(mu,sigma,data.loc[(data['timestep'] == t) & (data['sensor'] == 'SENSOR')],observedLandmarks)
        # Generate visualization plots
        fig = plot_state(mu,sigma,world_ldmrks,observedLandmarks,fig,ax)
        camera.snap()
    return camera, mu[0:3],sigma[0:3,0:3]

```

```python
observedLandmarks = np.zeros((1,N))
robSigma = np.zeros((3,3))
robMapSigma = np.zeros((3,2*N))
mapSigma = np.identity((2*N))*1000 # 1000 as a "infinite" or just "high" value
aux1 = np.concatenate((robSigma,robMapSigma),axis = 1)
aux2 = np.concatenate((robMapSigma.T,mapSigma), axis = 1)
sigma = np.concatenate((aux1,aux2),axis = 0)
mu = np.zeros((2*9+3,1),dtype = 'float')

fig,ax = plt.subplots()
camera = Camera(fig)

camera = ekf_loop(mu,sigma,observedLandmarks,fig,ax,camera)[0]

animation = camera.animate()
HTML(animation.to_html5_video())

```

## Testing

Here I compare the results obtained here with the results obtained by the original Octave code. Note that there are differences due to the different functions that the python and octave libraries have.

```python
def test_total(decimal = 2):
    # input to function
    fig,ax = plt.subplots()
    camera = Camera(fig)
    muini = np.zeros((2*9+3,1),dtype = 'float')
    robSigma = np.zeros((3,3))
    robMapSigma = np.zeros((3,2*N))
    mapSigma = np.identity((2*N))*1000 # 1000 as a "infinite" or just "high" value
    aux1 = np.concatenate((robSigma,robMapSigma),axis = 1)
    aux2 = np.concatenate((robMapSigma.T,mapSigma), axis = 1)
    sigmaini = np.concatenate((aux1,aux2),axis = 0)
    observedLandmarks = np.zeros((1,9))

    # desired output
    muresult = np.array([[5.0178],[4.6234],[1.5430]])
    sigmaresult = np.array([[0.1607056,  -0.0557768,  -0.0105228],\
                            [-0.0557768,   0.1671933,   0.0119245],\
                            [-0.0105228,   0.0119245,   0.0037868]])

    # function output
    _,testmu,testsigma = ekf_loop(muini,sigmaini,observedLandmarks,fig,ax,camera)

    # test
    try:
        print('====================================================================')
        np.testing.assert_almost_equal(testmu, muresult,decimal= decimal)
        print('\x1b[6;30;42m' + 'Test Success for mu in ekf_loop' + '\x1b[0m')
    except AssertionError as e:
        print('====================================================================')
        print('\x1b[0;30;41m' + 'Test Failed for mu in ekf_loop' + '\x1b[0m')
        print(e)

    try:
        np.testing.assert_almost_equal(testsigma, sigmaresult,decimal= decimal+1)
        print('====================================================================')
        print('\x1b[6;30;42m' + 'Test Success for sigma in ekf_loop' + '\x1b[0m')
    except AssertionError as e:
        print('====================================================================')
        print('\x1b[0;30;41m' + 'Test Failed for sigma in ekf_loop:' + '\x1b[0m')
        print(e)

def test_prediction(decimal = 2):
    # input to function
    predmu = np.array([[5.04492],[4.42067],[1.55460],[1.80649],[0.83467],[-0.10844],[3.87679],\
    [2.00993],[6.81895],[8.85230],[1.62635],[9.96294],[4.60379],[9.03066],[7.60910],[4.93532],\
    [4.76744],[4.87090],[2.75450],[5.02812],[8.76238]])
    u = data.loc[(data['timestep'] == 330) & (data['sensor'] == 'ODOMETRY')]
    presigma = np.array([[2.6108e-01,  -5.6749e-02,  -1.0594e-02,   1.1899e-01,  -1.9931e-02,   1.5414e-01,   1.2071e-03,   1.8575e-01 , -2.2452e-02,   1.2952e-01,  -9.6479e-02 ,  1.6147e-01,  -1.0754e-01,   1.9297e-01,  -9.7449e-02,   1.6260e-01,-5.3947e-02,   1.4095e-01,  -5.3326e-02,   2.0615e-01,  -5.5533e-02],\
    [  -5.6749e-02,   2.6897e-01,   1.2191e-02,  -1.0836e-02,   1.2655e-01,  -4.9441e-02,   1.0319e-01 , -8.4403e-02,   1.2944e-01,  -2.2330e-02,   2.1129e-01,  -5.7534e-02,   2.2349e-01,  -9.2427e-02,   2.1233e-01,  -5.8696e-02,1.6350e-01,  -3.4450e-02,   1.6275e-01,  -1.0540e-01,   1.6522e-01],\
    [-1.0594e-02,   1.2191e-02,   1.3796e-02,  -1.6596e-03,   3.8382e-03 , -9.4275e-03,  -8.1671e-04,  -1.6430e-02,   4.3996e-03,  -3.9440e-03,   2.0860e-02,  -1.1036e-02,   2.3321e-02,  -1.8038e-02 ,  2.1076e-02,  -1.1296e-02,1.1375e-02,  -6.4802e-03,   1.1235e-02,  -2.0938e-02,   1.1734e-02],\
    [1.1899e-01,  -1.0836e-02,  -1.6596e-03,   1.1266e-01,  -5.2917e-03,   1.1804e-01,  -1.9095e-03,   1.2291e-01,  -5.6064e-03,   1.1426e-01,  -1.6993e-02,   1.1916e-01,  -1.8694e-02,   1.2403e-01,  -1.7138e-02,   1.1932e-01,-1.0452e-02,   1.1600e-01,  -1.0357e-02,   1.2602e-01,  -1.0683e-02],\
    [-1.9931e-02,   1.2655e-01,   3.8382e-03,  -5.2917e-03,   1.1371e-01,  -1.7745e-02,   1.0593e-01,  -2.8980e-02,   1.1446e-01,  -8.9791e-03,   1.4079e-01,  -2.0313e-02,   1.4472e-01,  -3.1570e-02,   1.4112e-01,  -2.0700e-02,1.2566e-01,  -1.3012e-02,   1.2544e-01,  -3.6174e-02,   1.2619e-01],\
    [1.5414e-01,  -4.9441e-02,  -9.4275e-03,   1.1804e-01,  -1.7745e-02,   1.4908e-01,   7.4526e-04 ,  1.7629e-01,  -1.9837e-02,   1.2720e-01,  -8.4449e-02,   1.5505e-01,  -9.4097e-02,   1.8260e-01,  -8.5284e-02,   1.5603e-01,-4.7269e-02,   1.3714e-01,  -4.6730e-02,   1.9389e-01,  -4.8597e-02],\
    [1.2071e-03,   1.0319e-01 , -8.1671e-04,  -1.9095e-03,   1.0593e-01,   7.4526e-04 ,  1.0758e-01,   3.1316e-03 ,  1.0577e-01,  -1.1237e-03 ,  1.0016e-01,   1.2881e-03,   9.9327e-02,   3.6820e-03,   1.0009e-01 ,  1.3707e-03,1.0338e-01,  -2.6522e-04,   1.0343e-01,   4.6616e-03,   1.0327e-01],\
    [1.8575e-01,  -8.4403e-02,  -1.6430e-02,   1.2291e-01,  -2.8980e-02,   1.7629e-01,   3.1316e-03,   2.2457e-01,  -3.2848e-02,   1.3882e-01,  -1.4541e-01,   1.8735e-01,  -1.6223e-01,   2.3540e-01 , -1.4687e-01 ,  1.8903e-01,-8.0603e-02,   1.5610e-01 , -7.9612e-02,   2.5507e-01,  -8.2915e-02],\
    [-2.2452e-02,   1.2944e-01,   4.3996e-03,  -5.6064e-03,   1.1446e-01,  -1.9837e-02,   1.0577e-01,  -3.2848e-02,   1.1572e-01,  -9.8738e-03,   1.4580e-01,  -2.2882e-02,   1.5031e-01,  -3.5759e-02,   1.4619e-01,  -2.3327e-02,1.2843e-01,  -1.4501e-02,   1.2814e-01,  -4.1035e-02,   1.2905e-01],\
    [1.2952e-01 , -2.2330e-02,  -3.9440e-03,   1.1426e-01,  -8.9791e-03,   1.2720e-01,  -1.1237e-03,   1.3882e-01,  -9.8738e-03,   1.1842e-01,  -3.6883e-02,   1.2992e-01,  -4.0882e-02,   1.4147e-01,  -3.7199e-02,   1.3030e-01,-2.1392e-02,   1.2239e-01,  -2.1184e-02,   1.4618e-01,  -2.1935e-02],\
    [-9.6479e-02,   2.1129e-01,   2.0860e-02 , -1.6993e-02,   1.4079e-01,  -8.4449e-02,   1.0016e-01,  -1.4541e-01,   1.4580e-01,  -3.6883e-02,   2.8941e-01,  -9.8511e-02,   3.1067e-01,  -1.5933e-01,   2.9118e-01,  -1.0063e-01,2.0647e-01,  -5.8939e-02,   2.0514e-01,  -1.8441e-01,   2.0949e-01],\
    [1.6147e-01,  -5.7534e-02 , -1.1036e-02,   1.1916e-01 , -2.0313e-02,   1.5505e-01,   1.2881e-03,   1.8735e-01,  -2.2882e-02 ,  1.2992e-01,  -9.8511e-02,   1.6285e-01,  -1.0988e-01,   1.9480e-01,  -9.9591e-02,   1.6367e-01,-5.4965e-02,   1.4151e-01,  -5.4295e-02,   2.0797e-01,  -5.6577e-02],\
    [-1.0754e-01,   2.2349e-01,   2.3321e-02 , -1.8694e-02,   1.4472e-01,  -9.4097e-02,   9.9327e-02,  -1.6223e-01,   1.5031e-01,  -4.0882e-02,   3.1067e-01,  -1.0988e-01,   3.3472e-01,  -1.7781e-01,   3.1287e-01,  -1.1218e-01,2.1812e-01,  -6.5564e-02,   2.1662e-01,  -2.0584e-01,   2.2152e-01],\
    [1.9297e-01 , -9.2427e-02,  -1.8038e-02,   1.2403e-01,  -3.1570e-02,   1.8260e-01,   3.6820e-03,   2.3540e-01,  -3.5759e-02,   1.4147e-01,  -1.5933e-01,   1.9480e-01,  -1.7781e-01,   2.4811e-01,  -1.6079e-01,   1.9658e-01,-8.8286e-02,   1.6044e-01,  -8.7155e-02,   2.6931e-01,  -9.0957e-02],\
    [-9.7449e-02 ,  2.1233e-01,   2.1076e-02,  -1.7138e-02,   1.4112e-01,  -8.5284e-02,   1.0009e-01,  -1.4687e-01,   1.4619e-01,  -3.7199e-02,   2.9118e-01,  -9.9591e-02,   3.1287e-01,  -1.6079e-01,   2.9341e-01,  -1.0164e-01,2.0748e-01,  -5.9509e-02,   2.0612e-01,  -1.8624e-01,   2.1054e-01],\
    [1.6260e-01,  -5.8696e-02,  -1.1296e-02,   1.1932e-01,  -2.0700e-02,   1.5603e-01,   1.3707e-03,   1.8903e-01,  -2.3327e-02,   1.3030e-01,  -1.0063e-01,   1.6367e-01,  -1.1218e-01,   1.9658e-01,  -1.0164e-01,   1.6488e-01,-5.6116e-02,   1.4217e-01,  -5.5455e-02,   2.1010e-01 , -5.7733e-02],\
    [-5.3947e-02,   1.6436e-01,   1.1394e-02,  -1.0472e-02,  1.2570e-01,  -4.7379e-02,   1.0337e-01,  -8.0808e-02,   1.2849e-01 , -2.1446e-02,   2.0674e-01,  -5.5111e-02,   2.1841e-01,  -8.8508e-02,   2.0774e-01 , -5.6242e-02, 1.6184e-01,  -3.3445e-02,   1.6102e-01,  -1.0218e-01,   1.6339e-01],\
    [1.4095e-01,  -3.4946e-02,  -6.4906e-03,   1.1601e-01, -1.3041e-02,   1.3721e-01,  -2.5900e-04,   1.5622e-01, -1.4534e-02 ,  1.2242e-01,  -5.9092e-02,   1.4160e-01,  -6.5735e-02,   1.6057e-01,  -5.9664e-02,   1.4225e-01, -3.3445e-02,   1.2933e-01,  -3.3040e-02,   1.6835e-01,  -3.4366e-02],\
    [-5.3326e-02,   1.6360e-01,   1.1250e-02 , -1.0376e-02,   1.2548e-01 , -4.6834e-02,   1.0342e-01,  -7.9803e-02,   1.2819e-01,  -2.1235e-02 ,  2.0537e-01,  -5.4431e-02,   2.1688e-01,  -8.7360e-02,   2.0636e-01,  -5.5574e-02,1.6102e-01,  -3.3040e-02,   1.6050e-01, -1.0087e-01, 1.6259e-01],\
    [2.0615e-01,  -1.0699e-01,  -2.0970e-02,   1.2606e-01,  -3.6263e-02,   1.9411e-01,   4.6805e-03,   2.5546e-01,  -4.1141e-02,   1.4628e-01,  -1.8489e-01,   2.0825e-01,  -2.0638e-01 ,  2.6973e-01,  -1.8673e-01,   2.1035e-01, -1.0218e-01,   1.6835e-01 , -1.0087e-01,   2.9488e-01,  -1.0522e-01],\
    [-5.5533e-02,   1.6612e-01,   1.1753e-02,  -1.0704e-02,   1.2624e-01 , -4.8714e-02,   1.0326e-01,  -8.3133e-02,   1.2911e-01,  -2.1992e-02,   2.0978e-01,  -5.6735e-02,   2.2183e-01,  -9.1199e-02,  2.1083e-01,  -5.7870e-02,1.6339e-01,  -3.4366e-02,   1.6259e-01,  -1.0522e-01,   1.6541e-01]])

    # desired output
    predmuresult =np.array([[5.04656],[4.52065],[1.55445],[1.80649],[0.83467],[-0.10844],[3.87679],\
    [2.00993],[6.81895],[8.85230],[1.62635],[9.96294],[4.60379],[9.03066],[7.60910],\
    [4.93532],[4.76744],[4.87090],[2.75450],[5.02812],[8.76238]])
    sigmaresult = np.array([[2.6108e-01, -5.6749e-02, -1.0594e-02,   1.1899e-01,  -1.9931e-02,   1.5414e-01,   1.2071e-03,   1.8575e-01,  -2.2452e-02,   1.2952e-01,  -9.6479e-02,   1.6147e-01,  -1.0754e-01,   1.9297e-01,  -9.7449e-02,   1.6260e-01, -5.3573e-02,   1.4073e-01,  -5.2965e-02,   2.0545e-01,  -5.5147e-02],\
    [-5.5777e-02,   1.6719e-01,   1.1924e-02,  -1.0710e-02,   1.2625e-01,  -4.8728e-02,   1.0326e-01,  -8.3151e-02,   1.2910e-01,  -2.2023e-02,   2.0970e-01,  -5.6684e-02 ,  2.2172e-01,  -9.1057e-02,   2.1073e-01,  -5.7846e-02, 1.6350e-01,  -3.4450e-02,   1.6275e-01,  -1.0540e-01,   1.6522e-01],\
    [-1.0523e-02 ,  1.1924e-02,   3.7868e-03,  -1.6571e-03,   3.8324e-03,  -9.4136e-03,  -8.1547e-04,  -1.6404e-02,   4.3922e-03,  -3.9369e-03,   2.0828e-02,  -1.1017e-02,   2.3286e-02,  -1.8010e-02,   2.1044e-02,  -1.1280e-02, 1.1375e-02,  -6.4802e-03,   1.1235e-02,  -2.0938e-02,   1.1734e-02],\
    [1.1893e-01,  -1.0710e-02,  -1.6571e-03,   1.1266e-01,  -5.2842e-03,   1.1802e-01,  -1.9111e-03,   1.2287e-01, -5.5982e-03,   1.1425e-01,  -1.6955e-02,   1.1913e-01,  -1.8652e-02,   1.2399e-01,  -1.7100e-02,   1.1930e-01, -1.0452e-02,   1.1600e-01,  -1.0357e-02,   1.2602e-01,  -1.0683e-02],\
    [ -1.9801e-02,   1.2625e-01,   3.8324e-03,  -5.2842e-03,   1.1370e-01,  -1.7704e-02,   1.0593e-01,  -2.8907e-02,   1.1444e-01,  -8.9590e-03,   1.4070e-01,  -2.0261e-02,   1.4462e-01,  -3.1492e-02,   1.4103e-01,  -2.0653e-02,1.2566e-01,  -1.3012e-02 ,  1.2544e-01 , -3.6174e-02 ,  1.2619e-01],\
    [1.5382e-01,  -4.8728e-02,  -9.4136e-03,   1.1802e-01,  -1.7704e-02,   1.4898e-01,   7.3662e-04,   1.7611e-01,  -1.9791e-02,   1.2716e-01,  -8.4240e-02,   1.5492e-01,  -9.3865e-02,   1.8241e-01,  -8.5073e-02,   1.5592e-01,-4.7269e-02,   1.3714e-01,  -4.6730e-02,   1.9389e-01,  -4.8597e-02],\
    [1.1794e-03,   1.0326e-01,  -8.1547e-04,  -1.9111e-03,   1.0593e-01,   7.3662e-04,   1.0757e-01,   3.1162e-03,   1.0577e-01,  -1.1279e-03,   1.0018e-01,   1.2770e-03,   9.9347e-02,   3.6656e-03,   1.0011e-01,   1.3608e-03, 1.0338e-01,  -2.6522e-04,   1.0343e-01,   4.6616e-03,   1.0327e-01],\
    [1.8519e-01,  -8.3151e-02,  -1.6404e-02,   1.2287e-01,  -2.8907e-02,   1.7611e-01,   3.1162e-03,   2.2425e-01,  -3.2761e-02,   1.3873e-01,  -1.4503e-01,   1.8713e-01,  -1.6181e-01,   2.3506e-01,  -1.4648e-01,   1.8883e-01, -8.0603e-02,   1.5610e-01,  -7.9612e-02,   2.5507e-01,  -8.2915e-02],\
    [-2.2306e-02,   1.2910e-01,   4.3922e-03,  -5.5982e-03,   1.1444e-01,  -1.9791e-02,   1.0577e-01 , -3.2761e-02,   1.1570e-01,  -9.8519e-03 ,  1.4569e-01,  -2.2823e-02,   1.5019e-01,  -3.5667e-02,   1.4608e-01,  -2.3275e-02,1.2843e-01,  -1.4501e-02,   1.2814e-01,  -4.1035e-02,   1.2905e-01],\
    [1.2938e-01,  -2.2023e-02,  -3.9369e-03,   1.1425e-01,  -8.9590e-03,   1.2716e-01,  -1.1279e-03,   1.3873e-01,  -9.8519e-03,   1.1840e-01,  -3.6779e-02,   1.2986e-01,  -4.0767e-02,   1.4138e-01,  -3.7095e-02,   1.3024e-01,-2.1392e-02,   1.2239e-01,  -2.1184e-02,   1.4618e-01,  -2.1935e-02],\
    [-9.5795e-02,   2.0970e-01,   2.0828e-02,  -1.6955e-02,   1.4070e-01,  -8.4240e-02,   1.0018e-01,  -1.4503e-01,   1.4569e-01,  -3.6779e-02,   2.8891e-01,  -9.8239e-02,   3.1012e-01,  -1.5891e-01,   2.9067e-01,  -1.0039e-01,2.0647e-01,  -5.8939e-02,   2.0514e-01,  -1.8441e-01,   2.0949e-01],\
    [1.6109e-01,  -5.6684e-02,  -1.1017e-02,   1.1913e-01,  -2.0261e-02,   1.5492e-01,   1.2770e-03,   1.8713e-01,  -2.2823e-02,   1.2986e-01,  -9.8239e-02,   1.6268e-01,  -1.0957e-01,   1.9456e-01,  -9.9313e-02,   1.6352e-01, -5.4965e-02,   1.4151e-01,  -5.4295e-02,   2.0797e-01,  -5.6577e-02],\
    [-1.0677e-01,   2.2172e-01,   2.3286e-02,  -1.8652e-02,   1.4462e-01,  -9.3865e-02,   9.9347e-02,  -1.6181e-01,   1.5019e-01,  -4.0767e-02,   3.1012e-01,  -1.0957e-01,   3.3411e-01,  -1.7735e-01,   3.1231e-01,  -1.1192e-01, 2.1812e-01,  -6.5564e-02,   2.1662e-01,  -2.0584e-01,   2.2152e-01],\
    [1.9236e-01,  -9.1057e-02,  -1.8010e-02,   1.2399e-01,  -3.1492e-02,   1.8241e-01 ,  3.6656e-03,   2.3506e-01,  -3.5667e-02,   1.4138e-01,  -1.5891e-01,   1.9456e-01,  -1.7735e-01,   2.4774e-01,  -1.6037e-01,   1.9637e-01,-8.8286e-02,   1.6044e-01,  -8.7155e-02,   2.6931e-01,  -9.0957e-02],\
    [-9.6757e-02,   2.1073e-01,   2.1044e-02,  -1.7100e-02,   1.4103e-01 , -8.5073e-02,   1.0011e-01,  -1.4648e-01,   1.4608e-01,  -3.7095e-02,   2.9067e-01,  -9.9313e-02,   3.1231e-01,  -1.6037e-01,   2.9291e-01,  -1.0140e-01, 2.0748e-01,  -5.9509e-02,   2.0612e-01,  -1.8624e-01,   2.1054e-01],\
    [1.6222e-01,  -5.7846e-02,  -1.1280e-02,   1.1930e-01,  -2.0653e-02,   1.5592e-01,   1.3608e-03,   1.8883e-01,  -2.3275e-02,   1.3024e-01,  -1.0039e-01,   1.6352e-01,  -1.1192e-01,   1.9637e-01,  -1.0140e-01,   1.6475e-01, -5.6116e-02,   1.4217e-01,  -5.5455e-02 ,  2.1010e-01,  -5.7733e-02],\
    [-5.3573e-02,   1.6350e-01,   1.1375e-02,  -1.0452e-02 ,  1.2566e-01,  -4.7269e-02,   1.0338e-01,  -8.0603e-02,   1.2843e-01,  -2.1392e-02,   2.0647e-01,  -5.4965e-02,   2.1812e-01,  -8.8286e-02,   2.0748e-01,  -5.6116e-02,1.6184e-01,  -3.3445e-02,   1.6102e-01,  -1.0218e-01,   1.6339e-01],\
    [ 1.4073e-01,  -3.4450e-02,  -6.4802e-03,   1.1600e-01,  -1.3012e-02,   1.3714e-01,  -2.6522e-04,   1.5610e-01,  -1.4501e-02,   1.2239e-01,  -5.8939e-02,   1.4151e-01,  -6.5564e-02,   1.6044e-01,  -5.9509e-02,   1.4217e-01, -3.3445e-02,   1.2933e-01 , -3.3040e-02,   1.6835e-01,  -3.4366e-02],\
    [ -5.2965e-02,   1.6275e-01,   1.1235e-02,  -1.0357e-02,   1.2544e-01,  -4.6730e-02,   1.0343e-01,  -7.9612e-02,   1.2814e-01,  -2.1184e-02,   2.0514e-01,  -5.4295e-02,   2.1662e-01,  -8.7155e-02 ,  2.0612e-01 , -5.5455e-02,1.6102e-01,  -3.3040e-02,   1.6050e-01,  -1.0087e-01,   1.6259e-01],\
    [ 2.0545e-01 , -1.0540e-01,  -2.0938e-02,   1.2602e-01,  -3.6174e-02,   1.9389e-01,   4.6616e-03,   2.5507e-01,  -4.1035e-02,   1.4618e-01,  -1.8441e-01,   2.0797e-01,  -2.0584e-01,   2.6931e-01,  -1.8624e-01,   2.1010e-01,-1.0218e-01,   1.6835e-01,  -1.0087e-01,   2.9488e-01,  -1.0522e-01],\
    [-5.5147e-02,   1.6522e-01,   1.1734e-02,  -1.0683e-02,   1.2619e-01,  -4.8597e-02,   1.0327e-01,  -8.2915e-02,   1.2905e-01,  -2.1935e-02,   2.0949e-01,  -5.6577e-02,   2.2152e-01,  -9.0957e-02,   2.1054e-01,  -5.7733e-02, 1.6339e-01,  -3.4366e-02,   1.6259e-01,  -1.0522e-01,   1.6541e-01]])

    # function output
    testmu, testsigma = prediction_step(predmu,presigma,u)

    # test
    try:
        print('====================================================================')
        np.testing.assert_almost_equal(testmu, predmuresult,decimal= decimal)
        print('\x1b[6;30;42m' + 'Test Success for mu in prediction_step' + '\x1b[0m')
    except AssertionError as e:
        print('====================================================================')
        print('\x1b[0;30;41m' + 'Test Failed for mu in prediction_step' + '\x1b[0m')
        print(e)

    try:
        np.testing.assert_almost_equal(testsigma, sigmaresult,decimal= decimal+1)
        print('====================================================================')
        print('\x1b[6;30;42m' + 'Test Success for sigma in prediction_step' + '\x1b[0m')
    except AssertionError as e:
        print('====================================================================')
        print('\x1b[0;30;41m' + 'Test Failed for sigma in prediction_step:' + '\x1b[0m')
        print(e)

def test_correction(decimal = 2):
    # input values
    muinput = np.array([[5.04656],[4.52065],[1.55445],[1.80649],[0.83467],[-0.10844],\
               [3.87679],[2.00993],[6.81895],[8.85230],[1.62635],[9.96294],[4.60379],\
               [9.03066],[7.60910],[4.93532],[4.76744],[4.87090],[2.75450],[5.02812],[8.76238]])
    sigmainput = np.array([[2.6108e-01,  -5.6749e-02,  -1.0594e-02,   1.1899e-01,  -1.9931e-02,   1.5414e-01,   1.2071e-03,   1.8575e-01,  -2.2452e-02,   1.2952e-01,  -9.6479e-02,   1.6147e-01,  -1.0754e-01,   1.9297e-01,  -9.7449e-02,   1.6260e-01,-5.3947e-02,   1.4095e-01,  -5.3326e-02,   2.0615e-01,  -5.5533e-02],\
            [-5.6749e-02,   2.6897e-01,   1.2191e-02,  -1.0836e-02,   1.2655e-01,  -4.9441e-02 ,  1.0319e-01,  -8.4403e-02,   1.2944e-01,  -2.2330e-02,   2.1129e-01,  -5.7534e-02,   2.2349e-01,  -9.2427e-02,   2.1233e-01,  -5.8696e-02, 1.6436e-01,  -3.4946e-02,   1.6360e-01,  -1.0699e-01,   1.6612e-01],\
            [-1.0594e-02,   1.2191e-02,   1.3796e-02,  -1.6596e-03,   3.8382e-03,  -9.4275e-03,  -8.1671e-04,  -1.6430e-02,   4.3996e-03,  -3.9440e-03,   2.0860e-02,  -1.1036e-02,   2.3321e-02,  -1.8038e-02,   2.1076e-02,  -1.1296e-02,1.1394e-02,  -6.4906e-03,   1.1250e-02,  -2.0970e-02,   1.1753e-02],\
            [1.1899e-01 , -1.0836e-02,  -1.6596e-03,   1.1266e-01,  -5.2917e-03,   1.1804e-01,  -1.9095e-03,   1.2291e-01,  -5.6064e-03,   1.1426e-01,  -1.6993e-02,   1.1916e-01,  -1.8694e-02,   1.2403e-01,  -1.7138e-02,   1.1932e-01,-1.0472e-02,   1.1601e-01,  -1.0376e-02,   1.2606e-01,  -1.0704e-02],\
            [-1.9931e-02,   1.2655e-01,   3.8382e-03,  -5.2917e-03,   1.1371e-01,  -1.7745e-02,   1.0593e-01,  -2.8980e-02,   1.1446e-01,  -8.9791e-03,   1.4079e-01,  -2.0313e-02,   1.4472e-01,  -3.1570e-02,   1.4112e-01,  -2.0700e-02,1.2570e-01,  -1.3041e-02 ,  1.2548e-01,  -3.6263e-02,   1.2624e-01],\
            [1.5414e-01,  -4.9441e-02,  -9.4275e-03,   1.1804e-01,  -1.7745e-02,   1.4908e-01,   7.4526e-04,   1.7629e-01,  -1.9837e-02 ,  1.2720e-01,  -8.4449e-02,   1.5505e-01,  -9.4097e-02,   1.8260e-01,  -8.5284e-02,   1.5603e-01,-4.7379e-02,   1.3721e-01,  -4.6834e-02,   1.9411e-01,  -4.8714e-02],\
            [1.2071e-03,   1.0319e-01,  -8.1671e-04,  -1.9095e-03,   1.0593e-01,   7.4526e-04,   1.0758e-01,   3.1316e-03,   1.0577e-01,  -1.1237e-03,   1.0016e-01,   1.2881e-03,   9.9327e-02,   3.6820e-03,   1.0009e-01,   1.3707e-03,1.0337e-01,  -2.5900e-04,   1.0342e-01,   4.6805e-03,   1.0326e-01],\
            [1.8575e-01,  -8.4403e-02,  -1.6430e-02,   1.2291e-01,  -2.8980e-02,   1.7629e-01,   3.1316e-03,   2.2457e-01,  -3.2848e-02,   1.3882e-01,  -1.4541e-01,   1.8735e-01,  -1.6223e-01,   2.3540e-01,  -1.4687e-01,   1.8903e-01,-8.0808e-02,   1.5622e-01,  -7.9803e-02,   2.5546e-01,  -8.3133e-02],\
            [-2.2452e-02,   1.2944e-01,   4.3996e-03,  -5.6064e-03,   1.1446e-01,  -1.9837e-02,   1.0577e-01,  -3.2848e-02,   1.1572e-01,  -9.8738e-03,   1.4580e-01,  -2.2882e-02,   1.5031e-01,  -3.5759e-02,   1.4619e-01,  -2.3327e-02,1.2849e-01,  -1.4534e-02,   1.2819e-01,  -4.1141e-02,   1.2911e-01],\
            [1.2952e-01,  -2.2330e-02,  -3.9440e-03,   1.1426e-01,  -8.9791e-03,   1.2720e-01,  -1.1237e-03,   1.3882e-01,  -9.8738e-03,   1.1842e-01,  -3.6883e-02,   1.2992e-01,  -4.0882e-02,   1.4147e-01,  -3.7199e-02,   1.3030e-01,-2.1446e-02,   1.2242e-01,  -2.1235e-02,   1.4628e-01,  -2.1992e-02],\
            [-9.6479e-02,   2.1129e-01,   2.0860e-02,  -1.6993e-02,   1.4079e-01,  -8.4449e-02 ,  1.0016e-01,  -1.4541e-01,   1.4580e-01 , -3.6883e-02,   2.8941e-01,  -9.8511e-02,   3.1067e-01,  -1.5933e-01,   2.9118e-01,  -1.0063e-01,2.0674e-01,  -5.9092e-02,   2.0537e-01,  -1.8489e-01,   2.0978e-01],\
            [1.6147e-01,  -5.7534e-02,  -1.1036e-02,   1.1916e-01,  -2.0313e-02,   1.5505e-01,   1.2881e-03,   1.8735e-01,  -2.2882e-02,   1.2992e-01,  -9.8511e-02,   1.6285e-01,  -1.0988e-01,   1.9480e-01,  -9.9591e-02,   1.6367e-01,-5.5111e-02,   1.4160e-01,  -5.4431e-02,   2.0825e-01,  -5.6735e-02],\
            [-1.0754e-01,   2.2349e-01,   2.3321e-02,  -1.8694e-02,   1.4472e-01,  -9.4097e-02,   9.9327e-02,  -1.6223e-01,   1.5031e-01,  -4.0882e-02,   3.1067e-01,  -1.0988e-01,   3.3472e-01,  -1.7781e-01,   3.1287e-01,  -1.1218e-01,2.1841e-01,  -6.5735e-02,   2.1688e-01,  -2.0638e-01,   2.2183e-01],\
            [1.9297e-01,  -9.2427e-02,  -1.8038e-02,   1.2403e-01,  -3.1570e-02,   1.8260e-01,   3.6820e-03,   2.3540e-01,  -3.5759e-02,   1.4147e-01,  -1.5933e-01,   1.9480e-01,  -1.7781e-01,   2.4811e-01,  -1.6079e-01,   1.9658e-01,-8.8508e-02,   1.6057e-01,  -8.7360e-02,   2.6973e-01,  -9.1199e-02],\
            [-9.7449e-02,   2.1233e-01,   2.1076e-02,  -1.7138e-02,   1.4112e-01,  -8.5284e-02 ,  1.0009e-01,  -1.4687e-01,   1.4619e-01,  -3.7199e-02,   2.9118e-01,  -9.9591e-02,   3.1287e-01,  -1.6079e-01,   2.9341e-01,  -1.0164e-01, 2.0774e-01,  -5.9664e-02 ,  2.0636e-01,  -1.8673e-01,   2.1083e-01],\
            [1.6260e-01,  -5.8696e-02,  -1.1296e-02,   1.1932e-01,  -2.0700e-02,   1.5603e-01,   1.3707e-03,   1.8903e-01,  -2.3327e-02,   1.3030e-01,  -1.0063e-01,   1.6367e-01,  -1.1218e-01,   1.9658e-01,  -1.0164e-01,   1.6488e-01,-5.6242e-02,   1.4225e-01,  -5.5574e-02,   2.1035e-01,  -5.7870e-02],\
            [-5.3947e-02,   1.6436e-01,   1.1394e-02,  -1.0472e-02,   1.2570e-01,  -4.7379e-02,   1.0337e-01, -8.0808e-02,   1.2849e-01,  -2.1446e-02,   2.0674e-01,  -5.5111e-02,   2.1841e-01,  -8.8508e-02,   2.0774e-01,  -5.6242e-02, 1.6198e-01,  -3.3526e-02,   1.6115e-01,  -1.0244e-01,   1.6354e-01],\
            [1.4095e-01,  -3.4946e-02,  -6.4906e-03,   1.1601e-01,  -1.3041e-02,   1.3721e-01,  -2.5900e-04,   1.5622e-01,  -1.4534e-02,   1.2242e-01,  -5.9092e-02,   1.4160e-01,  -6.5735e-02,   1.6057e-01,  -5.9664e-02,   1.4225e-01, -3.3526e-02,   1.2938e-01,  -3.3115e-02,   1.6851e-01,  -3.4452e-02],\
            [-5.3326e-02,   1.6360e-01,   1.1250e-02,  -1.0376e-02,   1.2548e-01,  -4.6834e-02 ,  1.0342e-01,  -7.9803e-02,   1.2819e-01,  -2.1235e-02,   2.0537e-01,  -5.4431e-02,   2.1688e-01,  -8.7360e-02,   2.0636e-01,  -5.5574e-02,  1.6115e-01,  -3.3115e-02,   1.6062e-01,  -1.0111e-01,   1.6273e-01],\
            [2.0615e-01,  -1.0699e-01,  -2.0970e-02,   1.2606e-01,  -3.6263e-02,   1.9411e-01,   4.6805e-03,   2.5546e-01,  -4.1141e-02,   1.4628e-01,  -1.8489e-01,   2.0825e-01,  -2.0638e-01,   2.6973e-01,  -1.8673e-01,   2.1035e-01,  -1.0244e-01,   1.6851e-01,  -1.0111e-01,   2.9537e-01,  -1.0550e-01],\
            [-5.5533e-02,   1.6612e-01,   1.1753e-02,  -1.0704e-02,   1.2624e-01,  -4.8714e-02,   1.0326e-01,  -8.3133e-02,   1.2911e-01,  -2.1992e-02,   2.0978e-01,  -5.6735e-02,   2.2183e-01,  -9.1199e-02,   2.1083e-01,  -5.7870e-02,   1.6354e-01,  -3.4452e-02,   1.6273e-01,  -1.0550e-01,   1.6559e-01]])
    observedLandmarks = np.ones((1,9))
    # desired values
    corrmu = np.array([[5.01782],[4.62335],[1.54298],[1.80461],[0.83901],[-0.11898],[3.87586],\
          [1.99148],[6.82428],[8.84647],[1.65308],[9.94389],[4.63452], [9.00673],\
          [7.63752],[4.92296],[4.78357],[4.86310],[2.76855],[5.00123],[8.78071]])
    corrsigma = np.array([[1.6071e-01,  -5.5777e-02,  -1.0523e-02,   1.1893e-01,  -1.9801e-02,   1.5382e-01,   1.1794e-03,   1.8519e-01,  -2.2306e-02,   1.2938e-01,  -9.5795e-02,   1.6109e-01 , -1.0677e-01,   1.9236e-01,  -9.6757e-02,   1.6222e-01, -5.3573e-02,   1.4073e-01,  -5.2965e-02,   2.0545e-01,  -5.5147e-02],\
                        [-5.5777e-02,   1.6719e-01,   1.1924e-02,  -1.0710e-02,   1.2625e-01,  -4.8728e-02,   1.0326e-01,  -8.3151e-02,   1.2910e-01,  -2.2023e-02,   2.0970e-01,  -5.6684e-02,   2.2172e-01,  -9.1057e-02,   2.1073e-01,  -5.7846e-02, 1.6350e-01,  -3.4450e-02,   1.6275e-01,  -1.0540e-01,   1.6522e-01],\
                        [-1.0523e-02,   1.1924e-02,   3.7868e-03,  -1.6571e-03,   3.8324e-03,  -9.4136e-03,  -8.1547e-04 , -1.6404e-02,   4.3922e-03,  -3.9369e-03,   2.0828e-02 , -1.1017e-02,   2.3286e-02,  -1.8010e-02,   2.1044e-02,  -1.1280e-02,1.1375e-02,  -6.4802e-03,   1.1235e-02,  -2.0938e-02,   1.1734e-02],\
                        [1.1893e-01,  -1.0710e-02,  -1.6571e-03,   1.1266e-01,  -5.2842e-03,   1.1802e-01,  -1.9111e-03,   1.2287e-01,  -5.5982e-03,   1.1425e-01,  -1.6955e-02,   1.1913e-01,  -1.8652e-02,   1.2399e-01,  -1.7100e-02,   1.1930e-01,-1.0452e-02,   1.1600e-01,  -1.0357e-02,   1.2602e-01,  -1.0683e-02],\
                        [-1.9801e-02,   1.2625e-01,   3.8324e-03,  -5.2842e-03 ,  1.1370e-01,  -1.7704e-02,   1.0593e-01,  -2.8907e-02,   1.1444e-01,  -8.9590e-03,   1.4070e-01 , -2.0261e-02,   1.4462e-01,  -3.1492e-02,   1.4103e-01,  -2.0653e-02,1.2566e-01,  -1.3012e-02,   1.2544e-01,  -3.6174e-02,   1.2619e-01],\
                        [1.5382e-01,  -4.8728e-02,  -9.4136e-03,   1.1802e-01,  -1.7704e-02,   1.4898e-01,   7.3662e-04,   1.7611e-01,  -1.9791e-02,   1.2716e-01,  -8.4240e-02,   1.5492e-01,  -9.3865e-02,   1.8241e-01,  -8.5073e-02,   1.5592e-01,-4.7269e-02,   1.3714e-01,  -4.6730e-02,   1.9389e-01,  -4.8597e-02],\
                        [1.1794e-03,   1.0326e-01,  -8.1547e-04,  -1.9111e-03,   1.0593e-01,   7.3662e-04,   1.0757e-01,   3.1162e-03,   1.0577e-01,  -1.1279e-03,   1.0018e-01,   1.2770e-03,   9.9347e-02,   3.6656e-03,   1.0011e-01,   1.3608e-03,1.0338e-01,  -2.6522e-04,   1.0343e-01,   4.6616e-03,   1.0327e-01],\
                        [1.8519e-01,  -8.3151e-02,  -1.6404e-02,   1.2287e-01,  -2.8907e-02,   1.7611e-01,   3.1162e-03,   2.2425e-01,  -3.2761e-02,   1.3873e-01,  -1.4503e-01,   1.8713e-01,  -1.6181e-01,   2.3506e-01,  -1.4648e-01,   1.8883e-01,-8.0603e-02,   1.5610e-01,  -7.9612e-02,   2.5507e-01,  -8.2915e-02],\
                        [-2.2306e-02,   1.2910e-01,   4.3922e-03,  -5.5982e-03,   1.1444e-01,  -1.9791e-02,   1.0577e-01,  -3.2761e-02,   1.1570e-01,  -9.8519e-03,   1.4569e-01,  -2.2823e-02,   1.5019e-01,  -3.5667e-02,   1.4608e-01,  -2.3275e-02,1.2843e-01,  -1.4501e-02,   1.2814e-01,  -4.1035e-02,  1.2905e-01],\
                        [1.2938e-01,  -2.2023e-02,  -3.9369e-03,   1.1425e-01,  -8.9590e-03,   1.2716e-01,  -1.1279e-03,   1.3873e-01,  -9.8519e-03,   1.1840e-01,  -3.6779e-02,   1.2986e-01,  -4.0767e-02,   1.4138e-01,  -3.7095e-02,   1.3024e-01, -2.1392e-02,   1.2239e-01,  -2.1184e-02,   1.4618e-01,  -2.1935e-02],\
                        [-9.5795e-02,   2.0970e-01,   2.0828e-02,  -1.6955e-02,   1.4070e-01,  -8.4240e-02,   1.0018e-01,  -1.4503e-01,   1.4569e-01,  -3.6779e-02,   2.8891e-01,  -9.8239e-02,   3.1012e-01,  -1.5891e-01,   2.9067e-01,  -1.0039e-01,2.0647e-01,  -5.8939e-02,   2.0514e-01,  -1.8441e-01,   2.0949e-01],\
                        [1.6109e-01,  -5.6684e-02,  -1.1017e-02,   1.1913e-01,  -2.0261e-02,   1.5492e-01,   1.2770e-03,   1.8713e-01,  -2.2823e-02,   1.2986e-01,  -9.8239e-02,   1.6268e-01,  -1.0957e-01,   1.9456e-01,  -9.9313e-02,   1.6352e-01,-5.4965e-02,   1.4151e-01,  -5.4295e-02,   2.0797e-01,  -5.6577e-02],\
                        [-1.0677e-01,   2.2172e-01,   2.3286e-02,  -1.8652e-02,   1.4462e-01,  -9.3865e-02,   9.9347e-02,  -1.6181e-01,   1.5019e-01,  -4.0767e-02,   3.1012e-01,  -1.0957e-01,   3.3411e-01,  -1.7735e-01,   3.1231e-01,  -1.1192e-01,2.1812e-01,  -6.5564e-02,   2.1662e-01,  -2.0584e-01,   2.2152e-01],\
                        [1.9236e-01,  -9.1057e-02,  -1.8010e-02,   1.2399e-01,  -3.1492e-02,   1.8241e-01,   3.6656e-03,   2.3506e-01,  -3.5667e-02,   1.4138e-01,  -1.5891e-01,   1.9456e-01,  -1.7735e-01,   2.4774e-01,  -1.6037e-01,   1.9637e-01,-8.8286e-02,   1.6044e-01,  -8.7155e-02,   2.6931e-01,  -9.0957e-02],\
                        [-9.6757e-02,   2.1073e-01,   2.1044e-02,  -1.7100e-02,   1.4103e-01,  -8.5073e-02,   1.0011e-01,  -1.4648e-01,   1.4608e-01 , -3.7095e-02,   2.9067e-01,  -9.9313e-02,   3.1231e-01,  -1.6037e-01,   2.9291e-01,  -1.0140e-01,2.0748e-01,  -5.9509e-02,   2.0612e-01,  -1.8624e-01,   2.1054e-01],\
                        [1.6222e-01,  -5.7846e-02,  -1.1280e-02,   1.1930e-01,  -2.0653e-02,   1.5592e-01,   1.3608e-03,   1.8883e-01,  -2.3275e-02,   1.3024e-01,  -1.0039e-01,   1.6352e-01,  -1.1192e-01,   1.9637e-01,  -1.0140e-01,   1.6475e-01,-5.6116e-02,   1.4217e-01 , -5.5455e-02,   2.1010e-01,  -5.7733e-02],\
                        [-5.3573e-02,   1.6350e-01 ,  1.1375e-02,  -1.0452e-02,   1.2566e-01,  -4.7269e-02,   1.0338e-01,  -8.0603e-02,   1.2843e-01,  -2.1392e-02,   2.0647e-01,  -5.4965e-02 ,  2.1812e-01,  -8.8286e-02,   2.0748e-01,  -5.6116e-02,1.6184e-01,  -3.3445e-02,   1.6102e-01,  -1.0218e-01,   1.6339e-01],\
                        [1.4073e-01,  -3.4450e-02,  -6.4802e-03,   1.1600e-01,  -1.3012e-02,   1.3714e-01,  -2.6522e-04,   1.5610e-01,  -1.4501e-02,   1.2239e-01,  -5.8939e-02,   1.4151e-01,  -6.5564e-02,   1.6044e-01,  -5.9509e-02,   1.4217e-01,-3.3445e-02,   1.2933e-01,  -3.3040e-02,   1.6835e-01,  -3.4366e-02],\
                        [-5.2965e-02,   1.6275e-01,   1.1235e-02,  -1.0357e-02,   1.2544e-01,  -4.6730e-02,   1.0343e-01,  -7.9612e-02,   1.2814e-01,  -2.1184e-02,   2.0514e-01,  -5.4295e-02,   2.1662e-01,  -8.7155e-02,   2.0612e-01,  -5.5455e-02, 1.6102e-01,  -3.3040e-02,   1.6050e-01,  -1.0087e-01,   1.6259e-01],\
                        [2.0545e-01,  -1.0540e-01,  -2.0938e-02,   1.2602e-01,  -3.6174e-02,   1.9389e-01,   4.6616e-03,   2.5507e-01,  -4.1035e-02,   1.4618e-01,  -1.8441e-01,   2.0797e-01,  -2.0584e-01,   2.6931e-01,  -1.8624e-01,   2.1010e-01,-1.0218e-01,   1.6835e-01,  -1.0087e-01,   2.9488e-01,  -1.0522e-01],\
                        [-5.5147e-02,   1.6522e-01,   1.1734e-02 , -1.0683e-02,   1.2619e-01,  -4.8597e-02,   1.0327e-01,  -8.2915e-02 ,  1.2905e-01,  -2.1935e-02,   2.0949e-01,  -5.6577e-02,   2.2152e-01,  -9.0957e-02,   2.1054e-01,  -5.7733e-02,1.6339e-01,  -3.4366e-02 ,  1.6259e-01,  -1.0522e-01,   1.6541e-01]])
    # test values
    testmu, testsigma, observedLandmarks = correction_step(mu,sigma,data.loc[(data['timestep'] == 330) & (data['sensor'] == 'SENSOR')],observedLandmarks)
    # test
    try:
        print('====================================================================')
        np.testing.assert_almost_equal(testmu, corrmu,decimal= decimal)
        print('\x1b[6;30;42m' + 'Test Success for mu in correction_step' + '\x1b[0m')
    except AssertionError as e:
        print('====================================================================')
        print('\x1b[0;30;41m' + 'Test Failed for mu in correction_step' + '\x1b[0m')
        print(e)

    try:
        np.testing.assert_almost_equal(testsigma, corrsigma,decimal= decimal)
        print('====================================================================')
        print('\x1b[6;30;42m' + 'Test Success for sigma in correction_step' + '\x1b[0m')
    except AssertionError as e:
        print('====================================================================')
        print('\x1b[0;30;41m' + 'Test Failed for sigma in correction_step:' + '\x1b[0m')
        print(e)
```

```python
test_total(decimal = 1)
```

```python
test_prediction(decimal = 1)
```

    ====================================================================
    Test Success for mu in prediction_step
    ====================================================================
    Test Failed for sigma in prediction_step:

    Arrays are not almost equal to 2 decimals

    Mismatched elements: 3 / 441 (0.68%)
    Max absolute difference: 0.20181353
    Max relative difference: 5.28393366
     x: array([[ 3.63e-01, -5.80e-02, -1.20e-02,  1.19e-01, -2.03e-02,  1.55e-01,
             1.29e-03,  1.87e-01, -2.29e-02,  1.30e-01, -9.86e-02,  1.63e-01,
            -1.10e-01,  1.95e-01, -9.96e-02,  1.64e-01, -5.51e-02,  1.42e-01,...
     y: array([[ 2.61e-01, -5.67e-02, -1.06e-02,  1.19e-01, -1.99e-02,  1.54e-01,
             1.21e-03,  1.86e-01, -2.25e-02,  1.30e-01, -9.65e-02,  1.61e-01,
            -1.08e-01,  1.93e-01, -9.74e-02,  1.63e-01, -5.36e-02,  1.41e-01,...

```python
test_correction(decimal = 1)
```

    ====================================================================
    ====================================================================
    Test Failed for mu in correction_step

    Arrays are not almost equal to 1 decimals

    Mismatched elements: 17 / 21 (81%)
    Max absolute difference: 14.75384379
    Max relative difference: 2.71482664
     x: array([[ 0.1],
           [ 0. ],
           [ 0.1],...
     y: array([[ 5. ],
           [ 4.6],
           [ 1.5],...
    ====================================================================
    Test Failed for sigma in correction_step:

    Arrays are not almost equal to 1 decimals

    Mismatched elements: 123 / 441 (27.9%)
    Max absolute difference: 999.89243
    Max relative difference: 9295.27219485
     x: array([[0.0e+00, 0.0e+00, 0.0e+00, 0.0e+00, 0.0e+00, 0.0e+00, 0.0e+00,
            0.0e+00, 0.0e+00, 0.0e+00, 0.0e+00, 0.0e+00, 0.0e+00, 0.0e+00,
            0.0e+00, 0.0e+00, 0.0e+00, 0.0e+00, 0.0e+00, 0.0e+00, 0.0e+00],...
     y: array([[ 1.6e-01, -5.6e-02, -1.1e-02,  1.2e-01, -2.0e-02,  1.5e-01,
             1.2e-03,  1.9e-01, -2.2e-02,  1.3e-01, -9.6e-02,  1.6e-01,
            -1.1e-01,  1.9e-01, -9.7e-02,  1.6e-01, -5.4e-02,  1.4e-01,...

```python
mu
```

    array([[ 0.09956596],
           [ 0.01005956],
           [ 0.10086379],
           [ 1.78615901],
           [ 0.87720484],
           [-0.09141201],
           [ 3.85900198],
           [ 0.        ],
           [ 0.        ],
           [ 0.        ],
           [ 0.        ],
           [ 0.        ],
           [ 0.        ],
           [ 0.        ],
           [ 0.        ],
           [ 0.        ],
           [ 0.        ],
           [ 0.        ],
           [ 0.        ],
           [ 0.        ],
           [ 0.        ]])

```python
sigma
```

    array([[   0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,
               0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,
               0.,    0.,    0.],
           [   0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,
               0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,
               0.,    0.,    0.],
           [   0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,
               0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,
               0.,    0.,    0.],
           [   0.,    0.,    0., 1000.,    0.,    0.,    0.,    0.,    0.,
               0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,
               0.,    0.,    0.],
           [   0.,    0.,    0.,    0., 1000.,    0.,    0.,    0.,    0.,
               0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,
               0.,    0.,    0.],
           [   0.,    0.,    0.,    0.,    0., 1000.,    0.,    0.,    0.,
               0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,
               0.,    0.,    0.],
           [   0.,    0.,    0.,    0.,    0.,    0., 1000.,    0.,    0.,
               0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,
               0.,    0.,    0.],
           [   0.,    0.,    0.,    0.,    0.,    0.,    0., 1000.,    0.,
               0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,
               0.,    0.,    0.],
           [   0.,    0.,    0.,    0.,    0.,    0.,    0.,    0., 1000.,
               0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,
               0.,    0.,    0.],
           [   0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,
            1000.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,
               0.,    0.,    0.],
           [   0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,
               0., 1000.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,
               0.,    0.,    0.],
           [   0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,
               0.,    0., 1000.,    0.,    0.,    0.,    0.,    0.,    0.,
               0.,    0.,    0.],
           [   0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,
               0.,    0.,    0., 1000.,    0.,    0.,    0.,    0.,    0.,
               0.,    0.,    0.],
           [   0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,
               0.,    0.,    0.,    0., 1000.,    0.,    0.,    0.,    0.,
               0.,    0.,    0.],
           [   0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,
               0.,    0.,    0.,    0.,    0., 1000.,    0.,    0.,    0.,
               0.,    0.,    0.],
           [   0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,
               0.,    0.,    0.,    0.,    0.,    0., 1000.,    0.,    0.,
               0.,    0.,    0.],
           [   0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,
               0.,    0.,    0.,    0.,    0.,    0.,    0., 1000.,    0.,
               0.,    0.,    0.],
           [   0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,
               0.,    0.,    0.,    0.,    0.,    0.,    0.,    0., 1000.,
               0.,    0.,    0.],
           [   0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,
               0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,
            1000.,    0.,    0.],
           [   0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,
               0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,
               0., 1000.,    0.],
           [   0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,
               0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,    0.,
               0.,    0., 1000.]])

```python

```


```python
%load_ext autoreload
%autoreload 2

import numpy as np
import scipy as sp
import pandas as pd

from bokeh.plotting import figure, show, output_notebook
from bokeh.models import (
    ColumnDataSource, HoverTool
)
from bokeh.palettes import Blues, Reds

from bokeh.tile_providers import get_provider, Vendors

from sklearn.preprocessing import MinMaxScaler

from pyproj import Proj, transform

output_notebook()
```

    Loading BokehJS ...

# Why is Coronavirus all about Mathematics?

On 11 March 2020, the World Health Organization officiallly [declared](https://twitter.com/WHO/status/1237777021742338049?s=20) the coronavirus (COVID-19 or SARS-CoV-2) a [pandemic](https://en.wikipedia.org/wiki/Pandemic). But, fear not, this is not yet another opinionated response on the failing of society and the end of times. This is all about Mathematics!

## Data

```python
path = 'COVID-19/csse_covid_19_data/csse_covid_19_time_series/time_series_19-covid-{}.csv'

features = ['Confirmed', 'Deaths', 'Recovered']

dataframes = dict()

for feature in features:
    dataframes[feature] = pd.read_csv(path.format(feature))
```

Let's take a took at confirmed cases and prepare the dataset

## Vizualization

### Map

```python
df = dataframes['Confirmed'].copy()
df['total'] = df[df.columns[-1]]

# in order to easily transform from longitude/latitude (EPSG:4326) into another projection (for example WebMercator EPSG:3857
inProj, outProj = Proj(init="epsg:4326"), Proj("epsg:3857")
df['x'], df['y'] = transform(inProj, outProj, df['Long'].tolist(), df['Lat'].tolist())

#scale within 0 and 1
df['size'] =  150 * MinMaxScaler().fit_transform(df[['total']])
```

    /Users/g4brielvs/.local/share/virtualenvs/COVID-19-xLAGSW6c/lib/python3.8/site-packages/pyproj/crs/crs.py:55: FutureWarning: '+init=<authority>:' syntax is deprecated. '<authority>:' is the preferred initialization method. When making the change, be mindful of axis order changes: https://pyproj4.github.io/pyproj/stable/gotchas.html#axis-order-changes-in-proj-6
      return _prepare_from_string(" ".join(pjargs))

Let's prepare the map using [Bokeh](https://bokeh.org),

```python
p = figure(
    title="COVID-19: Confirmed Cases",
    x_axis_type="mercator",
    y_axis_type="mercator",
    width=800,
    tools='pan,box_zoom,wheel_zoom,save,help')

CARTODBPOSITRON = get_provider(Vendors.CARTODBPOSITRON)
p.add_tile(CARTODBPOSITRON)

source = ColumnDataSource(data=dict(
                        x=list(df['x']),
                        y=list(df['y']),
                        size=list(df['size']),
                        total=list(df['total']),
                        region=list(df['Country/Region']),
                        province=list(df['Province/State'])))

hover = HoverTool(tooltips=[
     ("Country/Region", "@region"),
     ("Province/State", "@province"),
     ("Confirmed Cases", "@total")

])

p.add_tools(hover)
p.circle(x ='x', y='y', source=source,size='size', line_color="#FF0000", fill_color="#FF0000", fill_alpha=0.05)

#p.toolbar.logo = None
p.xaxis.visible = False
p.yaxis.visible = False

show(p)
```

### Fit

```python
df = dataframes['Confirmed'].copy()

# unpivot dataframe
df = df.melt(id_vars=['Province/State', 'Country/Region', 'Lat', 'Long'], var_name="date", value_name="count")
df = df.groupby(['Country/Region', 'date']).agg(sum)

# set time series
df = df.reset_index()

df['date'] = pd.to_datetime(df['date'])
df = df.set_index('date').sort_index()
```

Let's take at closer look at China at the beginning of the epidemic,

```python
df = df[df.index < pd.Timestamp('2020-02-15')]
df = df[df['Country/Region'] == 'China']
```

Let's fit the data to an exponential,

```python
# expoenential
exp = lambda x, a, b : np.exp(a*(x-b))

# fit
y = df['count']
x = np.arange(len(y))

popt, pcov = sp.optimize.curve_fit(exp, x, y)

# put it back
df['fit'] = exp(x, *popt)
```

Let's prepare the map using [Bokeh](https://bokeh.org),

```python
p = figure(title="COVID-19: Confirmed Cases in China and Exponential", x_axis_type="datetime", y_axis_type="linear", plot_width=800, plot_height=400)

source = ColumnDataSource(df)

p.scatter('date', 'count', source=source, legend_label='Confirmed Cases')
p.line('date', 'fit', source=source, line_dash='dotted')

hover = HoverTool(tooltips=[
     ("Confirmed Cases", "@count")

])
p.add_tools(hover)

p.xaxis.axis_label = 'Date'
p.yaxis.axis_label = 'Cases'
p.legend.location = 'center_left'

show(p)
```

## Exponential Growth

An epidemic is a textbook example of an exponential growth. Not only an example, but its underlying principle is a centerpiece in the history of Mathematics and its discovery was fundamental for breakthroughs in Probability Theory, Mathematical Analysis, Differential Equations, Physics, Chemistry and Biology and even Pop Culture.

The idea is rather straightforward: it has do with the way a quantity changes. When the rate on which a given quantity changes is proportional to the quantity itself at any given moment, mathematicians called it exponential.

```
the rate of change is proportional to the quantity at any given time
```

Probably many in the past observed this behavior on how populations expand, diseases spread, biology decays or even how science progresses, but it was formally brought to life in the work on logarithms. Later, while studying compound interest - more money you have, more money you make - [Bernoulli](https://en.wikipedia.org/wiki/Jacob_Bernoulli) discovered a constant ironically known as [Euler's number](https://en.wikipedia.org/wiki/E_(mathematical_constant)).

We will have the opportunity to talk about the multiple "origins" of that constant in the future.

### Exponentiate!

Coming back to the virus. In our case, having an exponential growth suggests that the larger the infectious population, quicker we will have new cases and the infectious population will grow. We pratically experience that whenever people get sick around us (or when all of our friends start getting pregnant at the same...no! it is not exponential...or is it?)

Scientists denote ideas using mathematical script for convenience (sometimes laziness). Let us do it then,

- *I(t)*: infectious population

$$
\begin{equation}
\Delta I(t) \sim I(t) \implies \frac{dI}{dt} \sim I(t)
\end{equation}
$$

```
the rate of infection is proportional to the infectious population
```

Well, stating a relation of proportionality does not give us much. We still are not able to answer any questions. Is this virus dangerous? How contagious is it? Is it deadly? Are we all going to die?

We are going to formulate a model using variables and parameters to quantify *how proportial* that relation is.

- *$\beta$*: *contact rate* or how many people an infected person comes into contact with in given time
- *$\gamma$*: *recovery rate* or how many people recover in given time

Using our intuition for the model's formulation, those are the two parameters we will take into account. The rate on which people are infected should grow the more contact they have to each other and should decay as they recover. In another words, the rate of change is *positively* proportional to $\beta$ (*contact rate*) and *negatively* proportional to $\gamma$ (*recovery rate*)

Finally, we have our equation,

$$
\begin{equation}
\frac{dI}{dt} = \left(\beta - \gamma\right) I
\end{equation}
$$

Now let us take some time to check if we can take some conclusions from it.

If the *recovery rate* is greater than *contact rate*, the rate of change will always be negative, so the infection will eventually dissipate and even not outbreak. On the other hand, if *contact rate* is greater *recovery rate*, we will have a disaster. Apparently, those paramaters indicate how strong an epidemic can be.

Now, if $\gamma$ is the *recovery rate*, then $1/\gamma$ is the *infectious/recovery period* or the period of time during which an infected individual can pass it on, consider the product between the $\beta$ (*contact rate*) and $1/\gamma$ (*infectious period/recovery*) and we will have the average number of people an infected patient will pass it on.

$$
\begin{equation}
R_{0} = \frac {\beta}{\gamma}
\end{equation}
$$

In fact, that indicator is known as the *basic reproduction number*. Note that when $R_0 > 1$ the infection will be able to start spreading in a population, but not if $R_0 < 1$.

Probably everybody has that number in their minds at the present. When trying to mitigiate, we will see how important bringing it down is in order to slow down the epidemic.

After the outbreak in China, studies found that COVID-19's basic reproduction number is between [1.4-3.9](https://pubmed.ncbi.nlm.nih.gov/31995857-early-transmission-dynamics-in-wuhan-china-of-novel-coronavirus-infected-pneumonia/), which means that, on avarage, a sick patient transmits the infection to 1.4 and 3.9 others. The *recovery period* is around [10 days](https://www.statnews.com/2020/03/09/people-shed-high-levels-of-coronavirus-study-finds-but-most-are-likely-not-infectious-after-recovery-begins/).

We solved our equation numerically inputting different parameters to see how it plays out.

#### The I Model

```python
# total population
N = 10000000
# initial number of infected and recovered
I0, R0 = 1, 0
# initial susceptible
S0 = N - I0 - R0
# contact rate and recovery rate
beta, gamma = 0.2, 1./10
# time
t = np.linspace(0, 45, 45)
```

```python
# gradient vector
def gradient(y, t, N, beta, gamma):
    I = y
    dI = (beta - gamma) * I
    return dI

# initial conditions vector
y0 = I0
```

##### Simulate

```python
betas = [0.20, 0.25, 0.30, 0.35, 0.4, 0.41, 0.42]
results = dict()

for beta in betas:
    y = sp.integrate.odeint(gradient, y0, t, args=(N, beta, gamma))
    I = y.T[0]

    results[beta] = I
```

We wil solve the equation for multiple parameters,

```python
title = "The I Model: Basic Reproductive Number Sensibility"

p = figure(title=title, x_axis_type="linear", y_axis_type="linear", plot_width=750, plot_height=400)

for beta, color in zip(betas, reversed(Reds[len(betas)])):
    p.line(t, results[beta]/1000, color=color, legend_label=f'{beta*10}')

p.xaxis.axis_label = 'Days'
p.yaxis.axis_label = 'Infected Population (in thousands)'

p.legend.location = "top_left"
p.legend.click_policy = 'hide'
p.legend.inactive_fill_alpha = 0.6

show(p)
```

We visually check how small decrements *basic reproduction number* will have a huge accumulative gain over time.That is reason why we should take agressive action, either by social distancing, avoiding and cancelling events.

We saw that the *basic reproduction number* depends on *contact rate* and *recovery rate*. Since the *recovery rate* is barely in our control as it is typicallly a biological charateristic of the virus, controlling the *contact rate* is our best chance to flatten the curve. That does not mean we should not invest in treatments, best case scenario will bring both down.

### Thou shall not exponentiate!

But here is the plot twist: an epidemic is not purely exponential! Our first model assumed that the infection will spread indefinitely, the population is infinite and there is no immunity or cure.

Of course, that is nonsense. An rather evil way of think is that once the entire population is infected, there is none left to be infected!

Let us address that in a new model by assuming the following,

- fixed population (nobody dying, moving or being born)
- recovery and immunity

In COVID's case, [it is not clear how long the protection lasts](https://www.independent.co.uk/life-style/health-and-families/coronavirus-immunity-reinfection-get-covid-19-twice-sick-spread-relapse-a9400691.html). But we will assume it lasts longer than our simulated timeframe.

We will use a compartmental model. The idea is to divide the population into group (or *compartments*) and describe how the infection moves from one to another. It was first introduced by [Anderson Ogilvy Kermack](https://en.wikipedia.org/wiki/Anderson_Gray_McKendrick) and [Anderson Gray McKendrick](https://en.wikipedia.org/wiki/Anderson_Gray_McKendrick]).

Let the SIR model be as follows,

- *S(t)*: susceptible but not yet infected population
- *I(t)*: infectious population
- *R(t)*: recovered population

- *$\beta$*: *contact rate* or how many people an infected person comes into contact with in given time
- *$\gamma$*: *recovery rate* or how many people recover in given time
- *N*: total population

First, the *equation S* tell us that as the population is infected, the susceptible population becames smaller.

$$
\begin{equation}
\frac{dS}{dt} = - \frac{\beta SI}{N}
\end{equation}
$$

Second, the *equation I* is similar to our original model's, adding a factor of susceptibility.

$$
\begin{equation}
\frac{dI}{dt} = \frac{\beta SI}{N} - \gamma I
\end{equation}
$$

Last, the *equation R* represents that recovered population is proportinal to the recovery rate.

$$
\begin{equation}
\frac{dR}{dt} =  \gamma I
\end{equation}
$$

Unfortunately, that system of differential equations non-linear and does not have analytical solution. Fortunately, we solve it numerically with the following initial conditions,

#### The SIR Model

```python
# total population
N = 10000000
# initial number of infected and recovered
I0, R0 = 1, 0
# initial susceptible
S0 = N - I0 - R0
# contact rate and recovery rate
beta, gamma = 0.2, 1./10
# time
t = np.linspace(0, 180, 180)
```

```python
# gradient vector
def gradient(y, t, N, beta, gamma):
    S, I, R = y = y
    dS = -beta * S * I / N
    dI = beta * S * I / N - gamma * I
    dR = gamma * I
    return dS, dI, dR

# initial conditions vector
y0 = S0, I0, R0
```

```python
betas = [0.20, 0.25, 0.30, 0.35, 0.4, 0.41, 0.42]
results = dict()

for beta in betas:
    y = sp.integrate.odeint(gradient, y0, t, args=(N, beta, gamma))
    S, I, R = y.T

    results[beta] = (S, I, R)
```

```python
title = "The SIR Model: R = 4"

p = figure(title=title, x_axis_type="linear", y_axis_type="linear", plot_width=750, plot_height=400)

p.line(t, results[0.4][0]/1000, color='black', line_alpha=0.5, line_dash='dotted', legend_label=f'Susceptible')
p.line(t, results[0.4][1]/1000, color='red', legend_label=f'Infected')
p.line(t, results[0.4][2]/1000, color='green', line_alpha=0.5, line_dash='dotted', legend_label=f'Recovered')

p.xaxis.axis_label = 'Days'
p.yaxis.axis_label = 'Population (in thousands)'

p.legend.location = "center_left"
p.legend.click_policy = 'hide'
p.legend.inactive_fill_alpha = 0.6

show(p)
```

```python
title = "The SIR Model: Infected Population"

p = figure(title=title, x_axis_type="linear", y_axis_type="linear", plot_width=750, plot_height=400)

for beta, color in zip(betas, reversed(Reds[len(betas)])):
    #p.line(t, results[beta][0]/1000, color=color, legend_label=f'{beta*10}')
    p.line(t, results[beta][1]/1000, color=color, legend_label=f'{beta*10}')
    #p.line(t, results[beta][2]/1000, color=color, legend_label=f'{beta*10}')

p.xaxis.axis_label = 'Days'
p.yaxis.axis_label = 'Infected Population (in thousands)'

p.legend.location = "top_left"
p.legend.click_policy = 'hide'
p.legend.inactive_fill_alpha = 0.6

show(p)
```

```python

```


# Quasi-hyperbolic discounting and commitment savings

## The exponential discounter

Consider a simple extension of the standard intertemporal optimization problem (seen in [an earlier notebook](consume_opt1.ipynb) from two to three periods.

Our *exponential* or time-consistent discounter wishes use own-savings strategies and/or the services of a competitive financial service sector to exchange existing endowment income stream $(y_0,y_1,y_2)$ for a more optimal consumption stream.

The consumer optimization problem is to choose ${c_0,c_1,c_2}$ to solve:

$$\max_{c_0,c_1,c_2} u(c_0) + \delta u(c_1) + \delta^2 u(c_2)$$
$$ s.t.    c_0 + \frac{c_1}{1+r} + \frac{c_2}{1+r} = y_0 + \frac{y_1}{1+r} + \frac{y_2}{1+r}$$

where $\delta$  is the consumer's own personal psychic discount factor and $r$ is the financial sector's opportunity cost of funds.  Competitition drives banks to offer earn zero-profits, leading them to offer consumption contract $c^*=(c_0^*,c_1^*,c_2^*)$ in exchange for the consumer's original more volatile income stream $c^*=(y_0,y_1,y_2)$ of equal monetary present value.

Let us solve this by setting up a Lagrangean

$$  u(c_0) + \delta u(c_1) + \delta^2 u(c_2) - \lambda \left [ Ey - c_0 - \frac{c_1}{1+r} - \frac{c_2}{1+r} \right ]$$

The first-order necessary conditions for an interior optimum are:

$$u'(c_0^*) = \lambda$$
$$\delta u'(c_1^*) = \lambda \frac{1}{(1+r)}$$
$$\delta^2 u'(c_0^*) = \lambda \frac{1}{(1+r)^2}$$

If it just happens that the consumer's discount factor $\delta$ has the following relationship to the bank's opportunity cost of funds $r$:

$$\delta = \frac{1}{1+r}$$

Then the first order conditions collapse down to:

$$u'(c_0^*) = u'(c_1^*) =u'(c_2^*) = \lambda$

which of course then implies that the consumer has constant consumption across periods  $c_0^* = c_1^* = c_2^* $

It's easy to establish that if the consumer is sufficiently patient and/or the return to saving is high enough then the consumer will consume more in later periods:

$$\delta > \frac{1}{1+r}  \text{    then  } c_0^* < c_1^* < c_2^* $$

and if the situation is reversed so the consumer is relatively impatient and/or the return to savings is low then they will consume more in earlier periods:

$$\delta > \frac{1}{1+r}  \text{    then  } c_0^* > c_1^* > c_2^* $$

## The CRRA utility case

A Constant-Relative Risk Aversion (CRRA) felicity function is given by:

$$
\begin{equation}
u\left(c_{t}\right)=\begin{cases}
\frac{c^{1-\rho}}{1-\rho}, & \text{if } \rho>0 \text{ and } \rho \neq 1 \\
ln\left(c\right) & \text{if } \rho=1
\end{cases}
\end{equation}
$$

Te Arrow-Pratt measure of [relative risk aversion](https://en.wikipedia.org/wiki/Risk_aversion#Relative_risk_aversion)

$$R(c) =\frac{-cu''(c)}{u'(c)}$$

gives us a measure of how the curvature of the consumers felicity function -- or how averse consumers are to risks -- changes with wealth (consumption).  As its name implies the CRRA function has a constant measure of relative risk aversion given simply by $\rho$.

The elasticity of intertemporal substitution $\sigma$ (the responsiveness of the slope of the consumption path to changes in the interest rate) for a CRRA function is also constant and given simply by:

$$\sigma = \frac{1}{\rho}$$

If we plug our CRRA felicity function into our earlier derived FOC we can solve to find:

$$\frac{c_1^*}{c_0^*} = \frac{c_2^*}{c_1^*} = \left [\delta (1+r) \right ]^\frac{1}{\rho} $$

Match this to the zero profit condition and we can find a closed form solutions for the consumption path.

## Quasi-hyperbolic time-inconsistent preferences

A relatively simple way to model present-biased and time-inconsistent preferences is by moderling prefernces using quasi-hyperbolic preferences as follows.  The key idea in what follows is that the agent's preferences change from period to period.

The problem can be described in broad terms like this.  When first making plans for future consumption choices -- say the choice between period period 1 consumption and period 2 consumption -- the consumer's period 0 self wants to keep consumption smoothly balanced between those two periods.  However when period 1 arrives the consumer's preferences *change* in a way that makes him/her more impatient or present biased.  The new period 1 self consumer now wants to undo the optimal consumption plans laid out by her earlier period 0 self and to choose a new consumption plan that boosts present (period 1) consumption at the expense of future (period 2) consumption.

The 'sophisticated' (quasi) hyperbolic discounter anticipates his own future self's likely change of heart and this is likely to alter their original period 0 consumption plan.  This sets up in effect a conflict or a struggle between the consumer's earlier and future selves.  The period 0 self acts strategically, anticipating their own later period's optimal responses.

### The demand for commitment services

Let's see this formally.The consumer's "period-0" self wants to choose consumption path $(c_0,c_1,c_2)$ to maximize:

$$u(c_0)+\beta[\delta u(c_1)+\delta^2 u(c_2)]$$

subject to the earlier described intertemporal budget constraint.

This is as before except for the fact that we've now introduced a 'present-bias' parameter $\beta$  (where $0<\beta \leq 1$) that tilts the consumer's preferences toward period zero consumption.  Whenever $\beta$ is strictly less than one the consumer wants to smooth preferences in a way that tilts toward period 0 consumption but seeks to keep consumption between period 1 and 2 more balanced.

When period 1 rolls around however the consumer's preferences over remaining consumption bundles **change**.  The period 1 self now wants to re-arrange remaining consumption to now maximize:

$$u(c_1) +\beta\delta u(c_2)$$

While the period-0 self wanted to trade off period 1 and period 2 consumption to equalize marginal utilities like this:

$$u'(c_1^*) = \delta (1+r )u'(c_2^*)$$

the new period-1 self now pefers to tradeoff like this:

$$u'(c_1^*) = \beta \delta (1+r )u'(c_2^*)$$

The $\beta$ on the right-hand side of the last equation means the period 1 consumer now values period 2 consumption less relative to period 1 consumption compared to his period 0 self. Compared to the period-0 self's plans the new period-1 consumer wants to 'raid-savings' and/or 'take out a new loan'.

Lets look again at the CRRA case under the special assumption that $\delta = \frac{1}{1+r}$ and $r=0$ (and hence $\delta = 1$.  These last assumptions are without loss of generality and done only to simplify the math and spotlight the key mechanisms at work.

Let's assume also that the period zero consumer can contract with a bank *and that the bank can credibly commit to not renegotiating the terms of the contract* even when the consumer's period 1 self comes begging for the bank to do so.

In this case the problem is exactly as described in the earlier section. The period 0 consumer maximizes

$$u(c_0)+\beta[\delta u(c_1)+\delta^2 u(c_2)]$$

subject to the intertemporal budget constraint.

When $\delta = \frac{1}{1+r}$ the period 0 consumer would like to keep consumption between period 1 and period 2 flat.  The first order conditions now reduce to:

$$c_1^* = \beta^\frac{1}{\rho} c_0^*$$

$$c_2^* =  c_1^*$$

 Substituting these into the binding budget constraint yields:

 $$c_0^* = \frac{E[y]}{1+\beta^\frac{1}{\rho}}$$
 $$c_1^* = c_2^* = \beta^\frac{1}{\rho}c_0^*$$

We call this the **full commitment contract**.

The financial intermediary who offers such a contract is really providing *two* services to consumer's period 0 self: they're helping the consumer to smooth consumption between period zero and later periods and they're also helping the consumer resist his period 1 self's temptation to disrupt this optimal consumption plan.

For example suppose that consumer endowment income is such that she finds it optimum to save in period 0 and then finance constant consumption out of endowment income and savings in period 1 and 2.  When period 1 rolls around the consumer's period 1 self present bias would tempt them to want to 'raid savings' and/or take out a new loan to boost period 1 consumption at the expense of period 2 consumption.

To see this formally note that under the full commitment contract the consumer enters the period with contractual claims to the remaining consumption stream $(c_1^*,c_2^*) =({\bar c}^*, {\bar c}^*)$.  If they could however the period 1 self would re-contract with this bank (or another) to choose another contract $(\hat c_1, \hat c_2)$ that solves:

$$\max_{c_1, c_2} u(c_1) + \beta u(c_2)$$

subject to

$$c_1 + \frac{c_2}{1+r}  \leq {\bar c}^* + \frac{{\bar c}^*}{1+r} $$

The first order conditions for this problem are

$$u'(c_1) = \beta u'(c_2)$$

which in the CRRA case requires $c_2 =\beta ^\frac{1}{\rho} c_1$

which is clearly not satisfied along the original contract which had  $(c_1^*,c_2^*) =({\bar c}^*, {\bar c}^*)$

Substituting the FOC into the new period 1 budget constraint we can arrive at a solution or reaction function that states that when the period 1 self enters the period with claims ${c_1, c_2}$ they will want to renegotiate to:

$$\hat c_1 = \frac{ c_1 +c_2 }{1+\beta^\frac{1}{\rho} }$$
$$\hat c_2 =\beta ^\frac{1}{\rho} \hat c_1$$

If this renegotiation takes place then the period 1's welfare would increase but at the expense of period 0's welfare, since period 0's optimal consumption plan would have been undone.

From this discussion it should be clear that the period 0 self would like to have banks compete for her business in period 0 but offer a full-commitment contract which in practice means being locked into an exclusive relationship (if the relationship were not exclusive then the period 1 self would approach a second bank in period 1 and ask them to 'buy out' and then renegotiate their contract with the first bank.

## More detailed analysis
Much more detail on the analysis of contracts like this can be found in our 2016 working paper (with Karna Basu) entitled "Sticking to the Contract: Nonprofit vs For-profit Banks and Hyperbolic Discounters.” A github [repository](https://github.com/jhconning/renegotiation) is available with code and jupyter notebooks describing some of that analysis. I draw on that here, but most of the details of the methods are in those other notebooks.

## Commitment Savings products

Many economists believe that time-inconsistent preferences drive people to have to struggle with important issues of self-control that affects their ability to reach savings goals, and generates a demand for commitment services.  Indeed some economiss have gone so far as to argue that these problems are ubiquitous and that one reason for the demand for microfinance services is that they provide individuals with the discipline and commitment mechanisms that can help them with these problems of self-control.

Rotating Savings and Credit Associations (ROSCAs) for example place individuals into savings groups and then create pressures and social sanctions to help individuals achieve savings goals.  Iterant savings collectors may serve a similar purpose.  Some have even suggested that many microcredit loans which advance loans to individuals and then put them on tight weekly repayment schedules may be popular not so much because they are releasing the individual from a binding credit constraint as because they are helping the individual 'borrow to save.'  In this type of scenario the individual wants to save up to buy say a refrigerator or other household appliance but cannot find the discipline to save up for the item themselves.  A microcredit loan allows them to buy the refrigerator outright and then places them on a strict repayment schedule that commits the individual to putting aside the fraction of weekly income toward the item that they might have otherwise struggled to do on their own.

In all these stories the availabiliy of commitment contracts provides additional value to the consumer, value that they may be willing to pay for.

In the following section I will illustrate some of these ideas with contracts that we will solve for and illustrate for the CRRA case.  The code for much of what follows is in the [Contract.py](contract.py) module described in greater detail in the github repo.

```python
import Contract
```

Lets look at a competive contracting situation where $\beta = 0.8$ and $\rho = 0.5$

```python
cC = Contract.Competitive(beta = 0.8)
cC.rho = 0.5
```

Now lets give the individual an endowment income of $(y_0,y_1,y_2) = (150, 100, 50)$

```python
cC.y = [150,100,50]
```

The full commitment contract is:

```python
cCF = cC.fcommit(); cCF
```

    array([ 131.57894737,   84.21052632,   84.21052632])

Note how this involves some saving in period 0 ($y_0=150$ and consumption $c_0=131.6$) as well as additional saving for period 2.

If for some reason period 0 self agreed to this contract (sincerely believing that it would never be renegotiated) but then suddenly to everyone's surprise period 1 self had an opportunity to renegotiate the continuation of this contract, they would renegotiate to the following contract:

```python
cCR = cC.reneg(cCF); cCR
```

    array([ 131.57894737,  101.44337644,   64.92376092])

We can see here how period 1 self 'raids' the savings that the period 0 self had intended should be passed onto period 2 consumption.  The period 1 self boosts period 1 consumption from 84 to 101. Rather than pass 34 units of consumption into period 2 only 20 units are now passed.

If the bank cannot credibly commit to not renegotiate the contract then the sophisticated time-inconsistent consumer will defensively alter their consumption plans to try to thwart their future self's strategic renegotiations.  The period 0 consumer will insist on **renegotiation proof contracts** which will impose an additional constraint on the problem, namely that no bank find it profitable to renegotiate the contract.  In general this will be achieved by in a sense 'surrendering' to their future self's bias tilt toward period 1 consumption.

Details in the paper and in class lecture on the shape of this constraint.

The important thing to understand is that adding any constraint to the problem can only lower consumer welfare and push us away from the full-commitment optimum.

For the present setup the renegotiation proof contract can be solved for to find:

```python
cCRP = cC.reneg_proof().x; cCRP
```

    array([ 122.59354576,  108.17462856,   69.23182568])

Compare to the full commitment contract this contract involves more savings pased from from period 0 to period 1 but much less savings passed from period 1 to period 2.  Total savings (period 0 plus period 1 savings) are higher when credible commitment savings devices are available compared to when not.

## Replicating Ashraf, Karlan and Yin (2006) Commitment Savings paper
Ashraf, Karlan and Yin (2006) "Tying Odysseus to the Mast: Evidence from a Commitment Savings Product in the Philippines," *Quarterly Journal of Economics.

What follows are incomplete notes... just some pieces of the puzzle needed to replicate the paper. I hope to improve on this in time and I just offer this here so that you might form an idea on how this might be done.  Note that I am using the ipystata library which requires Stata to be running in the background.  This seems to only be possible to do on Windows machines as described in the ipystata library.

The Stata dataset and code replication files have been made available by the authors via the Harvard Dataverse [here](https://dataverse.harvard.edu/dataset.xhtml?persistentId=doi:10.7910/DVN/27854)

Notes to self:
Files in D:\Users\jconning\Google Drive\GC\DevII_S15\other\Karlan

renamed

"D:\Users\jconning\Google Drive\GC\DevII_S15\other\Karlan\seedanalysis_011204_1.dta"
"D:\Users\jconning\Google Drive\GC\DevII_S15\other\Karlan\seedanalysis_011204_080404_1.dta"
"D:\Users\jconning\Google Drive\GC\DevII_S15\other\Karlan\seedanalysis_080404_1.dta"

The posted files are Stata version 14.  These can be opened by python pandas but not by older versions of Stata.

```python
%matplotlib inline
import seaborn as sns
import pandas as pd
import ipystata
import statsmodels.formula.api as sm
```

### Open the datasets

One disadvantage of proprietary software is that you often cannot open a dataset saved with a later version of the software unless you pay to upgrade the software.  I'm going to load the Stata dataset into a python pandas dataframe using its read_stata method and then pass the dataset into the running Stata session (which on my home machine is Stata 11).

```python
#df = pd.read_stata(r"G:\GC\Dev-II\notebooks\seedanalysis_011204_1_v12.dta")
df = pd.read_stata(r"G:\GC\Dev-II\notebooks\data\savings1.dta")
```

```python
regVI_1 = 'balchange ~ treatment + marketing'
model = sm.ols(regVI_1, df)
fitted = model.fit(cov_type='HC1')
print(fitted.summary())
```

                                OLS Regression Results
    ==============================================================================
    Dep. Variable:              balchange   R-squared:                       0.002
    Model:                            OLS   Adj. R-squared:                  0.001
    Method:                 Least Squares   F-statistic:                     2.901
    Date:                Tue, 29 Mar 2016   Prob (F-statistic):             0.0553
    Time:                        17:00:31   Log-Likelihood:                -16270.
    No. Observations:                1777   AIC:                         3.255e+04
    Df Residuals:                    1774   BIC:                         3.256e+04
    Df Model:                           2
    Covariance Type:                  HC1
    ==============================================================================
                     coef    std err          z      P>|z|      [95.0% Conf. Int.]
    ------------------------------------------------------------------------------
    Intercept     40.6257     61.676      0.659      0.510       -80.257   161.509
    treatment    234.6785    101.748      2.306      0.021        35.256   434.101
    marketing    184.8506    146.982      1.258      0.209      -103.228   472.929
    ==============================================================================
    Omnibus:                     3458.895   Durbin-Watson:                   0.051
    Prob(Omnibus):                  0.000   Jarque-Bera (JB):          5716807.283
    Skew:                          14.932   Prob(JB):                         0.00
    Kurtosis:                     279.259   Cond. No.                         4.24
    ==============================================================================

    Warnings:
    [1] Standard Errors are heteroscedasticity robust (HC1)

```python
model = sm.ols('balchange ~ treatment', df[(df.treatment ==1) | (df.marketing ==1)])
fitted = model.fit(cov_type='HC1')
print(fitted.summary())
```

                                OLS Regression Results
    ==============================================================================
    Dep. Variable:              balchange   R-squared:                       0.000
    Model:                            OLS   Adj. R-squared:                 -0.001
    Method:                 Least Squares   F-statistic:                    0.1020
    Date:                Thu, 24 Mar 2016   Prob (F-statistic):              0.750
    Time:                        23:43:38   Log-Likelihood:                -12115.
    No. Observations:                1308   AIC:                         2.423e+04
    Df Residuals:                    1306   BIC:                         2.424e+04
    Df Model:                           1
    Covariance Type:                  HC1
    ==============================================================================
                     coef    std err          z      P>|z|      [95.0% Conf. Int.]
    ------------------------------------------------------------------------------
    Intercept    225.4763    133.405      1.690      0.091       -35.992   486.944
    treatment     49.8279    156.027      0.319      0.749      -255.979   355.635
    ==============================================================================
    Omnibus:                     2498.290   Durbin-Watson:                   0.056
    Prob(Omnibus):                  0.000   Jarque-Bera (JB):          3202671.430
    Skew:                          14.153   Prob(JB):                         0.00
    Kurtosis:                     243.756   Cond. No.                         3.11
    ==============================================================================

    Warnings:
    [1] Standard Errors are heteroscedasticity robust (HC1)

Table 6: Impact:  6 months