introduction to kalman filter -...
TRANSCRIPT
Introduction to Kalman Filter
Po-Chen Wu
Media IC and System Lab
Graduate Institute of Electronics Engineering
National Taiwan University
Outline
• Introduction to Kalman Filter
• Conceptual Overview
• The Theory of Kalman Filter
• Simple Example
• Extended Kalman Filter
Media IC & System Lab Po-Chen Wu (吳柏辰) 2
Takeaways
• What is a Kalman Filter?
• Why do we need Kalman Filters?
Media IC & System Lab Po-Chen Wu (吳柏辰) 3
Outline
• Introduction to Kalman Filter
• Conceptual Overview
• The Theory of Kalman Filter
• Simple Example
• Extended Kalman Filter
Media IC & System Lab Po-Chen Wu (吳柏辰) 4
Introduction
• Recursive data processing algorithm
– Generates optimal estimate of desired
quantities given the set of measurements
– Optimal: For linear system and white Gaussian
errors, Kalman filter is “best” estimate based
on all previous measurements.
– Recursive: doesn’t need to store all previous
measurements and reprocess all data each
time step.
Media IC & System Lab Po-Chen Wu (吳柏辰) 5
The Problem
• System state cannot be measured directly.
• Need to estimate “optimally” from measurements.
Media IC & System Lab Po-Chen Wu (吳柏辰) 6
Measuring
Devices Estimator
Measurement
Error Sources
System State
(desired but not
known)
External
Controls
Observed
Measurements
Optimal
Estimate of
System State
System
Error Sources
System
Black Box
Outline
• Introduction to Kalman Filter
• Conceptual Overview
• The Theory of Kalman Filter
• Simple Example
• Extended Kalman Filter
Media IC & System Lab Po-Chen Wu (吳柏辰) 7
Conceptual Overview (1/9)
• Lost on the 1-dimensional line
• Position – x(t)
• Assume Gaussian distributed measurements
Media IC & System Lab Po-Chen Wu (吳柏辰) 8
x
0 10 20 30 40 50 60 70 80 90 1000
0.02
0.04
0.06
0.08
0.1
0.12
0.14
0.16
Conceptual Overview (2/9)
• Sextant Measurement at 𝑡1: Mean = 𝑧1 and Variance = z1
• Optimal estimate of position is: ොx(t1) = z1
• Variance of error in estimate: x2(t1) = z1
2
• Boat in same position at time t2 - Predicted position is z1
Media IC & System Lab Po-Chen Wu (吳柏辰) 9
Conceptual Overview (3/9)
• So we have the prediction ොy−(t2)
• GPS Measurement at t2: Mean = z2 and Variance = z2
• Need to correct the prediction due to measurement to get ොx(t2)
• Closer to more trusted measurement – linear interpolation?
Media IC & System Lab Po-Chen Wu (吳柏辰) 10
0 10 20 30 40 50 60 70 80 90 1000
0.02
0.04
0.06
0.08
0.1
0.12
0.14
0.16
prediction ොx−(t2)
measurement z(t2)
Conceptual Overview (4/9)
• Corrected mean is the new optimal estimate of position
• New variance is smaller than either of the previous two
variances
Media IC & System Lab Po-Chen Wu (吳柏辰) 11
0 10 20 30 40 50 60 70 80 90 1000
0.02
0.04
0.06
0.08
0.1
0.12
0.14
0.16
corrected optimal
estimate ොx(t2)
prediction ොx−(t2)
measurement z(t2)
Conceptual Overview (5/9)
• Lessons so far:
Media IC & System Lab Po-Chen Wu (吳柏辰) 12
Make prediction based on previous data: ොx−, −
Take measurement: zk, z
Optimal estimate (ොy) = Prediction + (Kalman Gain) * (Measurement - Prediction)
Variance of estimate = Variance of prediction * (1 – Kalman Gain)
Conceptual Overview (6/9)
• At time t3, boat moves with velocity dx
dt= u
• Naïve approach: Shift probability to the right to predict
• This would work if we knew the velocity exactly (perfect model)
Media IC & System Lab Po-Chen Wu (吳柏辰) 13
0 10 20 30 40 50 60 70 80 90 1000
0.02
0.04
0.06
0.08
0.1
0.12
0.14
0.16ොx(t2)
Naïve Prediction ොx−(t3)
0 10 20 30 40 50 60 70 80 90 1000
0.02
0.04
0.06
0.08
0.1
0.12
0.14
0.16
Conceptual Overview (7/9)
• Better to assume imperfect model by adding Gaussian noise
•dx
dt= u + w
• Distribution for prediction moves and spreads out
Media IC & System Lab Po-Chen Wu (吳柏辰) 14
Prediction ොx−(t3)
ොx(t2)
Naïve Prediction ොx−(t3)
• Now we take a measurement at t3
• Need to once again correct the prediction
• Same as before
0 10 20 30 40 50 60 70 80 90 1000
0.02
0.04
0.06
0.08
0.1
0.12
0.14
0.16
Conceptual Overview (8/9)
Media IC & System Lab Po-Chen Wu (吳柏辰) 15
Corrected optimal estimate ොx(t3)
Measurement z(t3)
Prediction ොx−(t3)
• Lessons learnt from conceptual overview:
– Initial conditions (ොxk−1 and k−1)
– Prediction (ොxk− , k
−)
• Use initial conditions and model (eg. constant velocity) to
make prediction
– Measurement (zk)
• Take measurement
– Correction (ොxk , k)
• Use measurement to correct prediction by ‘blending’
prediction and residual – always a case of merging only two
Gaussians
• Optimal estimate with smaller variance
Conceptual Overview (9/9)
Media IC & System Lab Po-Chen Wu (吳柏辰) 16
Outline
• Introduction to Kalman Filter
• Conceptual Overview
• The Theory of Kalman Filter
• Simple Example
• Extended Kalman Filter
Media IC & System Lab Po-Chen Wu (吳柏辰) 17
Theoretical Basis
• Process to be estimated:
• Kalman Filter
18
Process Noise (w) with covariance Q
Measurement Noise (v) with covariance R
Predicted: ොxk− is estimate based on measurements at previous time-steps
Corrected: ොxk has additional information – the measurement at time k
ොxk− = Aොxk−1 + Buk
Pk− = APk−1A
T + Q
Optimal Kalman gain
Predicted (a priori) state estimate
Predicted (a priori) estimate covariance
xk = Axk−1 + Buk +wk
zk = Hxk + vk
ොxk = ොxk− + K(zk − Hොxk
−)
Pk = (1 − KH)Pk−
K = Pk−HT(HPk
−HT + R)−1
Blending Factor
• If we are sure about measurements:
– Measurement error covariance (R) decreases to zero
– K increases and weights residual more heavily than
prediction
• If we are sure about prediction
– Prediction error covariance Pk− decreases to zero
– K decreases and weights prediction more heavily than
residual
Media IC & System Lab Po-Chen Wu (吳柏辰) 19
Theoretical Basis
Media IC & System Lab Po-Chen Wu (吳柏辰) 20
Prediction (Time Update)
(1) Project the state ahead
(2) Project the error covariance ahead
ොxk− = Aොxk−1 + Buk
Pk− = APk−1A
T + Q
Correction (Measurement Update)
(1) Compute the Kalman Gain
(2) Update estimate with measurement zk
(3) Update Error Covariance
ොxk = ොxk− + K(zk − Hොxk
−)
Pk = (1 − KH)Pk−
K = Pk−HT(HPk
−HT + R)−1
Initial estimates for ොxk−1 and Pk−1
Outline
• Introduction to Kalman Filter
• Conceptual Overview
• The Theory of Kalman Filter
• Simple Example
• Extended Kalman Filter
Media IC & System Lab Po-Chen Wu (吳柏辰) 21
Simple Example (1/4)
• Consider a truck on perfectly frictionless, infinitely long straight roads.– Initially the truck is stationary at position 0, but it is buffeted
this way and that by random acceleration.
– We measure the position of the truck every Δt seconds, but these measurements are imprecise; we want to maintain a model of where the truck is and what its velocity is.
– We show here how we derive the model from which we create our Kalman filter.
Media IC & System Lab Po-Chen Wu (吳柏辰) 22
x
Simple Example (2/4)
• The position and velocity of the truck are described by
the linear state space:
𝐱𝑘 =𝑥ሶ𝑥
• We assume that between the (k−1) and k timestep the
truck undergoes a constant acceleration of ak that is
normally distributed, with mean 0 and standard
deviation σa.
Media IC & System Lab Po-Chen Wu (吳柏辰) 23
𝐱𝑘 = 𝐀𝐱𝑘−1 + 𝐆𝑎𝑘
𝐀 =1 Δt0 1
, 𝐆 =Δt2
2Δt
𝐱𝑘 = 𝐀𝐱𝑘−1 +𝐰𝑘
𝐰𝑘~𝑁 0,𝐐
𝐐 = 𝐆𝐆T𝜎𝑎2 =
Δt4
4
Δt3
2Δt3
2Δt2
Simple Example (3/4)
• At each time step, a noisy measurement of the true
position of the truck is made.
• Let us suppose the measurement noise 𝐯𝑘 is also
normally distributed, with mean 0 and standard
deviation σz.
Media IC & System Lab Po-Chen Wu (吳柏辰) 24
𝐳𝑘 = 𝐇𝐱𝑘 + 𝐯𝑘
𝐇 = 𝟏 𝟎𝐯𝑘~𝑁 0,𝐑𝐑 = 𝜎𝑧
2
Simple Example (4/4)
• We know the initial starting state of the truck with perfect
precision, so we initialize
• And to tell the filter that we know the exact position, we give it a
zero covariance matrix:
• If the initial position and velocity are not known perfectly the
covariance matrix should be initialized with a suitably large
number, say L, on its diagonal.
Media IC & System Lab Po-Chen Wu (吳柏辰) 25
ො𝐱0 =00
𝐏0 =0 00 0
( 𝐏0 =𝐿 00 𝐿
)
Outline
• Introduction to Kalman Filter
• Conceptual Overview
• The Theory of Kalman Filter
• Simple Example
• Extended Kalman Filter
Media IC & System Lab Po-Chen Wu (吳柏辰) 26
Extended Kalman Filter
• The basic Kalman filter is limited to a linear
assumption.
• The extended Kalman filter (EKF) is the
nonlinear version of the Kalman filter which
linearizes about an estimate of the current
mean and covariance.
Media IC & System Lab Po-Chen Wu (吳柏辰) 27
xk = Axk−1 + Buk +wk
zk = Hxk + vk
xk = 𝑓(xk−1 + uk) + wk
zk = ℎ(xk) + vk
Theoretical Basis
Media IC & System Lab Po-Chen Wu (吳柏辰) 28
Prediction (Time Update)
(1) Project the state ahead
(2) Project the error covariance ahead
ොxk− = 𝑓(ොxk−1 + uk)
Pk− = AkPk−1Ak
T + Q
Correction (Measurement Update)
(1) Compute the Kalman Gain
(2) Update estimate with measurement zk
(3) Update Error Covariance
ොxk = ොxk− + K(zk − ℎ(ොxk
−))
Pk = (1 − KHk)Pk−
K = Pk−Hk
T(HkPk−Hk
T + R)−1
Initial estimates for ොxk−1 and Pk−1
Ak = ቤ𝜕𝑓
𝜕xොxk−1,uk
Hk = ቤ𝜕ℎ
𝜕xොxk−
Reference
• Bishop, Gary, and Greg Welch. "An introduction to the
Kalman filter." Proc of SIGGRAPH, Course 8.27599-
23175 (2001): 41.
• Michael Williams. "Introduction to Kalman Filters."
Powerpoint Slides, 5 June 2003
Media IC & System Lab Po-Chen Wu (吳柏辰) 29