2D Image Processing Bayes filter implementation: Kalman filter Prof. Didier Stricker Kaiserlautern University http://ags.cs.uni-kl.de/ DFKI Deutsches Forschungszentrum für Künstliche Intelligenz http://av.dfki.de SA-1 Some slides based on G. Panin and K. Smith
Outline Recap: Bayes filter Some facts about Gaussians Kalman filter algorithm Some slides are based on G. Panin, S. Thrun, and K. Smith 2
Summary: Bayes filter framework 3
Summary: Bayes filters Probabilistic tool for recursively estimating the state of a dynamical system from noisy measurements and control inputs. Based on probabilistic concepts such as the Bayes theorem, Theorem of Total Probability (marginalization), and conditional independence. Make a Markov assumption according to which the state is a complete summary of the past. In realworld problems, this assumption is usually an approximation! 4
Markov assumption Underlying Assumptions Static world Independent noise Perfect model, no approximation errors 5
Markov assumption revisited Reality: sources of error and uncertainty Environment dynamics Approximate computation Random effects Sensor limitations Inaccurate models 6
Bayes update rule posterior likelihood motion model posterior at t-1 Provides a general concept Can in the presented form only be implemented for simple estimation problems, requires either or closed form solutions for multiplication and integral restriction to finite state spaces What is missing to be able to use this: Concrete representations for probability density functions Implementable and tractable filter approximations Applicability to continuous estimation problems 7
Representations of PDFs Example: Model the probability distribution of faces appearing in frame t 8
Representations of PDFs Example: Model the probability distribution of faces appearing in frame t 9
Representations of PDFs Example: Model the probability distribution of faces appearing in frame t Peak 1 Peak 3 Peak 2 10
Representations of PDFs One hypothesis, no uncertainty MAP=Maximum a posteriori: we look only for the maximum 11
Representations of PDFs One hypothesis + uncertainty 12
Representations of PDFs 13
Representations of PDFs (non-parametric) Dirac Any number and shape of modes 14
Representations of PDFs (non-parametric) Any number and shape of modes 15
Filter methods (rules-of-thumb) Different characteristics in terms of: Computational efficiency Accuracy of the approximation Ease of implementation Linear Gaussian models Bayes Filter Kalman Filter n 1 Kalman Filter banks Particle Filter Nonlinear models, Gaussian noises (Non)linear models, Gaussian noises, multimodal Highly nonlinear models, non-gaussian noises, multi-modal Unscented Kalman Filter Extended Kalman Filter 16
Kalman filter Published in 1960 Kalman, R. E. 1960. A New Approach to Linear Filtering and Prediction Problems, Transaction of the ASME Journal of Basic Engineering, pp. 35-45 (March 1960). Used for many problems Guidance Navigation Autopilots Radar Satellite Weather forecasting 17
Univariate Gaussian Exponentiation of a quadratic function 18
Multivariate Gaussian 19
Matlab example: Multivariate Gaussian 20
Matlab example: Samples drawn from a multivariate Gaussian 21
Properties of Gaussians 22
Properties of Gaussians Standardization 23
Matlab example: Two Gaussians with different variance (and mean) 24
Handling Gaussians 25
Matlab example: General Gaussian vs. transformed normal distribution 26
Drawing confidence regions of Gaussians (ex. 2D) Shape: draw a scaled and shifted circle 27
Matlab example: Confidence region 28
Tracking with Kalman filters: Gaussians! posterior prior Measurement All PDFs are assumed Gaussian closed-form solutions for integral and product of Gaussians tractable Bayes filter implementation for continuous spaces optimal (minimum variance) estimator for linear Gaussian systems 29
Product of Gaussians (fusion formula) Is again a Gaussian (though no longer normalized) Univariate case: Recursive formulation Multivariate case: 30
Fusion formula derived Maximum of quadratic function à set 1 st derivative to 0 à Mean of resulting distribution Curvature of quadratic function à 2 nd derivative à Inverse of covariance 31
Kalman filter models (state-space model) 32
Components of a Kalman Filter 33
Kalman filter update: prediction/time update Initial belief Prediction prior Diffusion Uncertainty increases 1D-case Gaussian Process noise covariance 34
Kalman filter update: correction/measurement update Initial belief Prediction prior posterior Measurement Gaussian Measurement noise covariance 1D-case Uncertainty decreases Fusion formula including linear functions! 35
Measurement update derivation... Whole derivation given, e.g., in Probabilistic Robotics by Thrun 36
Time update derivation... Whole derivation given, e.g., in Probabilistic Robotics by Thrun 37
Kalman filter algorithm (same structure as Bayes filter) 1. Kalman_filter(µ t-1, Σ t-1, u t, z t ): 2. Prediction/time update: 3. 4. 5. Correction/measurement update: 6. 7. 8. 9. return µ t, Σ t 38
Correction/Measurement update Innovation Covariance decrease Innovation covariance Kalman gain: how much the innovation is taken into account minimizes the posterior state covariance 39
Kalman filter illustration (2D) Notation differs throughout literature In the following illustration by Kevin Smith: Motion model without control input (often not given in visual tracking) State covariance, measurement and process noise covariances are denoted by different symbols Often used notation: hat indicates estimate (rather than true state) P often used to denote state covariance Estimate at time t given t-1 (before including the measurement) 40
Kalman filter example State vector predict Measurement measure correct 41
Kalman filter: initial conditions Initial state predict measure correct 42
Kalman filter: predict mean (const. velocity model) Prediction from the motion model (here without control) Update the mean State transition matrix predict measure correct 43
Kalman filter: predict covariance Prediction from the motion model (here without control) Update covariance predict measure correct 44
Kalman filter: predict covariance If we would only predict predict measure correct 45
Kalman filter: measurement predict measure correct 46
Kalman filter: predicted measurement Predicted measurement predict measure correct 47
Kalman filter: innovation predicted measurement Innovation (residual) and innovation covariance: actual measurement predict predicted covariance measurement covariance correct measure 48
Kalman filter: Kalman gain Kalman gain specifies how much the correction considers the prediction or the measurement predict predicted covariance Innovation covariance correct measure 49
Kalman filter: measurement update Correct the prediction using the measurement state prediction innovation predict measure includes innovation covariance predicted covariance correct 50
Kalman filter Predict, measure, correct cycle iteratively estimates the state at each time step predict measure correct 51
Summary: Kalman filter Pros Efficient Gaussian densities are easy to work with Optimal solution for linear Gaussian systems (minimal variance) Well established method Cons Restricted to Gaussian densities Uni-modal distribution: single hypothesis Only linear, continuous models Readings: Kalman filter introduction by Greg Welch (part about KFs): http://www.cs.unc.edu/~welch/kalman/kalmanintro.html 52
General purpose (translational) motion models State Motion model Assumptions 53
General purpose (translational) motion models State Unconstrained Motion models with control input Assumptions Smaller state, more efficient Control input enters estimate immediately, not filtered 54
General purpose (translational) motion models Ballistic trajectory (gravity modelled as noisy control input) State Motion model Assumptions The model does not have to be physically motivated Basically, we are free to model whatever we want Constrained Brownian motion (in 1D) State Motion model Assumptions Random walk in position 55
Thank you! 56