The document provides an overview of the Kalman filter, which is an optimal recursive estimator that minimizes the mean square error of estimated parameters. It describes the basic Kalman filter equations for state prediction and correction. As an example, it applies the Kalman filter to estimate the altitude of an airplane based on noisy measurements over time. It then expands on the example by incorporating control inputs and adding velocity as another state to estimate. Finally, it outlines the general discrete-time Kalman filter model and process for estimating state covariance.
2. What is a Kalman Filter
β’ A Kalman filter is an optimal recursive
estimator
β If all noise is Gaussian, the Kalman filter minimizes
the mean square error of the estimated
parameters.
β’ Versatile
β Estimation
β Filtering
β Prediction
β Fusion
Predict Correct
3. Why Kalman Filtering
β’ Efficient βleast-squaresβ implementation
β’ Past, present and future estimation
β’ Estimation of missing states
β’ Measure of estimation quality (variance)
β’ Robust
β Forgiving in many ways
β Stable given common conditions
β’ Convenient form for online real time processing.
β Easy to formulate and implement given a basic
understanding.
4. The Process Model
β’ Process Dynamics
π π = π¨π πβπ + π πβπ
β’ Measurement
π π = π―π π + π π
next state
measurement
previous state
measurement
noise
process
noise
state
dynamic model
measurement model
6. State Estimation (Predict)
β’ Rewrite the measurement equation into
π₯ π = π§ π β π£ π
β Problem: the current noise is unpredictable
β’ Estimate the state by taking into account both
the current measurement and the previous
estimated state
οΏ½Μ π = οΏ½Μ πβ1 + πΎπ(π§ π β οΏ½Μ πβ1)
β οΏ½Μ : the hat means it is estimated
β πΎπ is a gain expressing the tradeoff
οΏ½Μ π = πΎπ π§ π + (1 β πΎπ)οΏ½Μ πβ1
7. Computing the Gain
β’ Recall the measurement associated with a noise
π₯ π = π§ π β π£ π
β we donβt know the individual noise for an
measurement, but we typically know the average
noise, call it π
πΎπ = π πβ1/(π πβ1 + π)
β’ π has no subscript; it doesnβt depend on time
β’ where π π is a prediction error that is computed recursively
ππ = (1 β πΎπ) π πβ1
β’ think about the cases:
β ππβ1 previous prediction error was 0
β if there is very little noise, π is 0
β when the system is noisy, π becomes large
8. The Prediction and Correction
β’ Consider the state equation
π₯ π = π΄π₯ πβ1
β It seems to have vanished in the equation
οΏ½Μ π = οΏ½Μ πβ1 + πΎπ(π§ π β οΏ½Μ πβ1)
β’ we need both
β’ first equation represents a prediction about what the state should
be, and the second equation represents an correction to this
prediction, based on an observation
β’ Rewrite it
οΏ½Μ π = π΄οΏ½Μ πβ1
β’ We use the constant in π΄ a prediction of the error too
π π = π΄π πβ1 π΄
β we multiply twice by π΄ because the prediction error π π is
itself a squared error; hence, it is scaled by the square of
the coefficient associated with the state value π₯ π
12. A More Realistic Model
β’ Account for the time-varying control that the
pilot exercises over the airplane
β for example, moving the control column forward and
back
οΏ½Μ π+1 = π΄οΏ½Μ π + π΅π’ π
β π’ π represents the current value of the control signal
that the pilot is sending to the airplane
β this control signal can be scaled by a constant amount
B
β’ Measurement
π§ π = π»π₯ π + π£ π
14. Adding Velocity to the System
β’ For the state equation
οΏ½Μ π = π΄οΏ½Μ πβ1
β Considering equation expressing distance in terms
of velocity and time
πππ π‘ππππ ππ’πππππ‘
= πππ π‘ππππ ππππ£πππ’π + π£ππππππ‘π¦ ππππ£πππ’π β π‘ππππ π‘ππ
β vectors are used
π₯ π β‘
πππ π‘ππππ π
π£ππππππ‘π¦ π
π΄ =
1 π‘ππππ π‘ππ
0 1
β’ So
πππ π‘ππππ π
π£ππππππ‘π¦ π
=
1 π‘ππππ π‘ππ
0 1
πππ π‘ππππ πβ1
π£ππππππ‘π¦ πβ1
15. Discrete Kalman Filter
β’ Trying to estimate the state π₯ β β π
of a discrete-
time controlled process that is governed by the
linear stochastic difference equation
π₯ π = π΄π₯ πβ1 + π΅π’ π + π€ πβ1
β’ with a measurement π§ β β π
that is
π§ π = π»π₯ π + π£ π
β’ Random variables w and v represent process and
measurement noise, which normal probability
distributions
π π€ ~ π 0, π
π π£ ~ π(0, π )
16. Process Dynamics
π π+π = π¨π π + π π
state vector
π₯ π β β π
contains the states of the process
17. Process Dynamics
π π+π = π¨π π + π π
state transition matrix
ππ₯π matrix π΄ relates state at time step π to time
step π + 1
18. Process Dynamics
π π+π = π¨π π + π π
process noise
π€ π β β π
models the uncertainty of the process
π π€ π ~ π 0, π
19. Measurement
π π = π―π π + π π
measurement vector
π§ π β β π
is the process measurement
20. Measurement
π π = π―π π + π π
state vector
π₯ π β β π
contains the states of the process
21. Measurement
π π = π―π π + π π
measurement matrix
ππ₯π matrix π» relates state to measurement
22. Measurement
π π = π―π π + π π
measurement noise
π£ π β β π
models the uncertainty of the process
π π£ π ~ π 0, π
24. Chosen Kalman Gain
ππ = πΈ π₯ π β ΰ·π₯ π π₯ π β ΰ·π₯ π
π
(1)
οΏ½Μ π = οΏ½Μ π
β
+ πΎπ π§ π β π» ΰ·π₯ π
β
(2)
β’ The nxm matrix K in equation (2) is chosen to be
the factor that minimizes the a posteriori error
covariance equation (1)
β It can be accomplished by first substituting equation
(2) into the above definition for π π, substituting that
into equation (1), performing the indicated
expectations, taking the derivative of the trace of the
result with respect to K, setting that result equal to
zero, and then solving for K.
β Maybeck 1979; Jacobs 1993; Brown and Hwang 1996
πΎπ = ππ
β
π» π
(π»ππ
β
π» π
+ π )β1
25. The Update iterations
β’ Time Update (Predict)
1. state and covariance prediction
β’ Measurement Update (Correct)
2. Kalman gain correction and state correction
3. covariance correction
Predict Correct
27. Source Code
β’ C#
β KalmanDemo
β’ Python
β pykalman: the dead-simple Kalman Filter, Kalman
Smotther, and EM library
β’ ANSI C
β recovery of motion and 3D structure from a
sequence of images
29. Variations of the Filter
β’ Discrete-Discrete
β’ Continuous-Discrete
β’ Extended Kalman Filter
β’ TBC
30. References
β’ Kalman, R. E. 1960. βA New Approach to Linear Filtering
and Prediction Problemsβ, Transaction of the ASME β
Journal of Basic Engineering, March 1960
β’ Greg Welch, Gary Bishop, "An Introduction to the
Kalman Filter", University of North Carolina at Chapel
Hill Department of Computer Science, 2001
β’ Lindsay Kleeman, "Understanding and Applying Kalman
Filtering", Department of Electrical and Computer
Systems Engineering
β’ Simon D. Levy, βKalman Filter Interactive Tutorialβ
β’ M.S.Grewal, A.P. Andrews, "Kalman Filtering - Theory
and Practice Using MATLAB", Wiley, 2001
β’ MIT Video Lecture on the Kalman filter, YouTube
β’ Kalman filter, Wikipedia