EE3901/EE5901 Sensor Technologies

Week 4 Notes

Non-linear Kalman filters

Recap of the Kalman filter

Let’s remind ourselves of the structure of the Kalman filter, so we can consider how to extend it for the case of non-linear systems.

To design a Kalman filter, you must determine:

• Which variables go into the state vector $\mathbf{x}_k$.
• Which variables go into the control vector $\mathbf{u}_k$.
• Which variables go into the measurement vector $\mathbf{z}_k$.
• The prediction matrix $F_k$, which describes how the state vector tends to evolve over time.
• The control matrix $B_k$, which describes how the state vector is perturbed by control inputs.
• The process covariance $Q_k$, which describes the growth in the uncertainty in the predicted state.
• The measurement matrix $H_k$, which gives the mapping from state vector to measurements.
• The measurement covariance $R_k$, which is the covariance in the measurements.

The Extended Kalman Filter (EKF)

Motivation

The classical Kalman filter uses a prediction model of the form

where $F_{k}$ is a matrix of numbers.

However, what if the system dynamics cannot be expressed as a matrix of numbers? Suppose instead we have some arbitrary function

How can we develop a Kalman filter for such a system?

Deriving the EKF

The key insight is as follows. There is no particular reason that the prediction step must be matrix multiplication. If we know that some arbitrary function $f(\mathbf{x})$ describes the time evolution of the system, then we should just use it directly!

The EKF modifies the prediction step to read:

where $f(\cdot)$ is a function that steps the state vector forward in time, and $b(\cdot)$ is a function that calculates the influence of control inputs.

Similarly the measurement step can be updated to read:

where $h(\cdot)$ is a function that calculates the expected measurements based on the given state vector.

A further insight: the state covariance and measurement covariance can be calculated using the error propagation laws. Recall from week 2 that we have $\Sigma_{y}=J\Sigma_{x}J^{T}$ where $J$ is the Jacobian of $\boldsymbol{y}=f(\mathbf{x})$.

We will use the following notation. Let $F_{k}$ be the Jacobian of $f(\hat{\boldsymbol{x}}_{k-1})$ and let $H_{k}$ be the Jacobian of $h(\hat{\boldsymbol{x}}_{k|k-1}$). Then using the variance propagation law, the predicted state covariance is

which is the same equation as for the linear Kalman filter, except that $F_{k}$ is a Jacobian matrix that will change at every timestep. Note that we assume zero covariance in the control inputs $\boldsymbol{u}_{k}$. If you believe the control inputs $\boldsymbol{u}_{k}$ or control model $b\left(\cdot\right)$ have uncertainty then you must use the error propagation laws on the $b\left(\boldsymbol{u}_{k}\right)$ term as well.

Similarly the measurement residual covariance is:

Again the equation is the same, except that $H_{k}$ is a Jacobian matrix.

All other parts of the filter can remain the same.

Intuitively, the extended Kalman filter is simply finding a linear model that best matches the non-linear functions $f$ and $h$. It is effectively the (multi-dimensional) “tangent to the curve” at the current state estimate.

The extended Kalman filter is not necessarily an optimal estimator, in the sense that the linear Kalman filter is mathematically proven to minimise the sum of squares of the error for linear systems. If the system state is linear then the EKF reduces to the normal Kalman filter, in which case it is optimal. For non-linear systems, the optimality cannot be guaranteed. However, in practice the EKF often works well.

Example of an Extended Kalman Filter (EKF)

Suppose that you want to track the motion of a pendulum (Figure 1).

Define the state vector

By analysing the forces, we can write the differential equations that describe this state:

This is our physics model, which we will now use to develop an Extended Kalman Filter.

The prediction model

We must convert our system of continuous differential equations (13)-(14) into discrete time steps. There are many ways to do this. Here, we will use a method called “leapfrog integration”. Leapfrog integration is a very good scheme for classical mechanics problems, e.g. where the physics gives us an equation for acceleration ($F=ma$) and the goal is to solve for velocity and displacement. The leapfrog integration scheme is as follows:

where $x$ is displacement (or angular displacement) and dots indicate time derivatives. In words, this method says that the new velocity should be computed based on the average of the old acceleration and the new acceleration.

Applying the leapfrog method to Eqs. (13)-(14), we find:

This is our prediction model, which says how to step the state vector forward in time.

Recall that the EKF relies upon linearising the prediction model around the current operating point. In other words, we need the Jacobian of $f(\cdot)$. However, the Jacobian would be quite messy to calculate by hand. Notice that you would need to substitute the first element of the vector into the second, and then apply the chain rule to get the derivatives.

To reduce the risk of a mistake, it is recommended that you use a computer algebra system to calculate the derivatives. The provided Matlab code shows you how to automatically find Jacobians:

• Download runnable code as a Matlab notebook, or
• Download exported HTML for viewing.

The resulting Jacobian, copy-pasted from the provided Matlab code, is:

Measurement

Suppose that the measurement actually records the kinetic and potential energy, i.e. a non-linear function of the state.

where $v=L\dot{\theta}$ is the velocity of the pendulum and $h=L-L\cos\theta$ is the height of the pendulum above its lower-most point.

Again we need a Jacobian matrix

EKF example code

Refer to the attached Matlab script for a complete working example.

• Download runnable code as a Matlab notebook, or
• Download exported HTML for viewing.

Implementation notes

A few points to note:

• The Jacobian matrices $F$ and $H$ vary with each time step so you must re-calculate them inside your loop.
• You will typically need to tune the process covariance to get good results. Essentially you are trading off how much the filter believes the prediction vs how much it believes the measurement. As with the linear Kalman filter, a diagonal matrix with standard deviation proportional to the time step is a plausible starting point in the absence of a more detailed error model.

The Unscented Kalman Filter (UKF)

The UKF is another method for estimating a non-linear process. Its key advantage is that it does not require the computation of Jacobians. Hence it can be applied to models that are non-differentiable (for example arbitrary computer code, or models containing with discontinuous functions where derivatives would be undefined).

The key idea is that instead of propagating just the state vector $\hat{\boldsymbol{x}}$ through the model, we also propagate some so-called “sigma points” around $\hat{\boldsymbol{x}}$. These points are carefully chosen to sample the state covariance. You can think of them as exploring various possible states near the estimate, to test how the prediction model would behave on neighbouring states as well. This allows non-linearities in the model to be properly accounted for.

The Unscented Transform

The Unscented Transform solves this problem: given some calculation $\boldsymbol{y}=f(\boldsymbol{x})$ find the covariance $\Sigma_{y}$ without using a Jacobian matrix to approximate $f$. Hence it can be applied to any function $f$ including arbitrary computer code.

The steps are as follows:

First define some parameters: let $\alpha$ be a scaling factor of typical size $\alpha=0.001$ (chosen empirically). Let $\beta$ be a factor that incorporates knowledge of the statistical distribution of the states, where $\beta=2$ is the optimal value for a Gaussian distribution. Let $n$ be the number of elements in the state vector.

Calculate

and

where $\text{chol}\left(\Sigma_{x}\right)$ is the Cholesky factorisation of the covariance matrix $\Sigma_{x}$. The Cholesky factorisation is roughly akin to a matrix square root. It is implemented in Matlab with the chol function.

Next, expand the vector $\boldsymbol{x}$ into a matrix $X$

where $\boldsymbol{x}\pm A^{T}$ performs addition/subtraction between the vector $\boldsymbol{x}$ and each column in the matrix $A^T$. (If you are using Matlab, you can simply write + and - because that is the behaviour of those operators.) The matrix $X$ has $2n+1$ columns.

Each column of $X$ is one of the sigma points.

Transform each sigma point through the function to produce a new matrix $F$, i.e.

where each column of $F$ is given by

If $f$ is arbitrary computer code, then write a for loop to process each column of $X$ separately and concatenate these columns to form $F$.

We will construct the outputs $\boldsymbol{y}$ and $\Sigma_{y}$ as a weighted sum over the columns of $F$. For each column we define a mean weight $w^{m}$ and a covariance weight $w^{c}$:

Form vectors by concatenating these weights

Note that other definitions of the weights are also possible. Different authors use different weights. A necessary constraint is that $\sum_{j}w_{j}^{m}=1$.

Then the expected value of the output is given by:

The covariance is given by

where $\cdot$ is the elementwise product (i.e. the Matlab .* operator).

Applying the Unscented Transform to the Kalman filter

Prediction step

Given the previous state $\hat{\boldsymbol{x}}_{k-1}$ and covariance $P_{k-1}$, compute the $2n+1$ sigma points using the equations above and put these in the columns of the matrix $X$.

Propagate the sigma points through the prediction model $f$ to obtain the matrix $F$:

Following the Unscented Transform, use $F$ to calculate the predicted state $\hat{\boldsymbol{x}}_{k|k-1}$ and covariance $P_{k|k-1}$.

Finally add in the process covariance to $P_{k|k-1}$.

Measurement step

If the measurement is a linear function of the state, then you can use the normal Kalman filter from this point forwards.

If your measurement model is non-linear, then apply the Unscented Transform a second time to propagate the predicted state through the measurement model. Define a new set of sigma points based on the predicted state and predicted covariance. Then propagate these through the measurement model:

Reconstruct the predicted measurements

the measurement residual

and the measurement residual covariance

where $R_{k}$ is the measurement covariance.

Update step

If the measurement model is linear, use the standard Kalman filter update step. Otherwise, proceed as below.

The update step needs a slight adjustment because there is no measurement matrix to easily link measurement space and state space. Instead we can use the cross-correlation between state and measurement:

The Kalman gain in this case is

The updated state and covariance are given by

Example of an Unscented Kalman Filter (UKF)

Refer to the attached Matlab script for a complete working example.

• Download runnable code as a Matlab notebook, or
• Download exported HTML for viewing.

Implementation notes

• The initial state covariance cannot be 0 because Cholesky factorisation is only defined for positive definite matrices (those with positive eigenvalues). This is true for any legitimate covariance matrix. If the Cholesky factorisation throws an error then most likely your prediction model is diverging or you have made a mistake in the equations.

• A major advantage of the UKF is the ability to handle sophisticated prediction models where derivatives are impossible or unreasonable to compute. For example, your prediction model could be an entire computer program. All you need to do is run each of the sigma points through your prediction model and you can thereby use it in your Kalman filter.