The Kalman Filter ImPr Talk

Transcription

1 The Kalman Filter ImPr Talk Ged Ridgway Centre for Medical Image Computing November, 2006

2 Outline What is the Kalman Filter? State Space Models Kalman Filter Overview Bayesian Updating of Estimates Kalman Filter Equations Extensions

3 What is the Kalman Filter for? State estimation in Dynamical Systems

4 What is the Kalman Filter for? State estimation in Dynamical Systems In particular for discrete-time models with continuous, possibly hidden, state

5 What is the Kalman Filter for? State estimation in Dynamical Systems In particular for discrete-time models with continuous, possibly hidden, state It is optimal for Linear Gaussian systems

6 Why filter? The KF is a state-space filter

7 Why filter? The KF is a state-space filter Noisy measurements cleaned state estimate Estimates are updated online with each new measurement

8 Why filter? The KF is a state-space filter Noisy measurements cleaned state estimate Estimates are updated online with each new measurement Equivalent to an (optimally) adaptive low-pass (IIR) filter

9 Why Kalman? (!) R. E. Kalman A new approach to linear filtering and prediction problems. Journal of Basic Engineering, 82(1):35-45, 1960.

10 Why Kalman? (!) R. E. Kalman A new approach to linear filtering and prediction problems. Journal of Basic Engineering, 82(1):35-45, Rudolf Emil Kalman, born in Budapest, Hungary, May 19, 1930.

11 Outline What is the Kalman Filter? State Space Models Kalman Filter Overview Bayesian Updating of Estimates Kalman Filter Equations Extensions

12 State space examples - dynamical system Motion of a particle (in 1D) constant (but noisy) acceleration

13 State space examples - dynamical system Motion of a particle (in 1D) constant (but noisy) acceleration s k = s k 1 + ṡ k 1 T s k 1 ( T ) 2 ṡ k = ṡ k 1 + s k 1 T s k = s k 1

14 State space examples - dynamical system Motion of a particle (in 1D) constant (but noisy) acceleration s k = s k 1 + ṡ k 1 T s k 1 ( T ) 2 + ɛ k ṡ k = ṡ k 1 + s k 1 T + ɛ k s k = s k 1 + ɛ k

15 State space examples - dynamical system Motion of a particle (in 1D) constant (but noisy) acceleration s k = s k 1 + ṡ k 1 T s k 1 ( T ) 2 + ɛ k ṡ k = ṡ k 1 + s k 1 T + ɛ k s k = s k 1 + ɛ k s k 1 T ( T ) 2 /2 ṡ k = 0 1 T s k s k 1 ṡ k 1 s k 1 + w k

16 State space examples - dynamical system Motion of a particle (in 1D) constant (but noisy) acceleration s k = s k 1 + ṡ k 1 T s k 1 ( T ) 2 + ɛ k ṡ k = ṡ k 1 + s k 1 T + ɛ k s k = s k 1 + ɛ k s k 1 T ( T ) 2 /2 ṡ k = 0 1 T s k s k 1 ṡ k 1 s k 1 + w k x k = Fx k 1 + w k

17 State space examples - dynamical system Motion of a particle (in 1D) constant (but noisy) acceleration s k = s k 1 + ṡ k 1 T s k 1 ( T ) 2 + ɛ k ṡ k = ṡ k 1 + s k 1 T + ɛ k s k = s k 1 + ɛ k s k 1 T ( T ) 2 /2 ṡ k = 0 1 T s k s k 1 ṡ k 1 s k 1 + w k x k = Fx k 1 + w k w k N (w k 0, Q)

18 State space examples - time-series

19 State space examples - time-series Autoregressive AR(n) model s k s k 1 s k 2. s k (n 1) = a 1 a 2 a 3... a n s k 1 s k 2 s k 3. s k n + ɛ k

20 State space examples - time-series Autoregressive AR(n) model s k a 1 a 2 a 3... a n s k s k 2 = s k (n 1) s k 1 s k 2 s k 3. s k n ɛ k σ x k = Fx k 1 + w k Q k =

21 The state space model x k R n z k R m u k R p vector of hidden state variables vector of observations vector of control inputs Linear x k = F k x k 1 + B k u k z k = H k x k (1)

22 The state space model x k R n z k R m u k R p vector of hidden state variables vector of observations vector of control inputs Linear Gaussian x k = F k x k 1 + B k u k + w k z k = H k x k + v k w k N (w k 0, Q k ) v k N (v k 0, R k ) (1)

23 The state space model

24 Outline What is the Kalman Filter? State Space Models Kalman Filter Overview Bayesian Updating of Estimates Kalman Filter Equations Extensions

25 How does the KF work? Recursive (online) updating of current state estimate and precision (covariance)

26 How does the KF work? Recursive (online) updating of current state estimate and precision (covariance) 1. Using the system model

27 How does the KF work? Recursive (online) updating of current state estimate and precision (covariance) 1. Using the system model Predict the next state Estimate the new (reduced) precision

28 How does the KF work? Recursive (online) updating of current state estimate and precision (covariance) 1. Using the system model Predict the next state Estimate the new (reduced) precision 2. Using the new measurement

29 How does the KF work? Recursive (online) updating of current state estimate and precision (covariance) 1. Using the system model Predict the next state Estimate the new (reduced) precision 2. Using the new measurement Correct the state estimate Update the (increased) precision

30 How is it derived? Two equivalent alternatives

31 How is it derived? Two equivalent alternatives 1. Classical

32 How is it derived? Two equivalent alternatives 1. Classical Show form of Kalman gain update is unbiased Derive expressions for posterior covariance (MSE) Analytically find optimal Kalman gain

33 How is it derived? Two equivalent alternatives 1. Classical Show form of Kalman gain update is unbiased Derive expressions for posterior covariance (MSE) Analytically find optimal Kalman gain 2. Bayesian

34 How is it derived? Two equivalent alternatives 1. Classical Show form of Kalman gain update is unbiased Derive expressions for posterior covariance (MSE) Analytically find optimal Kalman gain 2. Bayesian Treat previous estimates as prior Measurement model gives likelihood Derive posterior distribution

35 Outline What is the Kalman Filter? State Space Models Kalman Filter Overview Bayesian Updating of Estimates Kalman Filter Equations Extensions

36 Bayesian updating A simple example p(x) = N ( ) x ˆx, σw 2 prior z = x + v v N (0, σv) 2 ( ) p(z x) = N z x, σv 2 likelihood

37 Bayesian updating A simple example p(x) = N ( ) x ˆx, σw 2 prior z = x + v v N (0, σv) 2 ( ) p(z x) = N z x, σv 2 likelihood p(x z) p(z x) p(x) from Bayes

38 Bayesian updating p(z x) p(x) = N ( ) ( ) z x, σv 2 N x ˆx, σw 2

39 Bayesian updating p(z x) p(x) = N ( ) ( ) z x, σv 2 N x ˆx, σw 2 log (p(z x) p(x)) = (z x)2 σ 2 v + (x ˆx)2 σ 2 w +

40 Bayesian updating p(z x) p(x) = N log (p(z x) p(x)) = ( ) ( ) z x, σv 2 N x ˆx, σw 2 (z x)2 σ 2 v = x 2 ( 1 σ 2 v (x ˆx)2 + σw ) ( z σw 2 2x σv 2 + ˆx ) σw 2 +

41 Bayesian updating p(z x) p(x) = N log (p(z x) p(x)) = ( ) ( ) z x, σv 2 N x ˆx, σw 2 (z x)2 σ 2 v (x ˆx)2 + σw 2 + ( 1 = x 2 σv ) ( z σw 2 2x σv 2 = x 2 /a 2bx + + ˆx ) σw 2 +

42 Bayesian updating p(z x) p(x) = N log (p(z x) p(x)) = ( ) ( ) z x, σv 2 N x ˆx, σw 2 (z x)2 σ 2 v (x ˆx)2 + σw 2 + ( 1 = x 2 σv ) ( z σw 2 2x σv 2 = x 2 /a 2bx + = 1 a (x ab)2 + + ˆx ) σw 2 +

43 Bayesian updating (Information form) log (p(x z)) 1 a (x ab)2 +

44 Bayesian updating (Information form) log (p(x z)) 1 a (x ab)2 + p(x z) = N (x ab, a) where 1 a = 1 σv σw 2 b = z σv 2 + ˆx σw 2

45 Bayesian updating (Information form) log (p(x z)) 1 a (x ab)2 + p(x z) = N (x ab, a) where 1 a = 1 σv σw 2 b = z σv 2 + ˆx σw 2 Note prior and measurement information add the posterior mean is the information-weighted mean

46 Bayesian updating (Information form) Summary 1 a = 1 σv 2 posterior cov = a ( z posterior mean = a + 1 σ 2 w σ 2 v + ˆx ) σw 2 (2)

47 Bayesian updating (Kalman-gain form) Now rearrange, for reasons that might become clear later... ( 1 a = σ 2 v + 1 ) 1 σw 2

48 Bayesian updating (Kalman-gain form) Now rearrange, for reasons that might become clear later... ( 1 a = σ 2 v + 1 ) 1 σw 2 = σ2 wσ 2 v σ 2 w + σ 2 v = σ2 w(σ 2 w + σ 2 v) σ 4 w σ 2 w + σ 2 v = σ 2 w σ4 w σ 2 v + σ 2 w

49 Bayesian updating (Kalman-gain form) Now rearrange, for reasons that might become clear later... ( 1 a = σ 2 v + 1 ) 1 σw 2 = σ2 wσ 2 v σ 2 w + σ 2 v = σ2 w(σ 2 w + σ 2 v) σ 4 w σ 2 w + σ 2 v = σ 2 w σ4 w σ 2 v + σ 2 w = (1 k)σ 2 w σ2 w k = σv 2 + σw 2

50 Bayesian updating (Kalman-gain form) Bear with me... a = (1 k)σ 2 w σ2 w k = σw 2 + σv 2

51 Bayesian updating (Kalman-gain form) Bear with me... a = (1 k)σ 2 w k = (1 k) = σ2 w σ 2 w + σ 2 v σ 2 v σ 2 w + σ 2 v = k σ2 v σ 2 w

52 Bayesian updating (Kalman-gain form) Bear with me... a = (1 k)σ 2 w k = σ2 w σ 2 w + σ 2 v σ 2 v (1 k) = σw 2 + σv 2 = k σ2 v σw ( 2 z ab = (1 k)σw 2 σv 2 + ˆx ) σw 2 = (1 k)σw 2 z + (1 k)ˆx σ 2 v = kz + ˆx k ˆx = ˆx + k(z ˆx)

53 Bayesian updating (Kalman-gain form) Summary σ2 w k = σw 2 + σv 2 posterior cov = (1 k)σ 2 w posterior mean = ˆx + k(z ˆx) (3)

54 Outline What is the Kalman Filter? State Space Models Kalman Filter Overview Bayesian Updating of Estimates Kalman Filter Equations Extensions

55 Derivation outline The prediction step is straight-forward. From the model x k = F k x k 1 + B k u k + w k

56 Derivation outline The prediction step is straight-forward. From the model x k = F k x k 1 + B k u k + w k ˆx k k 1 = F k ˆx k 1 k 1 + B k u k

57 Derivation outline The prediction step is straight-forward. From the model x k = F k x k 1 + B k u k + w k ˆx k k 1 = F k ˆx k 1 k 1 + B k u k P k k 1 = F k P k 1 k 1 F T k + Q k

58 Derivation outline The prediction step is straight-forward. From the model x k = F k x k 1 + B k u k + w k ˆx k k 1 = F k ˆx k 1 k 1 + B k u k P k k 1 = F k P k 1 k 1 F T k + Q k This gives us the time-k prior mean and covariance in terms of the time-(k 1) posteriors.

59 Derivation outline The correction step is algebraically tricky, but conceptually very similar to the simple example of Bayesian updating. (5)

60 Derivation outline The correction step is algebraically tricky, but conceptually very similar to the simple example of Bayesian updating. p(x k ˆx k k 1, z k ) p(z k x k, ˆx k k 1 ) p(x k ˆx k k 1 ) (5)

61 Derivation outline The correction step is algebraically tricky, but conceptually very similar to the simple example of Bayesian updating. p(x k ˆx k k 1, z k ) p(z k x k, ˆx k k 1 ) p(x k ˆx k k 1 ) p(z k x k ) p(x k ˆx k k 1 ) (5)

62 Derivation outline The correction step is algebraically tricky, but conceptually very similar to the simple example of Bayesian updating. p(x k ˆx k k 1, z k ) p(z k x k, ˆx k k 1 ) p(x k ˆx k k 1 ) p(z k x k ) p(x k ˆx k k 1 ) N (z k H k x k, R k ) N ( x k ˆx k k 1, P k k 1 ) (4) (5)

63 Derivation outline The correction step is algebraically tricky, but conceptually very similar to the simple example of Bayesian updating. p(x k ˆx k k 1, z k ) p(z k x k, ˆx k k 1 ) p(x k ˆx k k 1 ) p(z k x k ) p(x k ˆx k k 1 ) N (z k H k x k, R k ) N ( x k ˆx k k 1, P k k 1 ) p(x k ˆx k k 1, z k ) = N ( x k ˆx k k, P k k ) (4) (5)

64 Derivation outline The mean and covariance of the time-k posterior in (5) can be derived in information form by completing the square of the quadratic form in x that arises from (4).

65 Derivation outline Rearranging into Kalman-gain form then requires some fiddly algebra and the following two matrix inversion identities

66 Derivation outline Rearranging into Kalman-gain form then requires some fiddly algebra and the following two matrix inversion identities (A + BCD) 1 = A 1 A 1 B(C 1 + DA 1 B) 1 DA 1 (6) (A + BCD) 1 BC = A 1 B(C 1 + DA 1 B) 1 (7)

67 Results (information form) ˆx k k 1 = F k ˆx k 1 k 1 + B k u k P k k 1 = F k P k 1 k 1 F T k + Q k

68 Results (information form) ˆx k k 1 = F k ˆx k 1 k 1 + B k u k P k k 1 = F k P k 1 k 1 F T k + Q k P 1 k k = P 1 k k 1 + HT k R 1 k H k ˆx k k = P k k (P 1 k k 1ˆx k k 1 + H T k R 1 k z k ) (8) (Compare (2) from before)

69 Results (Kalman-gain form) ˆx k k 1 = F k ˆx k 1 k 1 + B k u k P k k 1 = F k P k 1 k 1 F T k + Q k

70 Results (Kalman-gain form) ˆx k k 1 = F k ˆx k 1 k 1 + B k u k P k k 1 = F k P k 1 k 1 F T k + Q k K = P k k 1 H T k ( ) 1 R k + H k P k k 1 Hk T P k k = (I KH k )P k k 1 ˆx k k = ˆx k k 1 + K ( z k H k ˆx k k 1 ) (9) (Compare (3), and see kalman update.m)

71 Outline What is the Kalman Filter? State Space Models Kalman Filter Overview Bayesian Updating of Estimates Kalman Filter Equations Extensions

72 Prediction & Smoothing The model allows us to predict ahead of the measurements, at each iteration we maintain the optimal estimate of the future state given the data we have so far.

73 Prediction & Smoothing The model allows us to predict ahead of the measurements, at each iteration we maintain the optimal estimate of the future state given the data we have so far. We can also estimate the state at a point before the current measurement known as fixed lag smoothing.

74 Prediction & Smoothing The model allows us to predict ahead of the measurements, at each iteration we maintain the optimal estimate of the future state given the data we have so far. We can also estimate the state at a point before the current measurement known as fixed lag smoothing. If we first collect all the data and want to process it offline ( fixed interval smoothing ), then a closely related algorithm can be derived.

75 Prediction & Smoothing The model allows us to predict ahead of the measurements, at each iteration we maintain the optimal estimate of the future state given the data we have so far. We can also estimate the state at a point before the current measurement known as fixed lag smoothing. If we first collect all the data and want to process it offline ( fixed interval smoothing ), then a closely related algorithm can be derived. The Rauch Tung Striebel Smoother (or Kalman Smoother) uses a forward-backward algorithm to achieve optimal offline smoothing with two passes of a Kalman-like recursion. (see kalman smoother.m).

76 Nonlinearity For non-linear state space models x k = f k (x k 1, u k ) + w k z k = h k (x k ) + v k

77 Nonlinearity For non-linear state space models x k = f k (x k 1, u k ) + w k z k = h k (x k ) + v k Either, repeatedly linearise about the current point (EKF)

78 Nonlinearity For non-linear state space models x k = f k (x k 1, u k ) + w k z k = h k (x k ) + v k Either, repeatedly linearise about the current point (EKF) Or, non-linearly propagate (deterministic) samples and re-estimate the Gaussian mean and covariance (UKF)

79 Non-Gaussianity If the process or measurement noise isn t normally distributed, then for the correction step p(x k ˆx k k 1, z k ) p(z k x k ) p(x k ˆx k k 1 ) we can sample from the distribution using Markov Chain Monte Carlo methods.

80 Non-Gaussianity If the process or measurement noise isn t normally distributed, then for the correction step p(x k ˆx k k 1, z k ) p(z k x k ) p(x k ˆx k k 1 ) we can sample from the distribution using Markov Chain Monte Carlo methods. This is known as Sequential Monte Carlo (SMC) or Particle Filtering, and is a very powerful technique. See, for example, the Condensation algorithm in computer vision.