EE3901/EE5901 Sensor Technologies
Chapter 2 Tutorial

Written by A/Prof Bronson Philippa and Laurance Papale

College of Science and Engineering, James Cook University

Last updated 21 February 2025

Question 1

In lectures we considered a Kalman filter for one dimensional movement. There were two state space variables: position and velocity. In this question we will extend this model to two dimensional movement.

Let position be represented by variables and , and let velocity be and (where the dot means a time derivative has been applied).

Assume that the underlying physics is a “constant velocity” model of the form:

The differential equations (1)-(4) can be converted to discrete time steps using the rule

where is any state variable.

Write down the state vector for this problem, and then derive the state update matrix in the Kalman filter prediction step

For a state space vector of the form

the prediction matrix is

Hint: you can check this by calculating (e.g. in Matlab) for some different values of and , to make sure that it behaves reasonably.

Question 2

This is a discussion question for the tutorial class.

In Question 1, you used position and velocity in the state vector. Why only these two? Why not include acceleration as well? In general, how do you decide which variables belong in the state vector?

Some hints for your consideration:

Consider that a system of point masses is governed by an equation of the form or more explicitly .
- Is an independent variable?
What does it really mean when someone says that a system is governed by a “first order differential equation” or a “second order differential equation”? In particular, how many initial conditions are required? How many unique variables are there?

Question 3

Consider the resistor-capacitor circuit of Figure Q3.

Suppose that the capacitor is charged to some initial voltage before it is connected to this circuit. The current flowing in this loop is given by the differential equation

You are designing a Kalman filter to estimate the current . There is a single variable, i.e. the state “vector” collapses to a single number in this case. Similarly, the covariance “matrix” reduces to a scalar variance.

(a) Write the equation for the predicted state .

Hint: Start with the differential equation above, then write the prediction step (as per question 1).

(b) Suppose that the circuit resistance is not known exactly but is instead subject to some measurement variance . Using your result from part (a) and the law of propagating variance (week 2), estimate the variance in due to .

(c) Using your results from part (a) and (b), write the equation for the prediction step of the state variance, i.e. the equation for .

(d) Suppose that you are not able to measure the current directly, but instead, you can measure the voltage across the resistor. Write the measurement “matrix” (in this case a scalar) in the form

where is the predicted measurement (i.e. the voltage) and is the measurement “matrix” (in this case a scalar).

(e) Let be the measured voltage with associated variance . Write equations for the measurement residual and variance .

(f) Write the equations for the Kalman gain, updated state, and updated covariance.

The Kalman filter for this system is as follows:

(a) State prediction:

(b) Process variance:

(d) Measurement model:

(e) Measurement residual and variance:

(f) Kalman gain and update step:

Question 4 — software exercise

Validate the performance of the filter from Question 3 by implementing it on a computer.

Here is Matlab starter code that generates noisy measurement data and plots the results. (You are also welcome to use a different programming language if you prefer.)

A properly designed Kalman filter has an impressive ability to handle measurement noise and produce an accurate estimate. In Figure Q4 , the noisy measurements (red) are used to obtain the filtered state (yellow). Your task is to reproduce this figure using your equations from Question 3.

The software simulation is a great way to build intuition about the behaviour of the filter in the presence of different kinds of issues. Using your software, evaluate how the filter performs when:

(a) The initial state is wrong, i.e. does not match the true initial current.

(b) The process variance is very large, i.e. add a constant term to so it’s much larger than its true value.

(c) The initial state is wrong and measurement variance is very large, i.e. add a constant term to so it’s much larger than its true value.

(d) The measurement process is extremely inaccurate, e.g. try Ω and V, but give the filter an accurate initial condition and the true process and measurement variances.

(e) The measurement process is extremely inaccurate and the initial condition is wrong.

Briefly:

(a) If the initial condition is wrong but everything else is set up correctly, then the filter will quickly converge to the true value of the state.

(b) When the process variance is large, the filter basically ignores the predictive model, and just tracks the noisy measurement.

(c) If the initial condition is wrong and the measurement variance is large, the filter is slower to converge to the true value. If the measurement variance is very large then it never converges to the proper value.

(d) The filter can still cope with very low quality measurement data, so long as it “knows” that the data are high variance. In that case it basically falls back on the process model. If the process model and initial condition are accurate then the system can still work reasonably.

(e) The system will fail if the initial condition is inaccurate and the measurements are very poor. In that case there’s simply not enough information available.

Question 5

Given a state prediction model of the form

the extended Kalman filter predicts the covariance using the equation:

where is the Jacobian of and is the process covariance.

Calculate the Jacobian for each prediction model given below.

(a)

(b)

(c)

(d)

(a) In this case the Jacobian is a scalar.

(b)

(c)

(d)

Question 6

Develop the equations for an extended Kalman filter for a vehicle moving in a 2D plane as shown in Figure Q6.

The vehicle is described by the state vector

where is the position along the east-west axis, is the position along the north-south axis, is the speed of movement (where positive means forward motion), and is the direction of heading measured clockwise from north.

(a) Write a prediction function .

(b) Calculate the Jacobian of and hence write the equation for the predicted covariance.

(c) Suppose that the sensors on the vehicle are GPS (measuring position), a speedometer, and a compass. Define which variables would go into the measurement vector and hence write down the measurement model in the expression

(d) Suppose that the vehicle has no GPS and the only sensors are a speedometer and compass. Repeat part (c) with a reduced set of sensors.

(a) Prediction step (based upon an assumption of constant speed)

If control inputs were available then additional terms could be added to reflect predicted changes in and .

(b) The Jacobian of is

for a covariance prediction of

where is the process covariance.

This is the same ordering as the state vector so the measurement model is linear and is given by

where the measurement matrix is

If your measurement vector had the measurements in a different order then the corresponding rows of would be swapped.

(d) In this case there are only two measurements

and the measurement matrix becomes

Question 7

Develop an unscented Kalman filter for the non-linear circuit shown in Figure Q7.1.

This is a second-order system, so it requires two state variables. Choose the following state vector:

The measurements are the voltages,

The LED is described by the Shockley diode equation,

where the device parameters (e.g. reverse bias saturation current, ideality factor, and temperature) have already been substituted into the equation. Notice that the voltage across the diode is the same as the voltage across . Recall that capacitors are described by .

(a) Analyse the circuit and show that it is described by the following set of differential equations:

(b) At the point where the diode begins to turns on, these equations are highly non-linear and are numerically challenging to solve. A simple prediction model of the form

fails to converge to the correct solution (unless is made impractically small). Figure Q7.2 shows instability resulting from the inadequacies of such a simple prediction model.

Figure 2:
Notice the failure of the simple prediction model at the moment where the diode begins to turn on.
Zoom:

The instability (indicated with an arrow) would cause problems for any kind of Kalman filter since the prediction model is failing. An improved prediction model requires the use of a differential equation solver that can dynamically choose to maintain accuracy. Hence, the prediction step is a computer program, and a derivative-free method like the UKF is required.

Write code to implement an unscented Kalman filter for this system. Starter code is provided that implements an accurate prediction step. Based upon this predictive model, you can build a reliable filter. Write your code and evaluate its performance.

Using the parameters in the starter code, the working UKF produces output like Figure Q7.3.

Figure 3:
The expected output for Question 7.
Zoom: