Lab 3 - Implementing the UKF

Implementing the UKF

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

\[\begin{split}\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}\end{split}\]

Weights

Remember that the weigths are calculated as follow:

\[\begin{split}\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}\end{split}\]

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

Example

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

-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:

\[\begin{split}\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}\end{split}\]

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

The subscript \(i\) is choosing the i-th row vector of the matrix.

What is the square root of a matrix \(\Sigma\)?

• We can define it as \(\Sigma = SS^T\)

• Where \(S\) can be computed with the Cholesky decomposition.

Warning

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

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

• Start by creating a matrix of size \((2n+1, n)\).

• Calculate the matrix containing \(\left( \sqrt{(n+\lambda)\Sigma}\right )\) for each i using cholesky.

Example

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

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

Unscented Transform

Recall the equations:

\[\begin{split}\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}\end{split}\]

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

• \(\mu\) is just the dot product of \(w_m\) and \(\mathcal X\).

• \(\Sigma\) is a square matrix of size \(n\).

  • \(\mathcal X^i-\mu\) is the substraction of two vectors of 1 dimension

  • Because they have only one dimension yoiu will need to use numpy.outer to multiply it by itself.

  • Remember to add the process noise \(R\) to \(\Sigma\).

• It should return the new mean and covariance.

Example

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

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 \(f\):

\[\mathcal Y = f(\mathcal X)\]

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

• sigma_points_fn is the function to calculate the sigma points defined earlier.

• fx is the nonlinear function \(f\).

• Calculate the sigma points.

• Pass each sigma points trough the function \(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:

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 \(h(x)\):

\[\mathcal Z = h(\mathcal Y)\]

\[\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:

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

and the new covariance is computed as :

\[\Sigma = \bar\Sigma - KSK^T\]

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

• hx is the measurement function

• mu, Sigma are the mean and covariance after the prediction step.

• z the measurement received

• sigmas_f the sigma points

• Q the measurement noise

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

+ show/hide code

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

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

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:

\[\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:

\[\begin{split}\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}\end{split}\]

1. Use the previous equations to define the state transition matrix \(A\)

The sensors provide position but not velocity.

2. Define the measurement function as a matrix \(H\).

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

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

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.

\[\begin{split}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\end{split}\]

3. Write the transifiton function f_cv.

4. Write the measurement function H_cv.

5. Use this function with the UKF and see what happens.