****************************
Lab 3 - Implementing the UKF
****************************

Implementing the UKF
====================

We will store the sigma points and weights in matrices, like so:

.. math::

    \begin{aligned}
    \text{weights} &= 
    \begin{bmatrix}
    w^0& w^1 & \dots & w^{2n}
    \end{bmatrix} 
    \\
    \text{sigmas} &= 
    \begin{bmatrix}
    \mathcal{X}^{0,0} & \mathcal{X}^{0,1} & \dots & \mathcal{X}^{0,n-1} \\
    \mathcal{X}^{1,0} & \mathcal{X}^{1,1} &  \dots & \mathcal{X}^{1,n-1} \\
    \vdots & \vdots &  \ddots & \vdots \\
    \mathcal{X}^{2n,0} & \mathcal{X}^{2n,1} & \dots & \mathcal{X}^{2n,n-1}
    \end{bmatrix}
    \end{aligned}


Weights
-------

Remember that the weigths are calculated as follow:

.. math::

    \begin{aligned}
        \lambda &= \alpha^2(n+\kappa)-n\\
        w^0_m &= \frac{\lambda}{n+\lambda}\\
        w^0_c &= \frac{\lambda}{n+\lambda}+1-\alpha^2+\beta\\
        w_m^i &= w_c^i = \frac{1}{2(n + \lambda)}, \quad i=1,2,\dots,2n
    \end{aligned}

Implement the function :code:`MerweWeightSigmaPoints(n, alpha, beta, kappa)` and return :math:`\lambda` and the vectors of weights :math:`W_m, W_c`.

.. admonition:: Example

    :code:`MerweWeightSigmaPoints(n=2, alpha=.1, beta=2., kappa=1.)` gives:

    .. code-block:: python 
        
        -1.97
        array([-62.67666667,  16.66666667,  16.66666667,  16.66666667,  16.66666667])
        array([-65.66666667,  16.66666667,  16.66666667,  16.66666667,  16.66666667])

Sigma Points
------------

Remember the equations for the sigma points are:

.. math::

    \begin{cases}
    \mathcal X^0 = \mu\\
    \mathcal X^i = \mu + \left( \sqrt{(n+\lambda)\Sigma}\right )_{i}& \text{for }i=1,\dots, n \\
    \mathcal X^i = \mu - \left( \sqrt{(n+\lambda)\Sigma}\right)_{i-n} &\text{for }i=(n+1),\dots, 2n
    \end{cases}

The hardest part is :math:`\left( \sqrt{(n+\lambda)\Sigma}\right )_i`. This term is a matrix because :math:`\Sigma` is a matrix.

The subscript :math:`i` is choosing the i-th row vector of the matrix.

What is the square root of a matrix :math:`\Sigma`?

*  We can define it as :math:`\Sigma = SS^T`
*  Where :math:`S` can be computed with the Cholesky decomposition.

.. warning::

    It's a common approximation, but you can find different definition.

Create the function :code:`SigmaPoints(mu, n, lambda_, Sigma)` that returns the sigma points.

*  Start by creating a matrix of size :math:`(2n+1, n)`.
*  Calculate the matrix containing :math:`\left( \sqrt{(n+\lambda)\Sigma}\right )` for each i using cholesky.

.. admonition:: Example

    :code:`SigmaPoints(np.array([10, 10]), 2, -1.97, np.array([[2., .1], [.1, 3]]))` gives:

    .. code-block:: python

        array([[10.        , 10.        ],
              [10.24494897, 10.01224745],
              [10.        , 10.2997499 ],
              [ 9.75505103,  9.98775255],
              [10.        ,  9.7002501 ]])

Unscented Transform
-------------------

Recall the equations:

.. math::

    \begin{aligned}
        \mu &= \sum_{i=0}^{2n}w_m^i\mathcal X^i\\
        \Sigma &= \sum_{i=0}^{2n} w_c^i(\mathcal X^i-\mu)(\mathcal X^i-\mu)^T
    \end{aligned}

Create the function :code:`unscented_transform(sigmas, wm, wc, R)`:

*  :math:`\mu` is just the dot product of :math:`w_m` and :math:`\mathcal X`.
*  :math:`\Sigma` is a square matrix of size :math:`n`.
  
   *  :math:`\mathcal X^i-\mu` is the substraction of two vectors of 1 dimension
   *  Because they have only one dimension yoiu will need to use :code:`numpy.outer` to multiply it by itself.
   *  Remember to add the process noise :math:`R` to :math:`\Sigma`.
*  It should return the new mean and covariance.

.. admonition:: Example

    Using :code:`unscented_transform(sigmas, wm, wc, R=np.array([[1.5,.5],[.5,1.5]]))`, we obtain:

    .. code-block:: python

        array([10., 10.])
        array([[3.5, 0.6], [0.6, 4.5]])
    
    
    The other values are the ones calculated with the previous examples.

UKF
---

Now we have everything to implement the filter itself.

Predict step
************

For the predict step, you need to pass each sigma point through the function :math:`f`:

.. math::

    \mathcal Y = f(\mathcal X)

Implement the function :code:`predict(sigma_points_fn, fx, mu, Sigma, wm, wc, R)`:

*  :code:`sigma_points_fn` is the function to calculate the sigma points defined earlier.
*  :code:`fx` is the nonlinear function :math:`f`.
*  Calculate the sigma points.
*  Pass each sigma points trough the function :math:`f`.
*  Apply the unscented transform on the new points.
*  Return the new mean and covariance and the sigma points

If we try with the previous values we obtain:

.. code-block:: python

    array([ 20. , 113.2])
    array([[   6.7      ,   66.7      ],
          [  66.7      , 1238.1479615]])


Update step
***********

Now, you need to implement the update step. Remember that the update step is converting the sigmas into measurement psace using the function :math:`h(x)`:

.. math::

    \mathcal Z = h(\mathcal Y)



.. math::

    \bar\Sigma^{x,z} = \sum_{i=0}^{2n}w_c^i(\mathcal X^i - \bar\mu)(\mathcal Z^i - \bar\mu)^T

Then, we compute the new state estimate using the reisdual and Kalman gain:

.. math::

    \begin{aligned}
    K &= \bar\Sigma^{x,z}S^{-1}\\
    \mu &= \bar\mu + Ky
    \end{aligned}

and the new covariance is computed as :

.. math::

    \Sigma = \bar\Sigma - KSK^T


Implement the update function :code:`update(hx, mu, Sigma, z, sigmas_f, wm, wc, Q)`:

*  :code:`hx` is the measurement function
*  :code:`mu, Sigma` are the mean and covariance after the prediction step.
*  :code:`z` the measurement received
*  :code:`sigmas_f` the sigma points
*  :code:`Q` the measurement noise

You can use the following measurement function to test the code:

.. hidden-code-block:: python

    def hx(x):
        return x[[0, 1]]

If we try with the previous values we obtain and the following parameters we obtain:

.. code-block:: python

    update(hx, mu, Sigma, np.array([11, 11]), sigmas_f, wm, wc, np.diag([0.2, 0.5]))

    array([11.38019055, 10.99044453])
    array([[1.67846715, 0.50288057],
          [0.50288057, 1.99941257]])

Using the UKF
=============

Let's solve some problems so you can gain confidence in how easy the UKF is to use. We will start with a linear problem you already know how to solve with the linear Kalman filter. 

Although the UKF was designed for nonlinear problems, it finds the same optimal result as the linear Kalman filter for linear problems. 

We will use the filter to track an object in 2D using a constant velocity model. 

We define the state of the robot as:


.. math::


    \mathbf x = \begin{bmatrix}x &  \dot x & y & \dot y \end{bmatrix}^\mathsf{T}


The engineers are telling us that the robot moves following the Newtonian equations:

.. math::

    \begin{aligned}
    x_k &= x_{k-1} + \dot x_{k-1}\Delta t \\
    y_k &= y_{k-1} + \dot y_{k-1}\Delta t
    \end{aligned}

1. Use the previous equations to define the state transition matrix :math:`A`

The sensors provide position but not velocity.

2. Define the measurement function as a matrix :math:`H`.

The sensor readings are in meters with an error of :math:`\sigma=0.3` meters in both *x* and *y*. This gives us a measurement noise matrix of 

.. math::

    Q = \begin{bmatrix}0.3^2 &0\\0 & 0.3^2\end{bmatrix}

Finally, let's assume that the process noise can be represented by the discrete white noise model - that is, that over each time period the acceleration is constant.

.. math::

    R = \begin{bmatrix}
    \frac{1}{4}\Delta t^4 & \frac{1}{2}\Delta t^3 \\
    \frac{1}{2}\Delta t^3 & \Delta t^2\end{bmatrix} \sigma^2

3. Write the transifiton function :code:`f_cv`.
4. Write the measurement function :code:`H_cv`.
5. Use this function with the UKF and see what happens.