Sensor Fusion Study - Ch13. Nonlinear Kalman Filtering [Ahn Min Sung]

Nonlinear Kalman Filtering
Min Sung Ahn
July 17, 2020
Sensor Fusion

Table of Contents
• Linearized Kalman Filter
• Extended Kalman Filter (EKF)
• “Higher-order” Nonlinear Kalman Filtering
• Kalman Filtering Parameter Estimation
7/17/20 Nonlinear Kalman Filtering 2

Linearized Kalman Filter

Consider the general nonlinear system model:

Taylor series of the system model:

Define nominal system trajectory:
Define deviation of state derivatives and measurement:
Using these definitions results in the following system:

Linearization Kalman Filter Summary
1. Given system equations and nominal trajectory ahead of time.
2. Compute partial derivative matrices evaluated at the nominal
trajectory values.
3. Compute the new covariance matrices.
4. Define Δ𝑦 = 𝑦 − 𝑦!.
5. Execute the Kalman filter equations.
6. Estimate the state as: %𝑥 = 𝑥! + Δ%𝑥.

Extended Kalman Filter

Extended Kalman Filter (Continuous)
Since the EKF tries to directly solve for %𝑥, we start by summing 𝑥! and
Δ ̇%𝑥 system equations:
Choosing 𝑥! 𝑡 = %𝑥(𝑡), the nominal measurement equation gets
updated:
And the state becomes:

Extended Kalman Filter (CT) Summary
1. Given system equations.
2. Compute the partial derivative matrices evaluated at the current
state estimate.
3. Compute the new covariance matrices.
4. Execute the Kalman filter equations.

Extended Kalman Filter (Hybrid)
Suppose a continuous-time system with discrete-time measurements:
Propagate state estimate according to nonlinear dynamics.
Propagate covariance as in the continuous-time EKF.
Time-update equations for hybrid EKF become:

Extended Kalman Filter (Hybrid)
At each measurement time, update the state estimate and covariance
as in the discrete-time Kalman filter:
𝐻" and 𝑀" are evaluated at %𝑥"
#
.
𝑃" and 𝐾" cannot be computed offline because of dependence on 𝐻"
and 𝑀".
Hence, (generally) no steady-state solution exists for EKF.

Extended Kalman Filter (Hybrid) Summary
1. Given system equations with continuous-time dynamics and
discrete-time measurements.
2. Initialize the filter as follows:
3. For 𝑘 = 1,2, … , perform the time update and measurement update as
follows:

Extended Kalman Filter Example
Given the system equations:
𝑥!: altitude 𝑥": velocity
1/𝑥#: constant ballistic coefficient
𝑤: process noise 𝑣: measurement noise
𝜌$: air density at seal level 𝑔: gravitational acceleration
𝑘: constant relationship between air density and altitude

Compute the partial derivatives: Initialize the filter:

Estimation error magnitudes
averaged over 100 simulations
CT EKF performs better in general
than hybrid EKF

Extended Kalman Filter (Discrete)
Suppose a discrete-time dynamics and discrete-time measurements.
Perform a Taylor series expansion of the state equation around 𝑥"#$ =
%𝑥"#$
%
and 𝑤"#$ = 0, and the measurement equation around 𝑥" = %𝑥"
#
and
𝑣" = 0.

Extended Kalman Filter (Discrete) Summary
1. Given the discrete-time system and discrete-time measurement
equations.
2. Initialize the filter as follows:
3. For 𝑘 = 1,2, … , compute the partial derivative matrices and perform
the time update and measurement update equations.

Higher-Order Approaches

Iterated EKF
Recall ℎ(𝑥", 𝑣") approximation by expanding it in a Taylor series around
%𝑥"
#
:
and how the measurement-update equations based on this linearization
are:
If we reformulate the Taylor series of ℎ(𝑥", 𝑣") about the a posteriori
estimate and recalculate the measurement-update equations, we
should get a better a posteriori estimate!

Iterated EKF
Notation: %𝑥",'
%
, 𝑃",'
%
, 𝐾",', 𝐻",'
Initialization:
For 𝑖 = 0, 1, … , 𝑁, evaluate:
Additional modification to obtain
the iterated Kalman filter:

Iterated EKF Summary
1. Given a nonlinear system and measurement equations.
2. Initialize the filter.
3. For 𝑘 = 1, 2, … , perform:
(a) Time-update
(b) Measurement-update
i) Initialize the iterated EKF estimate to the standard EKF estimate:
ii) For 𝑖 = 0, 1, … , 𝑁, evaluate the measurement update equations:
(c) Final a posteriori state estimate and estimation-error covariance are:

Second-Order EKF
Given a system (hybrid, in this case):
We previously conducted first-order Taylor series expansion to get the
time-update equation for EKF by evaluating at 𝑥 = %𝑥:
Second-order Taylor series expansion includes an additional term:

Second-Order EKF
The quadratic term can be re-written as follows:
But since the 𝑥 − %𝑥 (
(𝑥 − %𝑥) is unknown, we replace it with its
expected value, the Kalman filter covariance:
Substituting to the Taylor series expansion and evaluating at 𝑥 = %𝑥:

Second-Order EKF
Suppose the measurement-update equation for the state estimate is:
If we define the estimation error as:
Using the system equations and the supposed measurement-update
equation for state-estimate, the a posteriori error becomes:

Second-Order EKF
Performing the second-order Taylor series expansion of ℎ around the
nominal point %𝑥"
#
results in:
Substituting the above into 𝑒"
%
:

Second-Order EKF
Taking the expected value of both sides, and assuming 𝐸 𝑒"
#
= 0, in
order to have 𝐸 𝑒"
%
= 0, we must set:
Defining 𝑃"
%
as:
Leads to the following expression after some calculations:

Second-Order EKF
Defining a cost function that we want to minimize as a weighted sum of
estimation errors:
The 𝐾" that minimizes this is:
Substituting this back into 𝑃"
%
results in:
Λ" is complex, but 𝜙', 𝜙) are binary vectors with all zeros except at 𝑖, 𝑗,
which simplifies it:

Second-Order EKF
Measurement-update equations are then:

4. Run measurement-update:
Second-Order EKF Summary
1. Given a system equation.
2. Initialize the estimator:
3. Run time-update:

Other Approaches (Gaussian Sum Filter)
1. Given a discrete-time n-state system and measurement equations.
2. Initialize the filter by approximating the pdf of the initial state as:
3. For 𝑘 = 1, 2, … , do:
(a) Obtain the a priori estimate by first executing the time-update equations for
𝑖 = 1, 2, … , 𝑀:
The pdf of the a priori state estimate is then obtained by the following sum:

Other Approaches (Gaussian Sum Filter)
3. For 𝑘 = 1, 2, … , do:
(b) Obtain the a posteriori estimate by first executing the measurement-update
equations for 𝑖 = 1, 2, … , 𝑀:
The weighting coefficients 𝑎!" for the individual estimates are obtained as
follows:
And finally, the pdf:

Other Approaches
Grid-based Filtering:
- Similar to particle filtering, but has some computational requirements
that increase exponentially with the dimension of the state
- Has limited application
Linearizing the optimal nonlinear filter:
- Theoretically optimal nonlinear filter is very difficult to compute

Parameter Estimation

Parameter Estimation
Assume a discrete-time model with nonlinear system matrices
dependent on a constant parameter 𝑝:
Define an augmented state to derive an augmented system model:
Since 𝑓 is nonlinear, estimate the augmented state (which contains 𝑝)
using EKF or any other nonlinear filter.

Chapter Summary
Nonlinear filtering is the most widespread approach to state estimation
for nonlinear systems.
Unlike the linear Kalman filter , stability and convergence results are
more difficult to obtain for nonlinear Kalman filtering.
• Some convergence results have been found.
• If nonlinearities are bounded, the Riccati equation can be modified to
guarantee stability for CT EKF.
• Conditions to guarantee boundedness of DT EKF error covariance can be
related to the nonlinear system’s observability.

Sensor Fusion Study - Ch13. Nonlinear Kalman Filtering [Ahn Min Sung]

More Related Content

What's hot (6)

Similar to Sensor Fusion Study - Ch13. Nonlinear Kalman Filtering [Ahn Min Sung] (20)

More from AI Robotics KR (17)

Recently uploaded (20)

Sensor Fusion Study - Ch13. Nonlinear Kalman Filtering [Ahn Min Sung]