Kalman filter for Beginners

Kalman Filter
for Beginners
Winfred LU

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

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.

The Process Model
• Process Dynamics
𝒙 𝒌 = 𝑨𝒙 𝒌−𝟏 + 𝒘 𝒌−𝟏
• Measurement
𝒛 𝒌 = 𝑯𝒙 𝒌 + 𝒗 𝒌
next state
measurement
previous state
measurement
noise
process
noise
state
dynamic model
measurement model

Simple Example: Airplane Altitude
• Model
𝒙 𝒌 = 𝑨𝒙 𝒌−𝟏 + 𝒘 𝒌−𝟏
𝑎𝑙𝑡𝑖𝑡𝑢𝑑𝑒 𝑐𝑢𝑟𝑟𝑒𝑛𝑡_𝑡𝑖𝑚𝑒
= 0.98 ∗ 𝑎𝑙𝑡𝑖𝑡𝑢𝑑𝑒 𝑝𝑟𝑒𝑣𝑖𝑜𝑢𝑠_𝑡𝑖𝑚𝑒 + 𝑡𝑢𝑟𝑏𝑢𝑙𝑒𝑛𝑐𝑒 𝑝𝑟𝑒𝑣𝑖𝑜𝑢𝑠𝑡𝑖𝑚𝑒
𝒛 𝒌 = 𝒙 𝒌 + 𝒗 𝒌
𝑚𝑒𝑎𝑠𝑢𝑟𝑒𝑑_𝑎𝑙𝑡𝑖𝑡𝑢𝑑𝑒 𝑐𝑢𝑟𝑟𝑒𝑛𝑡_𝑡𝑖𝑚𝑒
= 𝑎𝑙𝑡𝑖𝑡𝑢𝑑𝑒 𝑐𝑢𝑟𝑟𝑒𝑛𝑡_𝑡𝑖𝑚𝑒 + 𝑛𝑜𝑖𝑠𝑒 𝑐𝑢𝑟𝑟𝑒𝑛𝑡_𝑡𝑖𝑚𝑒

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

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

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 𝑥 𝑘

Kalman Filter Equations
• Time Update (Predict)
�̂ 𝑘 = 𝐴�̂ 𝑘−1
• Measurement Update (Correct)
𝐾𝑘 = 𝑝 𝑘−1/(𝑝 𝑘−1 + 𝑟)
�̂ 𝑘 = �̂ 𝑘−1 + 𝐾𝑘(𝑧 𝑘 − �̂ 𝑘−1)
𝑃𝑘 = (1 − 𝐾𝑘) 𝑝 𝑘−1
Predict Correct

Simple Run
𝒌 0 1 2 3 4 5 6 7 8 9
𝑥 𝑘 1000 800 640 512 409.6 327.7 262.1 209.7 167.8 134.2
𝑧 𝑘 908.3 689.7 833 338.8 596.5 350 340.3 152.5 363.9 236.1
𝑝 𝑘 1
�̂ 𝑘 908.3
𝑔 𝑘
𝑥0 = 1000
r = 200
a = 0.8
𝑧 𝑘 = [1000*0.8**i + x
for i,x in enumerate(rnd_u(-200,200,10))]
Predict:
�̂ 𝑘 = 𝑎�̂ 𝑘−1
𝑝 𝑘 = 𝑎𝑝 𝑘−1 𝑎
Update:
�̂ 𝑘 = 𝑝 𝑘/(𝑝 𝑘 + 𝑟)
�̂ 𝑘 = �̂ 𝑘 + 𝑔 𝑘(𝑧 𝑘 − �̂ 𝑘)
𝑝 𝑘 = (1 − 𝑔 𝑘)𝑝 𝑘

Simple Run
𝒌 0 1 2 3 4 5 6 7 8 9
𝑥 𝑘 1000 800 640 512 409.6 327.7 262.1 209.7 167.8 134.2
𝑧 𝑘 908.3 689.7 833 338.8 596.5 350 340.3 152.5 363.9 236.1
𝑝 𝑘 1 0.63 0.41 0.26 0.17 0.11 0.07 0.04 0.02 0.01
�̂ 𝑘 908.3 726.5 581.7 465.2 372.4 297.9 238.4 190.7 152.6 122.1
𝑔 𝑘 0.003 0.002 0.001 0.001 0.000 0.000 0.000 0.000 0.000

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
𝑧 𝑘 = 𝐻𝑥 𝑘 + 𝑣 𝑘

�̂ 𝑘 = 𝐴�̂ 𝑘−1 + 𝐵𝑢 𝑘
𝐾𝑘 = 𝑝 𝑘−1 𝐻/(𝐻𝑝 𝑘−1 𝐻 + 𝑟)
�̂ 𝑘 = �̂ 𝑘−1 + 𝐾𝑘(𝑧 𝑘 − 𝐻�̂ 𝑘−1)
𝑃𝑘 = (1 − 𝐾𝑘 𝐻) 𝑝 𝑘−1
Predict Correct

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

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, 𝑅)

Process Dynamics
𝒙 𝒌+𝟏 = 𝑨𝒙 𝒌 + 𝒘 𝒌
state vector
𝑥 𝑘 ∈ ℛ 𝑛
contains the states of the process

Process Dynamics
𝒙 𝒌+𝟏 = 𝑨𝒙 𝒌 + 𝒘 𝒌
state transition matrix
𝑛𝑥𝑛 matrix 𝐴 relates state at time step 𝑘 to time
step 𝑘 + 1

Process Dynamics
𝒙 𝒌+𝟏 = 𝑨𝒙 𝒌 + 𝒘 𝒌
process noise
𝑤 𝑘 ∈ ℛ 𝑛
models the uncertainty of the process
𝑝 𝑤 𝑘 ~ 𝑁 0, 𝑄

Measurement
measurement vector
𝑧 𝑘 ∈ ℛ 𝑚
is the process measurement

Measurement
state vector
𝑥 𝑘 ∈ ℛ 𝑛
contains the states of the process

Measurement
measurement matrix
𝑚𝑥𝑛 matrix 𝐻 relates state to measurement

Measurement
measurement noise
𝑣 𝑘 ∈ ℛ 𝑚
models the uncertainty of the process
𝑝 𝑣 𝑘 ~ 𝑁 0, 𝑅

Estimate Covariances
a priori estimate error covariance
𝑃𝑘
−
= 𝐸[(𝑥 𝑘 − �̂ 𝑘
−
)(𝑥 𝑘 − �̂ 𝑘
−
) 𝑇
]
a posteriori estimate error covariance
𝑃𝑘 = 𝐸[(𝑥 𝑘 − �̂ 𝑘)(𝑥 𝑘 − �̂ 𝑘) 𝑇
]
– define a priori estimate errors as
• 𝑒 𝑘
− ≡ 𝑥 𝑘 − 𝑥 𝑘
−
• 𝑒 𝑘 ≡ 𝑥 𝑘 − �̂ 𝑘
– error covariance is then
• 𝑃𝑘
−
≡ 𝐸[𝑒 𝑘
− 𝑒 𝑘
−𝑇]
• 𝑃𝑘 ≡ 𝐸[𝑒 𝑘 𝑒 𝑇
𝑘]

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

The Update iterations
1. state and covariance prediction
2. Kalman gain correction and state correction
3. covariance correction
Predict Correct

1. state and covariance prediction
�̂ 𝑘
−
= 𝐴𝑥 𝑘−1
𝑃𝑘
−
= 𝐴𝑃𝑘−1 𝐴 𝑇
+ 𝑄
2. Kalman gain correction and state correction
𝐾𝑘 = 𝑃𝑘
−
𝐻 𝑇
(𝐻𝑃𝑘
−
𝐻 𝑇
+ 𝑅)−1
�̂ 𝑘 = �̂ 𝑘
−
+ 𝐾𝑘 𝑧 𝑘 − 𝐻ො𝑥 𝑘
−
3. covariance correction
𝑃𝑘 = (𝐼 − 𝐾𝑘 𝐻)𝑃𝑘
−

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

Variations of the Filter
• Discrete-Discrete
• Continuous-Discrete
• Extended Kalman Filter
• TBC

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

Kalman filter for Beginners

Recommandé

Recommandé

Contenu connexe

Tendances

Tendances (20)

Similaire à Kalman filter for Beginners

Similaire à Kalman filter for Beginners (20)

Dernier

Dernier (20)

Kalman filter for Beginners