1. Kalman Filtering COS 323, Spring 05

2. Kalman Filtering Assume that results of experiment (i.e., optical flow) are noisy measurements of system stateAssume that results of experiment (i.e., optical flow) are noisy measurements of system state Model of how system evolvesModel of how system evolves Optimal combination of system model and observationsOptimal combination of system model and observations Prediction / correction frameworkPrediction / correction framework Rudolf Emil Kalman Acknowledgment: much of the following material is based on the SIGGRAPH 2001 course by Greg Welch and Gary Bishop (UNC)

3. Simple Example Measurement of a single point z 1Measurement of a single point z 1 Variance  1 2 (uncertainty  1 )Variance  1 2 (uncertainty  1 ) – Assuming Gaussian distribution Best estimate of true positionBest estimate of true position Uncertainty in best estimateUncertainty in best estimate

4. Simple Example Second measurement z 2, variance  2 2Second measurement z 2, variance  2 2 Best estimate of true position?Best estimate of true position? z1z1z1z1 z2z2z2z2

5. Simple Example Second measurement z 2, variance  2 2Second measurement z 2, variance  2 2 Best estimate of true position: weighted averageBest estimate of true position: weighted average Uncertainty in best estimateUncertainty in best estimate

6. Online Weighted Average Combine successive measurements into constantly-improving estimateCombine successive measurements into constantly-improving estimate Uncertainty decreases over timeUncertainty decreases over time Only need to keep current measurement, last estimate of state and uncertaintyOnly need to keep current measurement, last estimate of state and uncertainty

7. Terminology In this example, position is state (in general, any vector)In this example, position is state (in general, any vector) State can be assumed to evolve over time according to a system model or process model (in this example, “nothing changes”)State can be assumed to evolve over time according to a system model or process model (in this example, “nothing changes”) Measurements (possibly incomplete, possibly noisy) according to a measurement modelMeasurements (possibly incomplete, possibly noisy) according to a measurement model Best estimate of state with covariance PBest estimate of state with covariance P

8. Linear Models For “standard” Kalman filtering, everything must be linearFor “standard” Kalman filtering, everything must be linear System model:System model: The matrix  k is state transition matrixThe matrix  k is state transition matrix The vector  k represents additive noise, assumed to have covariance QThe vector  k represents additive noise, assumed to have covariance Q

9. Linear Models Measurement model:Measurement model: Matrix H is measurement matrixMatrix H is measurement matrix The vector  is measurement noise, assumed to have covariance RThe vector  is measurement noise, assumed to have covariance R

10. PV Model Suppose we wish to incorporate velocitySuppose we wish to incorporate velocity

11. Prediction/Correction Predict new statePredict new state Correct to take new measurements into accountCorrect to take new measurements into account

12. Kalman Gain Weighting of process model vs. measurementsWeighting of process model vs. measurements Compare to what we saw earlier:Compare to what we saw earlier:

13. Results: Position-Only Model MovingStill [Welch & Bishop]

14. Results: Position-Velocity Model [Welch & Bishop] MovingStill

15. Extension: Multiple Models Simultaneously run many KFs with different system modelsSimultaneously run many KFs with different system models Estimate probability each KF is correctEstimate probability each KF is correct Final estimate: weighted averageFinal estimate: weighted average

16. Probability Estimation Given some Kalman filter, the probability of a measurement z k is just n -dimensional Gaussian whereGiven some Kalman filter, the probability of a measurement z k is just n -dimensional Gaussian where

17. Results: Multiple Models [Welch & Bishop]

18. Results: Multiple Models [Welch & Bishop]

19. Results: Multiple Models [Welch & Bishop]

20. Extension: SCAAT H can be different at different time steps H can be different at different time steps – Different sensors, types of measurements – Sometimes measure only part of state Single Constraint At A Time (SCAAT)Single Constraint At A Time (SCAAT) – Incorporate results from one sensor at once – Alternative: wait until you have measurements from enough sensors to know complete state (MCAAT) – MCAAT equations often more complex, but sometimes necessary for initialization

21. UNC HiBall 6 cameras, looking at LEDs on ceiling6 cameras, looking at LEDs on ceiling LEDs flash over timeLEDs flash over time [Welch & Bishop]

22. Extension: Nonlinearity (EKF) HiBall state model has nonlinear degrees of freedom (rotations)HiBall state model has nonlinear degrees of freedom (rotations) Extended Kalman Filter allows nonlinearities by:Extended Kalman Filter allows nonlinearities by: – Using general functions instead of matrices – Linearizing functions to project forward – Like 1 st order Taylor series expansion – Only have to evaluate Jacobians (partial derivatives), not invert process/measurement functions

23. Other Extensions On-line noise estimationOn-line noise estimation Using known system input (e.g. actuators)Using known system input (e.g. actuators) Using information from both past and futureUsing information from both past and future Non-Gaussian noise and particle filteringNon-Gaussian noise and particle filtering