## Description

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