The regression equation for the gain bears a striking resemblance to the regression equation for the estimate X t : -1k -1 P t t = Ctr tt 7.

Transcription

1 7.8 The Kalman Filter 307 Yt Figure 7.14 Network for implementing sequential Bayes estimate. The regression equation for the gain bears a striking resemblance to the regression equation for the estimate X t : -1k -1 P t t = Ctr tt P -l~ - H TR-1 t Xt - t t Yt In fact, the gain kt is just the impulse response of the regression equation for Xt: P -Ik HTR-l t t = t t et; This property can also be illustrated with the network of Figure If X-I is set to zero and Yo = 0, Yl = 0,..., Yt-l = 0, Yt = 1, then xt = k t. 7.8 THE KALMAN FILTER The Kalman filter is an algorithm for recursively estimating dynamical state vectors Xt that evolve according to the difference equation Xt+l = AXt + but. In this difference equation, the initial state Xo is drawn from an N[O, Ro1 distribution. From there on, the difference equation is driven by a sequence of independent and identically distributed random variables {u t }, each of which is independent of Xo and distributed as U t : N[O, 0'~1. Because of the way Xt is constructed from a linear combination of normal random vectors, we know that it is a p X 1 normal random vector with mean zero and covariance matrix R t = Extxi: Xt : N[O, R t ]. The recursion for R t is obtained from the difference equation : Rt+1 = ARtA T + O':bb T. The measurements available to us take the form Yt = CTXt + nt, ~/\ where the noise sequence {nt} is a sequence of independent and identically distributed 'I, '0 (( f.( ~.

2 308 7 Bayes Estimators N[O, O"~] random variables, each independent of the sequence {Ut} and the state Xt. The measurement Yt is normally distributed with mean zero and covariance O"t: Yt : N [0,0";] "; = ctrtc + O"~. A block diagram that illustrates how the variables Xt and Yt are generated is given in Figure Kalman posed for himself the problem of estimating the state Xt from the sequence of measurements Yt = [Yo Yl yd T. We shall derive the Kalman filter by using the Gauss-Markov theorem. We begin by noting that the conditional distribution of Xt> given Yt> will be normal: The problem, of course, is to find the a posteriori mean Xtlt and the covariance Ptlt. Suppose we had measurements only up to time t - 1, namely Yt-l = [Yo Yl Yt_d T. The conditional distribution of XI> given Yt-l, would also be normal: While we do not know yet what form the "predictor" Xtlt-l and prediction error covariance matrix P1lt-l will take, we do know two important things about the predictor Xtlt-l and prediction error Otlt-l = Xt - Xtlt-l: 1. The predictor Xtlt-l is a sufficient statistic for Xt. That is,!(xtiyt-l) =!(Xtlx t-l) : N[Xtit-l, Ptlt-d, meaning the measurements Yt-l may be replaced by the predictor Xtlt The predictor Xtl t -l and prediction error Otlt-l = Xt - Xtlt-l orthogonally decompose Xt. That is, Xt = Xtlt-l + Ollt-l Xtlt-l : N[O, RI - Ptlt-d 0t!t-l : N[O, Ptit-Il E Xtit-l A Dllt-l AT =. These results are illustrated in the lower two branches of Figure u, X, y, Figure 7.15 Block diagram for generating the state and the measurement.

3 7.8 The Kalman Filter 309 n, , Yt Xt ,.-- -~-_.-- ~-----4X, -/ G.--_ ~-_.--..._.---_4 X'lt_J Yt-J Figure 7.16 The state X"~ predictor Xtlt-I, and measurement Y t. Now let's suppose a new measurement Yt is obtained. This measurement is related to the state Xt by Yt = CTXt +n" but this may be written Yt = c T (Xtlt-l + fitlt- 1 ) + n t and illustrated with the top branch of Figure With the aid of Figure 7.16 we may write down a linear model for the state Xr, the predictor Xtlt-l, and the new measurement Yt, all in terms of the orthogonal variables fitlt-i> Xtlt-l, and nt:.. I I 0 fi t lt - 1 o I 0 Xtlt-l c T c T 1 nt The covariance matrix for the variables fitl t- I, Xtlt-l, and ntis n; 1 = 0 [ Ptlt-I o When combined with the linear transformation of Equation 7.147, this diagonal covariance structure produces the following covariance matrix for Xt, Xtlt-l, and Yt: +'HIX) [ ~T Xtl t- I R, ~tlt-i Rtc Yt 1 = Ptlt- 1 Ptlt- 1 Pdt-IC T~ ctrt c Ptlt- d'2 1 t R t Ptlt-I Rtc Ptlt-1 ctrt St This is a foreboding result, but it contains all of the information we need to compute

4 310 7 Bayes Estimators the conditional distribution of Xt, given Xtlt-I and Yt: xt/(xtlt-i, Yt) : N[xtlt, Ptjt] ~ [P~ R ]S-I[Xtlt-l] Xtlt = tlt-l t C t Yt The inverse of the patterned matrix St is P R [P~ RclS-l[Ptlt-I] tit = t - t!t-i t t ctr t ' S-l = [P~/-I 0] + -I [ -c) t OT 0 Y t Yt = if; - ct(rt - Ptlt-I)c = ctptlt_lc + if~. When this result is substituted into the formulas for Xtlt and Pt!t, the equations simplify as follows: Xtlt = Xtlt-l + Pt!t-ICy;1 (Yt - CTXrjt-d 1 T Ptlt = Ptlt Ptlt-ICC Ptlt-I' Yt These equations can be written a little more elegantly by defining the Kalman gain k t = Ptlt-IcYt Then the Kalman recursions are Yt = CTptlt-1c + if~. Xrjt = Xtlt-I + kt (Yt - CTXtlt-tl Pt!t = Pt!t-I - ktctptlt-1 = (I - ktct)ptlt-1 = Pt!t-I - Ytktk; These recursions are complete, except for the definitions of Xtlt-I and Pt!t-I. The predictor Xtlt-l is the conditional mean ofxt, given Yt-l. It may be written as Xt!t-l = E[xtIYt-IJ = E[Axt-1 + but-dyt-ij = AXt-llt-l + O. The error covariance Ptjt-I is defined as follows: Ptlt- I = E[xt - xtlt-d[xt - Xtlt-d T = E[Axt-I + but-i - AXt-1It-d[Axt-1 + but-l - AXt-Ilt-Il T = E [A(Xt-l - Xt-Ilt-I) + but-d [A(Xt-I - Xt-Ilt-d + but_dt = APt-llt-l AT + if~bbt

5 7.8 The Kalman Filter 311 y, + Figure 7.17 The Kalman filter. So now we may summarize the celebrated equations of the Kalman filter: xtjt = Xtlt-I + kt (Yt - CTXtlt-d Xtlt-I = AXt-Ilt-1 kt = Ptlt-IcYt Yt = ctptlt_ 1 C + (T~ Ptlt = Ptlt- I - Ytkt k; Pt+llt = APtltA T + (T~bbT. From the prediction error covariance Ptlt- I we compute the Kalman gain kt and the estimator error covariance Ptlt. From Ptlt, Pt+llt is computed, and the recursion continues. The filter is depicted in Figure There is one final wrinkle to this story. From the Kalman recursion for the estimate Xtlt, we may construct the following error equation: Xt - Xtlt = Xt - Xtlt-I - kt(ct(xt - Xtlt-I) + nt) From this equation, we compute the covariance matrix Ptlt: Ptlt = PtIt-1 + Ytktk; - Ptlt-Ick; - ktctptit-1 = Ptlt- I - YtktkJ This reproduces our previously derived recursion and shows that the cross-covariance between the prediction error Xt - Xtlt-I and the residual term Yt - ctxtlt-1 is, in fact, the product of the variance Yt and the Kalman gain k t : E(xt - Xtlt-I)(Yt - CTXtlt_I)T = E[xt - Xtlt-d[cT(xt - Xtlt-I) + nt]t = Ptll-Ic = ytkt