Sparse Gauss-Hermite Quadrature Filter For Spacecraft Attitude Estimation

Transcription

1 Aerca Cotrol Coferece Marrott Waterfrot, Baltore, MD, USA Jue -July, ha5.4 Sparse Gauss-Herte Quadrature Flter For Spacecraft Atttude Estato B Ja, Mg, Yag Cheg Abstract I ths paper, a ew olear flter based o Sparse Gauss-Herte Quadrature (SGHQ) s proposed for spacecraft atttude estato. Gauss-Herte Quadrature (GHQ) has bee wdely used uercal tegrato ad olear flterg. However, for ult-desoal probles, the covetoal GHQ based flter usg product operatos s dffcult to pleet because the uber of pots creases expoetally wth desos. o solve ths proble, the Solya s product rule has bee used to exted GHQ rule to hgh desoal probles. he cotrbuto of ths wor s to desg a ew sparse-grd GHQ flter usg Solya s product rule to allevate the curse-of-desoalty proble of the covetoal GHQ flter. he uber of SGHQ pots eeded for hgh desoal probles s cosderably saller tha the orgal GHQ ethod. Hece, the effcecy of usg GHQ ca be sgfcatly proved. he perforace of ths ew flter s deostrated by the applcato to the spacecraft atttude estato proble, whch shows better results tha the Exteded Kala Flter (EKF). M I. INRODUCION ANY olear flterg ethods [-4], such as EKF, Usceted Kala Flter (UKF), ad Partcle Flter (PF) have bee used for spacecraft atttude estato sce t s essetally a olear flterg proble. I [,], EKF, especally the for of ultplcatve exteded Kala flter, was desged for atttude estato. I [], Usceted Kala Flter (UKF) was used sce UKF s ore accurate ad robust tha EKF for geeral olear probles. Partcle flter was show [4] to acheve better accuracy for atttude estato at the expese of hgher coputatoal load. Aog the olear flterg techques, Gauss-Herte Quadrature Flter (GHQF) has proved to be effcet ad successful solvg estato probles whe the state ad ose dstrbutos are Gaussa [5, 6]. It s well ow that GHQ wors very well for oe desoal proble. It ca also be exteded to ult-desoal probles by the product rule [7]. However, ths approach suffers fro the curse of desoalty because the uber of pots creases expoetally wth the syste deso. Sparse grd ethod was orgally proposed ad wdely used to allevate the curse of desoalty proble B Ja s a PhD studet at Msssspp State Uversty, Starvlle, MS 976, USA (eal: bj79@sstate.edu) Mg s a Assstat Professor at Msssspp State Uversty, Starvlle, MS 976, USA (eal: x@ae.sstate.edu); Correspodg Author. hs wor was supported part by the Natoal Scece Foudato CAREER Award (ECCS ) Yag Cheg s a Assstat Professor at Msssspp State Uversty, Starvlle, MS 976, USA (cheg@ae.sstate.edu) uercal tegrato [7-]. he orgal dea of sparse grd ethod ca be traced bac to Russa atheatca Solya [7], who used a specal ethod to choose pots ad ade the uber of ecessary pots draatcally saller tha that usg the drect product rule. As a result, the coputatoal cost does ot crease expoetally usg the sparse grd ethod. I [8], the sparse grd ethod based o Cleshaw-Curts ad Gauss (Patterso) rules was used to solve the ultdesoal tegral proble. I [9], ult-desoal tegrals volved the lelhood fucto of ecooetrc odels were calculated based o the sparse grd ethod ad Gaussa Quadrature. Gaussa Quadrature ad sparse grd tegrato techology were used [] to estate the paraeters the logt odel. Deso adaptve quadrature ethod based o sparse grds was proposed [] to solve the uercal tegrato of ultvarate fuctos. Effcet pleetato of ths ethod was also dscussed. I ths paper, Solya s product rule s used to calculate the ultdesoal tegral that s used GHQF [6]. hs leads to a ew olear flterg algorth called Sparse Gauss-Herte Quadrature flter (SGHQF). Based o ths ew flter algorth, a ew atttude estato algorth s developed. he rest of ths paper s orgazed as follows: the Bayesa estato fraewor ad GHQF are brefly revewed Secto II. Secto III troduces the Solya s product rule. Secto IV revews the eatcs ad sesor odel for the spacecraft atttude estato proble. Secto V gves the SGHQF algorth for spacecraft atttude estato. Secto VI gves the sulato results llustratg the perforace of SGHQF for the atttude estato ad aes coparsos wth EKF. Soe cocluso rears are gve Secto VII. II. GAUSS-HERMIE QUADRAURE FILER I ths secto, the fraewor of Bayesa olear flterg ad GHQF [6] are brefly revewed. Cosder the dyac equato of a olear dscrete-te syste: x = f ( x ) ν wth the easureet equato y = h( x ) where ν ad are process ose ad easureet ose, respectvely. I ths paper, we assue that ν s whte Gaussa ose wth covarace Q ad s whte Gaussa ose wth covarace R //$6. AACC 87

2 . Bayesa olear flterg fraewor he Bayesa flterg cludes recursve dcto ad update procedures []. Gve the pror probablty desty fucto of the syste p( x y: ), the codtoal desty p( x y: ) satsfes the Chapa-Kologorov equato []. p x y = p x x p x y dx : : Whe the easureet at te s avalable, the eror codtoal desty satsfes the followg equato [] p( y x) p( x y: ) p( x y: ) = (4) p y x p x y dx where ( ) : p y x s the lelhood fucto. Forula s called dcto forula whle forula (4) s called update forula. he oets, such as ea ad p x y. covarace, ca be calculated fro. Gauss-Herte Quadrature Flter he tegral forulae ad (4) dscussed the vous subsecto ca be calculated by GHQ rule f the state ad ose dstrbutos are Gaussa [6]. he oe-desoal GHQ rule ca be descrbed by E( f ( x) ) = f ( x) N( x;,) dx w f ( γ ) (5) R where s the uber of pots used for oe-desoal γ ad w are Gauss-Herte quadrature pots ad tegral; weghts, respectvely. ( ;,) : N x deotes a Gaussa dstrbuto wth zero ea ad ut varace. he γ ad w ca be calculated by the followg procedures. Assue that J s a syetrc tr-dagoal atrx wth zero dagoal eleets ad J, = J, = /,. he the quadrature pot γ s calculated by γ = ε, where ε s the th egevalue of J ; he correspodg w s calculated by w = v, where ( v ) s the frst eleet of the th oralzed egevector of J [6]. For -desoal probles, the GHQ rule ca be exteded by the followg rule [6]: E ( f ( x) ) = f ( x) N ( x;, I ) dx (6) w (,, ) w ( γ ) w f γ γ f = = = where γ = γ,, γ ad w = w p= p Note: he uber of total pots eeded for -desoal probles s. It s obvous that the uber of pots crease expoetally wth the deso. Whe the GHQ rule s used to calculate the tegral, the Bayesa flter algorth ca be rewrtte by the GHQF algorth [6]. Gve the tal estated state ˆx ad covarace P, the correspodg dcto ad update steps ca be suarzed as follows [6]: Predcto Step Copute the factorzato of P = SS ad set = Sγ xˆ he xˆ = w f ( ) (7) ( ( ) ˆ )( ( ) ˆ ) (8) P = w f x f x Q Update Step Copute the factorzato of P = SS ad set = S γ xˆ xˆ = xˆ L y z (9) he xz P = P L P where y s the true easureet value at te ( ) z = h w ( ˆ ) ( ) P w x h z xz = ( ( ) ) ( ) P w h z h z zz = L = Pxz R P zz (4) he detals of the above dervato ca be foud [6]. III. SMOLYAK S PRODUC RULE FOR GAUSS-HERMIE QUADRAURE As ca be see Secto II, although the GHQ rule s effcet for the oe-desoal tegrato, the uber of pots eeded for hgh desoal probles creases expoetally. As a result, GHQF ca oly be pleeted for lower desoal probles. Usg the Solya s forula, however, the hgh desoal quadrature forula ca be replaced by a ew lear cobato of lower desoal tesor product forula. he uber of pots wll be sgfcatly reduced. he Solya s rule s a geeral ethod for ultvarate exteso of the uvarate approxato ad tegrato operators. he rule s descrbed by the forula [] f x N x;, I dx I f, = q= q Ξ N where q ( I I ) f, q (5) I f s the ultdesoal tegral of the fucto f ad a Gaussa dstrbuto wth the accuracy level, N. s the deso. q s a auxlary paraeter for the coveece of otato. deotes tesor product. 874

3 Deote { } Ξ=,,. For ay oegatve teger q, the equato (5) has the followg for Ξ : d = q for q N N q = d = for q < where N s the set of atural ubers wth eleets. I j N Fg. : Costructo of sparse grds for deso ad accuracy level N q s the uvarate tegral approxato I = (,) j f x N dx= f x w x (6) j x j where j =. j Ξ. j s the uvarate pot set. Fro the Solya s rule (5), the correspodg set of sparse grds defed by, s = (7), q= Ξ Nq where deotes uo operato of the pot sets. We use the Solya s rule to exted the oe-desoal Gauss-Herte quadrature rule to the ultdesoal probles. We call ths ew quadrature Sparse Gauss-Herte Quadrature (SGHQ). For better llustrato of SGHQ, we use = ad = (7) to show how to use the Solya s rule to costruct the ultdesoal Gauss-Herte Quadrature pot set, by usg uvarate Gauss-Herte Quadrature pot sets, ad as show Fg. I ths paper, we use as the uber of pots [] that are eeded for uvarate Gauss-Herte quadrature wth level. he uvarate pot sets, ad for level, ad respectvely are show o the top left of Fg.. It should be otced that the uvarate Gauss-Herte quadrature pot set at the dfferet level oly shares the dpot ad other pots could be dfferet. Sce = ad = for,, by the forula (7), q ca be or. Whe N,,, ; whe q =, q =, = q {{ } { }} = q {{, },{, },{,} } Covetoal tesor Product Rule N Sparse grd, N. he frst cobato of N s = ad =. hs geerates the tesor product. Slarly, the secod cobato of N s = ad =, whch geerates the tesor product. Other tesor products N ca be geerated the sae way. he fal sparse grd pot set, cludes all the cobatos the N ad N. he pot set s show o the botto rght of the fgure, whch cotas pots. However, for covetoal tesor product rule, the fal pot set eeded s show o the upper rght of Fg., whch cotas 49 pots because the tesor product uses the sae accuracy level each deso for varable Gauss-Herte product rule, whch results. For coveece, the uber of pots eeded for SGHQ ad GHQ of accuracy level, ad ad of deso fro to 6 are lsted able ad able respectvely. able Pots eeded for SGHQ Lev\D able Pots eeded for GHQ Lev\D Fro these two tables, t s obvous that SGHQ uses draatcally fewer pots tha GHQ for hgh desoal tegrals. Besdes, for a gve accuracy ad creasg deso, the uber of pots does ot crease expoetally as that of usg the covetoal product rule, but oly polyoally[]. IV. SPACECRAF AIUDE KINEMAICS MODEL Atttude ca be reseted by ay paraeterzatos such as Euler Agler, Geeralzed Rodrgues paraeters (GRPs) ad Quatero. Quatero s frequetly used sce t has o sgularty proble []. I ths secto, the atttude quatero eatcs ad gyro odel are gve for atttude estato. 4. Process Model Quatero Keatcs he atttude of spacecraft ca be reseted by quatero, q = q, q 4, where q = q, q, q s the vector copoet. he quatero eatcs ca be descrbed by [] q = Θ ( q t ) ω t (8) q q = wth ω s the agular velocty vector ad qi 4 [ q ] Θ ( q ) = where[ q ] q s the cross-product atrx, 875

4 q q q = q q q q [ ] he equvalet dscrete-te eatc equato to propagate the quatero s q =Ω ( ω ) q (9) wth cos(.5 ω Δt) I ψ ψ Ω ( ω ) = ψ cos(.5 ω Δt) whereω s the agular rate at the -th saplg terval. ( t) ψ = s.5 ω Δ ω / ω, Δt s the saplg te terval ad ψ s a cross product atrx. Agular Rate Sesor Measureet Model A wdely used odel for agular rate easureet s gve by [] ω t = ˆ ω t β t ηv t β t = η t where ( t) u ω s the easured agular rate, η ( t ) ad η ( t ) v u are depedet Gaussa whte ose processes wth zero ea ad stadard devatos of σ v ad σu respectvely. β ( t) s the gyro bas vector. Gve the update estate, the estated agular velocty s gve by [] ˆ ˆ ω = ω β ad the propagated gyro bas follows ˆ ˆ β = β 4. Observato odel he easureet odel for atttude estato s [4]: y = Ar (4) where s the easureet ose. It s assued to be whte Gaussa dstrbuto wth zero ea. r ad y are a observato par acqured two dfferet Cartesa coordate systes at te, ad A s the rotato atrx whch has the followg for A = A q = q4 qq I qq q4 q (5) where q = q, q 4 s the quatero at te. V. SPARSE GAUSS-HERMIE QUADRAURE AIUDE ESIMAION Although quateros have ay advatages, they ust obey the oralzato costrat, whch ca be volated by the stadard EKF [] ad GHQF. I ths paper, we use the ucostraed geeralzed Rodrgues paraeters (GRPs) [] to reset the three-copoet atttude error. Defe a 6 state vector as follows: x = δ p β (6) where β s the gyro bas ; s the geeralzed Rodrgues paraeters to reset atttude errors, whch s defed by = (7) a 4 where a s a paraeter [, ] ad s a scale factor. Whe a = ad f c =, equato (7) gves the Gbbs vector ad t gves the stadard vector of odfed Rodrgues paraeters (MRPs) whe a = f c =. ad 4 are error quateros whch wll be used later. I the followg atttude estato algorth, we assue the uber of quadrature pots s N. Gve the tal estates of atttude quatero ˆq ad gyro-bas, the tal covarace P, ad the tal estate of state ˆ ˆ x = β as well as the process ose covarace Q ad the easureet covarace R, the dcto step ad update step of SGHQF used for atttude estato ca be suarzed as follows: Predcto Step:. Copute the factorzato of P = SS by usg sgular value decoposto ad set ( ) = Sγ xˆ where = ˆ, =,,, N ; β ad s the pot dex. ( ) s the atttude error copoet ad s the gyro bas copoet. Note: Sce we use (9) for atttude propagato, we eed to trasfor ( ) to error quatero ad the calculate the correspodg quatero that wll be used propagato. he error quatero δq = δq, δq4 ca be calculated by [] δ δq = f a δq p c 4 a ( a ) 4 = Assue that ˆ = q he correspodg quatero s gve by * ˆ = q he superscrpt resets the dcto step. * deotes the quatero product operato.. he dcted quatero ( ) s propagated usg equato (9) ( ) ˆ ( ) =Ω ω 876

5 where ω = ω ˆ Sce the state vector s defed by GRPs, the quatero ( ) eeds to be trasfored to GRPs order to use the SGHQF algorth. Frst, the correspodg error quatero δq = [ δq, δq ] s calculated by 4 = * ˆ q he, the correspodg dcted atttude error ( ) gyro-bas of ( ) quatero: ad are calculated fro the error = a 4 =. he correspodg propagated state vector value ad covarace are gve by the SGHQF algorth N xˆ = w = N = ( ˆ )( ˆ ) = P w x x Q 4. Sce the observato odel (4) s a fucto of quatero, the frst three eleets of x ˆ eeds to be trasfored to error quatero for ad the calculate the dcted quatero, whch s the a pror estate quatero. he correspodg error quatero δq = δq, δq 4 s gve by ( ) c c ( ) = c 4 δq f a δq a f f a 4 = where s the frst three eleets of x ˆ. he dcted quatero s gve by = * Reset to zeros. Update Step:. Copute the factorzato P = SS ad set ζ = S γ xˆ. = Sce GRPs caot be drectly used the easureet equato (4), ζ eeds to be trasfored to the error quatero for ad the we ca calculate the correspodg quatero ( ) whch wll be used (4). where ζ ζ ζ ˆ Note: We use q to deote quateros used update step ˆ to avod cofuso wth quateros q used dcto step. he error quatero ( ) s gve by δq δq, δq where = 4 = ( ) δq f a δq ζ c 4 a ζ ( a ) ζ 4 = ζ he, the quatero ( ) ˆ δ s calculated by q q * =. States are updated usg the SGHQF algorth as follows xˆ = xˆ L y z P = P L Pxz where y s the true easureet value at te. N ( ˆ ) z w A q r = N Pxz = w ( ζ xˆ )( A( ) r z ) N zz = ( ( ˆ ) ) ( ( ˆ ) ) P w A q r z A q r z L = P R P xz zz. Post updated quatero q ˆ s calculated usg the frst three eleets of x ˆ,.e. ζ ad ˆ q ( ),.e. = * where δq δq, δq = 4 = c 4 δq f a δq ζ ( ) wth a a ζ 4 = f c ζ ζ Reset to zeros. VI. NUMERICAL RESULS AND ANALYSIS I ths secto, several test scearos are sulated. he orbt paraeters used here s obtaed fro RMM spacecraft []. Oly three-axs agetoeter (AM) ad gyroscopes are used. he agetc feld referece odel s the th teratoal Geoagetc Referece Feld Model. he ose of the AM odel s zero ea whte Gaussa ose wth stadard devato of 5. he gyro ose s also assued to be whte Gaussa dstrbuto wth zero ea 877

6 ad stadard devato of σ v 7 / σ u / =.6 rad / s ad =.6 rad / s. he paraeters GRPs are set to a =, f c = 4. I the frst scearo, EKF ad SGHQF (Level ad Level ) are tested wth o tal atttude estato errors, ad tal bas estates are set to zeros. he tal covarace s gve by P = dag.5,.5,.5,. / h,. / h,. / h Sce both flters have very slar results ths case, the results are ot show the paper. he secod case s to add degrees error to the tal atttude estate for each axs usg the sae paraeters as those the frst scearo. he tal covarace s assued to be P = dag ( ),( ),( ),(. / h ),(. / h ),(. / h) he atttude error of ths case (5 tes Mote Carlo sulato) s show Fg.. Both SGHQF (Level ) ad SGHQF (Level ) coverge wth ther σ error bouds whle EKF does ot. Moreover, SGHQF (Level ) coverges faster tha SGHQF (Level ). he or of atttude errors for ths case (5 tes Mote Carlo sulato) s show Fg.. Fro the coparso of perforace, t ca be see that both SGHQF (Level ) ad SGHQF (Level ) are ore accurate tha EKF. SGHQF (Level ) s slght better tha SGHQF (Level ). Sce Level eeds ore pots, Level for SGHGF s suffcet for atttude estato ths case. he thrd scearo s to add / h tal gyro bas the y-axs. he tal covarace s P = dag ( ),( ),( ),( / h ),( / h ),( / h) he result (5 tes Mote Carlo sulato) s show Fg. 4. It ca be see that SGHQF s ore accurate tha EKF. It should be also otced that Level has uch better perforace tha Level ths case. he uber of pots used SGHQF for all these cases are for level ad 9 for level whle the uber of pots the covetoal GHQF are 79 ad 54 respectvely. It s hard to use the covetoal GHQF sce too ay pots are eeded. It s obvous that SGHQF greatly proves the effcecy of pleetg ths flter. Roll (Deg) Ptch (Deg) Yaw (Deg) Fg. Atttude errors of SGHQF wth σ -errors bouds Upper Boud of sga EKF SGHQF (Lev ) SGHQF (Lev ) Lower Boud of sga e (hour) VII. CONCLUSION I ths paper, a ew sparse Gauss-Herte Quadrature olear flter was developed for the spacecraft atttude estato proble based o the Solya s rule. he uber of pots requred the covetoal ult-desoal Gaussa Quadrature rule s sgfcatly reduced, whch facltates the real-te pleetato of ths ew flter. he sulato results deostrated that the ew flter has better perforace tha EKF. Atttude Error (Deg) - - EKF SGHQF (Lev ) SGHQF (Lev ) e (Hr) Fg. Nor of atttude errors wth tal atttude errors oly Atttude Error (Deg) - - EKF SGHQF (Lev ) SGHQF (Lev ) e (Hr) Fg. 4 Nor of atttude errors wth tal atttude ad bas errors REFERENCES [] E. J. Lefferts, F. L. Marley, M. D. Shuster. Kala flterg for spacecraft atttude estato, Joural of Gudace, Cotrol, ad Dyacs, Vol. 6, No. 5, 98, pp [] J. L. Crassds. ad J. L. Jus., Optal Estato of Dyac Systes, Chapa & Hall/CRC, Boca Rato, FL, 4. [] J. L. Crassds, ad F. L. Marley, Usceted Flterg for Spacecraft Atttude Estato, Joural of Gudace, Cotrol, ad Dyacs, Vol. 6, No. 4, Aug., pp [4] Y. Osha ad A. Car, Atttude Estato fro Vector Observatos Usg Geetc-Algorth-Ebedded Quatero Partcle Flter, Joural of Gudace, Cotrol, ad Dyacs. Vol.9, No. 4, pp [5] S. Challa, Y. Bar-Shalo, ad V. Krshaurthy, Nolear flterg va geeralzed Edgeworth seres ad Gauss Herte Quadrature, IEEE ras. Sgal Process., vol.48, o.6, pp. 86-8, Ju.. [6] K. Ito ad K. og, Gaussa flters for olear flterg probles, IEEE ras. Auto. Cotrol, vol. 45, o. 5, pp. 9-97, May. [7] S. A.Solya, Quadrature ad terpolato forulas for tesor products of certa classes of fuctos, Sovet. Math. Dol. 4, pp. 4-4, 96 [8]. Gerster, ad M. Grebel, Nuercal tegrato usg sparse grds, Nuercal Algorths, vol. 8, o. 4, pp. 9-, 998. [9] H. Flora, ad W. Vtor, Lelhood approxato by uercal tegrato o sparse grds, Joural of Ecooetrcs, vol. 44, pp. 6-8, 8. [] H. Flora, ad W. Vtor, Estato wth uercal tegrato o Sparse Grds, Dscusso Papers Ecoocs 96, 6. []. Gerster, ad M. Grebel, Deso-adaptve tesor-product quadrature, Coputg, vol. 7, pp , [] A. Jazws, Stochastc Processg ad Flterg heory. Acadec Press, New Yor, NY 97. [] G. L ad A. M. artaovsy, A effcet, hgh-order probablstc collocato ethod o sparse grds for three-desoal flow ad solute trasport radoly heterogeeous porous eda, Advaces Water Resources, vol., pp