EE50238 – Navigation, Guidance & Communications

Dr Rob Wortham

Kalman Filter Example &

Extended Kalman Filter

Note: This lecture is being recorded using University of Bath Panopto

16/03/2024 EE50238 Navigation, Guidance & Communications - Lecture 12 1

Today’s Lecture
• A Linear Kalman Filter Example
• Extended Kalman Filter (EKF)
• Tutorial Task this week
• Applications for EKF
• Further Reading

16/03/2024 EE50238 Navigation, Guidance & Communications - Lecture 12 2

Example Problem1
• Use a Kalman filter to extract accurate position information from noisy sensor readings for a mobile robot.
• Robot system modelled in Simulink.
Position
• System state is position [e,n] and velocity [, ]
• Use a linear Kalman filter 1800

Actual
1600

1400

1200

North [m]
1000

800

600

400

200

0
-500 0 500 1000 1500
East [m]

1. EE50238_KalmanNavigationExample.mlx
16/03/2024 EE50238 Navigation, Guidance & Communications - Lecture 12 3
Example Problem – Simulink model

Sample and hold

16/03/2024 EE50238 Navigation, Guidance & Communications - Lecture 12 4

Example Problem – Sensor Readings with Noise
Position
2000

1800

1600
Actual
Measured
1400

1200
North [m]

1000

800

600

400

200

0
-1000 -500 0 500 1000 1500
East [m]

16/03/2024 EE50238 Navigation, Guidance & Communications - Lecture 12 5

Example Problem – Kalman Filter
Position
2000

1800

1600
Actual
Measured
1400

1200

North [m]
1000

800

600

400

200

0
-1000 -500 0 500 1000 1500
East [m]

16/03/2024 EE50238 Navigation, Guidance & Communications - Lecture 12 6

Example Problem – Kalman Filter Formulation

A : state transition model

Gw[n] : system noise (velocity errors)1
C : sensor model
v[n] : sensor noise
Ts : sampling interval n -> n+1

w[n] = []

1. Note: There is actually no system noise, but adjusting this

value improves performance because underlying system not
linear.

16/03/2024 EE50238 Navigation, Guidance & Communications - Lecture 12 7

Example Problem – Filter Output
Position
2000

1800

1600 Actual
Measured
Kalman filter estimate
1400

1200
North [m]

1000

800

600

400

200

0
-1000 -500 0 500 1000 1500
East [m]

16/03/2024 EE50238 Navigation, Guidance & Communications - Lecture 12 8

Tutorial task in MATLAB

• EE50238_KalmanNavigationExample.mlx
20

Velocity - East [m]

10

0
• Investigate how modifying the sensor noise alters
-10
the sensor values & filter response Actual
-20
Kalman filter

0 50 100 150 200 250 300 350 400 450

Time [s]

• Tune the filter by adjusting the Kalman filter

20
parameters to obtain a ‘best’ fit for the example Kalman filter: 1.652

Velocity Error - East [m]

path
10

-10
• Competition! – who can achieve the smallest
velocity error – what does ‘smallest’ mean? 0 50 100 150 200 250
Time [s]
300 350 400 450

16/03/2024 EE50238 Navigation, Guidance & Communications - Lecture 12 9

Non Linear Systems Equations
System Dynamics • State is determined by some function of
Xk = f( Xk−1 , Uk ) + Wk last state and control input, plus system
noise.
where
Xk : state of the system at time k
U : control-input
Wk : system noise
f() : non linear state transition model • Observation is some function of the
system state, plus noise.
Observations
Yk = h( Xk ) + Zk
Where
Yk : observation
Zk : observation noise
h() : non linear observation model
16/03/2024 EE50238 Navigation, Guidance & Communications - Lecture 12 10
Extended Kalman Filter
• Applicable to the non-linear model.
• linearizes around an estimate of the current
mean and covariance.
• Use the same prediction and update steps as for To determine these derivates
linear Kalman filter. f() and h() must be smooth
and differentiable
• State transition and observation matrices are i.e. must behave linearly over
defined to be matrices containing partial small perturbations in X and U
derivatives – known as Jacobian matrices.

• Remember
Xk = f( Xk−1 , Uk ) + Wk
Yk = h( Xk ) + Zk

16/03/2024 EE50238 Navigation, Guidance & Communications - Lecture 12 11

Extended Kalman Filter Applications and Extensions
• Standard filter used for GNSS and other navigation systems e.g. INS, TDoA
• Unlike linear KF, if parameters are incorrect the EKF can give very poor performance
• Output can diverge - since Fk is dependent on k-1
• Stability can be improved by increasing assumed system noise Wk

• Interactive EKF Tutorial - https://simondlevy.academic.wlu.edu/kalman-tutorial/

• Further extensions of the Kalman predict / update model have been developed, such
as Unscented Kalman Filter. These are particularly useful for sensor fusion where type
of information differs widely between sensor systems.
• Further reading: Julier, S., Uhlman, J., ‘Unscented filtering and nonlinear estimation’,
Proceedings of IEEE, 2004 https://doi.org/10.1109/JPROC.2003.823141

16/03/2024 EE50238 Navigation, Guidance & Communications - Lecture 12 12

 Questions?
In-Person Sessions or Moodle
Discussion Forum.

16/03/2024 EE50238 Navigation, Guidance & Communications 13