kalman filter an introduction to the course 8tracker/media/pdf/siggraph2001_slides_08.pdfscaat vs....
TRANSCRIPT
![Page 1: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/1.jpg)
Course 8Course 8
An Introduction to theAn Introduction to the
Kalman FilterKalman Filter
![Page 2: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/2.jpg)
SpeakersSpeakers
Greg WelchGreg WelchGary BishopGary Bishop
![Page 3: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/3.jpg)
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//
![Page 4: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/4.jpg)
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
![Page 5: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/5.jpg)
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.
![Page 6: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/6.jpg)
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
![Page 7: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/7.jpg)
A really simple exampleA really simple example
![Page 8: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/8.jpg)
Gary makes a measurementGary makes a measurement
ˆ x 1 = z1
ˆ σ 21 = σ 2z1 14121086420-2
N(z1,σz1
2 )
z1 σ 2z1
,
Conditional Density Function
![Page 9: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/9.jpg)
Greg makes a measurementGreg makes a measurement
Conditional Density Function
z2 σ 2z2
,
ˆ x 2 = ...?
ˆ σ 22 = ...? 14121086420-2
N(z1,σz1
2 )
![Page 10: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/10.jpg)
Combine estimatesCombine estimates
![Page 11: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/11.jpg)
Combine variancesCombine variances
![Page 12: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/12.jpg)
Combined EstimatesCombined Estimates
ˆ x =
ˆ σ 2 = σ 22
ˆ x 2
14121086420-2
Conditional Density Function
N( σ 2)ˆ x,ˆ
Online weighted average!
![Page 13: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/13.jpg)
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
![Page 14: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/14.jpg)
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
![Page 15: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/15.jpg)
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””
![Page 16: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/16.jpg)
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
![Page 17: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/17.jpg)
Example: 2D Position-OnlyExample: 2D Position-Only
(Greg Welch)(Greg Welch)
![Page 18: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/18.jpg)
Apparatus: 2D TabletApparatus: 2D Tablet
![Page 19: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/19.jpg)
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
![Page 20: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/20.jpg)
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
![Page 21: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/21.jpg)
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
![Page 22: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/22.jpg)
InitializationInitialization
P0
0
0=
εε
x Hz0 0=
![Page 23: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/23.jpg)
PREDICTPREDICT
x Axk k−
−= 1
P AP A Qk kT−
−= +1
transitionuncertainty
![Page 24: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/24.jpg)
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
![Page 25: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/25.jpg)
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= −( ) −
![Page 26: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/26.jpg)
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
![Page 27: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/27.jpg)
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
![Page 28: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/28.jpg)
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
![Page 29: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/29.jpg)
Example: 2D Position-VelocityExample: 2D Position-Velocity
(PV Model)(PV Model)
![Page 30: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/30.jpg)
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
![Page 31: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/31.jpg)
Measurement Model (Same)Measurement Model (Same)
H
Hx
y
0 0 0
0 0 0
x
ydx
dt
dydt
measurement matrix state
![Page 32: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/32.jpg)
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
![Page 33: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/33.jpg)
Example: 6D HiBall TrackerExample: 6D HiBall Tracker
(x, y, z, roll, pitch, yaw)(x, y, z, roll, pitch, yaw)
![Page 34: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/34.jpg)
ApparatusApparatus
HiBall with sixoptical sensors
Ceiling panelwith LEDs
![Page 35: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/35.jpg)
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)
![Page 36: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/36.jpg)
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
![Page 37: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/37.jpg)
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
![Page 38: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/38.jpg)
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
![Page 39: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/39.jpg)
Nonlinear SystemsNonlinear Systems
(Gary Bishop)(Gary Bishop)
![Page 40: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/40.jpg)
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
![Page 41: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/41.jpg)
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
![Page 42: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/42.jpg)
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
![Page 43: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/43.jpg)
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
![Page 44: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/44.jpg)
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
![Page 45: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/45.jpg)
System IdentificationSystem Identification
Model Form and ParametersModel Form and Parameters
(Greg Welch)(Greg Welch)
![Page 46: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/46.jpg)
Measurement Noise (R)Measurement Noise (R)
![Page 47: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/47.jpg)
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
![Page 48: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/48.jpg)
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
![Page 49: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/49.jpg)
Parameter OptimizationParameter Optimization
Measurement
model
“Truth”
"Clean" and
realistic
state data
“True”
measurement
data
+Noise Kalman filter
Error ( Cost)
True State
EstimatedState
-
![Page 50: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/50.jpg)
Multiple-Model ConfigurationsMultiple-Model Configurations
Off or On-Line Model SelectionOff or On-Line Model Selection
![Page 51: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/51.jpg)
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
![Page 52: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/52.jpg)
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 }
![Page 53: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/53.jpg)
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
![Page 54: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/54.jpg)
Final Combined EstimateFinal Combined Estimate
)x xp z
p z= ( )
( )∑ ∑ƒ,
,µµ
µ
νν
µν
ΠΠ
![Page 55: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/55.jpg)
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
![Page 56: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/56.jpg)
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)
![Page 57: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/57.jpg)
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]
![Page 58: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/58.jpg)
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]
![Page 59: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/59.jpg)
ConclusionsConclusions
Suggestions and ResourcesSuggestions and Resources
(Greg Welch)(Greg Welch)
![Page 60: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/60.jpg)
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
![Page 61: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/61.jpg)
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
![Page 62: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/62.jpg)
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/
![Page 63: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/63.jpg)
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)
![Page 64: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/64.jpg)
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
![Page 65: Kalman Filter An Introduction to the Course 8tracker/media/pdf/SIGGRAPH2001_Slides_08.pdfSCAAT vs. MCAAT SCAAT vs. MCAAT • Single or Multiple Constraint(s) at a Time Single or Multiple](https://reader034.vdocuments.us/reader034/viewer/2022051811/6026c46b0c07ba76034bd432/html5/thumbnails/65.jpg)
EndEnd