2.160 System Identification, Estimation, and Learning, Lecture Notes #7

2.160 System Identification, Estimation, and Learning Lecture Notes No. 7 4.7. Continuous Kalman Filter Converting the Discrete Filter to a Continuous Filter x = Fx + Gw(t ) Continuous process (49) y = Hx + v(t ) Measurement (50) Assumptions [ ] δ (t − s) = Dirac delta function E w(t )wT (s ) = Qδ (t − s ) (51) [ ] (52) [ ] (53) E v(t )vT (s) = Rδ (t − s) E v(t )wT (s) = 0 Converting Rt and Qt in the discrete Kalman filter to Q and R of the above equations, (see Brown and Hwang, Section 7.1 for detail) R ∆t = sampling interval Qt = Q∆t Rt = ∆t From (4) ( K t = Pt t −1H tT H t Pt t −1H tT + Rt ( ) −1 = ∆tPt t −1H tT ∆t ⋅ H t Pt t −1H tT + R ≅ ∆tPt t −1H R T t Define −1 ) −1 (54) R ∆t (55) for ∆t << 1 ∆ K = Pt t −1H tT R −1 (56) From (8) ∆tK Pt t −1 = At Pt AtT + Gt Qt GtT = At (I − K t H t ) Pt t −1 A + Gt Qt G T t T t (57) At = I + ∆tF 1 ( ) (58) Pt +1 t = Pt t −1 + ∆tFPt t −1 + ∆tPt t −1F T − ∆tKH t Pt + Gt ∆tQGtT (59) Ignoring higher-order small quantities; O ∆t 2 ≅ 0 Pt +1 t − Pt t −1 ∆t = FPt t −1 + Pt t −1F T − Pt t −1H tT R −1H t Pt + Gt QGtT (60) ∆t → 0 (61) lim Pt t −1 = Pt −1 ∆t → 0 P = FP + PF T − PH T R −1HP + GQGT (62) This is called the Matrix Riccati Equation. Similarly, we can reduce the discrete time form of state estimation correction to the one of continuous time: xˆ = Fxˆ + K ( y − Hxˆ ) (63) where the Kalman gain is given by K = PH T R −1 (64) This is called the Kalman-Bucy Filter The physical interpretation of the Matrix Riccati Equation P =  FP + PF T − PH T R −1HP + GQGT Unforced State Transition: The effect of the unforced system dynamics upon the covariance propagation The decrease of uncertainty as a result of measurement (62) The increase of uncertainty due to the process disturbance Q 4.8 The Algebraic Riccati Equation Assume that the Riccati differential equation has an asymptotically stable solution for P(t ) : 2 lim P(t ) = P∞ t →∞ (65) Then the time derivative vanishes lim t →∞ dP(t ) =0 dt (66) Substituting this into the Riccati equation yields 0 = FP∞ + P∞ F T − P∞ H T R −1HP∞ + GQGT This is called the Algebraic Riccati Equation. (67) This is a nonlinear matrix equation, and need a numerical solver to obtain a solution for P∞ . Consider a scalar case; P∞ ∈ R1×1 , F , H , Q, R, G ∈ R1×1 . The Algebraic Riccati Equation can be solved analytically H2 2 P∞ − 2FP∞ − G 2Q = 0 R P∞ =  R  Q  F ± F 2 + H 2G 2  2   H  R  (68) (69) These are two solutions; one positive and the other negative. Taking the positive solution lim P(t ) = P∞ = t →∞  R  Q  F + F 2 + H 2G 2  2   H  R  (70) Note that, regardless of the sign of F ( F < 0 means a stable process dynamics), the above limit P∞ is positive. Remarks 1) As the sensor variance R increases, P∞ increases 2) As the process noise variance Q increases, P∞ increases 3) When the process noise variance Q is zero, and the process is stable, F < 0 , P∞ becomes zero. 4.8 Convergence Analysis 4.8.1 Transient Response of the Covariance Matrix The Discrete Kalman Filter is hinged on the covariance matrix update law: 3 Pt = (I − K t H t )Pt t −1 (41) Pt +1 t = At Pt AtT + Gt Qt GtT (45) P Does this converge? And where P∞ ? time Continuous Kalman Filter: The covariance matrix is given by the Riccati Differential equation: d P (t ) = FP(t ) + P (t ) F T − P(t ) H T R −1HP(t ) + GQGT (62) dt Where F is a state transition matrix: d x(t ) = F (t ) x(t ) + G (t ) w(t ) (49) dt Let us examine the properties of the Riccati differential equation in order to gain insights as to whether the covariance of Kalman filter converges or not. 4.8.2 Matrix Fraction Decomposition The Riccati Differential Equation (62) can be solved by using a technique, called the Matrix Fraction Decomposition Consider a square matrix M(t) decomposed into two square matrices A(t) and B(t), M (t ) = A(t ) B −1 (t ) (71) where B is non-singular and both A and B are differentiable with respect to time t. The above expression is called a fraction decomposition of Matrix M . Differentiating B (t ) B(t ) −1 = I (identify matrix) with respect to time t, 4 B B −1 + BB −1 = 0 (72) d −1 d B (t ) = − B −1 ⋅ B (t ) ⋅ B −1 dt dt (73) Therefore Now let us represent the covariance matrix P(t ) by P (t ) = A(t ) B −1 (t ) (74) and applying eq.(73) dP(t )  −1 = AB + AB −1 dt = A B −1 − AB −1B B −1 (75) From the Riccati equation (62) dP(t ) = FAB −1 + AB −1F T − AB −1H T R −1HAB −1 + GQGT dt (76) Equating the right hand sides of (75) and (76), and post-multiplying B yield ( ) ( A − AB −1B = FA + GQGT B − AB −1 H T R −1HA − F T B ) (77) Therefore, if we find A and B that satisfy: A = FA + GQGT B (78) B = H T R −1HA − F T B (79) then P(t ) = A(t ) B −1 (t ) satisfies the Riccati differential equation. Note that (78) and (79) are linear differential equations with respect to matrices A and B They can be rearranged as The Hamiltonian Matrix F (t ) G (t )Q(t )G T (t ) A(t )  d  A(t )    =  T    dt  B (t )   H (t ) R −1 (t ) H (t ) − F (t )  B(t )  (80) 5 As for the initial conditions, we can set A(0) = P0 and B(0) = I . (81) 4.8.3 Convergence Properties of a Scalar Case Consider a scalar case : A(t ) → a(t ) , and B(t ) → b(t ) and assume that the process and measurement equations are time-invariant F (t ) = F G (t ) = G Scalar Q(t ) = Q R (t ) = R H (t ) = H Eq.(80) reduces to G 2Q   a   a   F    =  2    b   H R − F  b  (82) a (0) = P0 and b(0) = 1 . This can be solved with initial condition of The eigenvalues of the Hamiltonian Matrix are λ1,λ2 = ± F 2 + Q 2 2 G H = ±λ R (83) Using λ1 and λ2 a (t ) = b( t ) = 1 { [ P0 (λ + F ) + q ]eλt + [ P0 (λ − F ) + q ]e− λt } 2λ 1 2λq {(λ − F )[ P (λ + F ) + q]e 0 λt − (λ + F )[ P0 (λ − F ) − q ]e− λt } (84) where q = G 2Q . Therefore, the covariance is given by [ P0 ( λ + F ) + q ] + [ P0 (λ − F ) − q ]e−2 λt a (t ) =q P(t ) = (λ − F )[ P0 (λ + F ) + q ] − (λ + F )[ P0 (λ − F ) − q ]e− 2 λt b( t ) (85) 6 The steady-state solution is given by P∞ = lim P(t ) = t→∞  q R  Q = 2  F + F 2 + H 2G 2  R λ−F H   This agrees with the previous result, eq.(70). For H=1, F=0, R=1, Q=1 2 } 1 Stable solutions Po>-1 0 1 Stable 2 -1 -2 An important property of the Riccati Differential Equation (RDE): If the system is observable, i.e. (F, H): Observable Pair, then the RDE has a positive-definite, symmetric solution for an arbitrary positive-definite initial value of matrix Po>0; P(t ) > 0 p.d . , P(t ) = PT (t ) ∈ R n ×n , ∀t > 0 (86) 7