NAVIGATION USING LOW COST INERTIAL UNIT (IMU-MEMS) AND GPS RECEIVER WITH APPLICATION OF SIGMA-POINT KALMAN FILTER. Walter Einwoegerer

Transcription

1 wogrr W. t al. avgato usg low t rtal ut AIGATIO USIG LOW COST ITIAL UIT (IMU-MMS) A GPS CI WITH APPLICATIO OF SIGMA-POIT KALMA FILT Waltr wogrr IP - Av dos Astroautas 758 CP 7 São José dos Campos SP Brazl wwogrr@gmal.com Hélo Kot Kuga IP - Av dos Astroautas 758 CP 7 São José dos Campos SP Brazl h@dm.p.r Paulo Gácomo Mla IP - Av dos Astroautas 758 CP 7 São José dos Campos SP Brazl mla@dm.p.r Astract: Th proposal o ths wor s to prst a procss o data uso o th acclrato ad agular rat masurmts o a low t Irtal Masurmt Ut (IMU) - MMS (Mcro lctro-mchacal Systm) asd tchology corporatg masurd posto ad spd data rom a GPS rcvr. Th t dtals th quatos that hav usd to tgrat th movmt ad th atttud o a systm strapdow cogurato through a approprat algorthm usg th mthod o th Sgma-Pot Kalma ltr vryg ts prormac ad comparg rsults. It also dtals th masurmts ta dyamc codtos usg th IMU ad GPS smultaously ad dscrs th us o ths data o a Sgma-Pot Kalma Fltr dvlopd to us th GPS posto ad spd ormato wth th coordats prdctd rom th IMU. Kywords: IMU MMS Kalma ltr Sgma Pot Kalma Fltr Itroducto A Irtal Masurmt Ut (IMU) asd o MMS tchology (Mcro lctro-mchacal Systm) du to ts wd avalalty ad low t wll usd hr or th dvlopmt o a avgato algorthm. ata rom a IMU-MMS (Mcro lctro-mchacal Systm) allow th matc tgrato o drtal quatos o moto at hgh samplg rats (up to 33Hz th uts usd). Howvr caus o ts low t ad tchology th IMU cotas drt lvls that accumulat crasg rrors alog th tm mpactg gatvly th posto ad vlocty stmats ovr tm. Th GPS rcvr tur oprats at lowr samplg rats ( Hz) provdg mor accurat ad o-drtg posto ad vlocty ut s sujct to loss o sgal du to physcal ostacls. To compsat or ths lmtatos t s tdd to tgrat th IMU to th GPS rcvr whch allows a avgato soluto that coms th dpdc o th IMU dspt ts low accuracy to th stalty o th GPS rcvr sujct to sgal losss. Ths typ o tgrato s comg mor commo du to crasg dvlopmt o ths typ o IMU ot oly alg ts tgrato wth GPS rcvrs ut also to othr ssors ad dvcs to support avgato. Th charactrstcs o COSSBOW C4- MMS-IMU usd ad th procdurs or otag ts paramtrs ar dscrd dtal rrcs Kuga t al. (7a) ad Kuga t al. (7). Th dtalg o th rsults or ths xprmt s rportd ad aalyzd rrc wogrr (9). Mcazato o th rtal avgato I ths wor th avgato quatos drs: th IMU platorm strapdow cogurato th coordat systm rotatg wth th arth ad th stat varals rprstd y th coordats o posto vlocty ad atttud. Thus th drtal quatos whch mchaz th matc avgato to dtrm th trajctory ad atttud o th IMU-MMS ar gv y Farrl ad Barth (998): λ h () Prstd at th I Smpóso Braslro d ghara Ircal o d Jaro Brazl Octor Joural o Arospac grg Sccs ad Applcatos Sp. c. ol. II o 3

2 wogrr W. t al. avgato usg low t rtal ut Joural o Arospac grg Sccs ad Applcatos Sp. c. ol. II o 3 ( ) ( )... g ta s ta s () p r q p r q p ψ ta s s s ta ta s (3) whr w v u p w v u (4) ad [ h λ ] ar th godtc lattud logtud ad alttud; [ ] ar th vlocty compots o th platorm th drctos orth ast ad local adr; [ ψ ] ar th agls o roll ptch ad yaw s th ast radus plus th alttud h s th orth radus plus th alttud h p s th rotato matrx o th ody (platorm) or th (orth ast ad adr) avgato systm s th arth's rotato g th local gravty that s computd tag to accout th gravtatoal ad ctrptal acclrato [p q r] ar th agular rats masurd y th gyroscops ad [ p q r ] ar thr rspctv ass; [ u v w ] ar th acclromtr masurmts ad [ u v w ] thr ass rspctvly; ad [ ] ar acclromtr masurmts trasormd to th systm. W dr that th ass ad drts o acclromtrs ad gyros thr smplst orms ca modld as tm-ws tat mag:. g a (5) 3 o lar sgma pot alma ltr For olar prolms th covtoal olar ltrs such as th xtdd Kalma Fltr may hav poor prormac du to prolms hrt olar systms maly du to th ollowg assumptos (Mayc 979; Brow ad Hwag 996; Brma 977): Larzato s a good approxmato o th procsss (Jacoa matrcs o th dyamcs ad masurmts); Procsss ar Gaussa v or hghly olar prolms; Usually oly th avrag (rst momt) s prdctd as o-lar sc th covarac s larsd. u to such prolms a olar ral-tm Kalma Fltr (KF) calld Sgma-Pot or usctd (thr SPKF or UKF wth sam mag) s th ltr dvlopd. Th SPKF mthod sts o otag a mmum st o sampls th sgma-pots aroud th ma (Julr ad Uhlma 997; Julr t al. ad Julr ad Uhlma 4) o trst ad s stll rprstatv o th olar systm. Ths mthod ss to ota ormato aout th rst momts (ma covarac th thrd ctral momt or swss ad urtoss) o carully chos sampls. Thr th sgma-pots ar olarly prdctd as wll as ts prdctd covarac. Ths approach dos ot rqur th larzato o th systm l th xtdd Kalma ltr (KF) avodg complx aalytc Jacoa matrx that ds to calculatd at ach stp o tgrato. To ths d dg as th dmso o th vctor to stmatd t grats a st o sgma-pots y: ) ( ) ( P x χ P x χ x χ o κ κ (6)

3 wogrr W. t al. avgato usg low t rtal ut whr ( κ ) P s th -th row or colum o th squar root matrx o ( κ) P ad th κ actor s chos so as to scal momts gratr tha 3. I ( κ) 3 t s also possl to scal som o th ourthmomts (urtoss) wh x s gaussa (Julr t al. ; Julr ad Uhlma 4). Th wghts to comput th prdctd ma ad covarac ar gv y: W W W o κ ( κ ) ( κ ) ( κ ) (7) Th prdcto phas o th SPKF s mplmtd through th tgrato o th qs. ()-(3) ad (5) or ach sgma pot: χ ( χ ) (8) Th avrag ad th prdctd covarac ar th calculatd y: x W χ (9) [ χ x ] [ χ ] T W x P () usg th wghts accordg to q. (7). Th approach va SPKF to ths prolm appls oly to th stag o prdcto. Th phas o corrcto o th ltr (masurmt updat) uss th quatos o th covtoal Kalma Fltr (Brma 977) caus masurmts ar lar wth rspct to th stat vctor to stmatd: t K P H t P P H x x K t ( H P H ) ( I - K H ) P [ y H x ] 4 o lar sgma pot alma ltr th dtrmato o th mu-mms paramtrs W dvlopd a pror as Kuga t al. (7a 7) th procdurs or dtrmg th ass ad drts o th IMU-MMS usg th tchqu o SPKF. I th ollowg th prormacs wr aalyzd ad th rrors volvd comparsos tw: Smpl tgrato xact IC (Ital Codtos) drg a ad g Smpl tgrato IC xact valus o a ad g pr-calculatd y SPKF Usg SPKF wth 9 lmts ad ot drg a ad g Usg SPKF wth 5 lmts drg a ad g. Th coclusos o ths smulatos hav dtrmd that t s cssary to ta to accout th systmatc rrors t stmatd prvously or stmatd altogthr th SPKF. Th smallr th rror ad astr covrgc o th valus o ass or drts ths rrors a ad g ar part o th stat to stmatd ay ltr dsgd or avgato. Ths approach also mas th ltr roust to local varatos th IMU whr or drt worg loads th rrors hav ampld cotrutos o drts tm scal actor ad msalgmt. 5 o lar sgma pot th mugps tgrato algorthm Th Sgma-Pot Kalma Fltr or IMUGPS tgrato drs th rspctv ass o th acclromtrs ad drts o th gyros as lmts o th stat vctor to stmatd. Th algorthm rcvs th tal codtos ad squtal masurmts sychrozd wth th valus otad y th IMU ad th GPS rcvr. It s mad th mplmtato o th ltr SPKF wth th grato o sgmapots ad th susqut umrcal tgrato o th quatos o th rtal avgato mchazato. Th () Joural o Arospac grg Sccs ad Applcatos Sp. c. ol. II o 3

4 wogrr W. t al. avgato usg low t rtal ut ma ad covarac or ach prdctd stat vctor ar otad wth susqut corrcto (updat) va th covtoal Kalma ltr or atttud ad avgato. Th purpos o th avgato algorthm s to tm propagat th ormato o posto vlocty ad atttud as a ucto o ach radg rcvd rom th acclromtrs ad gyros tw th GPS rcvr radgs ad also wh th GPS rcvr has th sgals locd y physcal ostacls (loss o sgal). 6 xprmt stup Th xprmt usd a low-t MMS-IMU st a GPS rcvr attry ad a computr or rcvg ad storg th data as show Fg.. Th MMS-IMU (Crossow IMU Modl-C4-) 5 s a 6-axs masurmt systm. Lar acclrato masurmts aroud 3 tr-orthogoal axs ad rats aroud thr orthogoal axs al ull masurmt o th systm dyamcs. Th IMU masurmts ar otad rom a sral commucato port S-3 usd or storg th masurmts o th computr. Th GPS 3 rcvr usd or ths wor Ashtch Z- mas ull us o th postog systm ad provds accurat ad avgato wth arcrat qualcato. Th whol systm s composd o a rcvr attry ata cals ad harss. ata ca stord trally ad th rcovrd rom th rcvr va sral commucato S-3 or post-procssg or thy may usd ral tm tag advatag o th sam commucato port. I ths wor w usd th avgato solutos (posto vlocty POP ad th corrspodg tm) provdd y th rcvr. I ths xprmt or comparso rasos t s o trst th masurmt rror th dstacs rom th GPS rcvr to th GPS satllts wh dtrmg rcvr posto. It s charactrzd y th OP (luto o Prcso) actor. I ths wor t s usd spccally th POP (posto OP) whch s a masur o thrdmsoal posto rror accoutg or th gomtry tw th rcvr ad GPS satllts tllato. Fgur. GPS cvr IMU ad computr Th qupmt Fg. was moutd o a automol ad th chos vromt ad trajctory or th tst s dsplayd o th aral vw show Fg.. Joural o Arospac grg Sccs ad Applcatos Sp. c. ol. II o 3 3

5 wogrr W. t al. avgato usg low t rtal ut Fgur. Aral vw o th trajctory o th xprmt I th travld path th IMU data (acclrato agular rats ad th corrspodg tm) wr stord o th computr. Sc t s a post-procssd xprmt th corrlato tw drt sampls o th stord ls was otad through owldg o th commo lmt to all o thm amly th tm. It s olgatory to ow thr tmls whch ach masur was ta ad rlato to a tm stadard (GMT) so that you ca hav a data sychrozato tw th ordary tm o IMU th computr's ad th GPS rcvr's. Fgur 3 summarzs th oprato o th algorthm ad shows that atr th prdcto phas wh data provdd y th GPS rcvr P POP t (posto vlocty posto OP ad tm) ar avalal th posto ad vlocty otad y tgrato o masurmts provdd y th IMU (agular rats ad acclratos) ca updatd or corrctd (stat stmato loc Fg. 3). Th dcso o whthr or ot to prorm th updat acto s ta asd o th valus o POP (sort o qualty lag) or th corrspodg GPS masurmts. Thus th POP valu s usd as a rrc or th qualty o GPS sgal th SPKF ltr. 7 Smulatos Fgur 3. Schm o IMUGPS tgratd va SPKF For th algorthm o th Sgma-Pot Kalma Fltr (SPKF) or th applcato o qs. (6) to (9) w usd th valus: a) th stat vctor x th tal posto: X() - Lattud Godtc S X() - Godtc Logtud X(3) - Godtc Alttud m X(4) orth locty ms X(5) ast locty ms X(6) adr locty ms X(7) - oll Joural o Arospac grg Sccs ad Applcatos Sp. c. ol. II o 3 4

6 wogrr W. t al. avgato usg low t rtal ut X(8) - Ptch -.49 X(9) - Yaw X() - ax -. ms X() - ay.57 ms X() - az -.3 ms X(3) gx.5 o s X(4) gy.4 o s X(5) gz.38 o s ) th tal covarac [ P o ]: Posto: (m) horzotal coordats (lattud ad logtud) ad (3m) th vrtcal coordat locty: (. ms) or th thr compots Atttud: (. o ) or th oll ad Ptch ad (.5 o ) or th Yaw agls Acclromtrs Bas: (.-3 ms ) Gyro rt: (5.-3 rads) Th valus ar typcal or masurmts o posto ad vlocty otad rom GPS (Msra ad g ). Amog th atttud agls th yaw agl was drd th last osrval (hghr stadard dvato) practc. Th matrx o powr spctral dsty was drd dagoal ad tat os: qvv. 5m q. 5 s q a a q g g. 5m. 5 3 s 3 s s corrspodg to th os th vlocty drtal q. () rat o atttud q. (3) o ass ad drts o th gyros ad acclromtrs q. (5). Fally or th varacs o th masurmt os ( ) o adoptd: () rad () rad (33) 9 m (lattud) (logtud) (alttud) (44). (ms) ( ) (55). (ms) ( ) (66). (ms) ( ) (77) rad (88) rad (99) rad (roll) (ptch) (yaw) Th xprmt was sampld at a rat o Hz or th radgs o th IMU ad Hz or th GPS rcvr. Th trajctory ad th masurmts o dstacs ar gv coordats ad uts mtrs. Th algorthm addto to procssg th trajctory accordg to th radgs o acclromtrs ad gyroscops cotuously rcvs th posto ad spd dtrmd y GPS. To aalyz th prormac o th algorthm asd o th qualty o GPS sgal (ltrg usg th valus o POP) two cass ar studd whr t s possl to masur th havor o th ltr ad ts prormac. Joural o Arospac grg Sccs ad Applcatos Sp. c. ol. II o 3 5

7 wogrr W. t al. avgato usg low t rtal ut Cas Cosdrg all th radgs rom th GPS rcvr as vald (o GPS data rjcto) For ths cas all th radgs otad y th GPS rcvr ar d ac to th avgato algorthm rgardlss o ts qualty (wthout POP vrcato). Fgur 4 mas th comparso coordats tw th trac provdd y th GPS rcvr ad th otad y procssg th IMUGPS tgratd algorthm. Fgur 4. GPS trajctory x algorthm wthout vrcato o POP Th dstac covrd s sujct to sctos wth loss o GPS sgal whch ar th rgos o trst or valuatg th prormac o th algorthm udr study. For th codtos o ths aalyss th loss o rlal valus or posto ad spd provdd y th GPS rcvr s rsposl or th dvatos th trajctory procssd y th ltr du to th lac o sgal o qualty. Fgur 5 zooms th ara o trst whr thr wr losss o GPS sgal ad th ltr rspos ths rgo. Fgur 5. go wth losss o GPS sgal ad havor o th algorthm wthout rjctg poor qualty GPS data Joural o Arospac grg Sccs ad Applcatos Sp. c. ol. II o 3 6

8 wogrr W. t al. avgato usg low t rtal ut Cas - Cosdrg oly th radgs rom th GPS rcvr wth POP 5 (wth GPS data rjcto). I th sam codtos as cas th algorthm cotuously rcvs th posto ad vlocty rom th GPS rcvr. Howvr or ths cas ar procssd ad updatd oly data wth POP 5 (good qualty). For POP > 5 (ad qualty) computd prdctos ar usd or posto ad vlocty otad rom th radgs o th IMU ad GPS data ar ot procssd. I ths cas th IMUGPS trajctory otad y th SPKF algorthm s show Fg. 6. Fgur 6. GPS trajctory x algorthm wth data qualty POP 5 Th tdcy o th algorthm s to ollow th corrct path v rgos whr thr ar ostacls or lmtatos o vsl GPS satllts that rsult a lowr qualty o th GPS sgal. Fgur 7. go wth loss o GPS sgal ad prormac o th algorthm lmatg th GPS sgals wth POP > 5 Joural o Arospac grg Sccs ad Applcatos Sp. c. ol. II o 3 7

9 wogrr W. t al. avgato usg low t rtal ut I th aalyss o th rsults otad rgos wth loss o GPS sgal ad ad gomtry (POP > 5) th tgrato IMU GPS surd trpolato o th trajctory wth rrors smallr tha thos commttd y th GPS alo sc hrtly ad GPS data wr lmatd. Th rsults otad show that th trajctory s dpdt o th corrctos mposd to th avgato soluto provdd y th GPS. As th qualty otad rom th GPS rcvr s a ucto o GPS tllato gomtry ths drctly acts th qualty o avgato. For ths raso t s sstal to lmat th "ad" GPS data y allowg th trajctory to calculatd oly through IMU data. 8 Coclusos Ths papr dscrd th procdurs ad th rsults otad dtrmg th trajctory y usg IMU data tgratd wth a GPS rcvr va a algorthm usg th Sgma Pot Kalma Fltr tchqu. Th Sgma- Pot tchqu s attractv systms whr th xprssos o th dyamcs o th systm ar ot avalal or ar a orm that dos ot allow a asy larzato. Morovr t s al to scal hghr ordr momts wth slght cras th computatoal load. Wh valuatg th prormac o th tgrato o a MMS-IMU wth a GPS rcvr t was cocludd rom th rsults otad that mrgg th ormato o acclrato ad agular vlocty o a IMU wth th posto ad vlocty o a GPS rcvr va SPKF crass th autoomy o th sam IMU opratg cas o loss o GPS sgal. Th rsults hav show a rapd rspos o th ltr. Futur wor mght volv logr campags or th charactrzato o th ltr ad to assss ts roustss ad ault tolrac. 9 rcs Ashtch Survyg Products - Ashtch Z- GPS cvr Oprato ad rc Maual. vso A 999. Brma G. J. Factorzato Mthods or scrt Squtal stmato. Acadmc Prss w Yor 977. Brow.G. ad Hwag P.Y.C. Itroducto to adom Sgals ad Appld Kalma Fltrg. Joh Wly & Sos w Yor 996. Crossow. IMU Usr s Maual Modls IMU3CC IMU4CC IMU4C. vso B F. 7 ocumt wogrr W. Mchazato o rtal avgato systm wth data rom tgrato o IMU-MMS ad GPS rcvr. ( Portugus). MSc. Thss Spac Mchacs ad Cotrol IP - Isttuto acoal d Psqusas spacas São José dos Campos 9. (rport IP-5763-TI56) 4p.. Farrl Jay A. ad Barth Matthw 998 Th Gloal Postog Systm & Irtal avgato w Yor Y McGraw-Hll. Julr S.J. ad Uhlma J.K. A w xtso o th Kalma Fltr or olar Systms. Itratoal Symposum o Arospacs Ssg Smulato ad Cotrols SPI 997. Julr S.J. ad Uhlma J.K. Usctd Fltrg ad olar stmato. Procdgs o th I ol. 9 o.3 4. Julr S.J. ad Uhlma J.K. ad urrat-whyt H.F. A w mthod or th olar trasormato o mas ad covaracs ltrs ad stmators. I Trasactos o Automatc Cotrol ol. 45 Issu 3 p Kuga H.K. ad Lops..F. ad wogrr W. xprmtal statc calrato o a IMU (Irtal Masurmt Ut) asd o MMS. XIX Itratoal Cogrss o Mchacal grg - COBM 7 Brasla F Brazl ov a. Kuga H.K. ad Mla P.G. ad wogrrw. xprmts o algmt o a Irtal Masurmt Ut asd o MMS (Mcro lctro-mchacal Systms). ( Portugus). Procdgs o Brazla Symposum o Irtal grg SBI o d Jaro J Brazl 7. Mayc P.S. 979 Stochastc Modls stmato ad Cotrol. Acadmc Prss w Yor. Msra P. ad g P. Gloal Postog Systm: Sgals Masurmts ad Prormac. Gaga-Jamua Prss Lcol. Joural o Arospac grg Sccs ad Applcatos Sp. c. ol. II o 3 8