20 KALMAN FILTER 20.1 Introduction In the previous section, we derived the linear quadratic regulator as an optimal solution for the full-state feedback control problem. The inherent assumption was that each state was known perfectly. In real applications, the measurements are subject to disturbances, and may not allow reconstruction of all the states. This state estimation is the task of a model-based estimator having the form: ˆ˙x + Bu + H(y − Cˆx = Aˆ x) (241) The vector x represents the state estimate, whose evolution is governed by the nominal ˆA and B matrices of the plant, and a correction term with the estimator gain matrix H. H operates on the estimation error mapped to the plant output y, since C ˆ = yˆ. Given x statistical properties of real plant disturbances and sensor noise, the Kalman Filter designs an optimal H. + -y x + + + 1/s A H C xy Bu 20.2 Problem Statement We consider the state-space plant model given by: x˙ = Ax + Bu + W1 (242) y = Cx + W2. There are n states, m inputs, and l outputs, so that A has dimension n × n, B is n × m, and C is l × n. The plant subject to two random input signals, W1 and W2. W1 represents20.3 Step 1: An Equation for ˙Γ 99 disturbances to the plant, since it drives x˙ directly; W2 denotes sensor noise, which corrupts the measurement y. An important assumption of the Kalman Filter is that W1 and W2 are each vectors of unbiased, independent white noise, and that all the n + l channels are uncorrelated. Hence, if E(·) denotes the expected value, E(W1(t1)W1(t2)T ) = V1ζ(t1 − t2) (243) E(W2(t1)W2(t2)T ) = V2ζ(t1 − t2) (244) E(W1(t)W2(t)T ) = 0n×l. (245) Here ζ(t) represents the impulse (or delta) function. V1 is an n × n diagonal matrix of intensities, and V2 is an l × l diagonal matrix of intensities. The estimation error may be defined as e = x − ˆ x. It can then be verified that x + Bu + H(y − C ˆe˙ = [Ax + Bu + W1] [Aˆ x)] (246)− = (A − HC)e + (W1 − HW2). The eigenvalues of the matrix A − HC thus determine the stability properties of the es-timation error dynamics. The second term above, W1 + HW2 is considered an external input. The Kalman filter design provides H that minimizes the scalar cost function J = E(e T W e), (247) where W is an unspecified symmetric, positive definite weighting matrix. A related matrix, the symmetric error covariance, is defined as Γ = E(ee T ). (248) There are two main steps for deriving the optimal gain H. ˙20.3 Step 1: An Equation for � The evolution of Γ follows from some algebra and the convolution form of e(t). We begin with T˙Γ = E( ˙ee T + ee˙ ) (249) T = E[(A − HC)ee T + (W1 − HW2)e + ee T (AT − CT HT ) + e(W1 T − W2 T HT )]. The last term above can be expanded, using the property that100 20 KALMAN FILTER � t e(t) = e(A−HC)t e(0) + e(A−HC)(t−φ) (W1(φ) − HW2(φ)) dφ. 0 We have E(e(W T 1 − W T 2 HT )) = e(A−HC)tE(e(0)(W T 1 − W T 2 HT )) + � t 0 e(A−HC)(t−φ) E((W1(φ) − HW2(φ)) (W T 1 (t) − W T 2 (t)HT )) dφ � t = 0 e(A−HC)(t−φ) E((W1(φ) − HW2(φ)) (W T 1 (t) − W T 2 (t)HT )) dφ � t = 0 e(A−HC)(t−φ) (V1ζ(t − φ) + HV2HT ζ(t − φ)) dφ = 1 2 V1 + 1 2 HV2HT . To get from the first right-hand side to the second, we note that the initial condition e(0) is uncorrelated with W1 T − W2 T HT . The fact that W1 and HW2 are uncorrelated leads to the third line, and the final result follows from � t 1 ζ(t − φ )dφ = , 0 2 i.e., the written integral includes only half of the impulse. The final expression for E(e(W T − W T HT )) is symmetric, and therefore appears in Equa- 1 2 tion 249 twice, leading to ˙Γ = (A − HC)Γ + Γ(AT − CT HT ) + V1 + HV2HT . (250) This equation governs propagation of the error covariance. It is independent of the initial condition e(0), and depends on the (as yet) unknown estimator gain matrix H. 20.4 Step 2: H as a Function of � We now make the connection between Γ = E(eeT ) (a matrix) and J = E(eT W e) (a scalar). The trace of a matrix is the sum of its diagonal elements, and it can be verified that J = E(e T W e) = trace(ΓW ). (251) We now introduce an auxiliary cost function defined as J∗ = trace(ΓW + ΦF ), (252)� � � � 20.5 Properties of the Solution 101 where F is an n×n matrix of zeros, and Φ is an n×n matrix of unknown Lagrange multipliers. Note that since F is zero, J∗ = J, so minimizing J∗ solves the same problem. Lagrange multipliers provide an ingenious mechanism for drawing constraints into the optimization; the constraint we invoke is the evolution of Γ, Equation 250: J∗ = trace ΓW + Φ(− ˙Γ + AΓ − HCΓ + ΓAT − ΓCT HT + V1 + HV2HT ) � (253) If J∗ is an optimal cost, it follows that �J∗ /�H = 0, i.e., the correct choice of H achieves an extremal value. We need the following lemmas, which give the derivative of a trace with respect to a constituent matrix: trace(−ΦHCΓ) = −ΦT ΓCT �H trace(−ΦΓCT HT ) = −ΦΓCT �H trace(ΦHV2HT ) = ΦT HV2 + ΦHV2. �H Proofs of the first two are given at the end of this section; the last lemma uses the chain rule, and the previous two lemmas. Next, we enforce Φ = ΦT , since the values are arbitrary. Then the condition �J∗/�H = 0 leads to 0 = 2Φ(−ΓCT + HV2), satisfied if H = ΓCT V2−1 . (254) Hence the estimator gain matrix H can be written as a function of Γ. Inserting this back into Equation 250, we obtain ˙Γ = AΓ + ΓAT + V1 − ΓCT V −1CΓ. (255)2 Equations 254 and 255 represent the practical solution to the Kalman filtering problem, which minimizes the squared-norm of the estimation error. The evolution of Γ is always stable, and depends only on the constant matrices [A, C, V1, V2]. Notice also that the result is independent of the weighting matrix W , which might as well be the identity. 20.5 Properties of the Solution The solution involves a matrix Riccati equation, like the LQR, suggesting a duality with the LQR problem. This is in fact the case, and the same analysis and numerical tools can be applied to both methodologies. The steady-state solution for Γ is valid for time-invariant systems, leading to a more common MARE form of Equation 255: 0 = …