EE3901/EE5901 Sensor Technologies

Chapter 2 Notes

The Kalman filter

Building your intuition

The essence of the Kalman filter is summarised in Figure 1. In this example, the variable being measured is the position of a robot. The position cannot be known perfectly, so the Kalman filter represents its belief as a Gaussian probability distribution. The spread of the probability distribution indicates the uncertainty in the position.

As shown in the figure, the essence of the Kalman filter is to merge together information from a physics-based model (the prediction step) with information from the real world (the measurement step). This information is merged in such a way that the overall uncertainty is minimised.

Why use a prediction step?

You may be tempted to simply rely upon measurements alone. However, there is much to be gained by also incorporating knowledge of the physics of your system. As a human, you can use your intuition to recognise when an apparent measurement doesn’t “seem right”. If you disbelieve a single measurement, your uncertainty may increase, but you won’t necessarily be fooled by measurement noise. The prediction step in the Kalman filter allows a computer system to do a similar thing.

Notice in Figure 1 that the uncertainty in the new position is lower than the uncertainty in the measurement. This is true in general. A Kalman filter produces a better result than simply using the raw measurements alone.

The Kalman filter, by example

We will teach the Kalman filter by starting from a simple example. Our example problem is that of a robot moving along a one dimensional train track. The robot has 2 measurands: position and velocity . However, the position and velocity are measured by noisy sensors.

The position and velocity can change over time, hence the robot must periodically obtain new measurements in order to keep up. Let the time interval between measurements be . We will call each of these intervals a “time step”. Time steps will be counted using integer values , where the elapsed time . It is convenient in software to count in integer steps.

At time step , we write our best estimate of the state of the robot in a vector

where and are the estimated position and velocity at the time step, respectively. The hat indicates that these are estimates.

Equation is the state vector of the robot. It is a vector that contains all the variables that we wish to measure. The Kalman filter is an algorithm that allows us to calculate this state vector.

We will also need to define the covariance of the state vector, which we will call :

where is the standard deviation of position, is the standard deviation of velocity, and is the correlation between position and velocity. Recall that the standard deviation refers to our uncertainty in the position and velocity. If the standard deviation is small, then it means that we have high confidence. If the standard deviation is large, then we have low confidence.

The prediction step

Given the previous state estimate can you predict the new state estimate ?

Of course you can: there is a relationship between position and velocity!

In words, this says that the new position is the old position plus how far we travel during this time step.For velocity, we assume that the new velocity is equal to the old velocity. Of course if you have further knowledge about the physics of the robot, then you could amend these equations. For instance, if you know about the drag or friction forces, you may have a better estimate for the new velocity. However, for now, let’s assume the simplest model where the velocity tends to remain constant. It is fine if the predictive model is not perfect, because the measurement will be used to correct for any small errors.

We can write Eqs. and as a matrix:

Here is called the prediction matrix. It gives a mathematical model of the robot’s dynamics. The notation means “the estimate at time step given information from time step .”

To predict the new covariance, we use the variance propagation law from last week on Eq. . The covariance must be propagated through the calculation. The result will be where is the Jacobian of our update step (). The Jacobian of a simple matrix multiplication is just the matrix itself, i.e. the Jacobian of is simply . Therefore, we are ready to write down our first version of the prediction step of the Kalman filter:

To recap our progress so far: we have written down a physics-based model of the robot, and used that to write some equations for running our state vector and covariance forward in time.

Control inputs

Our current model says that velocity remains unchanged, but this is not always accurate. For example, the robot might be given a command to accelerate. Our software can see the acceleration command, so we should include that information in our model.

Suppose that our motor controller is generating an acceleration . We can amend our prediction model to read:

The last term in Eq. represents the change in displacement due to constant acceleration, while the last term in Eq. represents the change in velocity due to the acceleration.

In general there could be multiple control inputs. Hence we introduce a control vector which contains the control inputs at timestep . Our little robot only has one control input, so in our case .

This enables us to convert Eq. and to matrix form:

where is called the control matrix.

As an aside: if you have studied modern control theory, then you should recognise Eq. as the state-space representation of the system being measured.

Process uncertainty

What if our model is inaccurate? What if there are other random perturbations that we can’t predict, e.g. the robot being pushed by wind or its wheels slipping a bit?

Add some process covariance to represent these unknown factors.

The process covariance represents the “unknown unknowns” in the model. It tells the Kalman filter that the prediction model is never perfect. The model uncertainty grows bigger over time as we add onto the process covariance every single timestep.

Intuitively, this is like walking around blind-folded: in the beginning you know where you are pretty well (small ), but as more time goes by your uncertainty gets bigger. The role of the process uncertainty is to represent this trend of increasing uncertainty over time.

The only way to reduce the uncertainty is by making a measurement. We will see below how measurements allow us to reduce the state covariance.

Summary of the prediction step

Self check quiz 2.5

Select the item from the list below that best describes the yellow highlighted part of the equation.

Question 1/9: 🟨 ⬜ ⬜ ⬜ ⬜ ⬜ ⬜ ⬜ ⬜

(a)

Process covariance (unknown external influences)

(b)

Control inputs (known external influences)

(c)

Control matrix (mapping from external influences to their impact on the state)

(d)

Old state

(e)

Old covariance

(f)

Predicted state

(g)

Predicted covariance

(h)

Prediction matrix (mapping from old state to new state)

(i)

Propagation of covariance through prediction model

Check Start again

✔️ Correct!

Measurement predictions

Suppose our robot has a GNSS (global navigation satellite system) receiver giving position, an infrared rangefinder giving position, and a wheel encoder giving velocity. Note, here we have 3 sensors, but there are only 2 elements in state vector. Suppose GNSS coordinates are converted to metres but the rangefinder results in are mm.

We can predict what the measurements would be if our state estimate were correct:

where is called the measurement matrix.

This may seem “backwards” to you. We predict the measurements based on the state, instead of the other way around. This is a critical feature of the Kalman filter!

There does not need to be a unique 1:1 relationship between elements in the state vector and the sensors we use. We can have multiple sensors that each measure some function of the state. Mathematically, the measurement matrix does not need to be invertible. It doesn’t even need to be square.

We also have a predicted measurement covariance

The measurement residual

Above we used our model of the system to predict the measurements that we should get, if our state estimation were perfect. However, in reality, the state estimation is probably not perfect.

Let’s finally turn on the robot’s sensors and take some measurements. Suppose the sensors provide some measurements with covariance . The difference between the true measurement and the predicted measurement is called the residual:

There is also a covariance in the residual

If the prediction and measurement were perfect then would be zero. Any differences indicate new information that will be used to update the state.

Update step

Finally we are ready to bring it all together. We have:

1. A predicted state , which we calculated based on the physics of our robot.
2. A measurement residual, which tells us how much agreement there is between predicted state and the actual sensor readings.

The state estimation will be calculated as follows:

where is a matrix called the “Kalman gain”. The clever part of the Kalman filter is how to calculate this matrix. Its job is to convert from a measurement residual into a state update.

The proof of the Kalman gain is optional for students in this class. If you are happy to simply accept the result, it is:

There is one final piece, which is the equation used to produce the final updated state covariance:

The proof of Equations and is available by pressing the button below, if you are interested. The essence of the proof is to minimise the sum of the squares of the error in the state estimate. Hence, the Kalman filter is an optimal estimator in the sense that it minimises this sum of squares.

Summary of the update step

Self check quiz 2.7

Select the item from the list below that best describes the yellow highlighted part of the equation.

Question 1/11: 🟨 ⬜ ⬜ ⬜ ⬜ ⬜ ⬜ ⬜ ⬜ ⬜ ⬜

(a)

Updated state

(b)

Predicted state

(c)

Predicted state covariance

(d)

Updated state covariance

(e)

Kalman gain

(f)

Measurement residual

(g)

Measurement residual covariance

(h)

Actual measurements (sensor data)

(i)

Measurement matrix (mapping from state to measurements)

(j)

New information from measurements

(k)

Measurement covariance (covariance of sensor data)

Check Start again

✔️ Correct!

Example implementation

The outline of a Matlab implementation is shown below.

% Run this code for every time step:

% Prediction step
xhat_prediction = F * xhat;
P_prediction = F * P * F' + Q;

% Get a measurement
z = read_sensors(); % obtain latest sensor data

% Run the update step
y = z - H*xhat_prediction; % measurement residual
S = H*P*H' + R; % measurement residual covariance
K = P * H' / S; % Kalman gain

% Calculate the new state
xhat = xhat_prediction + K*y; % new state vector
P = (eye(2) - K*H)*P_prediction; % new covariance

Click here to download the complete Matlab source code. The output from this program is shown in Figure 2. Notice how the Kalman filter produces a high quality position estimate despite the extremely noisy sensors.

Implementation hints

• To design a Kalman filter you need to determine the system matrices , , , etc. Often these are independent of time, in which case the subscript can be dropped.
• Once all those matrices have been determined, you end up with a system of equations that you can simply enter into a computer.
• The Kalman gain is written as if it contains a matrix inverse, but in Matlab, you should not use inv(S) to calculate it. You should use the mrdivide (/) operator instead. This is because explicit matrix inverses are always a bad idea.
• In some cases you might have measurements arriving at different rates, for instance, a GNSS receiver may be quite slow in comparison to an on-board accelerometer. In this scenario, there are several options:
  1. Use some interpolation scheme to fill in the gaps between slow sensors, to pretend that you have data from all sensors at every time step; or
  2. Write a different measurement matrix for the scenario where only some of the sensors are reporting data. You can still perform the update step using partial sensor data.
• Often the process covariance is not obvious. There are several strategies that you can consider.
  • If your system is tracking position and velocity, then the main source of error will be accelerations (changes in velocity) that you did not predict. You could treat unknown accelerations as a random variable with expected value of 0 but some assumed or measured variance. You could then propagate this variance through to the state vector update, in much the same way as you did in Self-check Quiz 3.4.
  • If you truly have no idea about what are the sources of error, a plausible starting point is a diagonal matrix:
    Then you can use a trial-and-error approach to adjust the variances until the filter achieves good performance. You might consider choosing each to be proportional to and consider how much standard deviation you expect to accumulate per second.

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 .
• Which variables go into the control vector .
• Which variables go into the measurement vector .
• The prediction matrix , which describes how the state vector tends to evolve over time.
• The control matrix , which describes how the state vector is perturbed by control inputs.
• The process covariance , which describes the growth in the uncertainty in the predicted state.
• The measurement matrix , which gives the mapping from state vector to measurements.
• The measurement covariance , 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 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 describes the time evolution of the system, then we should just use it directly!

The EKF modifies the prediction step to read:

where is a function that steps the state vector forward in time, and is a function that calculates the influence of control inputs.

Similarly the measurement step can be updated to read:

where 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 where is the Jacobian of .

We will use the following notation. Let be the Jacobian of and let be the Jacobian of ). Then using the variance propagation law, the predicted state covariance is