Extensions and Variations on the Kalman Filter

Transcription

1 Extensions and Variations on the Kalman Filter The Information Filter - rewriting the KF recursion in terms of Based on the Matrix Inversion Lemma/Woodbury-Sherman-Morrison formula Liewise (A + BD 1 C) 1 = A 1 A 1 B(D + CA 1 B) 1 CA 1 with: A = P 1, B = H T, C = H, D = R = 1 + HT R 1 H, is equivalent to This yields either = Q 1 Q 1 F ( + F T Q 1 or a Riccati Difference Equation for P = P 1 P 1 H T (H P 1 H T + R ) 1 H P 1 P +1 = F P F T + Q, is equivalent to +1 = Q 1 Q 1 F ( + F T Q 1 F ) 1 F T Q 1 P +1 = F [P + F 1 Q F T ]F T, is equivalent to +1 = F 1 F T F 1 F T Q 1/2 [I + Q 1/2 F 1 F T Q 1/2 ] 1 Q 1/2 F 1 F T = F 1 F T F 1 F T Q 1/2 [I + Q 1/2 F 1 F T F ) 1 F T Q 1 Q 1/2 ] 1 Q 1/2 F 1 + H T R 1 H F T + H T R 1 1 H

2 The Information Filter continued The information filter uses the WSM-formula to write a recursion for the information matrix - the inverse of the filtered error covariance The Kalman filter gain has a formulation in terms of the information matrix L = P 1 H T (H P 1 H T + R ) 1 1 = H T R 1 The information filter has a number of applications Handling the case where the initial condition has infinite covariance but still has structure Handling multiple measurements in a single time period Dealing with some numerical issue connected with matrix inversion It is really useful for proving things and captures the idea of information as being the converse of covariance 2

3 Correlated Process and Measurement Noises So far, we have assumed that the noises {w} and {v} were zero-mean, white and mutually uncorrelated - independent since gaussian What should we do if they are correlated but still white? E[w ]=0, E[w w T ]=Q δ,, E[v ]=0, E[v v T ]=R δ,, E[w v T ]=M δ j, Run a modified (but similar) set of recursions ˆx = ˆx +1 + P +1 H T +1(H +1 P +1 H T +1 + R +1 ) 1 (y +1 H +1ˆx +1 ) P = P +1 P +1 H T +1(H +1 P +1 H T +1 + R +1 ) 1 H +1 P +1 ˆx +1 = (F M R 1 P +1 = (F M R 1 H )ˆx + G u + M R 1 y H )P (F M R 1 Accommodates the correlation How is this proven? Homewor! H ) T + Q M R 1 M T There is a need for care as to whether v-1 (Simon) or v (A&M) is correlated with w You get different answers none mysterious 3

4 Correlated Noises continued Why is this important? Some signal models come this way, e.g. AR, ARX, ARMA, etc y + a 1 y 1 + a 2 y a n y n = w State-variable realization x T = y 1 T y 2 T... y n T a 1 a 2... a n 1 a n I x +1 = 0 I I 0 x + I w y = a 1 a 2... a n 1 a n x + w 4

5 Colored Process and Measurement Noises Colored means non-white means correlated over time [Recall our Kalman filtering is done with second-order statistics only] We need to understand how the noise is correlated Equivalent sets of information Correlation function of the noise Spectrum of the noise State-variable realization Φ ww,vv (ω) R w,v (τ) ζ +1 = A ζ + p η +1 = A η + r w = C ζ + q v = C η + s p,q,r,s mutually independent white gaussian with nown covariances P, Q, R, S x +1 = F x + Gu + w y = H x + Ju + v 5

6 Colored Noises Continued x +1 = F x + Gu + w ζ +1 = A ζ + p η +1 = A η + r y = H x + Ju + v w = C ζ + q v = C η + s Aha! ζ +1 A 0 0 ζ 0 I 0 0 x +1 = C F 0 x + G u + 0 I 0 η A η I y = ζ 0 H C x + J u + s η This is a regular Kalman filtering problem with an augmented state p q r Computing the state estimates C ˆζ 1, C ˆη 1 ˆζ 1, ˆη 1 yields via the predictable parts of the noises The innovations are white remember This is a really important aspect of Kalman filtering Somebody needs to be able to describe the signal and the noise and how to tell the two apart Go on! Chec the observability and stabilizability properties 6

7 The Steady-State Kalman filter The Riccati difference equation (RDE) P +1 = F P 1 F T F P 1 H T (H P 1 H T + R ) 1 H P 1 F T + Q The Lyapunov form of the RDE P +1 = (F K H )P 1 (F K H ) T + K R K T + Q The Kalman predictor error equation x +1 = (F K H ) x 1 K v + w We expect the error equation to be stable provide P 1 > 0 and [F K H,Q + K R K T ] is uniformly controllable This follows in the time-invariant case if R>0 and [F, Q] has no uncontrollable modes on the unit circle But, if F K H is uniformly (exponentially) stable then the RDE must also be (doubly) stable 7

8 For the LTI system Steady-state Kalman filtering continued x +1 = Fx + Gu + w Q = Q, R = R y = Hx + Ju + v The Kalman filter is time-varying because of P 0 1, ˆx 0 1 But as The limiting Kalman predictor covariance satisfies P 1 P P P + K K P = FP F T FP H T (HP H T + R) 1 HP F T + Q This is the discrete Algebraic Riccati Equation ([d]are) K = FP H T (HP H T + R) 1 L = P H T (HP H T + R) 1 The steady-state Kalman predictor is ˆx +1 = (F K H)ˆx 1 + K y This is a linear, time-invariant observer The steady-state Kalman filter is ˆx = (I L H)F ˆx + L y +1 8

9 Copied from Simon copied from me and MAP Theorem 23 (p.196) The DARE has a unique positive semidefinite solution if and only if (i) [F,H] is detectable, and (ii) [F-MR -1 H,Q] is stabilizable The corresponding steady state Kalman Filter is exponentially stable Theorem 24 (pp ) The DARE has at least one positive semidefinite solution if and only if (i) [F,H] is detectable, and (ii) [F-MR -1 H,Q] is controllable on the unit circle Exactly one of these solutions results in a stable s.s. Kalman Filter Theorem 25 (p.197) The DARE has at least one positive semidefinite solution if and only if (i) [F,H] is detectable, and (ii) [F-MR -1 H,Q] is controllable on and inside the unit circle Exactly one of these solutions results in a stable s.s. Kalman Filter 9

10 An unstable system with Q=0, R=1 A Really Simple ARE Example x +1 = 2x y = x + v [F,H] = [2,1] is clearly observable and therefore detectable [F,Q] = [2,0] is not stabilizable but its uncontrollable mode is at 2 ARE p = 2 p 2 2p(p + 1) 1 2p +0 p 2 + p = 4p p 2 3p = 0 = 4p 4p2 p +1 = 4p2 +4p 4p 2 p +1 p = 0, 3 Two nonnegative solutions Two Kalman predictors = 2p p +1 =0, 3/2 ˆx +1 = 2ˆx 1 ˆx +1 = (2 3/2)ˆx 1 +3/2 y 10

11 The Hamiltonian (Symplectic) Approach to steady-state KF Real matrix H is Hamiltonian if If H is Hamiltonian and! is an eigenvalue of H then -! is also an eigenvalue Real matrix S is symplectic if If S is symplectic and! is an eigenvalue of S then! -1 is also an eigenvalue The 2n x 2n Hamiltonian matrix is symplectic H = F T QF T JH = H T J T, J = S T JS = J, J = 0 In I n 0 0 In I n 0 F T H T R 1 H F + QF T H T R 1 H 2n n Ψ12 Ψ 22 Suppose H has no eigenvalues on the unit circle Select n eigenvalues and corresponding eigenvectors of H The matrix P = Ψ 22 Ψ 1 12 satisfies the ARE The closed-loop KP eigenvalues are at the n values chosen There are 2n n solutions and one (the maximal one) stabilizes Matlab uses the QZ generalized eigenvalue algorithm to solve AREs 11

12 History (à la Simon) The Continuous-time Kalman filter Follin, Carlton, Hanson, Bucy developed continuous-time KF in late 1950s Kalman independently developed discrete time KF in 1960 Kalman and Bucy published the continuous KF in 1961 Hence, the Kalman-Bucy filter Continuous-time stochastic linear system Itô stochastic calculus wt and vt are standard Brownian motions of intensity I x t x s = t s A τ x τ dτ + t The special calculus is to eep xt Marovian s dx t = A t x t dt + B t u t dt + Q 1 2 t dw t dy t = C t x t dt + R 1 2 t dv t B τ u τ dτ + There is a need for technical care with continuous-time white noise processes E[dw t dwt T ]=Idt t s Q 1 2 τ dw τ For bandlimited noise processes Itô and regular calculus coincide 12

13 Continuous-time Kalman filter continued Linear time-varying gaussian stochastic system dx t = A t x t dt + B t u t dt + Q 1 2 t dw t dy t = C t x t dt + R 1 2 t dv t dyt dt ẋ t = A t x t + B t u t + Q 1 2 t w t =ȳ t = C t x t + R 1 2 t v t Linear continuous-time observer format with a special choice of gain ˆx 0 = E[x 0 ] P 0 = E (x 0 ˆx 0 )(x 0 ˆx 0 ) T K t = P t Ct T Rt 1 ˆx t = (A t K t C t )ˆx t + B t u t + K t ȳ t dˆx t = (A t K t C t )ˆx t dt + B t u t dt + K t dy t P t = A t P t + P t A T t P t Ht T R 1 H h P t + Q t Pt is still the estimate covariance t It is permissible in practice to use regular calculus Notice there is no difference between KP and KF 13

14 Variations on the continuous-time Kalman filter Correlated noises Kalman filter Colored process and/or measurement noise processes - augment the state The steady-state continuous-time Kalman filter A, C, Q, R constant Pt! constant The continuous ARE Hamiltonian E[w t w T ]=Iδ t, E[v t v T ]=Iδ t, E[w t v T ]=Mδ t P = AP + PA T + Q KRK T K = (PC T + M)R 1 ˆx = (A KC)ˆx + Ky H = P 0 0=AP + PA T PC T R 1 CP + Q A T C T R 1 C Q A The steady-state continuous-time Kalman filter is also nown as the Wiener filter The Wiener filter was developed in World War 2 for radar applications It was defined from the signal and noise spectra by factorization 14