kalman filtering "from basics to unscented kaman filter"
TRANSCRIPT
Objectives
Objectives
Applications
Basic Idea
KalmanAlgorithm
Extension to
nonlinear
Extended Kalman
Filter (EKF)
Unscented Kalman
Filter (UKF)
Kalman Filter Applications •The Kalman filter has been used as an optimal solution to many tracking and data prediction applications.
Kalman Filter (KF)
•Prof. Rudolf Kalman(Born 1930 in Hungary)
•Developed filter in 1960/61
•The purpose of a Kalman filter is to estimate the state of a system by processing all available measurements, regardless of their precision.
•It works optimally for linear models and Gaussian distributions.
Kalman Filter
The Kalman filter algorithm involves two stages:
1. Prediction 2. Measurement
tttttt uBxAx 1
tttt xCz
Kalman Filter
t
The state transition matrix which applies the effect of each systemstate parameter at time t-1 on the system state at time t without controls or noise.
tA
The control input matrix which applies the effect of each control input parameter in the vector ut on the state vector xt .tB
The transformation matrix that maps the state vector xt
parameters into the measurement domain zt .tC
tRandom variables representing the process and measurement noise that are assumed to be independent and normally distributed with covariance Rt and Qt respectively.
The state vector containing the terms of interest for the system(e.g., position, velocity, heading) at time t
The vector containing any control inputs (steering angle, braking force).
The vector of measurements.
tx
tu
tz
Kalman Filter
Kalman Filter
ttttt uBA 1
t
T
tttt RAA 1
1t
1t
tu
Prediction
Measurement /Observation
Previous data
tz
Kalman FilterMeasurement update/correction
)( tttttt CzK
tttt CKI )(
1)( t
T
ttt
T
ttt QCCCK Kalman gain
1)0( T
ttt
T
ttt CCCK
1111)( ttt
T
t
T
tt CCCC
tttttttttttt CCzCCzC 111 )(
tttttt zCzC 11
0tQAt :
Kalman Filter
Kalman filter algorithm
Kalman Filter
Most realistic problems involve nonlinear functions
Kalman Filter
Kalman Filter
Kalman Filter
•The non-linear functions lead to non-Gaussian distributions.•Kalman filter is not applicable anymore!
Solution?
Local Linearization
Prediction:
Correction:
Extended Kalman Filter (EKF)
)(),(),(
)(),(
),(),(
1111
11
1
111
ttttttt
tt
t
tttttt
xGugxug
xx
ugugxug
)()()(
)()(
)()(
ttttt
tt
t
ttt
xHhxh
xx
hhxh
Linearization using Taylor Series Expansion
Linear functions(Jacobian matrices)
Extended Kalman Filter
Extended Kalman Filter
Extended Kalman filter algorithm
Extended Kalman Filter
• Not optimal!• Can diverge if nonlinearities are large!
better way to do linearization?
Unscented Transform (UT)
Unscented Kalman Filter (UKF)
Unscented Kalman Filter
Unscented Kalman Filter
Unscented Kalman Filter
Unscented Kalman Filter
nin
wwn
nw
nw
i
c
i
mi
i
cm
2,...,1for )(2
1 )(
)1( 2000
Sigma points Weights
Unscented Kalman Filter
Unscented Kalman Filter
Unscented Kalman Filter
Unscented Kalman filter algorithm
Unscented Kalman Filter
References
1. S. Thrun, W. Burgard,“Probabilistic Robotics”, Chapter 3.
2. Julier and Uhlmann,“A New Extension of the Kalman Filter to Nonlinear Systems”, 1995.
3. Ramsey Faragher, “Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation”, IEEE SIGNAL PROCESSING MAGAZINE, Sept.2012.
4. Cyrill Stachniss, “Robot Mapping lectures”, Uni. Freiburg, WS 2013/14 .