Time-Varying Systems and Computations Lecture 6

Transcription

1 Tme-Varyng Systems and Computatons Lecture 6 Klaus Depold 14. Januar 2014 The Kalman Flter The Kalman estmaton flter attempts to estmate the actual state of an unknown dscrete dynamcal system, gven nosy measurements of ts output, for a general ntroducton see Kalath 2, see also Kalath, Sayed and Hassb 3, here we gve a bref account to connect up wth the prevous secton on QR and the followng secton on structured matrces. The tradtonal set up makes a number of assumptons, whch we summarze. Assumptons We assume that we have a reasonably accurate model for the system whose state evoluton we try to estmate. Let x be the evolvng state at tme pont - t s a vector of dmenson d. We further assume that the system s drven by an unknown nosy, zero mean, vectoral nput u, whose second order statstcal propertes we know. We assume the dynamcal system to be lnear and to be gven by the state-space model A B Σ = C 0 descrbng respectvely the recursve propagaton of the state-varable x and output y accordng to { x1 = A x B u y = C x v, where we nclude that the observable output y s contamnated by zero mean, vectoral measurement nose v, whose second order statstcs we know also. A data flow dagram of the state evoluton s shown n Fg. 1. Concernng the statstcal propertes of the drvng process u and the measurement nose v, we need only to defne the second order statstcs (the frst order means s already assumed zero, and no further assumptons are made on the hgher orders). We always work on the space of relevant, zero means stochastc varables, usng E{.} as the expectaton operator. In the present summary, we assume that u and ν are uncorrelated wth each other and wth any other u k, ν k, k, and that ther covarances are gven respectvely by E{u u } = Q and E{v v } = R, both non-sngular, symmetrc postve defnte matrces (ths assumes that there s enough nose n the system). On a space of scalar stochastc varables wth zero mean we defne a (second order statstcal) nner product as (x, y) = E{(xy )}. Ths can be 1

2 Lecture 6 2 u B x A x 1 C v y Fgure 1: The model flter extended to vectors by usng outer products such as u 1 E{uy } = E. y 1 y n u m = E{u 1 y 1} E{u 1 y 2} E{u 1 y n} E{u 2 y 1} E{u 2 y 2} E{u 2 y n} E{u m y 1} E{u m y 2} E{u m y n}. We also assume that the process whose state s to be estmated starts at the tme pont = 0. The ntal state x 0 has known covarance E{x 0 x T 0 } = Π 0. The Recursve Soluton We start wth summarzng the classcal soluton, based on the nnovatons model poneered by Kalath e.a., see 2. Let us assume that we have been able to predct x and attempt to predct x 1. The least squares predctor ˆx s the one that mnmzes the predcton error e x, = x ˆx n the covarance sense, assumng lnear dependence on the data. The Wener property asserts that ˆx s a lnear combnaton of the known data y 0, y 1, y 2,... y 1 and that the error, also called the state nnovaton, e s orthogonal on all the known data so far. These propertes wll be propagated to the next stage, gven the (nose contamnated) new nformaton y. It turns out that the only nformaton needed from the past of the process s precsely the estmated state ˆx, the new estmate beng gven by ˆx 1 = A ˆx K p, (y C ˆx ). In ths formula K p, denotes the Kalman gan, whch has to be specfed, and whch s gven by K p, = K R 1, R = R C P C, K = A P C, where R = E{e y, e y, }. In these formulas, the covarances P = E{e x, e T x, } and R are used, n vew of the formula for the latter, only P has to be updated to the next step, and s gven by P 1 = A P A B Q B K p, R K p,.

3 Lecture 6 3 The covarance P 1 s supposed to be postve defnte for all values of, a constrant whch may be volated at tmes because of numercal errors caused be the subtracton n the formula. In the next paragraph we shall ntroduce the square root verson of the Kalman flter, whch cannot create ths type of numercally caused problems. So far we have only gven summares of the known results, we gve a smple drect proof n the next paragraph. Startng values have to be determned snce ths soluton s recursve, and they are gven by ˆx 0 = 0, P 0 = Π 0. Dervaton of Square Root Algorthms We gve a dervaton of the square root algorthm for the Kalman flter usng a recursve approach based on the parameters of the model at tme pont. We assume recursvely that the error e = x ˆx at tme pont s orthogonal to all the prevously recorded data, and that the new estmate ˆx 1 s a lnear combnaton of all the data recorded up to that pont. We frst relax the orthogonalty condton, and only ask e 1 to be orthogonal to ˆx and to y the newly recorded data at tme pont. We show that ths estmator already produces an estmate that s orthogonal on all the prevously recorded data. From our model we know that x 1 = A x B u. We next ask that ˆx 1 be a lnear combnaton of the known data ˆx and y,.e. there exst matrces X and Y, to be determned, such that ˆx 1 = X ˆx Y y. Requestng second order statstcal orthogonalty of e 1 on ˆx we obtan E{(x 1 ˆx 1 )ˆx } = E{(A x B u X ˆx Y y )ˆx } = 0. We now observe that E{u ˆx } = 0 by assumpton and that E{ˆx ˆx } = E{x ˆx } because E{e ˆx } = 0 through the recursve asserton. The prevous equaton then reduces to (A X Y C )E{x ˆx } = 0, whch shall certanly be satsfed when X = A Y C. Next we request orthogonalty on the most recent data,.e. E{e 1 y } = 0. In fact, we can ask a lttle less, by usng the noton of nnovaton. The optmal predctor for y s smply ŷ = C ˆx, and ts nnovaton, defned as e y, = y ŷ, s e y, = C e v. We now just requre that e 1 s orthogonal on e y,, as t s already orthogonal on ˆx and E{e 1 y } = E{e 1(x C v ˆx C )}. We now obtan an expresson for the nnovaton e 1 n term of past nnovatons and the data of secton e 1 = A e B u (A Y C )ˆx Y y = A e B u Y e y,, whch we now requre to be orthogonal to e y,. Wth P = E{e e }, we have E{e e y, } = P C and E{e y, e y, } = C P C R. The orthogonalty condton becomes therefore Y (R C P C ) = A P C. Hence ths gves the formulas for the Kalman flter, after dentfyng K p, = Y and R = R C P C (actually the covarance of e y,.) Concernng the propagaton of the covarance of the nnovaton P, we rewrte the formula for e 1 as (revertng back to the notaton n the prevous paragraph) e 1 K p, e y, = A e B u.

4 Lecture 6 4 Remarkng that the terms of the left hand sde are orthogonal to each other, and those of the rght hand sde as well, we obtan the equalty P 1 K p, R K p, = A P A B Q B, whch shows the propagaton formula for the nnovaton covarance. Fnally, when y k s some data collected at a tme pont k <, we see that recurson hypothess states that e s orthogonal to all past collected data, n partcular to y k. Hence we see that the expresson s equal to zero, after workng out the ndvdual terms, that s we have E{e 1 y T k } = E{(A e B u K p, ŷ )y k} = 0. The Square Root (LQ) Algorthm The square root algorthm solves the Kalman estmaton problem effcently and n a numercal stable way, avodng the Rccat equaton of the orgnal formulaton. It computes an LQ factorzaton on the known data to produce the unknown data. An LQ factorzaton s the dual of the QR factorzaton, rows are replaced by columns and the order of the matrces nverted, but otherwse t s exactly the same process. Not to overload the symbol Q, already defned as a covarance, we call the orthogonal transformaton matrx at step, U, actng on a so called pre-array and producng a post-array C P 1/2 R 1/2 0 A P 1/2 0 B Q 1/2 U = R 1/2 0 0 K p, P 1/2 1 0 The square root algorthm gets ts name because t does not handle the covarance matrces P and R drectly, but ther so-called square roots, actually ther Cholesky factors, where one wrtes, e.g. P = P 1/2 P /2 assumng P 1/2 to be lower trangular, and then P /2 s ts upper trangular transpose (ths notatonal conventon s n the beneft of reducng the number of symbols used, the exact mathematcal square root s actually not used n ths context.) The matrx on the left hand sde s known from the prevous step, applyng U reduces t to a lower trangular form and hence defnes all the matrces on the rght hand sde. Because of the assumptons on the non sngularty of R, R shall also be a square matrx, the non-sngularty of P 1 s not drectly vsble from the equaton and s n fact a more delcate affar, the dscusson of whch we skp here. The rght hand sde of the square root algorthm actually defnes a new flter wth transton matrx A Kp, C R 1/2. One obtans the orgnal formulas n the recurson just by squarng the square root equatons (multplyng to the rght wth the respectve transposes). In partcular ths yelds A P A = K p, R and hence K p, = K p, R 1/2 = K R /2, whch denotes a dfferent verson of the Kalman gan. Ths form s called an outer flter,.e. a flter that has a causal nverse. The nverse can be found by arrow reversal (see Fg. 2 and t can rghtfully be called both the Kalman flter (as t produces ˆx 1 ) and the (normalzed) nnovatons flter, as t produces e = R 1/2 (y C ˆx ), the normalzed nnovaton of y gven the precedng data summarzed n ˆx..

5 Lecture 6 5 e K p, A x x 1 C R 1/2 y Fgure 2: The Kalman flter, alas nnovatons flter Square Roots back to Covarances We consder the pre-array as gven n the prevous secton and compute the Graman, whch amounts to C P 1/2 R 1/2 P T/2 C T P T/2 A T 0 A P 1/2 0 B Q 1/2 R T/2 C P 0 = C T R C P A T 0 Q T/2 A P C T B T A P A T B Q B T. We then perform a smlar acton wth the post-array, where we explot that the transformaton U s supposed to be orthogonal. Hence we compute R 1/2 R T/2 0 0 K T p, K p, P 1/2 0 P T/2 R R 1/2 K p, T = K 0 0 p, R T/2 K p, KT p, P 1 Comparng the components of the two resultng matrces and explotng the dentty of them based on the orthogonalty of U produces the equatons, 11 : C P C T R = R 12 : C P A T = R 1/2 K p, T = R Kp, T 21 : A P C T = K p, R T/2 = K p, R 22 : A P A T B Q B T = K p, KT p, P 1 P 1 = A P A T B Q B T K p, R Kp, T, whch make up the recursve computatons of the Kalman flter. Lteratur 1 G. Golub, Ch. van Loan. Matrx Computatons. John Hopkns, T. Kalath. Lectures on Wener and Kalman Flterng. Sprnger Verlag, CISM Courses and Lectures No. 140, Wen, New York, 1981.

6 Lecture T. Kalath, A. Sayed, B. Hassb. Lnear Esmtaton. Prentce Hall, Upper Saddle Rver, New Jersey, P. Dewlde, K. Depold. Large-Scale Lnear Computatons wth Dedcated Real-Tme Archtectures. In S. Chakraborty, J. Eberspächer (Eds.), Advances n Real-Tme Systems, pp.41-81, Sprnger, Berln, Hedelberg, 2012.