Course 8

An Introduction to the
Kalman Filter
Speakers

Greg Welch
Gary Bishop
Kalman Filters in 2 hours?

• Hah!
• No magic.
• Pretty simple to apply.
• Tolerant of abuse.
• Notes are a standalone reference.
• These slides are online at
http://www.cs.unc.edu/~tracker/ref/s2001/kalman/
Rudolf Emil Kalman

• Born 1930 in Hungary

• BS and MS from MIT
• PhD 1957 from Columbia
• Filter developed in 1960-61
• Now retired
What is a Kalman Filter?

• Just some applied math.

• A linear system: f(a+b) = f(a) + f(b).
• Noisy data in
hopefully less noisy out.
• But delay is the price for filtering...
• Pure KF does not even adapt to the data.
What is it used for?

• Tracking missiles
• Tracking heads/hands/drumsticks
• Extracting lip motion from video
• Fitting Bezier patches to point data
• Lots of computer vision applications
• Economics
• Navigation
A really simple example
Gary makes a measurement

Conditional Density Function

z1 , σ 2
z1

xˆ1 = z1 N(z1,σ z2 )
1

σˆ = σ
2
1
2
z1 -2 0 2 4 6 8 10 12 14
Greg makes a measurement

Conditional Density Function

z2 , σ 2
z2

xˆ2 = ...?
N(z1,σ z2 )
1

σˆ = ...?
2
2 -2 0 2 4 6 8 10 12 14
Combine estimates
Combine variances
Combined Estimates

Conditional Density Function

ˆ σˆ 2)
N( x,
xˆ = xˆ2
σˆ = σ 2
2 2
-2 0 2 4 6 8 10 12 14

Online weighted average!

But suppose we’re moving

-2 0 2 4 6 8 10 12 14

• Not all the difference is error

• Some may be motion
• KF can include a motion model
• Estimate velocity and position
Process Model

• Describes how the state changes over time

• The state for the first example was scalar
• The process model was “nothing changes”
A better model might be
• State is a 2-vector [ position, velocity ]
• positionn+1 = positionn + velocityn * time
• velocityn+1 = velocityn
Measurement Model

“What you see from where you are”

not
“Where you are from what you see”
Predict
Correct

KF operates by
• Predicting the new state and its uncertainty
• Correcting with the new measurement

predict correct
Example: 2D Position-Only

(Greg Welch)
Apparatus: 2D Tablet
Process Model

xk  1 0xk −1  ~ xk −1 
 =   +  
 yk  0 1 yk −1  ~ yk −1 
state
xk
state A xstate
transition k−1 w
noise
k−1

xk = Axk −1 + wk −1
Measurement Model

uk   H x 0  xk  ~ uk 
 =   +  
vk   0 H y  yk  ~ vk 
measurement
zk
measurement H
matrix xk
state vk
noise

z k = Hxk + vk
Preparation

1 0
A =  State Transition
0 1
Q 0  Process
Q = E{w ∗ w } =  Noise
xx

T
Covariance
 0 Qyy 
Rxx 0  Measurement
R = E{v ∗ v } = 
T

Noise
Covariance
 0 Ryy 
Initialization

x0 = Hz0
ε 0
P0 =  
0 ε 
PREDICT

−
x = Axk −1
k
−
P = APk −1 A + Q
k
T

transition
uncertainty
CORRECT

xk = x + K ( zk − Hx
−
k
−
k )
Pk = ( I − KH ) P
actual
k
−
predicted

K = P H ( HP H + R)
− T − T −1
k k

“denominator”
(measurement space)
Summary

K = P H ( HP H + R)
− T − T −1
−
x = Axk −1
k
k k

−
P = APk −1 A + Q
T xk = xk− + K ( zk − Hxk− )
Pk = ( I − KH ) Pk−
k
Results: XY Track

0.9

0.8

0.7
y [meters]

0.6

0.5
Truth
Estimate
0.4

0.3

0.6 0.8 1 1.2 1.4 1.6

x [meters]
Y Track: Moving then Still

0.9

0.8
y [meters]

0.7

0.6

0.5
Truth
0.4
Estimate

0.3

0.2
0 5 10 15 20 25 30
Time [seconds]
Motion-Dependent Performance
0.292
0.72
Truth 0.291
0.715
Estimate 0.29
0.71 Measurement
0.289
0.705
0.288

0.7
0.287

0.695 0.286

0.69 0.285

0.284
0.685

6.8 7 7.2 7.4 7.6 7.8 8 8.2 8.4 8.6 21.8 22 22.2 22.4 22.6 22.8 23 23.2

significant 0.9
1
…relatively
latency when 0.8

0.7 smooth
moving… when not
0.6

0.5

0.4

0.3

0.2
0 5 10 15 20 25 30
Example: 2D Position-Velocity

(PV Model)
Process Model (PV)

state transition state

1 0 dt 0 x
   
0 1 0 dt  y
0 0 1 0 dx dt
  dy 
0 0 0 1  dt 
Measurement Model (Same)

measurement matrix state

x
H x 0 0 0  
  y
0 Hy 0 0 dx dt
dy 
 dt 
Different Performance
0.72
0.291
Truth
0.715
Estimate 0.29

0.71 Measurement 0.289

0.705
0.288

0.7
0.287

0.695
0.286
0.69
0.285
0.685
0.284

6.8 7 7.2 7.4 7.6 7.8 8 8.2 8.4 8.6 21.8 22 22.2 22.4 22.6 22.8 23 23.2

improved 1

0.9
…relatively
latency when 0.8

0.7 noisy
moving… when not
0.6

0.5

0.4

0.3

0.2
0 5 10 15 20 25 30
Example: 6D HiBall Tracker

(x, y, z, roll, pitch, yaw)

Apparatus

HiBall with six

optical sensors

Ceiling panel
with LEDs
State Vector (PV)

x = [τ ρ λ]
dρ T
dτ
dt dt

τ= translation (3D)
ρ= rotation (3D)
dτ = linear velocity (3D)
dt

dt =
dρ
angular velocity (3D)
λ= LED position (3D)
Non-Linear Measurement Model

cx 
 
 
c y = V ⋅ rotate ( ρ ) ⋅ ( λ − τ )
cz  view matrix

u cx cz 
 = 
v cy cz 
SCAAT vs. MCAAT
• Single or Multiple Constraint(s) at a Time
• Dimension of the measurement
• Nothing about KF mathematics restricts it
• Can process in “batch” or sequential mode
• SCAAT
• Estimate 15 parameters with 2D measurements
• Temporal improvements
• Autocalibration of LED positions
HiBall Initialization

• Initialize pose using a brute-force (relatively

slow) MCAAT approach
• Initial velocities = 0
• Initial process covariance P0 = ~cm/degrees
• Transition to SCAAT Kalman filter
Nonlinear Systems

(Gary Bishop)
Kalman Filter assumes linearity

• Only matrix operations allowed

• Measurement is a linear function of state
• Next state is linear function of previous
state
• Can’t estimate gain
• Can’t handle rotations (angles in state)
• Can’t handle projection
Extended Kalman Filter

Nonlinear Process (Model)

• Process dynamics: A becomes a(x)
• Measurement: H becomes h(x)
Filter Reformulation
• Use functions instead of matrices
• Use Jacobians to project forward, and to relate
measurement to state
Jacobian?
• Partial derivative of measurement with
respect to state
• If measurement is a vector of length M
• And state has length N
• Jacobian of measurement function will be
MxN matrix of numbers (not equations)
• Often evaluating h(x) and Jacobian(h(x)) at
the same time cost only a little extra
Tips

• Don’t compute giant symbolic Jacobian with

a symbolic algebra package
• Do use an automatic method during
development
• Check out tools from optimization packages
• Differentiating your function line-by-line is
usually pretty easy
New Approaches

Several extensions are available that work

better than the EKF in some circumstances
 System Identification

Model Form and Parameters

(Greg Welch)
Measurement Noise (R)
Sampled Process Noise (Q)

For continuous model

dx
= Fx + Qc
dt
The sampled (discrete) Q is
dt
Qd = ∫ e Qc e
Fτ F τ
dτ
T

0
Example: 2D PV Model

For continuous model

dx 0 1 0 0
= x +  
dt 0 0 0 q
The sampled (discrete) Q is
dt 3q 3 dt 2 q 2
Qd =  2 
dt q 2 dt q 
Parameter Optimization
Error ( Cost)
“Truth”

-
"Clean" and True State
realistic
state data
Estimated
State

Measurement
“True”
measurement
+ Kalman filter
Noise
model data
Multiple-Model Configurations

Off or On-Line Model Selection

Off-Line Model Selection

simulated Optimizer 1
measurement
sequence
Z11,Z22,…,Zkk
Optimizer 2

Optimizer n
On-Line Multiple-Model Estimation

~
Xµµ1
KFµ1
1
p(µ1 | z,∏µ11)
Actual
meas. ~
Xµµ2
seq.
KFµ2 2
p(µ2 | z,∏µ22)

(
Z* x
Σ
~
Xµµn
KFµn n
p(µn | z,∏µnn)

∏µnn = { xk, Pk, H, R }

Probability of Model µ

For model µ with Πµ = { x, P, H , R}

1
− ( z − Hx )T C −1 ( z − Hx )
p(µ z, Πµ ) =
1 2
e
(2 π C )
n
2

where C = HPH + R T
Final Combined Estimate

)x = x p(µ z, Πµ )
∑ ƒµ p(ν z, Π )
µ ∑ ν
ν
Example: P/PV Multiple-Model

0.9 Truth
Estimate
0.8
y [meters]

0.7

0.6

0.5

0.4

0.3

0.2
0 5 10 15 20 25 30
Time [seconds]
MME Weighting
1

0.9
y [meters]
0.8

0.7

0.6

0.5

0.4

0.3

0.2
0 5 10 15 20 25 30

Time [seconds]
1

0.9
p(µP | z,πµP)

0.8

0.7

0.6

0.5

0.4

0.3

0.2

0.1

0
0 5 10 15 20 25 30
Low-Latency During Motion

Truth
ooooo P Estimate
0.715 +++++ PV Estimate
MM Estimate
0.71

0.705

0.7

0.695

0.69
1

0.9

0.8 0.685
0.7

0.6

0.5
0.68
0.4
7 7.5 8 8.5
0.3

0.2
0 5 10 15 20 25 30
Time [seconds]
Smooth When Still

0.29
Truth
ooooo P Estimate
0.2895 +++++ PV Estimate
0.289
MM Estimate
0.2885

0.288

0.2875

0.287

1
0.2865
0.9

0.8 0.286
0.7

0.6

0.5
0.2855
0.4
23.2 23.4 23.6 23.8 24 24.2 24.4 24.6 24.8
0.3

0.2
0 5 10 15 20 25 30
Time [seconds]
 Conclusions

Suggestions and Resources

(Greg Welch)
Many Applications (Examples)
• Engineering
• Robotics, spacecraft, aircraft, automobiles
• Computer
• Tracking, real-time graphics, computer vision
• Economics
• Forecasting economic indicators
• Other
• Telephone and electricity loads
Kalman Filter Web Site

http://www.cs.unc.edu/~welch/kalman/

• Electronic and printed references

• Book lists and recommendations
• Research papers
• Links to other sites
• Some software
• News
Java-Based KF Learning Tool

• On-line 1D simulation
• Linear and non-linear
• Variable dynamics

http://www.cs.unc.edu/~welch/kalman/
KF Course Web Page

http://www.cs.unc.edu/~tracker/ref/s2001/kalman/index.html

( http://www.cs.unc.edu/~tracker/ )

• Electronic version of course pack (updated)

• Java-Based KF Learning Tool
• KF web page

• See also notes for Course 11 (Tracking)

Closing Remarks
• Try it!
• Not too hard to understand or program
• Start simple
• Experiment in 1D
• Make your own filter in Matlab, etc.
• Note: the Kalman filter “wants to work”
• Debugging can be difficult
• Errors can go un-noticed
End