OPTIMUM FILTERS (5) DISCRETE KALMAN FILTER. Appendix 7B. Derivation of matrix differentiation formulas

Transcription

1 OPTIMUM FILTERS (5) DISCRETE KALMAN FILTER Klm filter: Klm filterig roblem Klm reitio roblem Summry of the imlemettio of isrete Klm filter Stey stte roerties of Klm filters Aeix 7B Deritio of mtrix ifferetitio formuls 50

2 DISCRETE KALMAN FILTER Klm filters re otimum filters tht re ifferet from Wieer filters he the folloig istitie fetures: (i) the roblems re formulte i terms of stte-se moel (or stte-se form); (ii) the solutio of the roblems is omute reursiely; (iii) they be use to tret sttiory rom roesses, s ell s o-sttiory roesses As e lert from the reious letures, otimum Wieer filter is esige usig the Wieer-of equtios i hih the utoorreltio r x () of the oise obsertio (tht otis the esire sigl ( ith r () ) the ross-orreltio r x () re use Use of r (), r x () r x () iites tht ( re ie-sese sttiory s ell s joitly ie-sese sttiory rom roesses Wheres, for o-sttiory rom roess, its sttistil roerties re ot sttiory; i other or, they re time ryig For exmle, etermiisti siusoil sigl ( Asi( ω + ϕ) iterfere ith oise ( is o-sttiory rom roess, x ( ( + (, beuse the me of, m x ( Asi( ω + ϕ ), x ee o time, the utoorreltio, r ( +, r ( ) + ( A ) os( ω) os ω( + ) + ϕ, ee ot oly o time lg but lso o time The ross-orreltio ( +, ( A ) os( ω) os ω( + ) + ϕ lso ee o time r x Therefore, i the otimum sese Wieer filter oly be use to tret sttiory rom roesses As e ill see belo, Wieer filterig roblems re just seil se, the stey-stte se, of Klm filterig roblems Klm filter: Klm filterig roblem Klm filterig resses the geerl roblem of tryig to get the best estimte of the stte of roess goere by the stte equtio (lier stohsti ifferee equtio x ( A( + ( (17) from mesuremets gie by the obsertio equtio y ( + ( (18) Fig 19 The igrm for ostrutig Klm filter Seifilly, he the mesuremets (mesuremet etor) re iut to the Klm filter h(, the filter oututs, hih is the best lier estimte etor of the stte etor of legth bse o the obsertios y (l) for 0 l (Fig 19) The best estimte of,, from the Klm filter is relize by miimizig the me-squre error 51

3 0 { } E e( ) ξ ( tr (19) here P ( E e( e ( (0) is lle estimtio error orie mtrix ( estimte error, gie by e( e (, e(,, e( ) mtrix), e ( ( etor of legth ) is the T [,,, ) ) T tr { A} mes the tre of mtrix A, hih is efie s the sum of the terms log the igol of A, (1) tr A () mm m 1 0 Thus, tr{ ( } E e( ) P Aother error orie mtrix tht e ee i eriig the Klm filter is reitio error orie mtrix, efie s ( E{ ( e e ( } P (3) here e ( (4) is the oe-ste reitio error ( etor of legth ), i hih is the reitio etor of the stte etor of legth, bse o the obsertios y (l) for 0 l To erie the Klm filter e shll rry out reursie roeure of to stes: reitio usig reious mesuremets, estimtio usig e mesuremet (refer to Fig 0) I the first ste, the stte is reite usig the reious obsertios l) u to time l 1 so s to obti I the seo ste, the e iformtio ε ( yˆ( (the iotio roess of ) from e mesuremet is e to the estimte so s to get the best estimte of, Iitiliztio ˆx (0 0) E{0)} Ste 1: reitig 1) from ˆx (0 0) > 1 0) A(0)ˆ(0 x 0) ; Ste : estimtig 1) by ig e ifo ε (1) > 1 1) 1 0) 1) ε (1) Reet 1: reitig ) from ˆx (1 1) > 1) A(1)ˆ(1 1 x ) ; Reet : estimtig ) by ig e ifo ε () > ) 0) ) ε () Reet 1: reitig > A( ˆ( x ; Reet : estimtig by ig e ifo ε ( > ε( Fig 0 The to-ste reursie roeure of eriig Klm filter Suose tht gie rom roess hs iitil stte 0) ith gie estimte ˆx (0 0) E{0)} The iitil estimtio error orie mtrix (Eq (0)) for this estimte is (0 0) E{ e(0 0) e (0 0) } With the gie estimte P ˆx (0 0) E{0)}, e first reit 1) usig the reltio ˆx (1 0) A(0) ˆx (0 0) 5

4 fi ŷ (1 0) 1) ˆx (1 0) s ell Seoly, e me otiml use of the ilble mesuremet 1) by ig to ˆx (1 0) the e iformtio ε ( 1) 1) yˆ(1 0) (the iotio roess of 1)), fi the best estimte of stte 1), ˆx (1 1) Otiml use of ε (1) is me by eightig ftor 1) tht miimizes the me-squre error ( 0 ξ t time 1, (1) tr{ 1 1) } E e( 1 1 ) ξ Reetig the boe to stes for the ext time istt, e use ˆx (1 1) to reit ) usig the reltio 1) A(1)ˆ(1 1) x fi y ˆ( 1) )ˆ( x 1) The e ute ˆx ( 1) by ig e iformtio ε ( ) ) yˆ( 1) from the ext ilble mesuremet ), fi the best estimte of ), ˆx ( ), by otimizig ), the eight o () 0 ε, tht miimizes () tr{ ) } E e( ) ξ We rry o this util time, the e formulte reursie roeure for time ; tht is for eh >0, gie P (, e reit obti ), the e me otiml use of the e iformtio ε ( yˆ( from e ilble mesuremet to fi the best estimte of the stte by fiig otiml tht miimizes the me-squre error 0 { } E e( ) ξ ( tr The Klm filter is estblishe bse o the boe reursie roeure, s follos I the first ste, is rojete he (reite) i the stte trsitio mtrix i the folloig mer A( ˆ( x (5) hih is obtie by igorig the otributio of ( i the stte equtio (Eq (17)) beuse ( is hite oise roess ith zero me is ot orrelte ith its reious lues, (l) for l< The reitio error i this se is e ( A ( + ( A( ˆ( x A ( ) e( + ( (6) Sie e ( is uorrelte ith ( (ue to the ft tht ( is hite oise thus uorrelte ith (l) for l ) so tht E{ e ( ( } 0, the reitio error orie mtrix beomes { e( e ( } A( A ( Q ( ) P ( ) E + (7) This omletes the first ste of eriig the Klm filter I the seo ste, the otimum estimtio of is oute by ig the e mesuremet to the estimte i terms of the iotio roess of, ε ( yˆ( (8) hih is the error betee its reitio y ˆ ( reresets the e iformtio tht ot be reite by y ˆ ( The reit y ˆ ( is gie by y ˆ ( ˆ( x (9) hih is erie by igorig otributio of ( i the obsertio equtio (Eq (18)) beuse ( is ssume to be zero-me hite oise is ot orrelte ith its reious lues, (l) for l< Therefore, the otimum estimte of turs out to be [ yˆ( ) (30) i hih the iotios roess is sle by otimum gi tht is etermie by miimizig mesqure error ξ ( Isertig Eq (9) i Eq (30), e he 53

5 [ ˆ( x ) () A feture i this equtio is tht the estimte is relte ith, the reit of usig reious lues of l) u to time istt l 1 Alyig Eq (5) to Eq () yiel A( ˆ( x A( ˆ( x (3) hih is the reursie reltio betee the estimte the reious estimte The gi is o s the Klm gi The reursie reltio i Eq (3) orreso to the isrete Klm filter, hih is reursie filter illustrte i Fig 1 Fig 1 The isrete Klm filter As e ill see belo, the reursie otimum estimtio of results from the otimiztio of Klm gi ξ ( tr, lie e i for otimum Wieer filters It is beuse the by miimizig MSE Klm gi is otimum tht the Klm filter is otimum filter! I geerl, A( 1), re time ryig, thus the Klm filters re lible to o-sttiory rom roesses s ell The estimte error (see Eq (1)) is e( x ( + ( ˆ( x e( 1) e( ( I K C e K (37) [ ( ( ( ( ( i hih Eqs () (18) re use Sie ( ( re uorrelte, the e( 1) both re uorrelte ith ( Thus, the reitio error orie mtrix beomes P ( E{ e( e ( } [ I [ I Q ( K ( (38) With Eq (38), e miimize the me squre error ( tr{ } ξ to get the otimum Klm gi To o so e ee to use the folloig mtrix ifferetitio formuls, tr { KC} C K (39) tr { KCK } KC K (40) here the eritie ith reset to the mtrix K mes 1 ; (41) K 1 54

6 The eritios of Eqs (39) (40) re gie i Aeix 7B With the hel of Eqs (39) (40) settig ξ ( 0, e he ξ ( tr { } [ I C ( Q ( Solig Eq (4) for e get the otimum Klm gi, 0 (4) [ C ( + Q ( ) 1 K ( C ( (43) It is iterestig to loo ito ho is te to mesuremet oise Whe Q ( ) 0 (mely, (0) the ( ) 1 K C ( so tht C (, tht is, the estimte is ietil to the stte Whe Q (, the 0 I this se, Eq (3) beomes A( ˆ( x, hih shos tht the mesuremets beome omletely urelible re ot use i the estimtio Lie e h miimum me squre error for otimum Wieer filter ue to the orthogolity riile, e fi the miimum me squre error for the otimum Klm filter by usig Eq (4) Re-rrgig the error orie mtrix P ( i Eq (38) i the folloig mer, P ( [ I + [ I C ( Q ( K ( (44) ) it follos from Eq (4) tht the seo term i the bres {} i Eq (44) is zero Thus, e obti the miimize error orie mtrix P ( I (45) ) Sie the Klm gi the error orie mtrix P ( re ieeet of the t, they be omute off-lie It shoul be oite out tht oe of the ery elig fetures of the Klm filter is the reursie ture (sho i Fig 0), hih mes rtil imlemettios muh more fesible th imlemettio of Wieer filter hih is esige to oerte o ll of the t iretly for eh estimte The Klm filter iste reursiely oitios the urret estimte o ll of the st mesuremet Klm reitio roblem A (lier) reitio of sigl is oere ith the estimtio (reitio of +m) for m>0 usig the mesuremets l) u to time l It be oe-ste or multile-ste reitio A Oe-ste Klm reitio For oe-ste reitio roblem, e erform oe-ste forr time shift o Eq (5), ie, rele ith +1, the e he + 1 A( ˆ( x (46) Isertig Eq () ito Eq (46), e obti the oe-ste isrete Klm filter + 1 A( ˆ( x + A( ˆ( x (47) hih is the reursie reltio betee the reit + 1 the reious reit For the reursie reitio reltio, e itroue Klm reitio gi K ( ) K ( A( A ( C ( [ C ( + Q ( 1 (48) With Klm reitio gi K ( ), Eq (47) beomes + 1 A( ˆ( x + K ( ˆ( x (49) 55

7 I reltio to the reursie reitio, it is iterestig to stuy the reitio error orie mtrix, P ( + 1 E e( + 1 e ( + 1 (50) Sie e ( ) + 1 A ( + ( + 1) A( ˆ( x K ( [ + ( ˆ( x A ( e( ) + ( + 1) K ( [ e( + ( [ A( K ( e( + ( + 1) K ( ( (51) (+1) e( 1) both re uorrelte ith (, the + 1 A( ) A ( + Q ( K ( A ( Usig Eq (48) i the boe equtio, e my he P ( + 1 A( A ( + Q ( A( C ( [ C ( + Q ( A ( (5) hih is o s Riti equtio tht gies the reursie reltio betee P ( + 1 P ( The best oe-ste reitio of is esily obtie from Eq (9) y ˆ( )ˆ( x + 1 (53) here + 1 is gie i Eq (47) or Eq (49) B m-ste Klm reitio A m-ste reitio of sigl, eote by + m for m>0, mes tht e reit the sigl l) t l+m usig the lues of mesuremet u to time, it be obtie by exteig the reltio i Eq (5) otig tht oly the lues of u to time re use, i the folloig y + m A( + m )ˆ( x + m (54) Performig the reursie omuttio o Eq (54), + m A( + m )ˆ( x + m A( + m ) A( + m )ˆ( x + m (55) e get the m-ste reitio of m 1 + m A( + ) (56) 0 hih is etermie by the estimte Similrly, by exteig the reltio i Eq (9) otig tht oly the sigl lues u to time re use, e fi the best m-ste reitio of m 1 ˆ y ( + m + m)ˆ( x + m A( + ) (57) 0 Summry of the imlemettio of isrete Klm filter Stte equtio: x ( A( + ( Obsertio (or mesuremet) equtio: y ( + ( Iitil lue: xˆ (0 0) E{ 0)}, 1 etor (ue to () for 1,,, ) 56

8 P (0 0) E{ 0) x (0)}, iitil error orie mtrix Reursie omuttio: For 1,, P ( ) A( A ( + Q ( (7) [ C ( + Q ( ) 1 K ( C ( (43) [ I 1) P ( (45) A( ˆ( x (5) [ ˆ( x ) () ltertiely, Eq () ith Eq (5) iserte i, A( ˆ( x A( ˆ( x (3) The oe-ste reitio of is + 1 A( ˆ( x + A( ˆ( x (47) The best m-ste reitio of is m 1 + m A( + ) (56) 0 The best m-ste reitio of is m 1 ˆ y ( + m + m)ˆ( x + m A( + ) (57) 0 Riti equtio is P ( + 1 A ( ) A ( + Q ( A( C ( [ C ( + Q ( A ( P ( P ( re error orie mtries is r mtrix For sigle obsertio, is 1 etor The roess hite oise rie mtrix Q ( E{ ( ( } B B ( σ is mtrix ( The mesuremet oise rie mtrix Q ( E{ ( ( } is r r mtrix For sigle obsertio, ( ) Q σ is slr Exmle 14 Klm filter for estimtig uo DC oltge Suose tht the mesuremet of uo DC oltge is orrute by oise ( tht is hrterize by zero-me hite oise roess ith rie σ Fi the Klm filter of the form A( ˆ( x A( ˆ( x for the estimtio of the uo DC oltge Solutio Sie is DC oltge, the the stte equtio be exresse s 1) The mesuremet equtio is + ( From the stte se form (the boe equtios), e my fi tht A( 1, 1, Q ( ) 0, ( ) Q σ Sie is slr, the e my he P ( 57

9 [ + 1 σ P σ σ ( [ 1 1 ( 1) P + σ ( 1) ( 1 1) P + σ P + σ P σ ( σ + σ Writig reursiely s follos 0 0) σ 1 1) 0 0) + σ 1 1) σ ) 1 1) + σ ) σ 3 3) ) + σ 0 0) σ 0 0) + σ 0 0) σ 30 0) + σ e my ifer geerl exressio for s follos 0 0) σ 0 0) + σ Sie 0 0) σ 0 0) +σ the the Klm filter for estimtig the uo DC oltge beomes 0) xˆ ( + [ 0) + σ Note tht 0 s, the, hih mes tht rohes stey stte lue If σ (hih imlies tht the mesuremets re omletely urelible), the 0, the estimte is I this se, the mesuremets re igore ˆx (0 0), the iitil estimte, hih hs error rie 0 0) If ˆx (0 0) 0 0 0) (hih orreso to the se of o riori iformtio bout ), the 1/, the estimte beomes 1 1 ˆ( + + x ) hih is simly reursie imlemettio of the smle me 1 xˆ ( ) 1 Exmle 15 Klm filter for estimtig AR(1) roess Cosier tht AR(1) roess 08 1) + ( 58

10 (here ( is hite oise ith rie σ 036) is mesure i oisy eiromet, s follos + ( here ( is the mesuremet oise, hite oise roess ith zero me uit rie, is uorrelte ith ( Fi Klm filter tht gies best estimte of i the folloig mer, A( ˆ( x A( ˆ( x Solutio To fi the Klm filter, e shll etermie the Klm gi Form the gie equtios, it follos tht A( 1) 08, B( 1 1, Q BB σ σ 036 Sie the stte etor is slr, the the estimtio equtio beomes [ 08 ) 08 e lso he P ( [ ) [ 1 1) K ( P ( Isertig K ( [ ito ( [ 1 P ( 1 ( ) ( 1) 1 P P P yiel Thus, Notig tht is zero-me roess ith x Q σ 1, r ( ) 0 8, ssumig tht ˆx (0 0) E{0)}0 P (0 0) E{ 0) } (0) 1, the reursie lultios of the Klm gi r x the error ories 1) for the first fe lues re sho i the folloig tble The Klm gi error ories ) From the tble e see tht fter fe itertios, 1) roh their stey-stte lues Thus, the Klm filter te to its stey-stte solutio Re-ritig the boe equtio i the folloig y, ˆ( x , or xˆ ( ( q ) 1 05q omrig it ith the IIR usl Wieer filter i Exmle 5, e see tht both ( q ) 's re ietil P ito 1 Substitutig ( K ( K ( K ( + 1, e he here is use Sie rohes ostt s, the e my ssume tht lim lim K, hih is lle stey-stte solutio The boe equtio beomes, 064K K, or K + 115K K

11 Solig the boe equtio for K, e my he K 0375 (other solutio, K 15, is uresoble thus, isre); lim 08 K Beuse the stey-stte solutio of for sttiory roess is ostt, the it is ieeet of iitil lue 0 0) (0)) I other or, the iitil lue 0 0) oes ot ffet the stey-stte solutio Exmle 15 shos tht i the se of stey stte ( ) the error orie mtries P ( P ( re ifferet, tht is, lim ) lim Therefore, if e ssume lim P ( P, the e shoul ssume lim P ( P', beuse P P' i most ses Stey stte roerties of Klm filters As e he see i Exmle 15, the stey stte solutio of the Klm filter is ietil to the Wieer filter I other or, the Wieer filter is seil se of the Klm filter i the stey stte se ere e re goig to el ith the stey stte roerties of Klm filters For WSS roess, eg, ARMA roess ( of the form ( ( q ) (, the stte-se form is of the form x ( A + B( (58) y ( Cx ( + ( (58b) here A, B, C re ll ostt mtries, ( ( he the ries of σ σ ith orresoig rie mtries Q E{ ( ( } BB Q σ σ, resetiely A Stey-stte Klm gi, estimtio error, reitio error I this se,, P ( P ( ) roh their stey stte lues for, mely, e my he lim K ( K, lim P ( lim P, lim P ( lim P ( ) P' I geerl, lim P ( lim ) For gie A, B, C, Q Q, the stey-stte reitio error lim P ( + 1 lim P ( P' be etermie usig the Riti equtio i the folloig mer, AP' A Q AP ' C [ CP' C + Q CP' A P' + (59) Whe AI, Eq (59) is of simler form 1 P ' C CP ' C Q CP Q 0 (59') ' + + Whe the reitio error P' is fou, the stey-stte Klm gi lim K ( K be etermie by [ CP' C + Q 1 K P' C (60) the stey-stte estimtio error lim P ( lim P be obtie ith [ I KC P' P (61) B Stey-stte system futio for estimtio ie, From Eq (3), it follos tht A + K CA A KCA + K, ) 60

12 [ I KC A K + The q-trsformig of the boe equtio yiel I I KC Aq K, { } ) the system futio for estimtio is, thus, etermie i the folloig y here { I [ I KC Aq } K ( q ), (6) ) { I [ I KC A } K ( q q (63) C Stey-stte system futio for reitio of Oe ste forr shiftig A yiel + 1 A, from Eqs (3) (63), e my he the oe-ste reitio of s follos + 1 A A( q ) (64) A I I KC Aq K ( q ) here { } ) 1 ( ) A( q ) A{ I [ I KC Aq } K Altertiely, sie ˆ( + 1 A + AK[ C 1 q (65) x e he + 1 A AKC + AK, ie, [ KC AK + 1 A I The q-trsform of the boe equtio is I A I KC q + 1 AK { } ) the system futio beomes here { I A[ I KC q } AK ( q ), + 1, (66) 1 ) { I A[ I KC } AK 1 ( q q (67) The best m-ste reitio of is m 1 + m A (68) 0 The best m-ste reitio of is m 1 y ˆ( + m C + m C A (69) 0 Exmle 16 A usl Wieer reitio filter - stey-stte solutio of Klm reitio filter Cosier rom roess of the form, x ( + ( here ( is zero-me hite oise ith rie σ 1 < 1, ssume y ( + ( to be oisy mesuremet of here ( is zero-me hite oise ith uit rie uorrelte ith ( 61

13 () Fi the stey-stte Klm filter gi K, ie, K lim, i terms of ; (b) Sho tht the usl IIR Wieer filter for oe-ste reitio stisfies the stey-stte solutio of the Klm filter s follos K Note tht y ˆ( ) + 1 Solutio () From gie stte-se form, it follos tht A, C1, The Riti equtio P ' AP' A + Q AP' C beomes of the slr form, P' P ' P' + 1 P' + 1 P ' P' 0 [ CP' C + Q CP' A Solig the boe equtio for P' osierig P ' The stey-stte Klm gi be fou by [ CP' C + Q 1 Q σ σ 1, Q 1 P' 0, e te the ositie soltio s follos P' K P' C ; > K P' (b) Isertig x ( + ( ito y ( + ( tig the q-trsform, e he ( y ( + ( 1 q The oer setrum of is z z P z y ( ) z 1 z 1 z 1 z ( )( ) Setrlly ftorig P y (z) yiel P y here ( 1 z )( 1 z) ( 1 z )( 1 z) ( z) σ ε 4 ( ) ( )( ) (tig ito out <1), σ ε + Thus, e my rite 1 q ε ( 1 q From Eq (166) i Otimum Filters (4) (or Exmle 6 i Otimum Filters ()) e fi the oe-ste reitio of, + yˆ( q From Eq (69), it follos tht y ˆ( + 1 C Thus, the boe equtio be exresse s

14 Sie xˆ ( + 1 ( ) [ ) ( ) the e he K ( )( ) ( ) K Exmle 17 Stey-stte solutio of Klm filter Cosier rom roess of the form, x ( + ( here ( is zero-me hite oise ith rie σ 1 < 1, ssume y ( + ( to be oisy mesuremet of here ( is zero-me hite oise ith uit rie Assumig tht the stey-stte Klm gi lim K is o ostt, () fi the stey-stte system futio ( q ) of the Klm filter for the otiml to-ste reitio, +, of (b) Sho tht the stey-stte Klm gi K stisfies 0 K < 1 the Klm filter ( q ) i () is stble Solutio () From gie stte-se form, it follos tht A, C1, B1, σ Q BB σ 1, Q 1 Sie the stey-stte Klm gi lim K is o is slr, the oe-ste reitio [ ( C ) + 1 A + AK y i this se beomes K The q-trsform of the boe equtio gies K xˆ ( K q ( ) Oe-ste forr shiftig of A( ˆ( x yiel + 1 A( ˆ( x Moreoer, m 1 sie the m-ste reitio of is + m A, e he the to-ste reitio of 0 + AA A + 1 here ( q ) K 1 ( 1 K ) q 1 (b) Sie K P' C [ CP' C + Q P' ( P' + 1) K ( ) ( 1 K ) q q is the esire system futio for the otiml to-ste reitio he (1 K) < 1 so tht the ole q ( 1 K) P' > 0, the 0 K < 1 As < 1 0 K < 1 is isie the uit irle, thus ( q ) is stble, e 63

15 Aeix 7B Deritio of mtrix ifferetitio formuls I this eix e ill erie the mtrix ifferetitio formuls i Eqs (39) (40) ere e osier the rel lue mtries so tht { KC} K tr T T C { KCK } K tr KC For the omlex-lue mtries e my rele the trsose ith the ojugte trsose 1 Assumig 3x lue mtrix 1 13 K 1, x3 mtrix C, the e he KC +, tr KC s The eritie of KC ith reset to K is 1 1 { KC} K tr K 1 1 T T C > tr { KC} C K Assumig 3x mtrix 1 T 1 K 1 x mtrix C, K, the e he KCK T K 1 T + + { T tr KCK } s T Thus { KCK } K tr K KC