Deriva'on of The Kalman Filter. Fred DePiero CalPoly State University EE 525 Stochas'c Processes

Transcription

1 Deriva'on of The Kalman Filter Fred DePiero CalPoly State University EE 525 Stochas'c Processes

2 KF Uses State Predic'ons KF es'mates the state of a system Example Measure: posi'on State: [ posi'on velocity accelera'on ]T State is used to predict subsequent measurements which are op'mally combined with new measurements. Process noise is used to describe unpredictable disturbances to the state.

3 Both Measurements and Process Noise Affect State Change Consider effect of process noise f(t) on state State: [p v a]t State changes described by x = F x + G f (t) p v a = p v a + f (t) f(t) = a 1/s 1/s v p F =? G =?

4 State Transi'on Matrix Used in Discrete- Time Implementa'on x k +1 = φ k x k + w k Phi describes determinis'c changes to state A D- T random process describes unpredictable changes to state (w_k) Typically a D- T white sequence is employed. For [p v a]t state, phi can be determined via the equa'ons of mo'on. p k +1 = p k + Δt 0 (v k + a k t) dt

5 Generalized Approach to Finding Star'ng with: Take L{} State Transi'on Matrix Iden'ty helps when factoring scalar Find x(t=delta t) Compare to: x = Fx sx(s) x(t = 0)= FX(s) [si F]X(s) = x(t = 0) X(s) = [si F] 1 x(t = 0) x(t = Δt) = L 1 {[si F] 1 } t= Δt x(t = 0) x k +1 = φ k x k

6 KF Op'mally Combines State Es'mate with New Measurements Op'mality criteria minimizes error in the state es'mate. Op'mal combina'on is defined by (Bob) Kalman as a linear blend - State Update Eq: ˆ x k = ˆ x k + K k (z k ˆ z k ) (State Est) = (State Est prior to measurement) + (Kalman Gain Mtx) * [New meas An'cipated meas]

7 K Gain Matrix Size? Value? x ˆ k = x ˆ + K k (z k z ˆ ) (State Est) = (State Est prior to measurement) + (Kalman Gain Mtx) * [New meas Est meas] Example: P V A Dimension of K? If measurements quite noisy / untrustworthy, do you expect large or small values in K? Entries + / - / zero? No closed form solu'on exists to find K! Values in K (may) converge while filter runs. ˆ x k = position velocity (Nx1), acceleration z k = [ position](mx1)

8 Es'mate Next Measurement Rewrite state update equa'on to include explicit es'mate of next measurement. Using state es'mate: ˆ x k = Want es'mated posi'on: position velocity acceleration H is the Measurement Matrix Dimensions of H? Values for P V A system? H = H is typically constant = H State Update Eq: x ˆ k = x ˆ + K k (z k x ˆ ) Note: x ˆ k and x ˆ k both associated with same itera'on, k k z ˆ k = x ˆ k

9 Seek Op'mal K Gain Matrix K is defined in order to minimize expected error in state es'mate Define the error in the state es'mate e k = x k x ˆ k dimension e_k: Define / Describe expected varia'ons in e_k P_k is a P_k = E{ } True state x_k unknown! Forge ahead

10 How Might We Minimize A Covariance Matrix? P_k has N x N entries Minimize all entries? General form: = 2 σ 1... ρσ 1 σ σ ρσ 1 σ 3... σ 3 Other op'ons? k

11 Op'mize K by Minimizing Trace{ P_k } Flow of Deriva'on: Find P_k P_k includes K_k (also unknown) d Trace{ } = 0 d K k Set to op'mize K Preview: Form of Results (in final version ) Find K_k as func'on of P_k Find P_k+1 as a func'on of P_k (an update eq.)

12 Defining E{ State Error } as P_k Define P_k True state x_k unknown Express x_k in terms of other quan''es? Expose x_k in state update eq State update eq involves K_k (Good!) This establishes a rela'onship between P_k and K_k Both P_k and K_k are needed quan''es = E{e k e T k } = E{(x k x ˆ k )(x k x ˆ k ) T } Express in terms of true measurement, z_k, + noise, v_k ˆ x k = ˆ x k + K k (z k ˆ x k ) Actual meas noise unknown, but covariance of noise can be es'mated Express in terms of true measurement z_k + noise v_k Z_k =

13 Finding e_k, The State Error Rewrite P_k: Using State Update: With: Yields Then for e_k: Simplifying = E{e k e k T } = E{(x k ˆ x k )(x k ˆ x k ) T } x ˆ k = x ˆ + K k (z k x ˆ ) z k = x k + v k x ˆ k = x ˆ + K k ( x k + v k x ˆ ) e k = x k ˆ x k e k = (x k ˆ x k ) K k ( x k + v k ˆ x k ) e k = (x k ˆ x k ) K k (x k ˆ x k ) K k v k

14 Find P_k via E{ State Error } Find P_k Using: Yields: Which produces many cross terms when mul'plied Recall: = E{e k e T k } e k = (x k x ˆ ) K k (x k x ˆ ) K k v k = E{[(x k x ˆ ) K k (x k x ˆ ) K k v k ][(x k x ˆ ) K k (x k x ˆ ) K k v k ] T } (A B) T = B T A T

15 Simplify E{} Simplifying P_k = E{(x k x ˆ )(x k x ˆ ) T } + E{(x k x ˆ )[K k (x k x ˆ )] T } E{(x k ˆ x k )(K k v k ) T } E{(K k )(x k ˆ x k )(x k ˆ x k ) T } +E{(K k )(x k x ˆ )[(K k )(x k x ˆ )] T } +E{(K k )(x k x ˆ )(K k v k ) T } E{(K k v k )(x k ˆ x k ) T } +E{(K k v k )[(K k )(x k ˆ x k )] T } +E{(K k v k )(K k v k ) T } Thus, we have an update equa'on for P_k = (K k ) T (K k ) + (K k ) (K k ) T + K k R k K k T

16 Further Simplify P_k for Homework P_k can be further reduced from = (K k ) T (K k ) + (K k ) (K k ) T + K k R k K k T To = [I (K k )] [I (K k )] T + K k R k K k T (For homework!)

17 Find Tr{P_k} To Use for Op'miza'on Given Trace, Tr{P_k} Tr{ } = Tr{ (K k ) T (K k ) + (K k ) (K k ) T + K k R k K k T } Rearrange to expose K_k, (prior to d/dk) Note Tr{A^T} = Tr{A}, so Thus Tr{ (K k ) T K k ( ) + K k ( T + R k )K k T } Tr{[ (K k ) T ] T } = Tr{(K k ) T } = Tr{(K k ) } = Tr{K k ( )} Tr{ } = Tr{ 2K k ( ) + K k ( T + R k )K k T }

18 Op'mize K_k by Minimizing Tr{P_k} Op'mize K_k by senng dtr{ } dk k = 0 Use Rela'ons: dtr{ab} da Provided A,B square and C symmetric Permits a reduc'on from: = B T dtr{aca T } da = 2AC d dk Tr{ } = d dk Tr{P 2K k ( P ) + K k ( P H T k + R k )K T k } d dk Tr{ } = 2(H K P ) T + 2K K (H K P H T k + R K ) = 0 To: K k = T ( T + R k ) 1

19 Use Op'mal K to Simplify P_k Update Given op'mal K K k = T ( T + R k ) 1 Subs'tute into: = [I (K k )] [I (K k )] T + K k R k K k T To yield a simpler update equa'on for P_k: = [I (K k )] S,ll need to project P_k ahead to P_k+1

20 Need P_k+1 From P_k, for Update Given defini'on of: Thus Where Using state transi'on x k +1 = φ k x k Which gives: = E{e k +1 = E{e k e k T } +1 e k +1 = E{e k +1 e T k +1 } = x k +1 x ˆ k +1 e k +1 e k +1 e k +1 = φ k x k + w k φ k x ˆ k = φ k (x k ˆ x k ) + w k = φ k e k + w k w_k is the process noise e T k +1 } = E{(φ k e k + w k )(φ k e k + w k ) T } = φ k E{e k e k T}φ k T + E{w k w k T } = φ k φ k T + Q k

21 Summary Order of Evalua'on? Linear blend of state est. with new meas. (Eq A ) Defined P, covariance of the error in the state est Found op'mal K to minimize P Yielded (Eq B, C, and D ) x ˆ k = x ˆ + K k (z k x ˆ ) = E{e k e k T } d Tr{ } dk k = 0 K k = T ( T + R k ) 1 = [I (K k )] Appropriate ordering? +1 = φ k φ k T + Q k