Lab 5 - Extended Kalman Filter

ECE 8540
Lab 5 - Extended Kalman Filter

1 Introduction
This lab report concerns the problem of applying an extended Kalman filter to a set of
measurements. The measurements are numerical data that has been recorded over time in
order to provide samples to filter. The provided data is to be filtered using a sinusoidal
model. The measurements provide recorded data on the current height of the sinusoid.
Similar to the Kalman filter, the extended Kalman filter would be used actively to filter
the measurements in real time. The extended Kalman filter is used to more accurately
determine the desired state variables in non-linear models. This expands upon the regular
Kalman filter to help handle more complex problems with more complex models. In practice
there will be some level of noise that we want to remove. The extended Kalman filter allows
us to weight the accuracy of our predictions and measurements to produce the best results.
Previously, methods such as the regular Kalman filter allowed us to filter linear models, but
this technique fails to produce accurate results when observing non-linear problems. The
extended Kalman filter uses methods to linearize non-linear problems and produce accurate
results.
2 Methods
The extended Kalman filter also follows a cycle of prediction and update. In this cycle the
next state and state covariance is predicted. Then measurements are obtained from the
sensor that is being actively used. After the measurements are obtained the Kalman gain is
calculated. The Kalman gain represents the weights that are placed on both the predictions
and measurements. The state and state covariance is then updated. This process is then
looped over the next time step. These steps can be modeled in the follow series of equations:
1. Predict next state
Xt,t−1 = f(Xt−1,t−1, 0) (1)
Equation 1 represents the matrix of nonlinear equations used to predict the next state.
The inputs to f include the state variables and noise. The noise is set to 0 when
predicting the next state in this step.
2. Predict next state covariance
St,t−1 =

∂f
∂x
St−1,t−1

∂f
∂xT
+

∂f
∂a
Q

∂f
∂aT
(2)
In equation 2 the partial derivative terms are the Jacobians of the state transition
equations, f.
1
3. Obtain measurement(s) Yt
4. Calculate the Kalman gain (weights)
Kt = St,t−1

∂g
∂xT
"
∂g
∂x
St,t−1

∂g
∂xT
+

∂g
∂n
R

∂g
∂nT
#−1
(3)
In equation 3 the partial derivative terms are the Jacobians of the measurement equations, g.
5. Update state
Xt,t−1 + Kt
[Yt − g(˜xt
, 0)] (4)
In equation 4, g(˜xt
, 0) represents a noiseless measurement of the predicted state.
6. Update state covariance
St,t =

I − Kt

∂g
∂x St,t−1 (5)
7. Loop (t is incremented by ∆t)
It is important to note that in this series of equations initial values need to be defined.
These values include the state at time 0, the state covariance at time 0, the dynamic noise
covariance and the measurement noise covariance. It is the job of the designer to determine
how these values should be defined based on available information. The designer must also
define the nonlinear equations that compose both f and g. After these equations are defined
the initial Jacobians must also be calculated for and defined. The Jacobians are calculated
by taking partial derivatives of all the transition and observation equations with respect to
the noises and state variables. Lastly, it is important to note that if the Jacobians are not
constant they must be updated each iteration.
2.1 EKF with Sinusoidal Model
For the measurements that were being filtered for this lab a sinusoidal model was used. There
were three state variables, where the most important variable was ht
. The state variable
ht represented the height of the sinusoid at the given time t. All the state variables were
represented in the following matrix:
Xt =


xt
x˙t
ht

 (6)
For this model at time 0 it was chosen that that the first two state variables were set to zero
and the height of the sinusoid was set to be the value obtained by the first measurement.
The state transition equations for the given model were defined as follows:
f(xt
, at) =


xt+1 = xt + ˙xtT
x˙t+1 = ˙xt + at
ht+1 = sin xt
10

 (7)
2
These equations allowed for the prediction of future states. The value, at represents the
dynamic noise. The dynamic noise is a random value obtained from a normal Gaussian
distribution. For measurements it was assumed that the sensor used detected the current
height of the sinusoid. The height was represented by dt
in the following matrix:
Yt =

dt

(8)
The observation equation for the measurements were represented by the following matrix.
g(xt
, nt) =
dt = ht + nt

(9)
In this matrix there is only the need for one equation, because only one state variable was
being observed. The inputs to the equation includes both the state variable (ht) and the
measurement noise (nt). The measurement noise represents a random value obtained from
a normal Gaussian distribution. After the transition equations, observation equations and
accompanying variables were defined four Jacobians were defined. The first two Jacobians
include the derivative of the state transition equations with respect to the state variables
and then the dynamic noise. The second two Jacobians are the derivative of the observation
equations with respect to the state variables and then the measurement noise. All four
Jacobians are defined as follows:
∂f
∂x =


1 T 0
0 1 0
1
10 cos x
10 0 0

 (10)
∂f
∂a =


0 0 0
0 1 0
0 0 0

 (11)
∂g
∂x =

0 0 1
(12)
∂g
∂n =

1

(13)
It is important to note that for this example the value T was set to equal one. T was set equal
to one because the time step for each iteration of new measurements was defined to be one
second. It can also be noted that all but one of the Jacobians are constant. These Jacobians
are constant because many of the state transition and observation equations were linear.
Finally it is important to note that during the beginning of each time iteration Jacobian 10
must be updated.
It was then important to define the initial state estimate covariance, St
. The state
estimate covariance represents the level of uncertainty that there is in the state variables
shown in matrix 6. Specifically, the diagonal of matrix 14 represents the uncertainty of the
individual state variable. The elements off of the diagonal represent the covariances of the
three state variables. The state covariance matrix was initialized to the following:
St =


1 0 0
0 1 0
0 0 1

 (14)
3