MODULE 2 LESSON 1

THE KALMAN FILTER

Module 2 | Linear and Nonlinear
Kalman Filters
In this module…
• A brief history and overview of the (linear) Kalman filter

• Kalman filter as the BLUE

• Extending the Kalman filter to nonlinear systems through linearization

• Limitations of linearization

• Unscented Kalman filter

Module 2 | Linear and Nonlinear
By the end of this video, you will be able to…

1. Describe the Kalman filter as a two stage filter: (1) prediction and (2)
correction


2. Understand the difference between motion and measurement models


3. Use the Kalman filter in a simple 1D localization example

The Kalman Filter

Rudolf E. Kálmán receiving the National Medal of Science

The Kalman Filter

The (extended) Kalman Filter became

widely known after its use in the
Apollo Guidance Computer for
circumlunar navigation.
Apollo Guidance Computer
 The Kalman Filter | Prediction and Correction
3. Fusion
2. Measurement

PDF 1. Prediction

Note: the x-axis is

𝒩(xk̂ , Pk̂ )
not time!
𝒩(xk−1 ̂ )
̂ , Pk−1 𝒩(x̌k, P̌k) 𝒩(yk, Rk) Position (p)

Prediction
 Measurement

Estimate at (k - 1) (Motion Model) Estimate at (k) (Observation Model)

e.g., 

e.g., GPS,
wheel odometry,
Lidar
inertial navigation
The Kalman Filter | Linear Dynamical System

• The Kalman Filter requires the following motion and measurement models:

Motion model: xk = Fk−1xk−1 + Gk−1uk−1 + wk−1

input noise

Measurement model: yk = Hkxk + vk

noise

• With the following noise properties:

vk ∼ 𝒩(0, Rk) wk ∼ 𝒩(0, Qk)

Measurement Noise Process or Motion Noise
The Kalman Filter | Recursive Least Squares
+ Process Model
• The Kalman filter is a recursive least squares estimator that also
includes a motion model

1 Prediction 2a Optimal Gain

x̌k = Fk−1xk−1 + Gk−1uk−1 T T −1 (yk − Hkx̌k)

Kk = P̌kHk (HkP̌kHk + Rk)
P̌ = F P̂ FT + Q
k k−1 k−1 k−1 k−1
is often called the
‘innovation’

2b Correction Prediction 

x̌k (given motion model) 

x̂k = x̌k + Kk(yk − Hkx̌k) at time k

P̂ k = (1 − KkHk)P̌k Corrected prediction

x̂k (given measurement) 

(yk − Hkx̌k) at time k
is often called the
‘innovation’
 The Kalman Filter | Prediction & Correction
3. Correction
2. Measurement

PDF 1. Prediction

̂ )
̂ , Pk−1
𝒩(xk−1 𝒩(x̌k, P̌k) 𝒩(xk̂ , Pk̂ ) 𝒩(yk, Rk) Position (x)

1. Prediction 2,3. Measurement & Correction

x̌k = Fk−1xk−1 + Gk−1uk−1 T T −1
Kk = P̌kHk (HkP̌kHk
+ Rk)
{ ̂ T
P̌k = Fk−1Pk−1Fk−1 + Qk−1 x̂k = x̌k + Kk(yk − Hkx̌k)
{ P̂ = (1 − K H )P̌
k k k k
 The Kalman Filter | Short Example
Motion/Process Model

[0 1 ] [Δt]
Our input is
1 Δt 0
PDF xk = xk−1 + uk−1 + wk−1 acceleration. This
could come from a
control system.

Position Observation
Position(p) This could be a
yk = [1 0] xk + vk GPS measurement.

Noise Densities
p
[ dt = p· ]
d 2p
x= dp u=a= 2 vk ∼ 𝒩(0, 0.05) wk ∼ 𝒩(0, (0.1)12×2)
dt
 The Kalman Filter | Short Example
Data

[5] [ 0 1]
0 0.01 0
x̂0 ∼ 𝒩( , )
PDF

Δt = 0.5s
2 y1 = 2.2 [m]
Position(p) u0 = − 2 [m/s ]
The Kalman Filter | Short Example Solution
Prediction

x̌k = Fk−1xk−1 + Gk−1uk−1

[pˇ· 1] [0 1 ] [5] [0.5] [4]

p̌1 1 0.5 0 0 2.5
= + (−2) =

P̌k = Fk−1P̂ k−1FTk−1 + Qk−1

[0 1 ] [ 0 1] [0 1 ] [ 0 0.1] [ 0.5 1.1]

T
1 0.5 0.01 0 1 0.5 0.1 0 0.36 0.5
P̌1 = + =
The Kalman Filter | Short Example Solution
Correction
K1 = P̌1HT1 (H1P̌1HT1 + R1)−1
−1

[ 0.5 1.1] [0] ( [ 0.5 1.1] [0] )

0.36 0.5 1 0.36 0.5 1
= [1 0] + 0.05

[1.22]
0.88
=

x̂1 = x̌1 + K1(y1 − H1x̌1)

Bonus!

[p· ̂ 1] [ 4 ] [1.22] [4] [3.63]

p1̂ 2.5 0.88 2.5 2.24
= + (2.2 − [1 0 ] ) = P̂ 1 = (1 − K1H1)P̌1

[0.06 0.49]
0.04 0.06
=
Summary | The Kalman Filter

• The Kalman Filter is very similar to RLS but includes a motion model that tells
us how the state evolves over time

• The Kalman Filter updates a state estimate through two stages:

1. prediction using the motion model

2. correction using the measurement model