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 cs.unc.edu/~tracker/ref/s2001/ /~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 z σ 2 1, z 1 Conditional Density Function x ˆ = z 1 1 N(z 1,σ 2 z1 ) σ ˆ 2 = σ 2 1 z 1-2 0 2 4 6 8 10 12 14
Greg makes a measurement z σ 2 2, z 2 Conditional Density Function x ˆ =...? 2 N(z 1,σ 2 z1 ) σ ˆ 2 =...? 2-2 0 2 4 6 8 10 12 14
Combine estimates
Combine variances
Combined Estimates Conditional Density Function x ˆ = x ˆ 2 N( 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 Not all the difference is error Some may be motion KF can include a motion model Estimate velocity and position 4 6 8 10 12 14
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 ] position n+1 = position n + velocity n * time velocity n+1 = velocity n
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 x y k k state x k x = 1 0 0 1 y state transition A k 1 k 1 xstate k 1 x = Ax + w k k 1 k 1 x + ~ k 1 ~ y k 1 wnoise k 1
Measurement Model u v k k z k measurement H = 0 x 0 H measurement matrix H y x y k k state x k u + ~ k ~ v k noise v k z = Hx + v k k k
Preparation A = 1 0 0 1 State Transition Q Q E w w T xx = { 0 }= 0 Q R R E v v T xx = { 0 }= 0 R yy yy Process Noise Covariance Measurement Noise Covariance
Initialization x = Hz 0 0 0 P = ε 0 0 ε
PREDICT x k k = Axk 1 P = AP A + Q k 1 transition uncertainty T
CORRECT ( ) k k k k x = x + K z Hx P = I actual KH P k ( ) k predicted ( ) T T 1 k k K = P H HP H + R denominator (measurement space)
Summary x k = Axk 1 T P = AP A + Q k k 1 ( ) T T K = P H HP H + R 1 k k ( ) x = x + K z Hx k k k k k = ( ) k P I KH P
Results: XY Track 0.9 0.8 0.7 y [meters] 0.6 0.5 0.4 Truth Estimate 0.3 0.6 0.8 1 1.2 1.4 1.6 x [meters]
Y Track: Moving then Still 1 0.9 0.8 y [meters] 0.7 0.6 0.5 0.4 Truth Estimate 0.3 0.2 0 5 10 15 20 25 30 Time [seconds]
Motion-Dependent Performance 0.72 0.715 0.71 0.705 0.7 0.695 0.69 0.685 Truth Estimate Measurement 0.292 0.291 0.29 0.289 0.288 0.287 0.286 0.285 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 significant latency when moving 1 0.9 0.8 0.7 0.6 0.5 0.4 relatively smooth when not 0.3 0.2 0 5 10 15 20 25 30
Example: 2D Position-Velocity (PV Model)
Process Model (PV) state transition 1 0 dt 0 0 1 0 dt 0 0 1 0 0 0 0 1 state x y dx dt dy dt
Measurement Model (Same) measurement matrix H 0 0 0 x 0 H 0 0 y state x y dx dt dy dt
Different Performance 0.72 0.715 0.71 0.705 0.7 0.695 0.69 0.685 Truth Estimate Measurement 6.8 7 7.2 7.4 7.6 7.8 8 8.2 8.4 8.6 0.291 0.29 0.289 0.288 0.287 0.286 0.285 0.284 21.8 22 22.2 22.4 22.6 22.8 23 23.2 improved latency when moving 1 0.9 0.8 0.7 0.6 0.5 0.4 relatively noisy when not 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 d T τ ρ τ ρ λ dt dt = [ ] τ = ρ = dτ = dt dρ = dt λ = translation (3D) rotation (3D) linear velocity (3D) angular velocity (3D) LED position (3D)
Non-Linear Measurement Model Non-Linear Measurement Model u v c c c c x z y z = c c c V x y z = ( ) ( ) rotate ρ λ τ view matrix
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 P 0 = ~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 t estimate gain Can t t handle rotations (angles in state) Can t 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 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 dt = Fx + Q c The sampled (discrete) Q is dt Q = e Fτ Q e F T τ dτ d c 0
Example: 2D PV Model For continuous model dx dt = 0 1 x + 0 0 0 0 0 q The sampled (discrete) Q is dt q Qd = dt 2 q 3 dt q 2 2 dt q 3 2
Parameter Optimization "Clean" and realistic state data Measurement model Truth True measurement data + Noise True State Error ( Cost) Estimated State - Kalman filter
Multiple-Model Configurations Off or On-Line Model Selection
Off-Line Model Selection simulated measurement sequence Z 1,Z 2,,Z k Optimizer 1 Optimizer 2 Optimizer n
On-Line Multiple-Model Estimation Actual meas. seq. Z* p(µ 1 z, µ1 ) KFµ p(µ 1 1 z, ) µ1 KFµ 2 KFµ n ~ X µ1 ~ X µ2 ~ X µn p(µ 2 z, ) µ2 p(µ n z, ) µn Σ x ( = { x µn k, P k, H, R }
Probability of Model µ For model µ with Π ={ xphr,,, } µ 1 ( z Hx) C ( z Hx) T p( 1 1 µ z, Π )= µ e 2 ( n π C ) 2 2 where C = HPH T + R
Final Combined Estimate ) x = µ xƒ µ ν p( µ z, Π ) µ p( ν z, Π ) ν
Example: P/PV Multiple-Model 1 0.9 0.8 Truth Estimate 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 y [meters] 0.9 0.8 0.7 0.6 0.5 0.4 0.3 0.2 0 5 10 15 20 25 30 1 Time [seconds] p(µ P z,π µ P ) 0.9 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 0.715 0.71 Truth ooooo P Estimate +++++ PV Estimate MM Estimate 0.705 0.7 0.695 0.69 1 0.9 0.8 0.685 0.7 0.6 0.5 0.4 0.3 0.2 0 5 10 15 20 25 30 0.68 7 7.5 8 8.5 Time [seconds]
Smooth When Still 0.29 0.2895 0.289 Truth ooooo P Estimate +++++ PV Estimate MM Estimate 0.2885 0.288 0.2875 0.287 1 0.9 0.8 0.7 0.6 0.5 0.4 0.3 0.2 0 5 10 15 20 25 30 0.2865 0.286 0.2855 23.2 23.4 23.6 23.8 24 24.2 24.4 24.6 24.8 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