Week 4 Tutorial

Question 1

Given a state prediction model of the form

\hat{\boldsymbol{x}}_{k|k-1}=f(\hat{\boldsymbol{x}}_{k-1}),

the extended Kalman filter predicts the covariance using the equation:

P_{k|k-1}=FP_{k-1}F^{T}+Q_{k},

where $F$ is the Jacobian of $f\left(\cdot\right)$ and $Q_{k}$ is the process covariance.

Calculate the Jacobian for each prediction model given below.

(a)

f(x)=x-x^{2}\Delta t

(b)

f(\boldsymbol{x})=f\left(\begin{array}{c} x\\ y \end{array}\right)=\left(\begin{array}{c} x+y^{2}\\ \left(1-\Delta t\right)y \end{array}\right)

(c)

f(\boldsymbol{x})=f\left(\begin{array}{c} x\\ y\\ z \end{array}\right)=\left(\begin{array}{c} x-yz\\ y-xz\\ z-xy \end{array}\right)

(d)

f(\boldsymbol{x})=f\left(\begin{array}{c} x\\ y\\ t \end{array}\right)=\left(\begin{array}{c} x+\cos t\\ y+\sin t\\ t+\Delta t \end{array}\right)

Question 2

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

Figure Q2: Top-down view of a vehicle with the state variables labelled. Zoom:

The vehicle is described by the state vector

\boldsymbol{x}=\left(\begin{array}{c} x\\ y\\ s\\ \theta \end{array}\right),

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

(a) Write a prediction function $\hat{\boldsymbol{x}}_{k|k-1}=f(\hat{\boldsymbol{x}}_{k-1})$ .

(b) Calculate the Jacobian of $f$ 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 $\boldsymbol{z}$ and hence write down the measurement model $h$ in the expression $\boldsymbol{z}_{predicted}=h\left(\hat{\boldsymbol{x}}_{k|k-1}\right).$

(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)

\hat{\boldsymbol{x}}_{k|k-1}=f\left(\hat{\boldsymbol{x}}_{k-1}\right)=\left(\begin{array}{c} x_{k-1}+s_{k-1}\Delta t\sin\theta_{k-1}\\ y_{k-1}+s_{k-1}\Delta t\cos\theta_{k-1}\\ s_{k-1}\\ \theta_{k-1} \end{array}\right).

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

(b) The Jacobian of $f$ is

F=\left[\begin{array}{cccc} 1 & 0 & \Delta t\sin\theta_{k-1} & s_{k-1}\Delta t\cos\theta_{k-1}\\ 0 & 1 & \Delta t\cos\theta_{k-1} & -s_{k-1}\Delta t\sin\theta_{k-1}\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{array}\right]

for a covariance prediction of

P_{k|k-1}=FP_{k-1}F^{T}+Q_{k},

where $Q_{k}$ is the process covariance.

(c) The measurements can be arranged in a measurement vector

\boldsymbol{z}=\left(\begin{array}{c} \text{x position from GPS}\\ \text{y position from GPS}\\ \text{speed}\\ \text{heading} \end{array}\right).

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

\boldsymbol{z}_{pred}=H\boldsymbol{x}_{k|k-1},

where the measurement matrix $H$ is

H=\left[\begin{array}{cccc} 1 & 0 & 0 & 0\\ 0 & 1 & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{array}\right].

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

(d) In this case there are only two measurements

\boldsymbol{z}=\left(\begin{array}{c} \text{speed}\\ \text{heading} \end{array}\right)

and the measurement matrix becomes

H=\left[\begin{array}{cccc} 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{array}\right].

Question 3

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

Figure Q3.1: The circuit to be analysed for Question 3. Zoom:

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

\hat{\boldsymbol{x}}=\left(\begin{array}{c} \hat{V}_{1}\\ \hat{V}_{2} \end{array}\right).

The measurements are the voltages,

\boldsymbol{z}=\left(\begin{array}{c} V_{1}\\ V_{2} \end{array}\right).

The LED is described by the Shockley diode equation,

i_{D}=10^{-12}\left(e^{V_{2}/0.026}-1\right),

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 $\text{C}_{2}$ . Recall that capacitors are described by $i=C\frac{dv}{dt}$ .

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

\begin{align*} \frac{dV_{1}}{dt} & =\frac{-V_{1}-V_{2}}{RC_{1}}+\frac{V_{i}}{RC_{1}}\\ \frac{dV_{2}}{dt} & =\frac{-V_{1}-V_{2}}{RC_{2}}-\frac{10^{-12}}{C_{2}}\left(e^{V_{2}/0.026}-1\right)+\frac{V_{i}}{RC_{2}}. \end{align*}

(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

\left(V_{2}\right)_{k|k-1}=\left(V_{2}\right)_{k-1}+\left(\frac{dV_{2}}{dt}\right)\Delta t

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

Figure Q3.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 $\Delta t$ 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.