MODULE 2 LESSON 3

GOING NONLINEAR: THE

EXTENDED KALMAN FILTER
The Extended Kalman Filter (EKF)
By the end of this video, you will be able to

• Describe how the EKF uses first-order linearization to turn a nonlinear

problem into a linear one

• Understand the role of Jacobian matrices in the EKF and how to compute
them

• Apply the EKF to a simple nonlinear tracking problem

 Recap | The Linear Kalman Filter
Linear motion / process model Linear measurement model

xk = Fk−1xk−1 + Gk−1uk−1 + wk−1 yk = Hkxk + vk

current previous inputs process measurement state measurement
state state noise noise

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

3. Correction
2. Measurement

PDF 1. Prediction
Linear discrete-time Kalman Filter:
̂ )
̂ , Pk−1
𝒩(xk−1 𝒩(x̌k, P̌k) 𝒩(xk̂ , Pk̂ ) 𝒩(yk, Rk) Position (x)
The Kalman
Filter is the best
linear estimator
1. Prediction 2/3. Measurement & Correction

{
x̌k = Fk−1xk−1 + Gk−1uk−1
P̌k = Fk−1P̂ k−1FTk−1 + Qk−1
{
Kk = P̌k HTk (Hk P̌k HTk + Rk)−1
x̂k = x̌k + Kk(yk − Hk x̌k)
P̂ k = (1 − Kk Hk)P̌k
Nonlinear Kalman Filtering
Linear Nonlinear
regime regime

Linear systems do not This is because the

Current resistor heats up,
exist in reality! which changes the
resistance!
Ohm’s Law: I = V/R

Voltage

xk = fk−1(xk−1, uk−1, wk−1)

How can we adapt the Kalman
current previous inputs process
Filter to nonlinear discrete-time state state noise
systems?
yk = hk(xk, vk)

measurement state measurement

noise
EKF | Linearizing a Nonlinear System
f(x)

Choose an operating point a and

approximate the nonlinear function by
a tangent line at that point
∂f(x)
f(a) slope =
∂x
x=a

x
a
Mathematically, we compute this linear approximation using a first-order Taylor expansion:

∂f(x) 1 ∂2f(x) 1 ∂ 2
f(x)
f(x) ≈ f(a) + (x − a) + (x − a)2 + (x − a)3 + . . .
∂x 2! ∂x 2 3! ∂x 2
x=a x=a x=a
First-order terms Higher-order terms
EKF | Linearizing a Nonlinear System
For the EKF, we choose the operating point to be our most recent state
estimate, our known input, and zero noise:
Linearized motion model

∂fk−1 ∂fk−1
xk = fk−1(xk−1, uk−1, wk−1) ≈ fk−1(x̂k−1, uk−1, 0) + (xk−1 − x̂k−1) + wk−1
∂xk−1 ∂wk−1
x̂k−1,uk−1,0 x̂k−1,uk−1,0

Fk−1 Lk−1
Linearized measurement model

∂hk ∂hk
yk = hk(xk, vk) ≈ hk(x̌k, 0) + (xk − x̌k) + vk
∂xk ∂vk
x̌k,0 x̌k,0

Hk Mk

We now have a linear system in state-space! The matrices Fk–1, Lk–1, Hk, and Mk are
called the Jacobian matrices of the system
EKF | Computing Jacobian Matrices
In vector calculus, a Jacobian matrix is the matrix of all first-order partial
derivatives of a vector-valued function
∂f1 ∂f1
∂x1
⋯ ∂xn
∂f
= [ ∂x ⋯ ∂xn ]
∂f ∂f
= ⋮ ⋱ ⋮
∂x 1
∂fm ∂fm
∂x1
⋯ ∂xn

Intuitively, the Jacobian matrix tells you how fast each output of the function is
changing along each input dimension

For example:
∂f1 ∂f1

[f2] [ x12 ]
x1 + x2
[2x1 0]
f1 ∂f ∂x1 ∂x2 1 1
f(x) = = = =
∂x ∂f2 ∂f2
∂x1 ∂x2
 EKF | Putting It All Together
With our linearized models and Jacobians, we can now use the Kalman Filter equations!

Linearized motion model Linearized measurement model

xk = fk−1(x̂k−1, uk−1, 0) + Fk−1 (xk−1 − x̂k−1) + Lk−1wk−1 yk = hk(x̌k, 0) + Hk (xk − x̌k) + Mkvk

Prediction Optimal gain

x̌k = fk−1(x̂k−1, uk−1, 0) Kk = P̌kHTk (HkP̌kHTk + MkRkMTk )−1

P̌ = F P̂ FT + L
k k−1 k−1 k−1 Q L T
k−1 k−1 k−1

Correction
Prediction
x̌k (given motion model) x̂k = x̌k + Kk(yk − hk(x̌k, 0))
P̂ = (1 − K H )P̌
at time k
k k k k
Corrected prediction
x̂k (given measurement)
at time k
EKF | Short Example
Height
Motion/Process model
xk = f(xk−1, uk−1, wk−1)

[0 1 ] [Δt]
S 1 Δt 0
= xk−1 + uk−1 + wk−1
ϕ
Landmark measurement model
Position
p yk = ϕk = h(pk, vk)
D

( D − pk )
S

[p]
p −1
= tan + vk
x= · u = p··

Noise densities
S and D are known in advance
vk ∼ 𝒩(0, 0.01) wk ∼ 𝒩(0, (0.1)12×2)
EKF | Short Example
Motion model Jacobians
Height

[0 1 ]
∂f 1 Δt
Fk−1 = =
S ∂xk−1
x̂k−1,uk−1,0
ϕ
∂f
Lk−1 = = 12×2
∂wk−1
x̂k−1,uk−1,0
Position
p D
Measurement model Jacobians

∂h
[ (D − p̌k)2 + S2 0]
S d 1
Hk = = −1
tan x =
∂xk dx 1+
x̌k,0

∂h
Mk = =1
∂vk
x̌k,0
EKF | Short Example
Height Data

([5] [ 0 1])
0 0.01 0
x̂0 ∼ 𝒩 ,
S
ϕ
Δt = 0.5 s
2 y1 = π/6 [rad]
u0 = − 2 [m/s ]
Position
p D S = 20 [m] D = 40 [m]

Using the Extended Kalman Filter equations, what is our updated position?
p1̂
EKF | Short Example Solution
Prediction
x̌1 = f0(x̂0, u0, 0) This is the same
result as in the

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

p̌1 1 0.5 0 0 2.5 linear Kalman Filter
= + (−2) = example because
the motion model is This is the same
already linear! result as in the
linear Kalman Filter
example because
̂ T T
P̌1 = F0P0F0 + L0Q0L0 the motion model is
already linear!

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

1 0.5 0.01 0 1 0 1 0 0.1 0 1 0 0.36 0.5
P̌1 = + =
EKF | Short Example Solution
Correction
T T T −1
K1 = P̌1H1 (H1P̌1H1 + M1R1M1 )
−1

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

0.36 0.5 0.011 0.36 0.5 0.011
= [0.011 0] + 1(0.01)(1)

[0.55]
0.40
=

Bonus!
x̂1 = x̌1 + K1(y1 − h1(x̌1, 0))
P̂ 1 = (1 − K1H1)P̌1
Notice that our state
estimate has not

[p̂ 1] [ 4 ] [0.55] [4.02]

p1̂ 2.5 0.40 2.51
[0.50 1.1 ]
become more certain in
· = + (0.52 − 0.49) = 0.36 0.50 this case, because the
= measurement model
varies slowly at this
distance
Summary | Extended Kalman Filter (EKF)

• The EKF uses linearization to adapt the Kalman filter to nonlinear systems

• Linearization works by computing a local linear approximation to a nonlinear

function about a chosen operating point

• Linearization relies on computing Jacobian matrices, which contain all the first-
order partial derivatives of a function