kalman filter an introduction to the course 8tracker/media/pdf/siggraph2001_slides_08.pdf · kalman...
TRANSCRIPT
Course 8Course 8
An Introduction to theAn Introduction to the
Kalman FilterKalman Filter
SpeakersSpeakers
Greg WelchGreg WelchGary BishopGary Bishop
Kalman Filters in 2 hours?Kalman Filters in 2 hours?
•• Hah!Hah!
•• No magic.No magic.
•• Pretty simple to apply.Pretty simple to apply.
•• Tolerant of abuse.Tolerant of abuse.
•• Notes are a standalone reference.Notes are a standalone reference.
•• These slides are online atThese slides are online athttp://www.http://www.cscs..uncunc..eduedu/~tracker/ref/s2001//~tracker/ref/s2001/kalmankalman//
Rudolf Emil KalmanRudolf Emil Kalman
•• Born 1930 in HungaryBorn 1930 in Hungary
•• BS and MS from MITBS and MS from MIT
•• PhD 1957 from ColumbiaPhD 1957 from Columbia
•• Filter developed in 1960-61Filter developed in 1960-61
•• Now retiredNow retired
What is a Kalman Filter?What is a Kalman Filter?
•• Just some applied math.Just some applied math.
•• A linear system: f(a+b) = f(a) + f(b).A linear system: f(a+b) = f(a) + f(b).
•• Noisy data in Noisy data in �������� hopefully less noisy out. hopefully less noisy out.•• But delay is the price for filtering...But delay is the price for filtering...
•• Pure KF does not even adapt to the data.Pure KF does not even adapt to the data.
What is it used for?What is it used for?
•• Tracking missilesTracking missiles
•• Tracking heads/hands/drumsticksTracking heads/hands/drumsticks
•• Extracting lip motion from videoExtracting lip motion from video
•• Fitting Bezier patches to point dataFitting Bezier patches to point data
•• Lots of computer vision applicationsLots of computer vision applications
•• EconomicsEconomics
•• NavigationNavigation
A really simple exampleA really simple example
Gary makes a measurementGary makes a measurement
ˆ x 1 = z1
ˆ σ 21 = σ 2z1 14121086420-2
N(z1,σz1
2 )
z1 σ 2z1
,
Conditional Density Function
Greg makes a measurementGreg makes a measurement
Conditional Density Function
z2 σ 2z2
,
ˆ x 2 = ...?
ˆ σ 22 = ...? 14121086420-2
N(z1,σz1
2 )
Combine estimatesCombine estimates
Combine variancesCombine variances
Combined EstimatesCombined Estimates
ˆ x =
ˆ σ 2 = σ 22
ˆ x 2
14121086420-2
Conditional Density Function
N( σ 2)ˆ x,ˆ
Online weighted average!
But suppose we’re movingBut suppose we’re moving
•• Not Not allall the difference is error the difference is error•• Some may be motionSome may be motion•• KF can include a motion modelKF can include a motion model•• Estimate velocity and positionEstimate velocity and position
14121086420-2
Process ModelProcess Model
•• Describes how the Describes how the statestate changes over time changes over time
•• The The statestate for the first example was scalar for the first example was scalar
•• The The processprocess modelmodel was was ““nothing changesnothing changes””
A better model might beA better model might be
•• State is a 2-vector [ position, velocity ]State is a 2-vector [ position, velocity ]
•• positionpositionnn+1+1 = = positionpositionnn + + velocityvelocitynn * time * time
•• velocityvelocitynn+1+1 = = velocityvelocitynn
Measurement ModelMeasurement Model
““What you see from where you areWhat you see from where you are””notnot
““Where you are from what you seeWhere you are from what you see””
Predict ���� CorrectPredict ���� Correct
KF operates byKF operates by
•• Predicting the new state and its uncertaintyPredicting the new state and its uncertainty
•• Correcting with the new measurementCorrecting with the new measurement
predictpredict correctcorrect
Example: 2D Position-OnlyExample: 2D Position-Only
(Greg Welch)(Greg Welch)
Apparatus: 2D TabletApparatus: 2D Tablet
Process ModelProcess Model
x
y
x
y
x
yk
k
k
k
k
k
=
+
−
−
−
−
1 0
0 11
1
1
1
~
~
x Ax wk k k= +− −1 1
xk xk−1A wk−1state statestate
transition noise
Measurement ModelMeasurement Model
z Hx vk k k= +
u
v
H
Hx
y
u
vk
k
x
y
k
k
k
k
=
+
0
0
~
~zk xkH vk
measurement statemeasurementmatrix noise
PreparationPreparation
A =
1 0
0 1
R E v vR
RT xx
yy
= ∗{ } =
0
0
State Transition
ProcessNoiseCovarianceQ E w w
Q
QT xx
yy
= ∗{ } =
0
0MeasurementNoiseCovariance
InitializationInitialization
P0
0
0=
εε
x Hz0 0=
PREDICTPREDICT
x Axk k−
−= 1
P AP A Qk kT−
−= +1
transitionuncertainty
CORRECTCORRECT
x x K z Hxk k k k= + −( )− −
K P H HP H RkT
kT= +( )− − −1
P I KH Pk k= −( ) −
“denominator”(measurement space)
predictedactual
x Axk k−
−= 1
P AP A Qk kT−
−= +1
SummarySummary
x x K z Hxk k k k= + −( )− −
K P H HP H RkT
kT= +( )− − −1
P I KH Pk k= −( ) −
0.6 0.8 1 1.2 1.4 1.6
0.3
0.4
0.5
0.6
0.7
0.8
0.9
Results: XY TrackResults: XY Track
x [meters]
y [m
eter
s]
TruthEstimate
0 5 10 15 20 25 300.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
Y Track: Moving then StillY Track: Moving then Still
Time [seconds]
y [m
eter
s]
TruthEstimate
0 5 10 15 20 25 300.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
Motion-Dependent PerformanceMotion-Dependent Performance
6.8 7 7.2 7.4 7.6 7.8 8 8.2 8.4 8.6
0.685
0.69
0.695
0.7
0.705
0.71
0.715
0.72
21.8 22 22.2 22.4 22.6 22.8 23 23.2
0.284
0.285
0.286
0.287
0.288
0.289
0.29
0.291
0.292
TruthEstimateMeasurement
significantlatency whenmoving…
…relativelysmooth
when not
Example: 2D Position-VelocityExample: 2D Position-Velocity
(PV Model)(PV Model)
Process Model (PV)Process Model (PV)
1 0 0
0 1 0
0 0 1 0
0 0 0 1
dt
dt
x
ydx
dt
dydt
state transition state
Measurement Model (Same)Measurement Model (Same)
H
Hx
y
0 0 0
0 0 0
x
ydx
dt
dydt
measurement matrix state
21.8 22 22.2 22.4 22.6 22.8 23 23.2
0.284
0.285
0.286
0.287
0.288
0.289
0.29
0.291
6.8 7 7.2 7.4 7.6 7.8 8 8.2 8.4 8.6
0.685
0.69
0.695
0.7
0.705
0.71
0.715
0.72
Different PerformanceDifferent Performance
0 5 10 15 20 25 300.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
TruthEstimateMeasurement
improvedlatency whenmoving…
…relativelynoisy
when not
Example: 6D HiBall TrackerExample: 6D HiBall Tracker
(x, y, z, roll, pitch, yaw)(x, y, z, roll, pitch, yaw)
ApparatusApparatus
HiBall with sixoptical sensors
Ceiling panelwith LEDs
State Vector (PV)State Vector (PV)
x ddt
ddt
T= [ ]τ ρ λτ ρ
τ =ρ =
ddt
τ =
λ =d
dtρ =
translation (3D)rotation (3D)
linear velocity (3D)
angular velocity (3D)
LED position (3D)
Non-Linear Measurement ModelNon-Linear Measurement Model
u
v
c c
c cx z
y z
=
c
c
c
Vx
y
z
= ⋅ ( ) ⋅ −( )rotateρ λ τ
view matrix
SCAAT vs. MCAATSCAAT vs. MCAAT
•• Single or Multiple Constraint(s) at a TimeSingle or Multiple Constraint(s) at a Time•• Dimension of the measurementDimension of the measurement
• Nothing about KF mathematics restricts it
• Can process in “batch” or sequential mode
•• SCAATSCAAT• Estimate 15 parameters with 2D measurements
• Temporal improvements
• Autocalibration of LED positions
HiBall InitializationHiBall Initialization
•• Initialize pose using a brute-force (relativelyInitialize pose using a brute-force (relativelyslow) MCAAT approachslow) MCAAT approach
•• Initial velocities = 0Initial velocities = 0
•• Initial process covariance Initial process covariance PP00 = ~cm/degrees = ~cm/degrees
•• Transition to SCAAT Transition to SCAAT KalmanKalman filter filter
Nonlinear SystemsNonlinear Systems
(Gary Bishop)(Gary Bishop)
Kalman Filter assumes linearityKalman Filter assumes linearity
•• Only matrix operations allowedOnly matrix operations allowed
•• Measurement is a linear function of stateMeasurement is a linear function of state
•• Next state is linear function of previousNext state is linear function of previousstatestate
•• CanCan’’t estimate gaint estimate gain
•• CanCan’’t handle rotations (angles in state)t handle rotations (angles in state)
•• CanCan’’t handle projectiont handle projection
Extended Kalman FilterExtended Kalman Filter
Nonlinear Process (Model)Nonlinear Process (Model)• Process dynamics: A becomes a(x)
• Measurement: H becomes h(x)
Filter ReformulationFilter Reformulation• Use functions instead of matrices
• Use Jacobians to project forward, and to relatemeasurement to state
Jacobian?Jacobian?
•• Partial derivative of measurement withPartial derivative of measurement withrespect to staterespect to state
•• If measurement is a vector of length MIf measurement is a vector of length M•• And state has length NAnd state has length N•• Jacobian Jacobian of measurement function will beof measurement function will be
MxN MxN matrix of numbers (not equations)matrix of numbers (not equations)•• Often evaluating h(x) and Often evaluating h(x) and JacobianJacobian(h(x)) at(h(x)) at
the same time cost only a little extrathe same time cost only a little extra
TipsTips
•• DonDon’’t compute giant symbolic t compute giant symbolic Jacobian Jacobian withwitha symbolic algebra packagea symbolic algebra package
•• Do use an automatic method duringDo use an automatic method duringdevelopmentdevelopment
•• Check out tools from optimization packagesCheck out tools from optimization packages
•• Differentiating your function line-by-line isDifferentiating your function line-by-line isusually pretty easyusually pretty easy
New ApproachesNew Approaches
Several extensions are available that workSeveral extensions are available that workbetter than the EKF in some circumstancesbetter than the EKF in some circumstances
System IdentificationSystem Identification
Model Form and ParametersModel Form and Parameters
(Greg Welch)(Greg Welch)
Measurement Noise (R)Measurement Noise (R)
Sampled Process Noise (Q)Sampled Process Noise (Q)
dxdt
Fx Qc= +
For continuous model
Q e Q e ddF
dt
cFT
= ∫ τ τ τ0
The sampled (discrete) Q is
Example: 2D PV ModelExample: 2D PV Model
dxdt
xq
=
+
0 1
0 0
0 0
0
Qdt q dt q
dt q dt qd =
3 2
2
3 2
2
For continuous model
The sampled (discrete) Q is
Parameter OptimizationParameter Optimization
Measurement
model
“Truth”
"Clean" and
realistic
state data
“True”
measurement
data
+Noise Kalman filter
Error ( Cost)
True State
EstimatedState
-
Multiple-Model ConfigurationsMultiple-Model Configurations
Off or On-Line Model SelectionOff or On-Line Model Selection
Off-Line Model SelectionOff-Line Model Selection
Optimizer 1Optimizer 1
Optimizer 2Optimizer 2
Optimizer nOptimizer n
Z1,Z2,…,ZkZ1,Z2,…,Zk
simulatedmeasurement
sequence
simulatedmeasurement
sequence
On-Line Multiple-Model EstimationOn-Line Multiple-Model Estimation
KFµ1KFµ1 p(µ1 | z,∏µ1)p(µ1 | z,∏µ1)
KFµ2KFµ2
KFµnKFµn
Xµ1Xµ1
~~
p(µ2 | z,∏µ2)p(µ2 | z,∏µ2)
Xµ2Xµ2
~~
p(µn | z,∏µn)p(µn | z,∏µn)
XµnXµn
~~
xx
( (
Actualmeas.seq.
Z*
Actualmeas.seq.
Z*ΣΣ
∏µn = { xk, Pk, H, R }∏µn = { xk, Pk, H, R }
Probability of Model µProbability of Model µ
p zC
en
Tz Hx C z Hxµ
πµ,
( ) ( )Π( ) =
( )− − −−1
2 2
112
C HPH RT= +where
Π = { }µ x P H R, , ,For model µ with
Final Combined EstimateFinal Combined Estimate
)x xp z
p z= ( )
( )∑ ∑ƒ,
,µµ
µ
νν
µν
ΠΠ
0 5 10 15 20 25 300.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
Example: P/PV Multiple-ModelExample: P/PV Multiple-Model
Time [seconds]
y [m
eter
s]
TruthEstimate
MME WeightingMME Weighting
0 5 10 15 20 25 300.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
0 5 10 15 20 25 300
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
Time [seconds]
y [m
eter
s]p(
µ P |
z,π µ
P)
Low-Latency During MotionLow-Latency During Motion
7 7.5 8 8.5
0.68
0.685
0.69
0.695
0.7
0.705
0.71
0.715
0 5 10 15 20 25 300.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
Truthooooo P Estimate+++++ PV Estimate
MM Estimate
Time [seconds]
Smooth When StillSmooth When Still
23.2 23.4 23.6 23.8 24 24.2 24.4 24.6 24.8
0.2855
0.286
0.2865
0.287
0.2875
0.288
0.2885
0.289
0.2895
0.29
0 5 10 15 20 25 300.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
Truthooooo P Estimate
+++++ PV EstimateMM Estimate
Time [seconds]
ConclusionsConclusions
Suggestions and ResourcesSuggestions and Resources
(Greg Welch)(Greg Welch)
Many Applications (Examples)Many Applications (Examples)
•• EngineeringEngineering• Robotics, spacecraft, aircraft, automobiles
•• ComputerComputer• Tracking, real-time graphics, computer vision
•• EconomicsEconomics• Forecasting economic indicators
•• OtherOther• Telephone and electricity loads
Kalman Filter Web SiteKalman Filter Web Site
http://www.cs.unc.edu/~welch/kalman/
•• Electronic and printed referencesElectronic and printed references• Book lists and recommendations
• Research papers
• Links to other sites
• Some software
•• NewsNews
Java-Based KF Learning ToolJava-Based KF Learning Tool
• On-line 1D simulation• Linear and non-linear• Variable dynamics
http://www.cs.unc.edu/~welch/kalman/
KF Course Web PageKF 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 RemarksClosing Remarks
•• Try it!Try it!• Not too hard to understand or program
•• Start simpleStart simple• Experiment in 1D
• Make your own filter in Matlab, etc.
•• Note: the Note: the KalmanKalman filter filter ““wants to workwants to work””• Debugging can be difficult
• Errors can go un-noticed
EndEnd