Appendix. Kalman Filter

Transcription

1 Appndix A Kalman Filtr OPTIMAL stimation thory has a vry broad rang of applications which vary from stimation of rivr ows to satllit orbit stimation and nuclar ractor paramtr idntication. In this appndix w prsnt an informal dscription of th Kalman ltr, which is on of th basic tools stmming from stimation thory. W bgin with a short dnition of th optimal stimation domain to indicat th rol of th Kalman ltr. According to (Glb, 1974) \an optimal stimator is a computational algorithm that procsss masurmnts to dduc a minimum rror (in accordanc with som statd critrion of optimality) stimat of th stat of a systm by utilizing: nowldg of systm and masurmnt dynamics, assumd statistics of systm noiss and masurmnt rrors, and initial condition information." Th y fatur of this formulation is that all masurmnts and th nowldg about th systm ar usd to valuat th stimat and that th stimation rror is minimizd in a wll d- nd statistical sns. Thr ar thr main stimation problm classs: ltring, prdiction, and smoothing. Th ltring problm corrsponds to th cass whr an stimat is ndd at th momnt of th last masurmnt. In th cas of prdiction th stimat is rquird for an instant aftr th last masurmnt. Whn th tim of rquird stimat is btwn th rst and last masurmnt th problm falls into th smoothing catgory. As th nam indicats, th Kalman ltr provids an optimal stimat for th last masurmnt instant. It is on of th basic ltring tchniqus which is applicabl for stimation of th stat of a linar systm. It is also a good xampl of optimal stimators' capabilitis and limitations. Th gnral structur of th ltring procss is givn in Figur A.1. Hr it is assumd that a linar systm modl and modls charactrizing systm and masurmnt rrors ar availabl. In this cas th Kalman ltr procsss th masurmnts to provid an optimal stimat of th systm stat which minimizs th man squar stimation rror. It should b undrlind that whil th Kalman ltr

2 270 Kalman Filtr provids th information how to procss th masurd data, it dos not indicat th optimal msurmnt schdul. Bfor dscribing th Kalman ltr in mor dtail, w will introduc som basic concpts and modls usd in th ltring procdur. rror rror prior nowldg sourcs sourcs about th systm SYSTEM W(t) systm stat MEASURE- MENT ~ Z(t) obsrvation KALMAN FILTER ^ W(t) stat stimat Figur A.1. Gnral structur of th filtring procss. A.1 Linar Systm Modl In gnral th dynamics of a linar systm can b dscribd in ithr th frquncy domain or th tim domain. In th following w us th tim domain, which is mor convnint from mathmatical and notational viwpoints. Also, such a dscription is mor natural, which rsults in a bttr undrstanding of th systm's bhavior. Lt W (t) =[W 1 (t) W 2 (t) ::: W n (t)] T dnot th systm stat vctor dscribd by n paramtrs which ar a function of tim t. Th dynamics of this systm can b dscribd by th following rst-ordr dirntial quation: _W (t) =F (t)w (t)g(t)(t)l(t)c(t) (A.1) whr (t) is a random forcing function, c(t) is a control (dtrministic) function, and F (t), G(t), L(t) ar matrics dning th dynamics of th systm. Th diffrntial quation dtrmins th systm's subsqunt bhavior assuming that th stat vctor at a crtain point in tim and a dscription of th forcing and control functions ar givn. A bloc rprsntation of th linar systm dynamics is shown in Figur A.2. Transition matrix Lt us considr a systm without th forcing and control functions: _W (t) =F (t)w (t) (A.2) For this systm on can dn th transition matrix (t t 0 ) which dns th systm stat at a tim t basd on th nowldg of th stat at t 0 : W (t) =(t t 0 )W (t 0 ) (A.3)

3 A.1 Linar Systm Modl 271 (t) G(t) (t) L(t) W(t) F(t) Figur A.2. Linar systm dynamics. Obviously th transition matrix is a function of matrix F (t). following gnral rlations can b drivd: In particular th d dt (t t 0)=F (t)(t t 0 ) j(t t 0 )j = xp Z t t 0 trac[f ()]d (A.4) (A.5) In th cas of stationary systms th transition matrix only dpnds on th dirnc t;t 0 and th F matrix is tim invariant. This lads to th following dnition of th transition matrix: (t ; t 0 )= (t;t0)f (A.6) Discrt rprsntation Up to now w considrd a continuous tim modl. Nvrthlss, in many cass only discrt points in tim, t, =1 2 ::: ar of intrst. In this cas th systm dynamics can b dscribd by th following dirnc quation: W 1 = W ; c (A.7) whr =(t 1 t ) (A.8) ; = Z t1 t (t 1 )G()()d (A.9)

4 272 Kalman Filtr Z t1 c = (t 1 )L()c()d (A.10) t A bloc rprsntation of th discrt systm dynamics is givn in Figur A.3. In th rmaindr of this appndix w will considr only discrt systms. Obviously, most of th modls and faturs to follow hav thir corrsponding rprsntation in th continuous tim domain (Glb, 1974). Γ Λ c W 1 dlay W Φ Figur A.3. Discrt systm dynamics. A.1.1 Obsrvability and controllability To discuss obsrvabilitywintroduc th concpt of masurmnts, ~ Z, =1 2 :::. Th masurmnts ar assumd to b linarly rlatd to th systm stat: ~Z = H W u (A.11) whr H is th obsrvation matrix and u is th masurmnt nois. A systm is obsrvabl if it is possibl to dtrmin W 1 ::: W basd on corrsponding masurmnts in a nois fr nvironmnt. A prcis obsrvability condition xprssd in trms of matrics and H can b found in (Glb, 1974). Th issu of controllability is rlatd to th ability of achiving an arbitrary stat, in a givn numbr of stps, in a dtrministic (nois fr) linar dynamic systm. In particular a systm is controllabl in tim t if for any arbitrary pair of stats, W 1, W, thr is a control, c 1 ::: c which can driv th systm from stat W 1 to stat W. A prcis controllability condition xprssd in trms of matrixs and can b found in (Glb, 1974). In th rmaindr of this appndix w do not considr systms with control and w assum c =0. Introduction of th masurmnt modl complts th basic linar systm modl dscription for stimation purpos which is givn by W 1 = W ; (A.12)

5 A.1 Linar Systm Modl 273 Z = H W u (A.13) It should b mntiond that this modl is not uniqu in th sns that for givn systm input and output valus, thr ar many dirnt sts of,;,andh which will giv th sam input-output bhavior which corrsponds to th choic of a coordinat systm (tim). A.1.2 Covarianc matrix Th concpt of covarianc matrix is important in stimation rror analysis. Lt us bgin with th dnition of cross-covarianc matrix C of two vctors, A and B, whos lmnts ar random variabls: C(A B) =E[(A ; E[A])(B ; E[B]) T ]=E[AB T ] ; E[A] E[B T ] (A.14) If A = B, th covarianc matrix C dns th scond cntral momnts of th vctor lmnts. In particular th matrix diagonal consists of vctor lmnts' variancs whil othr matrix lmnts ar covariancs of two vctor's lmnts idntid by th matrix lmnt indics. In this appndix w considr systms whos forcing functions ar vctors of random variabls. It is assumd that any two valus of forcing function,, ;i, i = 1 2 :::, ar uncorrlatd, which mans that th forcing function gnrats a whit squnc. Obsrv that onc th forcing function is a random variabl, th systm stat is also a random variabl. To simplify prsntation it is also assumd that th forcing function is unbiasd (zro nsmbl avrag valus). This dos not rstrict th gnrality of th prsntd modls sinc th bias can b asily rmovd by subtraction. Lt us dn th rror in th stimat of th systm stat as a dirnc btwn th stimatd valu ^W and th actual valu W : = ^W ; W (A.15) Thn th stimation rror covarianc matrix is dnd as P = E[ T ] (A.16) Th covarianc matrix P xprsss th statistical masur of stimation uncrtainty. A covarianc matrix is also usd for dscription of th uncorrlatd random squnc ;. Hr w hav E[(; )(; ) T ]=; Q ; T (A.17) whr Q is th covarianc matrix of th whit squnc.

6 274 Kalman Filtr Estimation rror propagation Basd on th transition matrix on can dn th stimat of th prdictabl portion of th nxt stat as ^W 1 = ^W (A.18) Hncforth ^W is calld stat stimat xtrapolation. By subtracting Equation (A.12) from Equation (A.18) w gt 1 = ; ; (A.19) This quation can b usd to driv a rlation for xtrapolation of th rror covarianc matrix from tim t to t 1 P 1 = P T ; Q ; T (A.20) This rsult indicats that in som cass th rror covarianc can bcom unboundd if thr ar no stat masurmnts. A.2 Discrt Kalman Filtr In this sction w considr a linar discrt systm whos dynamics ar givn by W 1 = W (A.21) whr systm stat W is a n-dimnsional vctor and is a whit squnc vctor with zro man and covarianc matrix Q. Th systm masurmnts ar dnd by ~Z = H W u (A.22) whr masurmnt Z ~ is an l-dimnsional vctor and u is a whit squnc vctor with zro man and covarianc matrix Y ~. Lt us dn an optimal, unbiasd, and consistnt stimator. Hr optimality is dnd as minimization of th man squar stimation rror which corrsponds to minimization of J = E[ T I ] = trac[p ] (A.23) whr I is idntity matrix. An unbiasd stimat is dnd as th on whos xpctation is qual to th xpctation of th actual stat. A consistnt stimat convrgs to th actual valu with th incras in th numbr of masurmnts. Th Kalman ltr provids an optimal, unbiasd, consistnt stimat which can b xprssd in th linar and rcursiv form ^W = K 0 ^W K ~ Z (A.24) whr K 0 and K ar wighting matrics. It can b shown (Glb, 1974) that in ordr to hav th stimat unbiasd th following condition must hold: K 0 = I ;K H (A.25)

7 A.2 Discrt Kalman Filtr 275 W -1 Φ -1 DELAY W modl rror: -1 SYSTEM MODEL masurmnt rror: u ~ Z H MEASUREMENT MODEL - H Kalman gain K ^ W KALMAN FILTER ^ W Φ -1 ^ W -1 DELAY Figur A.4. Systm modl and Kalman filtr. Using this rlation in Equation (A.24) givs th stat stimat updat ^W = ^W K [ ~ Z ; H ^W ] (A.26) Basd on this rlation on can driv th rror covarianc matrix updat P =(I ;K H )P (I ;K H ) T K ~ Y K T (A.27) Th optimum valu of K can b found from minimization of xprssion A.23 which corrsponds to minimization of th lngth of th stimation rror vctor. This can b don by valuating th partial drivativ ofj with rspct to K and solving it for zro valu. Basd on th gnral rlation for th partial drivativ of th trac of th product of two matrics th solution givs K = P H T [H P H T ~ Y ] ;1 (A.28) which dns th Kalman gain matrix. Using this matrix in Equation (A.27) dns, aftr som transformations, th optimizd valu of th updatd rror covarianc matrix P =(I ;K H )P (A.29)

8 276 Kalman Filtr Equations (A.26), (A.28), and (A.29), togthr with initial conditions, W 0 P 0, dn th discrt Kalman ltr which is illustratd in Figur A.4. From a practical point of viw it is important that th Kalman ltr provids its own rror analysis by mans of th stimation rror covarianc matrix, P. It has bn also shown (Wiss, 1970) that, dspit its simpl rcursiv natur and linarity, th Kalman ltr is th optimal ltr if, u ar Gaussian (in othr words a non-linar ltr cannot b bttr). Othrwis th Kalman ltr is th optimal linar ltr. A.3 Discussion and Bibliographic Nots Th rst signicant contribution to th stimation thory can b tracd bac to Gauss (circa 1800) who usd th tchniqu of dtrministic last-squars in simpl masurmnt problms (Mhra, 1970). Fishr (circa 1910) invntd th maximum lilihood stimation which is basd on probability dnsity function (Wiss, 1970). Th dsign of statistically optimal ltrs in th frquncy domain is du to Winr (circa 1940) who addrssd th continuous tim problm using corrlation functions and th continuous ltr impuls rspons (Mhra, 1971 Abramson, 1968). Th Kalman ltr, an optimal linar ltr dsignd in tim domain, was dvlopd by Kalman and othrs s.g. (Kalman and Bucy, 1961 Uttam, 1971 Aoi and Huddla, 1967 Ts and Athans, 1970). It is intrsting to not that th Kalman ltr basically constituts a rcursiv solution to th original last-squars problm formulatd by Gauss. In this appndix th Kalman ltr prsntation follows in principl a dscription of this tchniqu givn in (Glb, 1974) which provids a simpl and intrsting pictur of th cntral issus undrlying stimation thory and practic. Morovr, it ts vry wll into th stimation problm tratd in Chaptr 4. Thr ar many othr wors daling with both stimation thory in gnral and th Kalman ltr in particular,.g. (Papoulis, 1991 Nahi, 1969 Proais, 1989). Rfrncs Abramson, P.D. Jr Simultanous stimation of th stat and nois statistics in linar dynamic systms. Ph.D. Thsis, M.I.T., TE-25. Aoi, M., and Huddla, J.R Estimation of stat vctor of a linar stochastic systm with a constraind stimator. IEEE Transactions on Automatic Control, AC-12(4). Glb, A Applid Optimal Estimation. Th M.I.T. Prss. Kalman, R.E., and Bucy, R.S Trans. ASME Sr. D.J. Basic Eng., 83:95{107. Dcmbr. Kalman, R.E Fundamntal study of adaptiv control systms. Tchnical Rport No.ASD-TR-61-27, Vol.I, Flight Control Laboratory, Wright-Pattrson Air Forc Bas, Ohio.

9 Rfrncs 277 Mhra, R.K On th idntication of variancs and adaptiv Kalman ltring. IEEE Transactions on Automatic Control, AC-15(2):175{184. Mhra, R.K On-lin idntication of linar dynamic systms with applications to Kalman ltring. IEEE Transactions on Automatic Control, AC- 16(1):12{21. Nahi, N.E Estimation Thory and Applications. Wily. Papoulis, A Probability, Random Variabls, and Stochastic Procsss. 3rd d. McGraw-Hill. Proais, J.G Digital Communications. 2nd d. McGraw-Hill. Ts, E., and Athans, M Optimal minimal-ordr obsrvr stimators for discrt linar tim-varying systms. IEEE Transactions on Automatic Control, AC-15(4). Uttam, B.J On th stability of tim-varying systms using an obsrvr for fdbac control. Intrnational Journal of Control, Dcmbr. Wiss I.M A survy of discrt Kalman-Bucy ltring with unnown nois covariancs. In Procdings of AIAA Guidanc, Control and Flight Mchanics Confrnc, Papr No.70{955, Santa Barbara, Calfornia.

10 278 Kalman Filtr