Desensitized Kalman Filtering with Analytical Gain

Transcription

1 Desenstzed Kalman Flterng wth Analytcal Gan ashan Lou School of Electrc and Informaton Engneerng, Zhengzhou Unversty of Lght Industry, Zhengzhou, 45002, Chna, Abstract: he possble methodologes to handle the uncertan parameter are revewed. he core dea of the desenstzed Kalman flter s ntroduced. A new cost functon consstng of a posteror covarance trace and trace of a weghted norm of the state error senstvtes matrx s mnmzng to obtan a well-nown analytcal gan matrx, whch s dfferent from the gan of the desenstzed Kalman flter. he pre-estmated uncertan parameter covarance s set as a referental senstvty-weghtng matrx n the new framewor, and the ratonalty and valdty of the covarance are tested. hen, these results are extended to the lnear contnuous system. Keywords: Desenstzed Kalman flter; Uncertan parameter; Analytcal gan; Robust flter I. Introducton he Kalman flter can obtan optmal estmaton based on a fundamental assumpton that the dynamc models can be accurately modeled wthout any colored nose or uncertan parameters. However, n practce, these models always nclude many addtonal parameters, whose uncertantes always result n the poor state estmate. Recent lterature n optmal estmaton theory plays more nterestng n mtgatng these negatve effects of the parameter uncertanty [1-6]. here are four possble methodologes to handle the parameter uncertanty problem. he frst one s to completely neglect uncertan parameter. Ignorng low mpact or well-calbrated parameter s reasonable, but gnorng hgh 1

2 mpact parameter uncertanty can brng large bas errors n the estmated state. he second one s to expand the state vector to nclude the parameters that may be uncertan as addtonal states. hs method wll lead to consderable computatonal power and processng tme requred, especally for the large dmenson systems. he thrd one s to consder the parameters, whch s nown as the Schmdt-Kalman flter [1] or consder Kalman flter[2]. hs method s that the state and covarance estmate are updated by usng the pre-estmated parameter covarance, wthout estmatng these parameters drectly. hs approach decreases the cost of the computatonal power and processng tme requred comparson to the second. One drawbac s that t requres nowledge of covarance of the parameter uncertantes. he fourth one s to decrease the senstve to devatons, whch ncludes the robust flters and the recent presented desenstzed Kalman flter by mnmzng a cost functon augmented by a penalty functon [6,7]. hese robust flters always need the norm-bounded parameter uncertanty. he desenstzed Kalman flter proposed by Karlgaard and Shen s another type of the robust Kalman flter wth nowledge of the senstvty-weghtng matrx [6]. Desenstzed Kalman flter (DKF) was frst developed by Karlgaard and Shen as means to account for the model parameter uncertantes by usng desenstzed optmal control technque n reference [6]. hey penalzed the cost functon consstng of the posteror covarance trace by a weghted norm of the state error senstvtes, whch means that ths cost functon was augmented wth the penalty functon consstng of a weghted norm of the state error senstvtes. Desenstzed state estmates were obtaned by mnmzng the above cost functon. hen, they extended the concept of the DKF to desenstzed unscented Kalman flterng [8], desenstzed dvded dfference flterng [9], n whch the cost functon was augmented the same penalty 2

3 functon. he DKF s non-mnmum varance, but exhbts reduced senstvty to devatons n the assumed dynamc model parameters. he DKF was appled nto an nducton motor state estmaton problem wth parameter uncertantes [10], and the effectveness of DKF was demonstrated. However, the DKF has two dsadvantages over the conventonal Kalman flter. he frst s to nown the senstvty-weghtng matrx. he second s to that obtan the gan matrx only by solvng a lnear equaton wthout an analytcal soluton. When the dmenson of the state vector or parameter vector s large, the cost of the computatonal power and processng tme requred ncrease sharply. hs note recombnes the state error senstvtes vector of each parameter to a total senstvty matrx of all parameters n the condton of the lnear dscrete model. A new cost functon s the sum of the posteror covarance trace and the trace of a weghted norm of the state error senstvtes matrx, not the sum of each penalty functon n lterature. Mnmzng ths new cost functon gves an analytcal soluton of the gan matrx, whch has the same structure wth the conventonal Kalman flter. Based on ths new framewor, we extend the results to the lnear contnuous model. II. Desenstzed Lnear Kalman Flter wth Analytcal Gan 2.1 Desenstzed Dscrete Lnear Kalman Flter wth Analytcal Gan Wthout loss of generalty, we dscard the determnstc nput and some matrces n the lnear dscrete model n reference [7] to smplfy the algorthm. Consder the process and measurement models gven by x Φ ( px ) w (1) z H () p x v (2) 3

4 where x s the n 1 state vector, and z s the m 1 measurement vector. Φ s the state 1 transton matrces, H s the measurement matrx. p s referred to as the 1 uncertan parameter vector. w and v are ndependent zero-mean Gaussan nose processes, and ther covarance are respectvely Q and R. hey satsfy E[ ww ] Q, E[ vv ] R, E[ wv ] 0 (3) j j j j j where j s the Kronecer delta functon, and Q 0, R 0. In ths wor, the uncertan model parameter s gven an estmate, p p ˆ, wth the a pror nowledge. In the Kalman flter, the estmated state propagaton equaton xˆ x ˆ (4) 1 1 and the measurement updated equaton zˆ H x ˆ (5) where the superscrpts denote a pror and + denote a posteror, and the overbar ndcates the correspondng estmate functon of the parameter, such as Φ Φ( p ˆ). hen, the Kalman flter provdes an optmal blendng of the x ˆ and z to obtan the a posteror state estmate va xˆ xˆ K ( z z ˆ ) (6) We defne the a pror estmaton error as e xˆ x and the a posteror estmaton error as e xˆ x, and assume these estmaton errors to be zero-mean. hen, the assocated error covarance matrces are P E[ e e ] Φ P Φ Q (7) P E[ ee ] ( IKH) P ( IKH) KRK (8) whch s vald for any K. he Kalman optmal gan K s chosen by mnmzng the cost functon J r( P ), and t s gven by 4

5 K P H Ξ (9) 1 where Ξ HPH R, and r denotes the trace of the matrx. Under the fundamental assumptons of the Kalman flter (zero-mean whte-nose sequence, unbased a pror estmaton errors, no model and parameter uncertanty, nown process and measurement models, etc.), the state estmate and state estmaton error covarance updates are optmal. In the presence of model parameter uncertantes, the dynamc model cannot match the true model. hs means that the fundamental assumptons of the Kalman flter cannot be satsfed, and the state estmates may be based and even dvergence. Karlgaard and Shen [7] proposed a desenstzed optmal flterng to mtgate the negatve effects of the uncertan parameters based on the cost functon of the state error senstvtes. hey defned the state error senstvtes and propagaton equatons of each parameter component p of p as e xˆ Φ σ Φ σ x (10) 1 ˆ, 1, 1 1 p p p e xˆ σ σ K γ (11),, p p H where γ H σ ˆ, x. Note that the senstvty of the true state s x p 0 n above p formulatons, and t s assumed that K p 0 n Eq. (11). A cost functon consstng of the posteror covarance and a weghted norm of the posteror senstvty s proposed as,, 1 J r( P ) σ W σ (12) where W s a n n symmetrc postve sem-defnte weghtng matrx for the th senstvty. hen, tang the dervatve wth respect to K, usng the trace dervatve propertes found n Appendx A, and settng J K 0, yelds 5

6 ,,,, 1 1 K Ξ WK γ γ PH Wσ γ (13) Note that the gan K must be solved wth the lnear equaton n Eq. (13) dfferng the analytcal gan matrx n the conventonal Kalman flter. hs mples that the cost of the computatonal power and the processng tme requred wll ncrease rapdly, especally when the dmenson of the state s large. In ths wor, we redefne the cost functon, whch also conssts of the posteror covarance and another weghted norm of the posteror senstvty, and obtan an analytcal soluton of the gan matrx. We redefne the state error senstvtes and propagaton equatons of the parameter vector p as e xˆ S Φ S Ψ p p (14) e ˆ x S S K γ p p (15) where Ψ Φ p xˆ (16) γ HS H. (17) p where H p H p xˆ. We redefne a new cost functon based on the trace of the weghted norm of the posteror senstvty matrx gven by J r( P ) r( S WS ) (18) a a where W a s a symmetrc postve sem-defnte weghtng matrx for the uncertan parameters. Substtutng Eqs. (8) and (15) nto Eq. (18) and tang the dervatve wth respect to 6

7 the gan K, and usng the trace dervatve propertes n Appendx A, yelds J a r( P ) r( SWS a ) K K K 2K Ξ 2PH 2SWγ 2K γ W γ a (19) Settng K 0 and smplfyng the formulaton gves the analytcal gan matrx as J a follows K ( P H S Wγ )( Ξ γ W γ ) (20) 1 a a Note that the formulaton of the gan K n Eq. (20) s the same as the conventonal Kalman flter n form and t s an analytcal soluton, too. Correspondng to the method n reference [6,7], the proposed cost functon and gan formulaton are completeness and clear at a glance, and the cost of the computatonal power and the processng tme requred decrease greatly. Even more mportant, t provdes a new algorthm framewor for the desenstzed optmal flterng ncludng the dscrete lnear flter and the contnuous lnear flter. 2.2 Desenstzed Contnuous Lnear Kalman Flter wth Analytcal Gan In ths study, the correspondng result for the lnear contnuous model wth the new cost functon and the analytcal gan n the new framewor s summarzed. he KSDKF for the contnuous case s n Appendx B. Consder the contnuous lnear system x () t Φ( p,) t x() t w() t (21) z() t H( p,) t x() t v () t (22) where x ( t) s the n 1 state vector, and z ( t) s the m 1 measurement vector. Φ( p, t) and Hp (, t) are the state transton matrx and the measurement matrx. w ( t) and v ( t) satsfy w N(0, Q( t) ( t )) and v N(0, R( t) ( t )). 7

8 he state estmate error s defne as e() t xˆ () t x () t. hen, the senstvty of the error to the parameter vector p s e() t xˆ () t S p p (23) and the correspondng senstvtes obey the propagaton equaton Φ S ΦS xˆ Kγ p (24) where Φ Φ( p ˆ, t), H H( p ˆ, t) and H γ HS xˆ p. he new cost functon, whch s reformulated to mnmze the rate of change of the state error covarance, augmented by the new penalty functon s J r( P ) 2 r( SWS ) (25) a a In the contnuous flter case, the optmal gan s obtaned as the dscrete flter, whch s found by tang the dervatve wth respect to the gan K, and the result s 1 K( PH γws a ) R (26) 2.3 Remars Remar 1: he penalty functons n the two algorthms for the dscrete case are dfferent, because the KSDKF consders the senstvty to each parameter respectvely and the ADKF does ths as a whole. From ther defntons, they satsfy S ( σ1,, σ2,,, σ, ) n (27) S ( σ1,, σ2,,, σ, ) n (28) he aforementoned two dfferent defntons generate the two dfferent penalty functons, σ 1 Wσ,, and r( SWS a ). he optmal gan K of the KSDKF n Eq. (13) s obtaned by solve a lnear matrx equaton, and the one of the ADKF n Eq. (20) s founded as the conventonal 8

9 Kalman flter n a well-nown form and t also s a closed-form soluton. he optmal gan of the ADKF greatly decrease the computatonal power cost and the processng tme requred. Remar 2: he relatons of the two senstvty defntons are the same as the dscrete case. he defnton of the penalty functon of the KSDKF n Appendx B, whch s dfferent from the dscrete case, s the product of the state error senstvty, ts rate of change and the correspondng senstvty matrx. Because the state error senstvty has not the gan matrx, so the analytcal gan s obtaned by tang the dervatve wth respect to the gan. So s n the ADKF for the contnuous case. Remar 3: he senstvty-weghtng matrx W n Eq. (12) s a n n weghtng matrx for the th senstvty respect to the state. But, how to determne each senstvty-weghtng matrx n Eq. (12) s not proposed n reference [6,7], and s an open problem to be resolved. he senstvty-weghtng matrx W n Eq. (18) s a weghtng matrx respect to the uncertan a parameters. Consderng the roles of the pre-estmated uncertan parameter covarance as n the consder Kalman flter [2,11]. hs covarance may be chosen as a referental senstvty-weghtng matrx n ADKF. It s reasonable that assgnng each covarance of the uncertan parameter to the correspondng senstvty-weghtng respect to all the states as n the consder Kalman flter. hen, we demonstrate ths n the followng numercal smulatons. Remar 4: he gan formulaton of ADEKF maes the DEKF recover the well-nown form of the Kalman flter, n whch the flterng algorthm manly ncludes fve basc equatons. Even more mportant, t provdes a new algorthm framewor for the DEKF ncludng the dscrete nonlnear model, the contnuous nonlnear model and the mxed contnuous-dscrete nonlnear model. 9

10 III. Numercal Results o compare the performance of the proposed ADKF and the KSDKF, the lnear dscrete-tme dynamc stochastc system n lterature [6] s consdered. he correspondng dynamc and measurement equatons are x x w (29) z x v (30) where and are two scalar uncertan model parameters, whch are assumed to be constants that ~ U( 0.1,0.1) and ~ U( 0.5,0.5),ndependently. As n lterature [6], we assumed that the ntal state x0 [10, 10] and P I. he noses are w ~ N( 0 21,0.1 I 22 ) and v ~ N ( 0, I ). In ths wor, we only consder two sets of values of the senstvty-weghtng matrx n an effort to compare these two methods. he senstvty-weght matrx of the ADKF s set as W a dag[0.003, 0.075], whch s nnety percent of the true covarance of the two uncertan parameters; the senstvty-weght matrces of the KSDKF have two values, W1 W 2 dag[0.003,0.075] and W1 W2 0.1I 2 2, whch s n lterature [6]. wo 5000 case Monte-Carlo smulatons wth 50 samples for the two flters runs are performed. In addtonal, the nomnal values for the uncertan parameters are ˆ 0, ˆ 0. he root mean square (RMS) errors of the state estmate and the penalty and total cost functons are calculated to compare the performance of the two methods at each epoch. Fgures 1 and 2 show the results when these senstvty-weghtng matrces are made equal as n the frst set. It can be seen that the RMS errors of the frst state x 1 of the ADKF are better than 10

11 the KSDKF n Fg.1 (a), and the RMS errors of the second state x 2 are about the same for the two methods n Fg.1 (b). Fgure 2 shows that the cost/penalty functons of the ADKF are all slghtly smaller than these of the KSDKF are. In a word, the performance of the ADKF s better than the KSDKF when they have the same senstvty-weghtng matrces. When these senstvty-weghtng matrces are set as n the second set, Fgures 3 and 4 show the results. Fg.3 (a) shows that the RMS errors of the frst state x 1 of the ADKF are better than the KSDKF, and n Fg.3 (b) the RMS errors of the second state x 2 have almost dentcal performance for the two methods. he cost and penalty functons of the ADKF are all smaller than the KSDKF n Fg.4. hs s maybe because the frst parameter senstvty-matrx of the KSDKF s not sutable, whch s one order greater n magntude than that of the ADKF. herefore, t s reasonable to choose the a pror covarance of the uncertan parameters as the senstvty-weghtng matrx n Eq. (18). Fg. 1 State RMS errors for the second set values. 11

12 Fg. 2 Cost functon and penalty functon for the second set values. Fg. 3 State RMS errors for the thrd set values. 12

13 Fg. 4 Cost functon and penalty functon for the thrd set values. References: [1] Schmdt, S. F., "Applcaton of state space methods to navgaton problems," Advanced n control systems, Academc Press, NewYor, 1966, pp [2] Woodbury, D. P., Majj, M., and Junns, J. L., "Consderng measurement model parameter errors n statc and dynamc systems," Journal of the Astronautcal Scences, Vol. 58, No. 3, 2011, pp [3] Yoon, M. G., Ugrnovs, V. A., and Petersen, I. R., "Robust fnte horzon mnmax flterng for dscrete-tme stochastc uncertan systems," SYSEMS & CONROL LEERS, Vol. 52, No. 2, 2004, pp [4] Gao, H. J., Meng, X. Y., and Chen,. W., "A new desgn of robust H-2 flters for uncertan systems," SYSEMS & CONROL LEERS, Vol. 57, No. 7, 2008, pp [5] Foo, Y. K., and Soh, Y. C., "Robust Kalman flterng for uncertan dscrete-tme systems wth probablstc parameters bounded wthn a polytope," SYSEMS & CONROL LEERS, Vol. 57, No. 6, 2008, pp [6] Karlgaard, C. D., and Shen, H. J., "Desenstzed optmal flterng," AIAA Gudance, Navgaton, and Control Conference, AIAA Paper , AIAA, Portland, Oregon, [7] Karlgaard, C. D., and Shen, H. J., "Desenstzed Kalman flterng," IE Radar, Sonar & Navgaton, Vol. 7, No. 1, 2013, pp [8] Shen, H. J., and Karlgaard, C. D., "Desenstzed unscented Kalman Flter about uncertan model parameters," Insttute of Navgaton Internatonal echncal Meetng, Newport Beach, Calforna, USA, [9] Karlgaard, C. D., and Shen, H. J., "Desenstzed dvded dfference flterng for nducton motor state estmaton," th Southeastern Symposum on System heory, Jacsonvlle, FL, [10] Karlgaard, C. D., and Shen, H. J., "Robust state estmaton usng desenstzed dvded dfference 13

14 flter," ISA transactons, Vol. 52, No. 5, 2013, pp [11] apley, B. D., Schutz, B. E., and Born, G. H., Statstcal orbt determnaton, 1st ed., Academc Press, New Yor, 2004, pp Appendx A- Matrx race Calculus o get the optmal gan from the cost functon n Kalman flter dervatons, tang the partal dervatve of the trace of matrx s often used. he correspondng results about the dervatves are r( KP) P K r( PK ) P K r( KPK ) KP KP K (31) (32) (33) Where K and P are two arbtrary matrces satsfyng matrx multplcaton rules. Appendx B- Desenstzed Contnuous Lnear Kalman Flter In ths appendx, the desenstzed contnuous lnear Kalman flter, whch was proposed by Karlgaard and Shen [6], s revewed. Consder the contnuous lnear system n Eqs. (21) and (22), the lnear Kalman flter for the contnuous case s xˆ () t Φxˆ () t K{ z() t Hxˆ ()} t (34) P ( Φ KH) P P( Φ KH) Q KWK (35) hen, the senstvty of the error to the parameter p s and the correspondng senstvty propagaton equaton s H where γ = ˆ + ˆ Hσ x ( t). p e xˆ( t) σ ˆ (36) p p Φ σ ˆ ˆ Φσ xˆ( t) Kγ (37) p he cost functon of the conventonal Kalman flter, J r( P ), s augmented by a penalty functon, whch s the product of the state error senstvty, ts rate of change and the correspondng senstvty matrx. he augmented cost functon s 14

15 J r( P ) 2σˆ ˆ Wσ (38) 1 he optmal gan s obtaned by tang the dervatve wth respect to the gan K, and the correspondng result s K ( PH Wσγ ˆ ) R 1 1 (39) 15