2D Image Processing. Bayes filter implementation: Kalman filter

Transcription

1 2D Image Processing Bayes filter implementation: Kalman filter Prof. Didier Stricker Kaiserlautern University DFKI Deutsches Forschungszentrum für Künstliche Intelligenz SA-1 Some slides based on G. Panin and K. Smith

2 Outline Recap: Bayes filter Some facts about Gaussians Kalman filter algorithm Some slides are based on G. Panin, S. Thrun, and K. Smith 2

3 Summary: Bayes filter framework 3

4 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

5 Markov assumption Underlying Assumptions Static world Independent noise Perfect model, no approximation errors 5

6 Markov assumption revisited Reality: sources of error and uncertainty Environment dynamics Approximate computation Random effects Sensor limitations Inaccurate models 6

7 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

8 Representations of PDFs Example: Model the probability distribution of faces appearing in frame t 8

9 Representations of PDFs Example: Model the probability distribution of faces appearing in frame t 9

10 Representations of PDFs Example: Model the probability distribution of faces appearing in frame t Peak 1 Peak 3 Peak 2 10

11 Representations of PDFs One hypothesis, no uncertainty MAP=Maximum a posteriori: we look only for the maximum 11

12 Representations of PDFs One hypothesis + uncertainty 12

13 Representations of PDFs 13

14 Representations of PDFs (non-parametric) Dirac Any number and shape of modes 14

15 Representations of PDFs (non-parametric) Any number and shape of modes 15

16 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

17 Kalman filter Published in 1960 Kalman, R. E A New Approach to Linear Filtering and Prediction Problems, Transaction of the ASME Journal of Basic Engineering, pp (March 1960). Used for many problems Guidance Navigation Autopilots Radar Satellite Weather forecasting 17

18 Univariate Gaussian Exponentiation of a quadratic function 18

19 Multivariate Gaussian 19

20 Matlab example: Multivariate Gaussian 20

21 Matlab example: Samples drawn from a multivariate Gaussian 21

22 Properties of Gaussians 22

23 Properties of Gaussians Standardization 23

24 Matlab example: Two Gaussians with different variance (and mean) 24

25 Handling Gaussians 25

26 Matlab example: General Gaussian vs. transformed normal distribution 26

27 Drawing confidence regions of Gaussians (ex. 2D) Shape: draw a scaled and shifted circle 27

28 Matlab example: Confidence region 28

29 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

30 Product of Gaussians (fusion formula) Is again a Gaussian (though no longer normalized) Univariate case: Recursive formulation Multivariate case: 30

31 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

32 Kalman filter models (state-space model) 32

33 Components of a Kalman Filter 33

34 Kalman filter update: prediction/time update Initial belief Prediction prior Diffusion Uncertainty increases 1D-case Gaussian Process noise covariance 34

35 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

36 Measurement update derivation... Whole derivation given, e.g., in Probabilistic Robotics by Thrun 36

37 Time update derivation... Whole derivation given, e.g., in Probabilistic Robotics by Thrun 37

38 Kalman filter algorithm (same structure as Bayes filter) 1. Kalman_filter(µ t-1, Σ t-1, u t, z t ): 2. Prediction/time update: Correction/measurement update: return µ t, Σ t 38

39 Correction/Measurement update Innovation Covariance decrease Innovation covariance Kalman gain: how much the innovation is taken into account minimizes the posterior state covariance 39

40 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

41 Kalman filter example State vector predict Measurement measure correct 41

42 Kalman filter: initial conditions Initial state predict measure correct 42

43 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

44 Kalman filter: predict covariance Prediction from the motion model (here without control) Update covariance predict measure correct 44

45 Kalman filter: predict covariance If we would only predict predict measure correct 45

46 Kalman filter: measurement predict measure correct 46

47 Kalman filter: predicted measurement Predicted measurement predict measure correct 47

48 Kalman filter: innovation predicted measurement Innovation (residual) and innovation covariance: actual measurement predict predicted covariance measurement covariance correct measure 48

49 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

50 Kalman filter: measurement update Correct the prediction using the measurement state prediction innovation predict measure includes innovation covariance predicted covariance correct 50

51 Kalman filter Predict, measure, correct cycle iteratively estimates the state at each time step predict measure correct 51

52 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): 52

53 General purpose (translational) motion models State Motion model Assumptions 53

54 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

55 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

56 Thank you! 56