Kalman Filters. Jonas Haeling and Matthis Hauschild

Transcription

1 Jonas Haeling and Matthis Hauschild Universität Hamburg Fakultät für Mathematik, Informatik und Naturwissenschaften Technische Aspekte Multimodaler Systeme November 9, 2014 J. Haeling and M. Hauschild - 1

2 Table of Contents 1. Motivation 2. The Discrete Kalman Filter 3. Model Process of a Kalman Filter Constant Model Filling Tank Non-linear Model Summary 4. Extended Kalman Filter 5. Conclusion J. Haeling and M. Hauschild - 2

3 Motivation Universität Hamburg Table of Contents 1. Motivation 2. The Discrete Kalman Filter 3. Model Process of a Kalman Filter Constant Model Filling Tank Non-linear Model Summary 4. Extended Kalman Filter 5. Conclusion J. Haeling and M. Hauschild - 3

4 Motivation Universität Hamburg Robot localization scenario A robot drives along a one dimensional road It localizes itself using Odometry Sonar sensor J. Haeling and M. Hauschild - 4

5 Motivation Universität Hamburg Current estimation of position J. Haeling and M. Hauschild - 5

6 Motivation Universität Hamburg Current estimation of position J. Haeling and M. Hauschild - 6

7 Motivation Universität Hamburg Current estimation of position J. Haeling and M. Hauschild - 7

8 Motivation Universität Hamburg Current estimation of position J. Haeling and M. Hauschild - 8

9 Motivation Universität Hamburg Current estimation of position J. Haeling and M. Hauschild - 9

10 Motivation Universität Hamburg History of the Kalman Filter[5] The Kalman Filter is a linear filter producing an optimal estimate of the system state using noisy input data Named after Rudolf Emil Kálmán Born 1930 in Budapest Hungarian-US-American electrical engineer & mathematician Invented in 1960 (with assistance from Richard Bucy) First use: trajectory estimation in the Apollo program Special case of non-linear filter by Stratonovich invented ealier J. Haeling and M. Hauschild - 10

11 Motivation Universität Hamburg Applications of the Kalman Filter Generally position estimation Robotics: robot localization, (moving) object or human tracking Military: navigation of missiles, submarines Aeronautics: position of a plane, attitude control of the ISS Electronics: phase-locked loop Computer graphics: stabilizing depth measurements, fitting Bezier patches J. Haeling and M. Hauschild - 11

12 The Discrete Kalman Filter Table of Contents 1. Motivation 2. The Discrete Kalman Filter 3. Model Process of a Kalman Filter Constant Model Filling Tank Non-linear Model Summary 4. Extended Kalman Filter 5. Conclusion J. Haeling and M. Hauschild - 12

13 The Discrete Kalman Filter The Discrete Kalman Filter[3][1] Tries to estimate the state x R n of a discrete-time controlled process Is an optimal linear filter Incorporates all available data Produces a statistically minimized error Assumes white gaussian noise both for process prediction and measurement J. Haeling and M. Hauschild - 13

14 The Discrete Kalman Filter The Discrete Kalman Filter[3] Time update projects the current state estimate ahead in time Measurement update adjusts the projected estimate by an actual measurement at that time J. Haeling and M. Hauschild - 14

15 The Discrete Kalman Filter Time Update ( Predict ) - Step 1/2[3] State estimation ˆx k = A ˆx k 1 + B u k 1 ˆx k : The observed state at timestep k A: Relates the state at timestep k 1 to the state at k u k 1 : Control input at timestep k 1 B: Relates optional control input to state x J. Haeling and M. Hauschild - 15

16 The Discrete Kalman Filter Time Update ( Predict ) - Step 2/2[3] Error covariance projection P k = A P k 1 A T + Q P k : A priori estimate error covariance P k : A posteriori estimate error covariance A: Relates the state at timestep k 1 to the state at k Q: Process noise covariance J. Haeling and M. Hauschild - 16

17 The Discrete Kalman Filter Time Update ( Predict ) Recap[3] J. Haeling and M. Hauschild - 17

18 The Discrete Kalman Filter Measurement Update ( Correct ) - Step 1/3[3] Kalman Gain computation K k = P k HT (H P k HT + R) 1 K k : Controls the influence of the measurement on the a posteriori state estimation at timestep k P k : A priori estimate error covariance H: Relates measurement to state R: Measurement noise covariance J. Haeling and M. Hauschild - 18

19 The Discrete Kalman Filter Measurement Update ( Correct ) - Step 2/3[3] State estimation update with measurement ˆx k = ˆx k + K k(z k H ˆx k ) ˆx k : A posteriori state estimate ˆx k : The observed state at timestep k K k : Controls the influence of the measurement on the a posteriori state estimation at timestep k z k : Measurement at timestep k H: Relates measurement to state J. Haeling and M. Hauschild - 19

20 The Discrete Kalman Filter Measurement Update ( Correct ) - Step 3/3[3] Error covariance update P k = (I K k H) P k P k : A posteriori estimate error covariance I: Identity matrix K k : Controls the influence of the measurement on the a posteriori state estimation at timestep k H: Relates measurement to state P k : A priori estimate error covariance J. Haeling and M. Hauschild - 20

21 The Discrete Kalman Filter Measurement Update ( Correct ) Recap[3] J. Haeling and M. Hauschild - 21

22 The Discrete Kalman Filter Operation of the Kalman Filter[3] J. Haeling and M. Hauschild - 22

23 The Discrete Kalman Filter Summary: The Discrete Kalman Filter Is used for combining noisy data Is an optimal filter Has a cyclic recursive approach Assumes white gaussian noise Predicts an estimate of the current state ˆx with a measurement scaled through the Kalman gain K J. Haeling and M. Hauschild - 23

24 Model Process of a Kalman Filter Table of Contents 1. Motivation 2. The Discrete Kalman Filter 3. Model Process of a Kalman Filter Constant Model Filling Tank Non-linear Model Summary 4. Extended Kalman Filter 5. Conclusion J. Haeling and M. Hauschild - 24

25 Model Process of a Kalman Filter Model Definition Process[2] The Kalman Filter removes noise by assuming a pre-defined model of a system. 1. Understand the situation 2. Model the state process 3. Model the measurement process 4. Model the noise 5. Test the filter 6. Refine filter J. Haeling and M. Hauschild - 25

26 Universität Hamburg Model Process of a Kalman Filter - Constant Model 1. Understand the situation[2] Task: Measure the level of water in a tank Measurements obtained via floating device Average water level could be changing or static Water could be sloshing or stagnant J. Haeling and M. Hauschild - 26

27 Model Process of a Kalman Filter - Constant Model 2. Model the state process[2] Water level L is constant State ˆx k is the estimate of L Constant model: Ak is 1 for any k 0 Control variables B and u are 0 Reminder: Time Update ˆx k P k = A ˆx k 1 + B u k 1 = A P k 1 A T + Q J. Haeling and M. Hauschild - 27

28 Universität Hamburg Model Process of a Kalman Filter - Constant Model 3. Model the measurement process[2] Float gives us the measurement z k Measurement scale is the same scale as state estimate H = 1 Reminder: Measurement update K k = P k HT (H P k HT + R) 1 ˆx k = ˆx k + K k(z k H ˆx k ) P k = (I K k H) P k J. Haeling and M. Hauschild - 28

29 Model Process of a Kalman Filter - Constant Model 4. Model the noise[2] Error due to process Process variance matrix Q = q Noise from the measurement Measurement variance matrix R = r Noise from the estimation State variance matrix P k = p (scalar) J. Haeling and M. Hauschild - 29

30 Model Process of a Kalman Filter - Constant Model 5. Test the filter[2] Simplified equations: Predict ˆx k P k = ˆx k 1 = P k 1 + q Update K k = P k (P k + r) 1 ˆx k = ˆx k + K k(z k ˆx k ) P k = (1 K k ) P k Filter is completely defined, let s test it! J. Haeling and M. Hauschild - 30

31 Model Process of a Kalman Filter - Constant Model 5. Test the filter[2] True water level L = 1 Start state x 0 arbitrarly initialized to 0 Start variance P 0 is 1000, system noise q = , measurement noise r = 0.1 (z 1 = 0.9) Predict ˆx 1 = 0 P1 = = Update K 1 = ( ) 1 = ˆx 1 = (0.9 0) = P 1 = ( ) = J. Haeling and M. Hauschild - 31

32 Model Process of a Kalman Filter - Constant Model 5. Test the filter[2] Another step: Predict ˆx 2 = P2 = = Hypothetical measurement of z 2 = 0.8 Update K 2 = ( ) 1 = ˆx 2 = ( ) = P 2 = ( ) = J. Haeling and M. Hauschild - 32

33 Model Process of a Kalman Filter - Constant Model 5. Test the filter[2] t ˆx k P k z k K k ˆx k P k J. Haeling and M. Hauschild - 33

34 Model Process of a Kalman Filter - Constant Model 5. Test the filter[2] J. Haeling and M. Hauschild - 34

35 Model Process of a Kalman Filter - Filling Tank Filling Tank Model[2] A 20 % error produced a 5 % inaccuracy But what if the true situation is not static? Static model, but the tank is filling at a constant rate Tank level at time k: L k = L k 1 + f Filling rate f = 0.1 per time step Tank level starts at L 0 = 0 Measurement and process noise remains the same Let s see what happens! J. Haeling and M. Hauschild - 35

36 Model Process of a Kalman Filter - Filling Tank Filling Tank model with q = and r = 0.1[2] t ˆx k P k z k K k ˆx k P k L J. Haeling and M. Hauschild - 36

37 Model Process of a Kalman Filter - Filling Tank Filling Tank model with q = and r = 0.1[2] J. Haeling and M. Hauschild - 37

38 Model Process of a Kalman Filter - Filling Tank Filling Tank model with q = 0.01 and r = 0.1[2] J. Haeling and M. Hauschild - 38

39 Model Process of a Kalman Filter - Filling Tank Filling Tank model with q = 0.1 and r = 0.1[2] J. Haeling and M. Hauschild - 39

40 Model Process of a Kalman Filter - Filling Tank Filling Tank model with q = 1 and r = 0.1[2] J. Haeling and M. Hauschild - 40

41 Model Process of a Kalman Filter - Filling Model A Filling Model[2] You can relax a model by increasing your estimated error But a bad model will not give good estimates! 2. Model the state process State x = (x l, x f ) T where x l is the estimated level and x f the estimated filling rate ( ) 1 k A k = represents the filling tank with timestep k 0 1 B and u still ignored J. Haeling and M. Hauschild - 41

42 Model Process of a Kalman Filter - Filling Model A Filling Model[2] Cannot measure filling rate But noisy measurement of L 3. Model the measurement process Scaling remains the same: H = ( 1, 0 ) z = ( z, 0 ) T J. Haeling and M. Hauschild - 42

43 Model Process of a Kalman Filter - Filling Model A Filling Model[2] 4. Model the noise Measurement process is unchanged: R = r State process is changed: ( ) pl p Estimate error covariance no longer scalar: P = lf ( ) qf /3 q Discrete noise model: Q = f /2 with filling noise q q f /2 f (Q derived from the continuous Q, skipped here) q f p lf p f J. Haeling and M. Hauschild - 43

44 Model Process of a Kalman Filter - Filling Model A Filling Model[2] 5. Test the model Measurement noise r = 0.1 Process noise of q f = , which is quite accurate Initial state x 0 = ( 0, 0 ) T ( ) Initial variance P 0 = True filling rate f = 0.1 per timestep J. Haeling and M. Hauschild - 44

45 Model Process of a Kalman Filter - Filling Model A Filling Model - example[2] J. Haeling and M. Hauschild - 45

46 Model Process of a Kalman Filter - Filling Model A Filling Model with a constant level[2] J. Haeling and M. Hauschild - 46

47 Model Process of a Kalman Filter - Non-linear Model Constant, but sloshing model[2] Another model: Water level is constant, but it is sloshing Sloshing modeled as a sine wave: L = c sin(2 π r k) + l c: scales the amplitude r: cycle rate l: average level We use c = 0.5, r = 0.05, l = 1 What do you notice? J. Haeling and M. Hauschild - 47

48 Model Process of a Kalman Filter - Non-linear Model Constant, but sloshing model - example[2] J. Haeling and M. Hauschild - 48

49 Universität Hamburg Model Process of a Kalman Filter - Summary Summary of the Three Tank Examples[2] Six steps for defining a Kalman Filter model: 1. Understand the situation 2. Model the state process 3. Model the measurement process 4. Model the noise 5. Test the filter 6. Refine filter Filter will fit measurements to provided model May not always be desirable (sloshing could be just noise) Initialization and noise components affect the results Think of the outcome of your filter (linear model works, but lags) An Extended Kalman filter is required to model non-linearity correctly J. Haeling and M. Hauschild - 49

50 Extended Kalman Filter Table of Contents 1. Motivation 2. The Discrete Kalman Filter 3. Model Process of a Kalman Filter Constant Model Filling Tank Non-linear Model Summary 4. Extended Kalman Filter 5. Conclusion J. Haeling and M. Hauschild - 50

51 Extended Kalman Filter Extended Kalman Filter[3] Previously: linear stochastic difference equation Process or measurement may be non-linear KF that linearizes about the current mean and covariance is called an Extended Kalman Filter Uses partial derivatives of the process and measurement function Process with state x R n x k = f (x k 1, u k 1, w k 1 ) Measurement with z R m z k = h(x k, v k ) J. Haeling and M. Hauschild - 51

52 Extended Kalman Filter EKF Time Update Equations[3] State estimation ˆx k = f (ˆx k 1, u k 1, 0) Error covariance projection P k = A k P k 1 A T k + W k Q k 1 W T k Jacobian matrix of partial derivatives of f with respect to x A [i,j] = δf [i] δx [j] (ˆx k 1, u k 1, 0) Jacobian matrix of partial derivatives of f with respect to w W [i,j] = δf [i] δw [j] (ˆx k 1, u k 1, 0) J. Haeling and M. Hauschild - 52

53 Extended Kalman Filter EKF Measurement Update Equations[3] Kalman Gain Computation K k = P k HT K (H k P k HT k + V k R k Vk T ) 1 State estimation update with measurement ˆx k = ˆx k + K k (z k h(ˆx k, 0)) Error covariance update P k = (I K k H k ) P k H [i,j] = δh [i] ( x k, 0) and V δx [i,j] = δh [i] ( x k, 0) [j] δv [j] x: approximate state, w: process noise, v: measurement noise J. Haeling and M. Hauschild - 53

54 Extended Kalman Filter Predict-Correct-Cycle of EKF[3] J. Haeling and M. Hauschild - 54

55 Extended Kalman Filter Summary - Extended Kalman Filter[4][3] EKF are needed when you either have a non-linear process or measurement relationship Uses function f for difference equation and function h for the measurement equation No longer optimal estimator (only in linear cases) Considered by some as the de facto standard for non-linear state estimation Heavily used in navigation systems and GPS J. Haeling and M. Hauschild - 55

56 Conclusion Universität Hamburg Table of Contents 1. Motivation 2. The Discrete Kalman Filter 3. Model Process of a Kalman Filter Constant Model Filling Tank Non-linear Model Summary 4. Extended Kalman Filter 5. Conclusion J. Haeling and M. Hauschild - 56

57 Conclusion Universität Hamburg Advantages and Disadvantages Advantages Optimal if you have a linear system with Gaussian noise Recursive Real-time capable EKF can handle non-linearity Relatively easy to use Wide use in practice speaks for itself Disadvantages Loses optimality in non-linear systems Unimodal because of Gaussians Only one hypothesis Models may be too complex Sensivity analysis required because of imprecisions Or may not be useful at all J. Haeling and M. Hauschild - 57

58 Conclusion Universität Hamburg Comparison to other filters Particle Filter or Sequential Monte Carlo methods Estimate density represented with particles Multimodal Does not require Gaussian noise Often used in complex non-linear models Large state space dimensionality requires lots of particles Hybrid Particle Filter superior for dealing with multi-modal data EKF superior for dealing with updates with little noise Use PF until variance is below a certain level, switch to KF J. Haeling and M. Hauschild - 58

59 Conclusion Universität Hamburg Conclusion The Kalman Filter is a good and easy to use filter to get more reliable output from your sensors The recursive approach makes it usuable for real-time purposes such as in robots But: you have to be able to describe the underlying model properly J. Haeling and M. Hauschild - 59

60 Conclusion Universität Hamburg Thank you for your attention! Jonas Haeling and Matthis Hauschild and Universität Hamburg Fakultät für Mathematik, Informatik und Naturwissenschaften Technische Aspekte Multimodaler Systeme J. Haeling and M. Hauschild - 60

61 Conclusion Universität Hamburg Bibliography [1] Peter Maybeck. Stochastic models, estimation, and control. Air Force Institute of Technology, [2] Ashutosh Saxena. Kalman Filter Applications. Cornell University, cs4758/2012sp/materials/mi63slides.pdf. [3] G. Welch and G. Bishop. An Introduction to the Kalman Filter. University of North Carolina at Chapel Hill, [4] Wikipedia. Extended Kalman filter, http: //en.wikipedia.org/wiki/extended_kalman_filter. [5] Wikipedia. Kalman filter, J. Haeling and M. Hauschild - 61