Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdf · Kalman Filters in 2 hours? Kalman Filters in 2 hours? • Hah! • No magic. • Pretty

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