KF Extension to

Nonlinear Systems

M. Sami Fadali
Professor EE
University of Nevada

1
Outline

 Linearized KF Linearize about

a nominal trajectory.
 Extended KF Linearize about
the estimated trajectory.
 Example.

2
Linearized KF

 Linearize about a nominal

trajectory.
 Assume that the actual trajectory
is approximately known.
 Nonlinear process and/or
measurement model.

3
Extended Kalman Filter
 Linearize about the estimated
trajectory 𝒙
ෝ𝑘|𝑘 instead of nominal.
 Proof is similar to linearized KF.
 Can include higher order terms but
the performance is not necessarily
better.

4
 Derivation of LKF/EKF
Process and measurement models
ሶ
𝒙(𝑡) = 𝒇 𝒙(𝑡), 𝒖𝑑 (𝑡), 𝑡 + 𝒖(𝑡)
𝒛(𝑡) = 𝒉 𝒙(𝑡), 𝑡 + 𝒗(𝑡)
𝒙 𝑡 = 𝑛 × 1 state vector
𝒖𝑑 𝑡 = 𝑝 × 1 reference input vector.
𝒖 𝑡 = 𝑝 × 1 noise vector.
𝒛 𝑡 = 𝑧 × 1 measurement vector
𝒇, 𝒉 = vectors of known continuous functions.

5
 Linearized KF:Nominal Trajectory
𝒙∗ 𝑡 = 𝑛 × 1 nominal (reference) trajectory
 Approximately equal to the actual trajectory.
 Corresponds to the deterministic input 𝒖𝑑 𝑡
𝒙ሶ ∗ 𝑡 = 𝒇 𝒙∗ 𝑡 , 𝒖𝑑 𝑡 , 𝑡
𝒙 𝑡 = 𝒙∗ 𝑡 + Δ𝒙 𝑡
𝒙ሶ ∗ 𝑡 + Δ𝒙ሶ 𝑡 = 𝒇 𝒙∗ 𝑡 + Δ𝒙 𝑡 , 𝒖𝑑 𝑡 , 𝑡 + 𝒖(𝑡)

6
Trajectories

Actual Trajectory x

*
Nominal Trajectory x

7
 Linearization: State Equation
𝒙ሶ ∗ 𝑡 + Δ𝒙ሶ 𝑡 = 𝒇 𝒙∗ + Δ𝒙, 𝒖𝑑 , 𝑡 + 𝒖 𝑡
𝝏𝒇 𝒙, 𝒖𝑑 , 𝑡
≈𝒇 𝒙∗ , 𝒖𝑑 , 𝑡 + ቉ Δ𝒙 + 𝒖 𝑡
𝜕𝒙 𝒙∗ ,𝒖𝑑

▪ Cancel nominal terms.

𝝏𝒇 𝒙, 𝒖𝑑 , 𝑡
Δ𝒙ሶ 𝑡 ≈ ቉ Δ𝒙 + 𝒖 𝑡 = 𝐹Δ𝒙 + 𝒖 𝑡
𝜕𝒙 𝒙∗ ,𝒖𝑑

 Discretize for KF: Δ𝒙𝑘+1 = 𝜙𝑘 Δ𝒙𝑘 + 𝒘𝑘

8
 Discretization
𝒙ሶ 𝑡 = 𝒇 𝒙(𝑡), 𝒖𝑑 (𝑡), 𝑡
 Euler forward approximation
𝒙 𝑡𝑘+1 − 𝒙 𝑡𝑘
𝒙ሶ 𝑡 ≈ = 𝒇 𝒙 𝑡𝑘 , 𝒖𝑑 𝑡𝑘 , 𝑡𝑘
Δ𝑡
𝒙 𝑡𝑘+1 = 𝒙 𝑡𝑘 + 𝒇 𝒙 𝑡𝑘 , 𝒖𝑑 𝑡𝑘 , 𝑡𝑘 Δ𝑡
 Simple but can cause numerical instability
 Better approximations available (e.g. RK)
 Consider the simpler form 𝒙ሶ 𝑡 = 𝒇 𝒙(𝑡), 𝑡
9
 Linearization: Output Equation
𝒛 𝑡 = 𝒉 𝒙∗ + Δ𝒙, 𝑡 + 𝒗 𝑡
∗
𝝏𝒉 𝒙, 𝑡
≈ 𝒉 𝒙 ,𝑡 + ቉ Δ𝒙 + 𝒗 𝑡
𝜕𝒙 𝒙∗
Δ𝒛 𝑡 = 𝒛 𝑡 − 𝒉 𝒙∗ , 𝑡
𝝏𝒉 𝒙, 𝑡
≈ ቉ Δ𝒙 + 𝒗 𝑡 = 𝐻Δ𝒙 + 𝒗 𝑡
𝜕𝒙 𝒙∗

 Discretize the system then design a DKF.

10
 Linearized (Extended) KF:
Predictor

ෝ𝑘+1|𝑘 = solution of nonlinear equation 𝒙(𝑡)

• 𝒙 ሶ =
𝒇 𝒙(𝑡), 𝒖𝑑 (𝑡), 𝑡 at 𝑡𝑘+1 with IC 𝒙∗𝑘 𝒙
ෝ𝑘|𝑘
• Use 𝒙∗𝑘 𝒙
ෝ𝑘|𝑘 for the linearized (extended)
Kalman filter.
• Solve numerically or use a simple
approximation.

11
 KF Discrete Model
• Often use an approximate discretization
𝒙 𝑡𝑘+1 = 𝒇 𝒙 𝑡𝑘 , 𝒖𝑑 𝑡𝑘 , 𝑡𝑘 + 𝒘 𝑡𝑘
𝝏𝒇 𝒙, 𝒖𝑑 , 𝑡𝑘
≈ 𝒇 𝒙∗ , 𝒖𝑑 , 𝑡𝑘 + ቉ Δ𝒙 𝑡𝑘 + 𝒘 𝑡𝑘
𝜕𝒙 𝒙∗
Δ𝒙 𝑡𝑘 = 𝒙 𝑡𝑘 − 𝒙∗ 𝑡𝑘
 Linearized KF: Use 𝒙∗𝑘 =nominal trajectory
 Extended KF: Use 𝒙∗𝑘 = 𝒙
ෝ𝑘|𝑘
12
 Predictor
𝒙 𝑡𝑘+1
∗
𝝏𝒇 𝒙, 𝒖𝑑 , 𝑡𝑘
≈ 𝒇 𝒙 , 𝒖𝑑 , 𝑡𝑘 + ቉ Δ𝒙 𝑡𝑘 + 𝒘 𝑡𝑘
𝜕𝒙 𝒙∗
 Extended KF Predictor
ෝ𝑘+1|𝑘 = 𝐸 𝒙 𝑡𝑘+1 𝒛1:𝑘 = 𝒇 𝒙
𝒙 ෝ𝑘|𝑘 , 𝒖𝑑 , 𝑡𝑘
• Use 𝒙∗𝑘 instead of 𝒙𝑘 for the linearized KF.
ෝ+

13
 A Priori Error Covariance
ෝ𝑘|𝑘 + 𝒘 𝑡𝑘
ෝ𝑘|𝑘 , 𝒖𝑑 , 𝑡𝑘 + 𝜙𝑘 Δ𝒙
𝒙 𝑡𝑘+1 ≈ 𝒇 𝒙
ෝ𝑘|𝑘
𝒙𝑘|𝑘 = 𝒙 𝑡𝑘 − 𝒙
Δෝ
ෝ𝑘+1|𝑘 = 𝒇 𝒙∗𝑘 , 𝒖𝑑 , 𝑡𝑘
𝒙
• Error Δෝ ෝ𝑘+1|𝑘
𝒙𝑘+1|𝑘 = 𝒙 𝑡𝑘+1 − 𝒙
ෝ𝑘|𝑘 + 𝒘 𝑡𝑘
= 𝜙𝑘 Δ𝒙
• Error Covariance Matrix (discretize)
𝑃𝑘+1|𝑘 = 𝜙𝑘 𝑃𝑘|𝑘 𝜙𝑘𝑇 + 𝑄𝑘
14
 Linearized/Extended KF: Corrector

 Minor changes from the DKF

Δෝ
𝒙𝑘|𝑘 = Δෝ
𝒙𝑘|𝑘−1 + 𝐾𝑘 Δ𝒛𝑘 − 𝐻𝑘 Δෝ
𝒙𝑘|𝑘−1
ෝ𝑘|𝑘−1 + Δෝ
𝒙 𝒙𝑘|𝑘
=𝒙ෝ𝑘|𝑘−1 + Δෝ
𝒙𝑘|𝑘−1
ෝ𝑘|𝑘−1 , 𝑡𝑘 − 𝐻𝑘 Δෝ
+ 𝐾𝑘 𝒛𝑘 − 𝒉 𝒙 𝒙𝑘|𝑘−1
ෝ𝑘|𝑘 = 𝒙
𝒙 ෝ𝑘|𝑘−1 + 𝐾𝑘 𝒛𝑘 − 𝒉 𝒙
ෝ𝑘|𝑘−1 , 𝑡𝑘 − 𝐻𝑘 Δෝ
𝒙𝑘|𝑘−1
ෝ𝑘|𝑘−1 + 𝐾𝑘 𝒛𝑘 − 𝒛ො 𝑘|𝑘−1
=𝒙
15
 A Posteriori Error covariance
Matrix
ෝ𝑘|𝑘 = 𝒙
𝒙 ෝ𝑘|𝑘−1 + 𝐾𝑘 𝒛𝑘 − 𝒉 𝒙
ෝ𝑘|𝑘−1 , 𝑡𝑘 − 𝐻𝑘 Δෝ
𝒙𝑘|𝑘−1
ෝ𝑘|𝑘−1 + 𝐾𝑘 𝐻𝑘 Δ𝒙𝑘 + 𝒗 𝑘 − 𝐻𝑘 Δෝ
≈𝒙 𝒙𝑘|𝑘−1
Δෝ
𝒙𝑘|𝑘 = 𝒙𝑘 − 𝒙ො 𝑘|𝑘
ෝ𝑘|𝑘−1 − 𝐾𝑘 𝒗 𝑘
ෝ𝑘|𝑘−1 − 𝐾𝑘 𝐻𝑘 Δ𝒙
= 𝒙𝑘 − 𝒙
Δෝ
𝒙𝑘|𝑘 = 𝐼𝑛 − 𝐾𝑘 𝐻𝑘 Δෝ
𝒙𝑘|𝑘−1 − 𝐾𝑘 𝒗 𝑘
ෝ ෝ 𝑇
𝑃𝑘|𝑘 = 𝐸 Δ𝒙𝑘|𝑘 Δ𝒙𝑘|𝑘
= 𝐼𝑛 − 𝐾𝑘 𝐻𝑘 𝑃𝑘|𝑘−1 𝐼𝑛 − 𝐾𝑘 𝐻𝑘 𝑇
+ 𝐾𝑘 𝑅𝑘 𝐾𝑘𝑇
16
EKF Loop

Enter initial state estimate and Measurements

its error covariance 𝒙 ෝ− −
0 , 𝑃0 𝒛0 , 𝒛1 , …

Compute Kalman Gain

𝐾𝑘 = 𝑃𝑘− 𝐻𝑘𝑇 (𝐻𝑘 𝑃𝑘− 𝐻𝑘𝑇 + 𝑅𝑘 )−1

Linearize
𝐹 = 𝜕𝒇 𝜕𝒙 𝒙−𝒌+𝟏 , 𝜙𝑘 , 𝐻𝑘
Update estimate with measurement 𝒛𝑘
ෝ+
𝒙 ෝ−
𝑘 =𝒙
∗
ෝ−
𝒌 + 𝑲𝑘 𝒛𝑘 − 𝒉(𝒙𝑘 , 𝑡𝑘 ) − 𝐻𝑘 Δ𝒙 𝑘|𝑘−1
Project Ahead:
ෝ−
𝒙 𝑘+1 = 𝒇( 𝒙ෝ+𝑘 )Δ𝑡 + 𝒙𝑘
−
𝑃𝑘+1 = 𝜙𝑘 𝑃𝑘+ 𝜙𝑘𝑇 + 𝑄𝑘
State Estimates
ෝ+
𝒙 ෝ1+ , …
0, 𝒙
Compute error covariance
𝑃𝑘+ = (𝐼𝑛 − 𝐾𝑘 𝐻𝑘 )𝑃𝑘−
17
EKF Loop: Discrete-time Model

Enter initial state estimate and its

error covariance 𝒙 ෝ0|−1 , 𝑃0|−1
Compute Kalman Gain Measurements
−1 𝒛0 , 𝒛1 , …
𝐾𝑘 = 𝑃𝑘|𝑘−1 𝐻𝑘𝑇 𝐻𝑘 𝑃𝑘|𝑘−1 𝐻𝑘𝑇 + 𝑅𝑘

Linearize & Project Ahead:

ෝ𝑘+1|𝑘 = 𝒇 𝒙
𝒙 ෝ𝑘|𝑘 , 𝑡𝑘 Update estimate with measurement 𝒛𝑘
𝜙𝑘 = 𝜕𝒇 𝜕𝒙 𝒙ෝ𝑘|𝑘 , ෝ𝑘|𝑘
𝒙 ෝ𝑘|𝑘−1 + 𝑲𝑘 𝒛𝑘 − 𝒉 𝒙
=𝒙 ෝ−
ෝ𝑘|𝑘−1 , 𝑡𝑘 − 𝐻𝑘 Δ𝒙 𝑘|𝑘−1
𝐻𝑘 = 𝜕𝒉 𝜕𝒙 𝒙ෝ𝑘|𝑘
𝑇
𝑃𝑘+1|𝑘 = 𝜙𝑘 𝑃𝑘|𝑘 𝜙𝑘 + 𝑄𝑘
State Estimates
ෝ0|0 , 𝒙
𝒙 ෝ1|1 , …
Compute error covariance
𝑃𝑘|𝑘 = (𝐼𝑛 − 𝐾𝑘 𝐻𝑘 )𝑃𝑘|𝑘−1
18
Which linearization is better?
 Linearization: suboptimal filters
 Estimate of the true trajectory better
in a statistical sense (on the
average).
 Extended KF better in a statistical
sense.
 Linearized KF may perform better.
 Choice depends on the application.
19
Hybrid EKF
 Continuous-time system with discrete
Kalman filter.
 Sample output.
 Predictor
◼ Use continuous dynamics to derive
the update for the error covariance.
◼ Use an approximation to update the
state and the error covariance.

20
 Covariance Equation
 From the theory of the continuous time
Kalman filter
𝑃ሶ 𝑡
= 𝐹 𝑡 𝑃 𝑡 + 𝑃 𝑡 𝐹𝑇 𝑡 + 𝐺 𝑡 𝑄 𝑡 𝐺 𝑇 𝑡
− 𝑃 𝑡 𝐻𝑇 𝑡 𝑅−1 𝐻 𝑡 𝑃 𝑡
 Between measurements: no knowledge
makes 𝑅 → ∞
𝑃ሶ 𝑡 = 𝐹 𝑡 𝑃 𝑡 + 𝑃 𝑡 𝐹 𝑇 𝑡 + 𝐺 𝑡 𝑄 𝑡 𝐺 𝑇 𝑡
𝐹𝑘 = 𝐹 𝑡𝑘 , 𝐺𝑘 = 𝐺 𝑡𝑘 , 𝑄𝑘 = 𝑄 𝑡𝑘
21
Hybrid KF Loop

Enter initial state estimate and Measurements

its error covariance 𝒙ෝ0|−1 , 𝑃−0 𝒛0 , 𝒛1 , …

Compute Kalman Gain

𝐾𝑘 = 𝑃𝑘− 𝐻𝑘𝑇 (𝐻𝑘 𝑃𝑘− 𝐻𝑘𝑇 + 𝑅𝑘 )−1

Linearize
𝐹 = 𝜕𝒇 𝜕𝒙 𝒙ෝ𝒌+𝟏|𝒌 , 𝜙𝑘 , 𝐻𝑘 Update estimate with measurement 𝒛𝑘
𝒙 ෝ−
ෝ𝒌|𝒌 = 𝒙 𝒌 + 𝑲𝒌 𝒛𝒌 − 𝒉 𝒙 ෝ𝒌|𝒌−𝟏 , 𝑡𝑘
− 𝑯𝑘 Δ𝒙ෝ𝑘|𝑘−1
Project Ahead:
ෝ𝑘+1|𝑘 = 𝒇 𝒙
𝒙 ෝ𝑘|𝑘 Δ𝑡 + 𝒙 ෝ𝑘|𝑘
−
𝑃𝑘+1 = 𝑃𝑘+
State Estimates
+ 𝐹𝑘 𝑃𝑘+ + 𝑃𝑘+ 𝐹𝑘𝑇 + 𝐺𝑘 𝑄𝑘 𝐺𝑘𝑇 Δ𝑡
ෝ0|0 , 𝒙
𝒙 ෝ1|1 , …
Compute error covariance
𝑃𝑘+ = (𝐼𝑛 − 𝐾𝑘 𝐻𝑘 )𝑃𝑘−
22
EKF Limitations
 Results are only reliable if the error
propagation is approximately linear.
 If the linear approximation is bad, the
filter can diverge.
 Requires costly calculation of the
Jacobian.
 Cannot be used for systems with
abrupt parameter changes (Jacobian
does not exist)
23
Example: Space Vehicle

 Vehicle near earth.

 Assume
◼ Circular orbit.
◼ Planar motion and
measurements.
 Estimate vehicle position.

24
Coordinates of Space Vehicle
 local vertical
earth’s horizon
0 
Re 
 space vehicle

r
 = angle between the earth’s horizon and the local vertical
 = angle between the local vertical a known reference line
𝑅𝑒 𝑟 = radius of earth (vehicular orbit)
𝑟, 𝜃 = polar coordinates of vehicular motion.
25
Equations of Motion of a
Particle
y

1 r1
𝒓1 cos 𝜃 sin 𝜃 𝐢
𝜽1 =
− sin 𝜃 cos 𝜃 𝐣  j r sin()
 x
𝒊 cos 𝜃 −sin 𝜃 𝒓1 i
= 𝜽1
r cos()
𝒋 sin 𝜃 cos 𝜃
𝒓1 , 𝜽1 are the two unit vectors in the
directions of the two body-fixed axes.
26
Velocity
𝒓1 cos 𝜃 sin 𝜃 𝐢
𝜽1 = − sin 𝜃 cos 𝜃 𝐣
Changes in 𝑟 do not change the two unit
vectors (zero coefficients of 𝑟). ሶ
𝒓ሶ 1 𝜕𝒓1 /𝜕𝑟 𝜕𝒓1 /𝜕𝜃 𝑟ሶ
=
𝜽ሶ 1 𝜕𝜽1 /𝜕𝑟 𝜕𝜽1 /𝜕𝜃 𝜃ሶ
ሶ − sin 𝜃 cos 𝜃 𝐢 ሶ 𝜽1
=𝜃 =𝜃
− cos 𝜃 − sin 𝜃 𝐣 −𝒓1
𝑑𝑟𝒓1 𝑑𝒓1 𝒓1
𝒗= = 𝑟𝒓ሶ 1+𝑟 = 𝑟ሶ 𝑟𝜃ሶ 𝜽
𝑑𝑡 𝑑𝑡 1
27
Acceleration

𝑑𝒗 𝑑 𝒓1
𝒂= = 𝑟ሶ 𝑟𝜃ሶ 𝜽
𝑑𝑡 𝑑𝑡 1
𝒓1 𝜽1
= 𝑟ሷ 𝑟ሶ 𝜃ሶ + 𝑟𝜃ሷ 𝜽 + 𝑟ሶ ሶ
𝑟𝜃ሶ 𝜃 −𝒓
1 1
𝒓1
= 𝑟ሷ − 𝑟𝜃ሶ 2 2𝑟ሶ 𝜃ሶ + 𝑟𝜃ሷ 𝜽
1

Used to write equation of motion

28
Equations of Motion

Use polar coordinates (planar motion)

2
𝑅𝑒
𝑟ሷ − 𝑟𝜃ሶ 2+𝑔 = 𝑢𝑟 𝑡
𝑟
2𝑟ሶ 𝜃ሶ + 𝑟𝜃ሷ = 𝑢𝜃 𝑡
Re = radius of earth.
𝑢𝑟 , 𝑢𝜃 = small random functions in 𝑟, 𝜃
directions = white noise

29
 Nominal Trajectory
 Constant angular velocity 𝜃ሶ = 𝜔0 𝜃ሷ = 0
𝜃 ∗ = 𝜔0 𝑡
 Fixed nominal radius 𝑟 ∗ = 𝑅0 𝑟ሶ = 0
 Zero random perturbation
2
𝑅𝑒
ሶ2
𝑟ሷ − 𝑟𝜃 + 𝑔 = 0, 𝑟𝜃ሷ + 2𝑟ሶ 𝜃ሶ = 0
𝑟
2
2
𝑅𝑒 𝑔𝑅𝑒2 𝐾
−𝑅0 𝜔0 + 𝑔 = 0 ⇒ 𝜔0 = 3 =
𝑅0 𝑅0 𝑅03 30
Nonlinear State Equations
𝑇 𝑇
𝒙 = 𝑥1 𝑥2 𝑥3 𝑥4 = 𝑟 𝑟ሶ 𝜃 𝜃ሶ
2
𝑅𝑒
ሶ2
𝑟ሷ − 𝑟𝜃 + 𝑔 = 𝑢𝑟 𝑡 , 2𝑟ሶ 𝜃ሶ + 𝑟𝜃ሷ = 𝑢𝜃 𝑡
𝑟
𝑥ሶ 1 = 𝑥2
𝐾
𝑥ሶ 2 = 𝑥1 𝑥42 − 2 + 𝑢𝑟 𝑡
𝑥1
𝑥ሶ 3 = 𝑥4
𝑥ሶ 4 = −2𝑥2 𝑥4 /𝑥1 + 𝑢𝜃 𝑡 /𝑥1
31
Linearization
0 1 0 0
𝜕𝑓2 𝜕𝑓2 0 0
0 0
𝜕𝑥1 𝜕𝑥4 1 0
Δ𝒙ሶ = Δ𝒙 + 𝒖
0 0 0 1 0 0
𝜕𝑓4 𝜕𝑓4 𝜕𝑓4 0 1/𝑥1∗
0
𝜕𝑥1 𝜕𝑥2 𝜕𝑥4 𝒙∗
𝑓2 = 𝑥1 𝑥42 − 𝐾 𝑥12 +
𝑢𝑟 𝑡
𝑓4 = −2𝑥2 𝑥4 /𝑥1 + 𝑢𝜃 𝑡 /𝑥1
𝜕 𝑢𝜃 /𝑥1 /𝜕𝑢𝜃 = 1/𝑥1
32
 Linearized State Equation
 Substitute the nominal values
𝑥1∗ = 𝑟 ∗ = 𝑅0 , 𝑥2∗ = 0, 𝑥3∗ = 𝜃 ∗ = 𝜔0 𝑡, 𝑥4∗ = 𝜔0
0 1 0 0
2𝐾 0 0
𝜔02 + 3 0 0 2𝑅0 𝜔0
𝑅0 1 0
Δ𝒙ሶ = Δ𝒙 + 𝒖
0 0 0 1 0 0
2𝜔0 0 1/𝑅0
0 − 0 0
𝑅0

33
 Measurement Model

𝑇 𝑇
𝒛 = 𝑧1 𝑧2 = 𝛾 𝛼
𝑇 𝑇
−1
𝑅𝑒 −1
𝑅𝑒
= sin 𝛼0 − 𝜃 = sin 𝛼0 − 𝑥3
𝑟 𝑥1

earth’s horizon local vertical
0 
Re 
 space vehicle

r
34
 Linearization
𝑥1∗ = 𝑟 ∗ = 𝑅0
𝑇
∗ 𝑅𝑒
∗
𝒛 =𝒉 𝒙 ∗
= 𝛾 𝛼∗ 𝑇 = sin −1
𝛼0 − ∗
𝑥3
𝑥1∗
𝑅𝑒
− 0 0 0
Δ𝑧1
= 𝑅0 𝑅02 − 𝑅𝑒2 Δ𝒙 + 𝒗
Δ𝑧2
0 0 −1 0
𝐻 matrix
35
Extended KF: Corrector
ෝ+
𝒙𝑘 = ෝ
𝒙 −
𝑘 + 𝐾 𝒛
𝑘 𝑘 − 𝒉 𝒙∗
𝑘 − 𝐻𝑘 Δෝ
𝒙 −
𝑘
−1
𝐾𝑘 = 𝑃𝑘− 𝐻𝑘𝑇 𝐻𝑘 𝑃𝑘− 𝐻𝑘𝑇 + 𝑅𝑘
𝑃𝑘+
= 𝐼𝑛 − 𝐾𝑘 𝐻𝑘 𝑃𝑘− 𝐼𝑛 − 𝐾𝑘 𝐻𝑘 𝑇
+ 𝐾𝑘 𝑅𝑘 𝐾𝑘𝑇
 Minor change from the standard
covariance filter.

36
 Extended KF: Predictor
 Solve the nonlinear diff. eqs. for 𝒙
ෝ−𝑘+1
(usually use an approximation)
𝑥ሶ 1 = 𝑥2
𝑥ሶ 2 = 𝑥1 𝑥42 − 𝐾/𝑥12
𝑥ሶ 3 = 𝑥4
𝑥ሶ 4 = −2𝑥2 𝑥4 /𝑥1 , IC 𝒙
ෝ+𝑘
 Error Covariance Matrix (discrete model)
−
𝑃𝑘+1 = 𝜙𝑘 𝑃𝑘+ 𝜙𝑘𝑇 + 𝑄𝑘

37
 Iterated KF (Jazwinski, 1970)
 Iterate to reduce approximation errors:
ෝ0𝑘|𝑘 = 𝒙
𝒙 ෝ𝑘|𝑘−1 , ෥𝑖𝑘|𝑘−1 = 𝒙
𝒙 ෝ𝑖𝑘|𝑘
ෝ𝑘|𝑘−1 − 𝒙

For 𝑖 = 0, … , 𝐿 − 1

ෝ𝑖+1
𝒙𝑘|𝑘 = ෝ
𝒙 𝑘|𝑘−1 +𝐾 ෝ
𝒙 𝑖
𝑘|𝑘 𝒛𝑘 − 𝒉 ෝ
𝒙 𝑖
𝑘|𝑘 − 𝐻 ෝ
𝒙 𝑖
෥
𝒙 𝑖
𝑘|𝑘 𝑘|𝑘−1

 For each step, calculate 𝐾 𝑘 , 𝑃𝑘+1|𝑘 , 𝑃𝑘|𝑘

 Stop after 𝐿 iterations: 𝒙

ෝ𝐿𝑘|𝑘 − 𝒙
ෝ𝐿−1
𝑘|𝑘 ≤𝜖

38
Block diagram

ෝ𝑘−1|𝑘−1
𝒙 ෝ𝑘|𝑘−1
𝒙 ෝ1𝑘|𝑘
𝒙 ෝ𝐿−1
𝒙 𝑘|𝑘 ෝ𝐿𝑘|𝑘
𝒙
Corrector … Corrector
Predictor
#1 #𝐿

One additional corrector 𝐿 = 2

substantially improves results.

39
Iterated KF Computation
 Corrector 1: Compute
𝐾 𝑘 , 𝑃𝑘+1|𝑘 , 𝑃𝑘|𝑘 using 𝒙 ෝ1𝑘|𝑘
ෝ𝑘|𝑘−1 ⇒ 𝒙

 Corrector 2: Compute
ෝ1𝑘|𝑘 ⇒ 𝒙
𝐾 𝑘 , 𝑃𝑘+1|𝑘 , 𝑃𝑘|𝑘 using 𝒙 ෝ2𝑘|𝑘
⋮
 Corrector L: Compute
ෝ𝐿−1
𝐾 𝑘 , 𝑃𝑘+1|𝑘 , 𝑃𝑘|𝑘 using 𝒙𝑘|𝑘 ⇒ ෝ
𝒙 𝐿
𝑘|𝑘
40
Conclusion
 Discretization of nonlinear
equations is difficult.
 KF is the optimal linear filter.
 The optimal filter may be nonlinear.
 Retaining more Taylor series terms
is not always better than
linearization.

41
 References
• B.D.O. Anderson and J.B. Moore, Optimal Filtering,
Prentice-Hall, Englewood Cliffs, N.J. 1979.
• R. G. Brown and P. Y. C. Hwang, Introduction to
Random Signals and Applied Kalman Filtering,
4ed., J. Wiley, NY, 2012.
• C.-B. Chang and K.-P. Dunn, Applied State
Estimation and Association, MIT Press, Cambridge,
MA, 2016.
• A. H. Jazwinski, Stochastic Processes and Filtering
Theory, Academic Press, NY, 1970.

42