Converted Measurement Kalman Filter with Nonlinear Equality Constrains

Transcription

1 Converted Measureent Kalan Flter wth Nonlnear Equalt Constrans Xaoxue Feng, Yan Lang, Laneng Jao College of Autoaton Northwestern Poltechncal Unverst X an, Chna fengxaoxue@al.nwpu.edu.cn Abstract For nonlnear sstes, Converted Measureent Kalan flter as one of varous odfcatons of the Kalan flter can be used to estate the state wth the non-lnear easurng equatons, effectvel. Although the Converted Measureent Kalan flter s powerful tools for nonlnear state estaton, we ght have nforaton about a sste that the Converted Measureent Kalan flter does not ncorporate. For exaple, we a now that the states satsf equalt or nequalt constrants. In ths paper we odf the Converted Measureent Kalan flter to explot ths addtonal nforaton. A target tracng exaple s presented to llustrate the effectveness of Converted Measureent Kalan flter wth constrants, whch gets better flterng perforance than the unstraned Converted Measureent Kalan flter provdes. Sulaton results between frst-order and second-order nonlnear state constrants also show that the second-order soluton for hgher order nonlneart as present n ths paper outperfors the frst-order soluton. Kewords-state estaton; Converted Measureent Kalan flter; nonlnear equaton constrans I. INODUCION Kalan flters are coonl used to estate the state of a dnac sste based on ts state process and easureent odels, whch has wdel applcatons n an felds, such as navgaton and gudance sstes, radar tracng, sonar rangng, and satellte or arplane orbt deternaton[]. In actve sonar and radar sstes the easureent of the poston of a target s reported n polar coordnates (ts range and azuth wth respect to the sensor locaton). However, n target tracng the target oton can be best odeled n Cartesan coordnates. racng n Cartesan coordnates usng polar easureent can be handled n two was. One ethod s to use an extended Kalan flter (EKF)[], whch ncorporates the orgnal easureent n a nonlnear fashon nto the target state estate, resultng n a xed coordnate flter. However, t has been found that n case cross-range easureent errors of the target poston are large, the perforance of EKF degrades consderabl due to nonneglgble nonlnear effects. he other approach s to use converted easureent Kalan flter ()[3,], whch converts the polar poston easureent to Cartesan coordnates usng the falar nonlnear appng between the two coordnate sstes, eldng a easureent odel that s a lnear functon of the target s Cartesan state, after whch the classcal Kalan flterng algorth can be used to trac target entrel n Cartesan coordnates [5]. Apart fro the flterng sste equatons, we ght have nforaton about a sste that the Kalan flter does not ncorporate. For exaple, we a now that the states satsf equalt or nequalt constrants. here are an exaples of state-constraned sstes n engneerng applcatons. Soe of these exaples nclude, fault dagnoss [6], vson-based sstes [7], target tracng [8, 9], robotcs [], and navgaton []. We beleve that f we odf the Kalan flter to explot ths addtonal nforaton, t wll get better flterng perforance than the Kalan flter provdes. Ang at the estaton of the lnear sste, there are an ethods whch ncorporate the constrants nto the flterng process, such as the projecton ethods n [, 3]. In the nonlnear sste, there are also soe ethods to use. E.g., the ovng horzon estaton (MHE) s the ost accurate ethod, but t has heav burden n the coputaton []. In [5, 6], projecton ethods are used n the sga ponts under the unscented Kalan flter (UKF), whch can get a better result. In [7], the constrants had been ncorporated nto the partcle flter (PF), and the perforance of the sste s greatl proved b ncorporatng these constrants. In [8] Quadrature Kalan flter (QKF) wth equalt constrants was proposed, whch had better error perforance than that of extended Kalan flter (EKF) wth constrants. Consderng the portance of the estaton proble of Kalan flterng wth nonlnear constrants, we cobne the wth the projecton ethods through frst-order and second-order lnearzaton, and present a new nonlnear flterng ethod wth constrants. Eplo the alor seres expanson of the nonlnear constrants and antan the frst ter and second ter [9], separatel. he approxate lnear constrant s obtaned and we dscuss the two lnearzed ethods nfluenced on the state estaton results. In ths paper, our contrbuton s that we solve the proble of the wth nonlnear equalt constrants, explot secondorder nonlnear state constrants provdng better approxaton for hgher order nonlneartes and deonstrate the effectveness of the new ethod on a nonlnear vehcle tracng exaple, copared wth the unstraned Converted Measureent Kalan flter. he paper s organzed as follows. Secton II presents a bref suar of nonlnearl constraned state estaton. Secton III detals the Converted Measureent Kalan Flter to solve the nonlnear state estaton, and the frst-order and second-order lnearzaton to extend the projecton ethod to hs paper s supported b the Natonal Natural Scence Foundaton of Chna (Grant No. 635, 6779). 8

2 nonlnear cases. Secton IV presents soe sulaton results to llustrate the algorth. Fnall, secton V closes wth soe concludng and suggestons for future wors. II. POBLEM FOMULAION Consder the followng dnac equaton and easureent equaton of the sste gven b: x f ( x ) + v () h ( x ) + n () where f and h are state transton functon and easureent functon, the are nonlnear or lnear. { v } and { n } are nose nputs. he subject to Gauss dstrbuton, zero-ean and wth covarance Q and. he two tpes of nose are utuall ndependent. Gven the ntal state vector and the assocated covarance, the proble s to estate the state vector of ever step b usng correspondng easureent data. he above descrpton s the general nonlnear flterng proble. Now suppose there are soe nonlnear constrants n the state evolveent whch are forulated usng a equalt forula as g( x ) d (3) Dx d () where g denotes a state constraned functon and d s a vector. Partcularl, D denotes a state constraned atrx. In a sense, (3) s a general forulaton. he are acqured fro the analss of the state estaton. At ths te, the case s called a nonlnear flterng proble wth nonlnear or lnear equalt constrants. For the nequalt, we can use the expanson of the equalt constrants le []. For convenence, n ths paper we onl dscuss the equalt constrants. One ethod to deal wth the equalt lnear constrants has been presented b Dan Son n [, 3]. It s a projecton ethod n essence. It s an effectve ethod and has a good perforance. he detaled dervaton s gven n the followng. Let x denotes the odfed state estaton, W denotes arbtrar setrc postve defnte atrx, ˆx denotes the general state estaton before consderng the condton of constrants. he estaton proble wth state constrants translates nto an optzaton proble as below n J ( x ) ( x xˆ) W( x xˆ) s.t. Dx d Use the Lagrange ultpler ethod to solve (5). Frstl, we construct the forula (5) J ( x, λ) ( x xˆ) W( x xˆ) + λ ( Dx d) (6) Subsequentl, calculate the partal dervatves of (6), and the results of each coponent are set zero,.e., J W( x xˆ ) + D λ x J Dx d λ B solvng equaton set (7) we obtan x x W D DW D Dx d (7) ˆ ( ) ( ˆ ) (8) he ethod s called projecton ethod va odfng state estaton. hat s to sa, the current state estaton s projected on the constraned subspace. III. CONVEED MEASUEMEN KALMAN FILE WIH EQUALIY CONSAINS A. In 3D target tracng scenaro, a sensor easures a target s poston, producng the sphercal poston easureent where [,, ] Z ρ ρ ρ + ρ s the target s true poston n sphercal coordnates (range, azuth, and elevaton) and ρ,, s a whte, zero-ean, Gaussan easureent nose wth covarance ρ cov ( ρ,, ) σ σ (9) σ. () he sphercal easureent s transfored to Cartesan coordnate easureent usng the classcal converson x ρ cos cos η: ρcos sn. () z ρsn So, the converted easureent s error s x x x z z z. () ρ cos cos ρcos cos ρcos sn ρcos sn ρsn ρsn For Equaton () we can see that the converted easureent error s no longer zero-ean Gaussan dstrbuted because of the nonlnear coordnate transforaton. So, debasng the raw converted easureent s necessar before the classcal Kalan flterng. 8

3 Now there are several approaches to copute the practcal bas and covarance approxatons of the converted easureent s error. Lerro and Bar-Shalo[, ] frstl studed explct solutons for the ean and covarance of the converted D easureent and presented a debased (D) algorth whch provdes ore accurate state estate than EKF and tradtonal. Now e detals the process of as follows. Let λ E cos e ' λ E cos e λ E e ' [ cos ] [ cos ] σ σ λ E e σ σ. (3) hen, the real ean of converted easureent error s wth eleents Let {[ ] ρ} μ( ρ,, ) E x,, z,, [ μ, μ, μ ] x z () μx ρcos cos ( λλ ) μ ρsn cos ( λλ ). (5) μ ρsn ( λ ) z x α x sn snhσ + cos coshσ α sn coshσ + cos snhσ. (6) αz sn snhσ + cos coshσ α sn snhσ + cos coshσ hen, the real covarance of debased converted easureent error s wth eleents [ x z ] ( ρ,, ) cov(,, ρ,, ) xx x xz x z xz z zz [ ρ ( α α cos cos ) + σ α α ] λ λ xx x x ρ x x ' x [ ρ ( αx λ cos ) + σραx ]sn cos λλ ' xz [ ρ ( λ ) + σρ]cos sn cosλλ [ ρ ( α α sn cos ) + σ α α ] λ λ x x ρ x ' z [ ρ ( λ ) + σρ]snsn cosλλ zz [ ρ ( αz sn ) + σραz ] λ (7). (8) p p p Substtutng the state predcton ( ρ,, ) nto Equaton () and (7), we can get the ean and covarance of the converted easureent error condtoned on the state predcton and p μ μ (,, ) (9) p p p ρ ρ ρ,, p (,, ) () p p p ρ ρ ρ,, B. Frst-order lnearzaton nonlnear constrants For the nonlnear constrant (3), we can perfor a alor seres expanson of the constrant equaton around xˆ to obtan gx ( ) gx ( ˆ ) + g ( xˆ )( x xˆ ) s +/ e ( x xˆ ) g ( xˆ )( x xˆ ) () where s s the denson of g( x ), e s the th natural bass s vector n, and the entr n the p th row and qth colun of the n natrx g ( x) s gven b g ( x) () pq x x [ g ( x) ] Neglectng the second-order ter gves [7] p q g ( xˆ ) x b g( xˆ ) + g ( xˆ ) xˆ (3) hs equaton s equvalent to the lnear constrant Dx df D g ( xˆ ) () d b g( xˆ ) + g ( xˆ ) xˆ (5) hus nonlnear constrants are lnearzed. Soetes, though, we can do better than sple frst-order lnearzaton, as dscussed n the followng sectons. C. Second-order lnearzaton nonlnear constrants If we eep the second-order ter n () then the constraned estaton proble can be approxatel wrtten as such that x argn ( x xˆ ) W( x xˆ ) (6) x xmx+ x+ μ (,, s) (7) where W s a weghtng atrx, and M, and μ can obtaned fro ().he optzaton proble gven n (6)- (7) can be solved wth a nuercal ethod. A Lagrange ultpler ethod for solvng ths proble s gven below. Followng the constraned Kalan flterng of [7], we can forulate the projecton of an unconstraned state estaton 83

4 onto a nonlnear constrant surface as the constraned leastsquare optzaton proble x argn ( x xˆ ) W( x xˆ ) (8) x subject to f( x ) (9) Let W H H and z Hxˆ, the forulaton n (8) becoes the sae as n (6). In a sense, (8) s a ore general forulaton because t can also be nterpreted as a nonlnear constraned easureent update or a projecton n the predcted easureent doan. he soluton to the constraned optzaton (8) can be obtaned agan usng the Lagrangan ultpler technque, as xˆ G V( I λ ) e( λ) + Σ Σ (3) e ( λσ ) e ( λ) t q( λ) + + j (3) ( + λσ ) + λσ where G s an upper rght dagonal atrx resultng fro the Choles factorzaton of W H H as W( H H) G G (3) V, an orthonoral atrx, and Σ, a dagonal atrx wth ts dagonal eleents denoted b σ, are obtaned fro the sngular value decoposton (SVD) of the atrx LG as LG UΣ V (33) where U s the other orthonoral atrx of the SVD and L results fro the factorzaton M LL, and e e V G H z (3) (35) ( λ) [ ( λ) ] ( ) ( λ ) t [ t ] ( V G ) As a nonlnear equaton n λ, t s dffcult to fnd a closed-for soluton n general for the nonlnear equaton q( λ ) n (3). Nuercal root-fndng algorths a be used nstead. IV. SIMULAION ESULS Assue that a target does crcular oveent, the center of the trajector s the orgn of a Cartesan coordnate sste, and the radus s r. he angular veloct s θ deg/s, and the ntal state vector s x [ ξ ξ ζ ζ] [ ].here s a sensor whch locates at the orgn to easure the trajector of the target. he saplng nterval s s.he easureent equatons are as follows: r ξ + ζ + v (36) r ξ θ atan( ) + v θ (37) ζ arget state transton equaton used n the tracng s.5 x+ x + n (38).5 where process nose n and easureent nose v subject to Gaussan dstrbuton, zero-ean wth covarance Q dag([ ]) and dag([9. ]), and the are utuall ndependent. he ntal state estaton covarance s P dag([5 5 ]). Fro trajector of the target, we are eas to now that there s a constrant forulated as below: gx ( ) ξ + ζ (39) otal sulaton step s,.e., the target turn around twce centered at orgnal. he above process s repeated for Monte-Carlo sulatons. MES of poston x MSE of veloct x MES of poston MSE of veloct 8 6 wth constrants 6 8 Sulaton Steps: 3 wth constrants 6 8 Sulaton Steps: 8 6 wth constrants 6 8 Sulaton Steps: 3 wth constrants 6 8 Sulaton Steps: Fg. MSE of and constraned Fg. l show that state estaton error of unconstraned and wth constrants. It can be seen that the constraned results n uch ore accurate estates of 8

5 poston than the unconstraned. he unconstraned flter results n average poston errors of about, whle the constraned flter results n poston errors of about. In addton for ths sulaton scenaro, the unconstraned perfors veloct errors dentcall wth constraned. he reason of ths phenoena s that the state constrants just about poston x and, wthout veloct. In the sulaton, for coparson the frst-order nonlnear constrants and second-order nonlnear constrants of flterng accurac of the proposed algorth, we plot ever coponent of the state estaton results usng the unstraned, wth frst-order lnearzed constrants (LC) and wth second-order expanson constrants (C), as shown n Fg.. It can be seen that the estaton results of LC s uch cruder than C, even cruder than unstraned. he reason of ths phenoenon s anl the state constrants s hgh order nonlneart. hus we can conclude that for hgher-order nonlnear constrants, the second-order soluton as presented n ths paper would outperfor a frst-order soluton. x() Vx(/s) () V(/s) - x-eal x-second-order-constr x-constr x-est Sulaton Steps: - Vx-eal Vx-second-order-constr Vx-constr Vx-Est Sulaton Steps: - -eal -second-order-constr -constr -Est Sulaton Steps: - V-eal V-second-order-constr V-constr V-Est Sulaton Steps: Fg. State estaton of dfferent ethods V. CONCLUSION In order to solve the state estaton proble wth nonlnear constrants n the nonlnear sste, n ths paper, we ncorporated second-order nonlnear state constrants nto. It can be vewed as an extenson of wth lnear state equalt constrants to nonlnear cases. Sulaton results deonstrated that, the proposed ethod got a ore accurate error perforance than the unconstraned provdes. For hgher-order nonlnear constrants, the second-order lnearzaton nonlnear constrants would outperfor a frstorder soluton. he contrbuton of ths paper enrches the nonlnear flterng under the condton of constrants. In ths paper, we utlze the Newton s ethod to fnd the nuercal root n (3). Our future wors wll nclude searchng for ore effcent root fndng algorth to solve for the Lagrangan ultpler. Other drectons of our future wor wll show ore theoretcal results related to convergence and stablt for nonlnear constraned flters, such as the UKF and MHE. EFEENCES [] W.Sorensor. "Kalan Flterng: theor and applcaton", IEEE Press, 985. [].L.Song, J.L.Speer. "A stochastc analss of a odfed gan extened Kalan Fllter wth applcaton to estaton wth bearng onl easureents ", IEEE ranscaton on autoatc Control. 985, 3(): 9-99 [3] D. Lerro and Y. Bar-Shalo, "racng wth debased consstent converted easureents versus EKF" [J], IEEE ransactons on Aerospace and Electronc Sstes, vol.9, pp.5-, Jul 993. [] M. Longbn, S. Zaoquan, Z. Yu, S. Zhong Kang, and Y. Bar-Shalo, "Unbased converted easureents for tracng" [J], IEEE ransactons on Aerospace and Electronc Sstes, vol.3, no.3, pp.3-7, Jul 998. [5] Y. Bar-Shalo and X.. L, "Estaton and tracng: prncples", echnques, and Software [M], Norwood, MA: Artech House, 993. [6] Son D., Son D.L. "Kalan flterng wth nequalt constrants for urbofan engne health estaton", IEE Proc. Control heor Appl., 6, 53, (3), pp [7] Porrll J. "Optal cobnaton and constrants for geoetrcal sensor data", Int. J. obot. es., 988, 7, (6), pp [8] Alouan A., Blar W."Use of a neatc constrant n tracng constant speed, aneuverng targets", IEEE rans. Auto. Control, 993, 38, (7), pp. 7 [9] Wang L., Chang Y., Chang F. "Flterng ethod for nonlnear sstes wth constrants", IEE Proc. Control heor Appl.,, 9, (6), pp [] Spong M., Hutchnson S., Vdasagar M.: "obot odelng and control" (John Wle & Sons, 5) [] Son D., Cha. "Kalan flterng wth state equalt constrants", IEEE rans. Aerospace Electron. Sst.,, 38, (), pp [] D. Son, "Kalan flterng wth state constrants: a surve of lnear and nonlnear algorths," IE Control heor & Applcatons,, Vol., Iss. 8, pp [3] D. Son and. Cha, "Kalan flterng wth state equalt constrants," IEEE rans on Aerospace and Electronc Sstes, vol. 38,no. I, pp. 8-36,. [] H. Mchalsa and D. Mane, "Movng horzon observers and observer based control," IEEE rans on Autoatc Control, vol., no. 6, pp ,995. [5] S. KoJls, B. A. Foss,. S. Sche, "Constraned nonlnear state estaton based on the UKF approach," Coputers and Checal Engneerng, vol. 33, pp. 386-,9. 85

6 [6] S. Juler and J. LaVola, "On Kalan flterng wth nonlnear equalt constrants," IEEE ransactons on Sgnal Processng, vol. 55, no. 6, pp , 7. [7] Y. Boers and H. Dressen, "Partcle flter trac-before-detect applcaton usng nequalt constrants," IEEE rans on Aerospace and Electronc Sstes, vol., no., pp. 8-87,5. [8] Jnguang Chen, Ll Ma, "Quadrature alan flter wth equalt constrants" IEEE Ffth Internatonal Conference on Bo-Inspred Coputng: heores and Applcatons (BIC-A), 3-6, Sept., Changsha, Chna, pp:97-. [9] Yang C., Blasch E. "Kalan flterng wth nonlnear state constrants", IEEE rans. Aeros. Electron. Sst., 8, 5, (), pp. 7 8 [] P. Suchos, "Explct expressons for debased statstcs of 3D converted easureents" [J], IEEE ransactons on Aerospace and Electronc Sstes, vol.35, pp , Januar 999. [] Z. Duan, C. Han, and X.. L, "Coents on Unbased converted easureents for tracng" [J], IEEE ransactons on Aerospace and Electronc Sstes, vol., no., pp , October [] Yang, C., Bach, M., and Blasch, E. "Nonlnear constraned tracng of targets on roads". In Proceedngs of the 8th Internatonal Conference on Inforaton Fuson, Phladelpha, PA, Jul 5, A8-3, 8. [3] Yang, C., and Blasch, E. "Kalan flterng wth nonlnear state constrants". In Proceedngs of the 9th Internatonal Conference on Inforaton Fuson, Florence Ital, Jul 6. 86