Unscented Kalman Filter. Cyrill Stachniss. 2. KF, EKF and UKF. â« Kalman filter requires linear models. â« EKF linearizes via Taylor expansion. Is there a better way ...
KF, EKF and UKF
Robot Mapping
! Kalman filter requires linear models ! EKF linearizes via Taylor expansion
Unscented Kalman Filter
Is there a better way to linearize? Cyrill Stachniss
Unscented Transform
Unscented Kalman Filter (UKF) 1
Taylor Approximation (EKF)
2
Unscented Transform
Linearization of the non-linear function through Taylor expansion
Compute a set of (so-called) sigma points
3
4
Unscented Transform
Unscented Transform
Transform each sigma point through the non-linear function
Compute Gaussian from the transformed and weighted points
5
6
Unscented Transform Overview
Sigma Points
! Compute a set of sigma points ! Each sigma points has a weight ! Transform the point through the nonlinear function ! Compute a Gaussian from weighted points
! How to choose the sigma points? ! How to set the weights?
! Avoids to linearize around the mean as the EKF does 7
8
Sigma Points Properties
Sigma Points
! How to choose the sigma points? ! How to set the weights? ! Select so that:
! Choosing the sigma points
First sigma point is the mean
! There is no unique solution for 9
10
Sigma Points
Matrix Square Root
! Choosing the sigma points
! Defined as ! Computed via diagonalization
matrix square root dimensionality
column vectors scaling parameter 11
12
Matrix Square Root
Cholesky Matrix Square Root
! Thus, we can define
! Alternative definition of the matrix square root
! Result of the Cholesky decomposition ! Numerically stable solution ! Often used in UKF implementations ! L and have the same Eigenvectors
! so that
13
Sigma Points and Eigenvectors
14
Sigma Points Example
! Sigma point can but do not have to lie on the main axes of
15
16
Sigma Point Weights
Recover the Gaussian
! Weight sigma points
! Compute Gaussian from weighted and transformed points
for computing the mean
parameters
for computing the covariance 17
Example
18
Examples
19
20
Unscented Transform Summary
UT Parameters
! Sigma points
! Free parameters as there is no unique solution ! Scales Unscented Transform uses Influence how far the sigma points are away from the mean
! Weights
Optimal choice for Gaussians 21
Examples
22
Examples
23
24
EKF Algorithm
EKF to UKF – Prediction Unscented
replace this by sigma point propagation of the motion
25
UKF Algorithm – Prediction
26
EKF to UKF – Correction Unscented
replace this by sigma point propagation of the motion use sigma point propagation for the expected observation and Kalman gain
27
28
UKF Algorithm – Correction (1)
UKF Algorithm – Correction (1)
29
UKF Algorithm – Correction (2)
(from EKF)
30
UKF Algorithm – Correction (2)
31
(see next slide)
32
From EKF to UKF – Computing the Covariance
UKF vs. EKF
33
UKF vs. EKF (Small Covariance)
34
UKF vs. EKF – Banana Shape EKF approximation
UKF approximation
35
36
UKF vs. EKF
UT/UKF Summary ! Unscented transforms as an alternative to linearization ! UT is a better approximation than Taylor expansion ! UT uses sigma point propagation ! Free parameters in UT ! UKF uses the UT in the prediction and correction step
Courtesy: E.A. Wan and R. van der Merwe
37
38
UKF vs. EKF
Literature
! Same results as EKF for linear models ! Better approximation than EKF for non-linear models ! Differences often “somewhat small” ! No Jacobians needed for the UKF ! Same complexity class ! Slightly slower than the EKF ! Still requires Gaussian distributions
Unscented Transform and UKF ! Thrun et al.: “Probabilistic Robotics”, Chapter 3.4 ! “A New Extension of the Kalman Filter to Nonlinear Systems” by Julier and Uhlmann, 1995 ! “Dynamische Zustandsschätzung” by Fränken, 2006, pages 31-34
