Greg Welch and Gary Bishop. University of North Carolina at Chapel Hill Department of Computer Science.

Transcription

1 STC Lecture Series An Introduction to the Kalman Filter Greg Welch and Gary Bishop University of North Carolina at Chapel Hill Department of Computer Science UNC Chapel Hill Computer Science Slide 1

2 Where We re Going Introduction & Intuition The Discrete Kalman Filter A Simple Example Variations of the Filter Relevant Applications & References UNC Chapel Hill Computer Science Slide 2

3 The Kalman filter Seminal paper by R.E. Kalman, 1960 Set of mathematical equations Optimal estimator minimum mean square error Versatile Estimation Filtering Prediction Fusion predict correct UNC Chapel Hill Computer Science Slide 3

4 Why a Kalman Filter? Efficient least-squares implementation Past, present and future estimation Estimation of missing states Measure of estimation quality (variance) Robust forgiving in many ways stable given common conditions UNC Chapel Hill Computer Science Slide 4

5 Some Intuition UNC Chapel Hill Computer Science Slide 5

6 First Estimate Conditional Density Function z σ 2 1, z 1 x ˆ = z 1 1 N(z 1,σ 2 z1 ) σ ˆ 2 = σ 2 1 z UNC Chapel Hill Computer Science Slide 6

7 Second Estimate Conditional Density Function z σ 2 2, z 2 x ˆ =...? 2 N(z 2,σ 2 z2 ) σ ˆ 2 =...? UNC Chapel Hill Computer Science Slide 7

8 Combine Estimates x ˆ = [ σ 2 ( σ 2 + σ 2 )]z + [ σ 2 ( σ 2 +σ 2 )]z 2 z2 z1 z2 1 z1 z1 z2 2 = ˆ x + K [ z x ˆ ] where K 2 = σ 2 z1 ( σ 2 + σ 2) z1 z2 UNC Chapel Hill Computer Science Slide 8

9 Combine Variances 1 σ 2 = ( 1 σ 2 )+ 1σ 2 2 z1 z2 ( ) UNC Chapel Hill Computer Science Slide 9

10 Combined Estimate Density Conditional Density Function x ˆ = x ˆ 2 N( x,ˆ ˆ σ 2 ) σ ˆ 2 = σ UNC Chapel Hill Computer Science Slide 10

11 Add Dynamics dx/dt = v + w where v is the nominal velocity w is a noise term (uncertainty) UNC Chapel Hill Computer Science Slide 11

12 Propagation of Density UNC Chapel Hill Computer Science Slide 12

13 Some Details x= Ax+ w z= Hx... UNC Chapel Hill Computer Science Slide 13

14 Discrete Kalman Filter Maintains first two statistical moments process state (mean) z y x error covariance UNC Chapel Hill Computer Science Slide 14

15 Discrete Kalman Filter The Ingredients A discrete process model change in state over time linear difference equation A discrete measurement model relationship between state and measurement linear function Model Parameters Process noise characteristics Measurement noise characteristics UNC Chapel Hill Computer Science Slide 15

16 Necessary Models previous state state dynamic model measurement model next state image plane (u,v) measurement UNC Chapel Hill Computer Science Slide 16

17 The Process Model Process Dynamics Measurement x k+1 = Ax k + w k z k = Hx k + v k UNC Chapel Hill Computer Science Slide 17

18 Process Dynamics x k+1 = Ax k + w k state vector x k R n contains the states of the process UNC Chapel Hill Computer Science Slide 18

19 Process Dynamics x k+1 = Ax k + w k state transition matrix nxn matrix A relates state at time step k to time step k+1 UNC Chapel Hill Computer Science Slide 19

20 Process Dynamics x k+1 = Ax k + w k process noise w k R n models the uncertainty of the process w k ~ N(0, Q) UNC Chapel Hill Computer Science Slide 20

21 Measurement z k = Hx k + v k measurement vector z k R m is the process measurement UNC Chapel Hill Computer Science Slide 21

22 Measurement z k = Hx k + v k state vector UNC Chapel Hill Computer Science Slide 22

23 Measurement z k = Hx k + v k measurement matrix mxn matrix H relates state to measurement UNC Chapel Hill Computer Science Slide 23

24 Measurement z k = Hx k + v k measurement noise z k R m models the noise in the measurement v k ~ N(0, R) UNC Chapel Hill Computer Science Slide 24

25 State Estimates a priori state estimate x ˆ k a posteriori state estimate xˆ k UNC Chapel Hill Computer Science Slide 25

26 Estimate Covariances a priori estimate error covariance P k = E[(x k - ˆ x k )(x k - ˆ x k ) T ] a posteriori estimate error covariance P k = E[(x k - xˆ k )(x k - ˆ x k ) T ] UNC Chapel Hill Computer Science Slide 26

27 Filter Operation Time update (a priori estimates) Project state and covariance forward to next time step, i.e. predict Measurement update (a posteriori estimates) Update with a (noisy) measurement of the process, i.e. correct UNC Chapel Hill Computer Science Slide 27

28 Time Update (Predict) state error covariance UNC Chapel Hill Computer Science Slide 28

29 Measurement Update (Correct) UNC Chapel Hill Computer Science Slide 29

30 Time Update (Predict) a priori state and error covariance xˆ k+1 = Ax k P k+1 = AP k A + Q UNC Chapel Hill Computer Science Slide 30

31 Measurement Update (Correct) Kalman gain K k = P k H T (HP k H T + R) -1 a posteriori state and error covariance xˆ k = xˆ k + K k (z k - Hxˆ k ) P k = (I - K k H)P k UNC Chapel Hill Computer Science Slide 31

32 Filter Operation Time Update (Predict) Measurement Update (Correct) UNC Chapel Hill Computer Science Slide 32

33 A Simple Example Estimating a Constant UNC Chapel Hill Computer Science Slide 33

34 Estimating a Constant A, H, P k, R, z k, and K k are all scalars. In particular, A = 1 H = 1 UNC Chapel Hill Computer Science Slide 34

35 Process Model Process Dynamics Measurement x k+1 = x k z k = x k + v k UNC Chapel Hill Computer Science Slide 35

36 Time Update a priori state and error covariance xˆ k+1 = x k P k+1 = P k UNC Chapel Hill Computer Science Slide 36

37 Measurement Update Kalman gain K k = P k / (P k + R) a posteriori state and error covariance xˆ k = xˆ k + K k (z k - ˆ x k ) P k = (1 - K k )P k UNC Chapel Hill Computer Science Slide 37

38 Simulations z ˆ k and x k P k UNC Chapel Hill Computer Science Slide 38

39 Variations of the Filter Discrete-Discrete (a.k.a. discrete ) Continuous-Discrete Extended Kalman Filter UNC Chapel Hill Computer Science Slide 39

40 Continuous-Discrete The Process (Model) Continuous model of system dynamics Discrete measurement equation Why, When, How Flexibility in prediction Irregularly spaced (discrete) measurements At measurement, integrate state forward (e.g. Runge-Kutta integrator) UNC Chapel Hill Computer Science Slide 40

41 Extended Kalman Filter Nonlinear Model(s) Process dynamics: A becomes a(x,w) Measurement: H becomes h(x,z) Filter Reformulation Use functions instead of matrices Use Jacobians to project forward, and to relate measurement to state UNC Chapel Hill Computer Science Slide 41

42 Relevant Applications Rebo Rebo, Robert K A Helmet-Mounted Virtual Environment Display System, M.S. Thesis, Air Force Institute of Technology. Azuma Azuma, Ronald Predictive Tracking for Augmented Reality, Ph.D. dissertation, The University of North Carolina at Chapel Hill, TR UNC Chapel Hill Computer Science Slide 42

43 Relevant Applications Friedmann et al. Friedmann, Martin, Thad Starner, and Alex Pentland Device Synchronization Using an Optimal Filter, Proceedings of 1992 Symposium on Interactive 3D Graphics (Cambridge MA) Liang et al. Liang, Jiandong, Chris Shaw, and Mark Green. On Temporal- Spatial Realism in the Virtual Reality Environment, Proceedings of the 4th annual ACM Symposium on User Interface Software & Technology, UNC Chapel Hill Computer Science Slide 43

44 Relevant Applications Van Pabst & Krekel Van Pabst, Joost Van Lawick, and Paul F. C. Krekel. Multi Sensor Data Fusion of Points, Line Segments and Surface Segments in 3D Space, TNO Physics and Electronics Laboratory, The Hague, The Netherlands. [cited 19 November 1995]. UNC Chapel Hill Computer Science Slide 44

45 SCAAT Tracking Latency & Rate Simultaneity Assumption SCAAT Observability Family of unobservable systems Calibration VE Tracking, GPS, etc. UNC Chapel Hill Computer Science Slide 45

46 Kalman Filter Papers... Kalman Kalman, R. E A New Approach to Linear Filtering and Prediction Problems, Transaction of the ASME Journal of Basic Engineering, pp (March 1960). Sorenson Sorenson, H. W Least-Squares estimation: from Gauss to Kalman, IEEE Spectrum, vol. 7, pp , July UNC Chapel Hill Computer Science Slide 46

47 Some Books Brown Gelb Jacobs Lewis Introduction to Random Signals and Applied Kalman Filtering (2 nd ) Applied Optimal Estimation Introduction to Control Theory Optimal Estimation with an Introduction to Stochastic Control Theory Maybeck Stochastic Models, Estimation, and Control, Volume 1 UNC Chapel Hill Computer Science Slide 47

48 Further Information Welch & Bishop Welch, Greg and Gary Bishop An Introduction to the Kalman Filter, The University of North Carolina at Chapel Hill, TR UNC Chapel Hill Computer Science Slide 48