Development of a Deeply-Coupled GPS/INS Integration Algorithm using Quaternions

Transcription

1 Developmet o a Deeply-Coupled GPS/INS Itegratio Algorithm usig Quaterios Yuhog Yag Juchua Zhou Holger Nies Otmar oeld Ceter or Sesor Systems (ZESS) Uiversity o Siege Siege Germay Stea Kedli imar GmH Im Reihersruch 3 St. Igert Germay Astract A deeply-coupled Gloal Positioig System (GPS)/ Iertial Navigatio System (INS) itegratio algorithm is proposed i this paper. he mathematical system process ad oservatio models are provided. Due to the oliearity o the system models a Exteded Kalma Filter (EKF) is employed hich uses quaterios as the represetatio o attitude. he itegrated algorithm is tested usig the IFEN GPS radio requecy (RF) sigal simulator. Both static ad dyamic scearios are simulated. Numerical results are compared ad aalyzed. Keyords Deeply-coupled; GPS/INS; Quaterios I. INRODUCION Deeply-coupled GPS/INS itegratio research has dra more attetio i the recet e years. raditioal GPS/INS itegratio employs a loosely or a tightly-coupled architecture. Oe example o dieret implemetatios o GPS/INS itegratio systems is illustrated i Fig.. RF Frot Ed GNSS Receiver IMU Fig.. Acquisitio IMU (3 Gyro + 3 Acc) racig loops (D F/P) Pseudorage & Delta Rage Navigatio Filter GPS/INS itegratio architectures. P. V. t 3 Deeply Discrimiator Output Based o Is ad Qs 2 ightly PR. DPR. oosely P. V. Correctios Measuremets Itegratio Kalma Filter Strap-do Processig Navigatio Solutio Itegratio Algorithm he loosely-coupled itegratio has a decetralized estimatio architecture hich uses the output iormatio o the avigatio solutios rom a GPS receiver ad a INS. he mai advatages o usig loosely-coupled itegratio are: a) the system oservatio model is simpler ad accordigly it requires much less computatioal urde; ) the umer o measuremet iputs (i.e. positio ad velocity) or the Kalma ilter is ixed; c) redudat GPS avigatio solutios are availale. Hoever the disadvatages are: a) i case o usig to separate KFs (i.e. oe or GPS avigatio processig ad the other or itegratio purpose) it opes the possiility o presetig istale avigatio solutios caused y mutual eedacs o estimatio errors hich is coied as cascaded ilterig prolem; ) usually more tha 4 satellites are required to maitai a GPS avigatio solutio. Ulie the loosely-coupled maer i a tightly-coupled itegratio oly a cetralized KF is used. he pseudorage ad delta rage (or Doppler) measuremets are directly used i the ilter. he advatages o applyig the tightly-coupled itegratio are maily i the olloig aspects: a) the cascaded ilterig prolem arisig through the mutual eedacs o the estimatio errors etee to separate KFs is elimiated; ) the system does ot require a GPS avigatio solutio to aid the INS. hereore eve ith less tha 4 satellites i vie the remaiig satellite ased measuremets ca still e used i the algorithm hich promotes the roustess o the avigatio system; c) all systematic errors ad oise sources o the distriuted sesors are modeled i the same ilter hich esures that all error correlatios are accouted or. Nevertheless the disadvatage o tightly-coupled approach arises i the icreased dimesio o the oservatio vector ith respect to the loosely-coupled maer. Ad the umer o GPS measuremets as iputs to the Kalma ilter is varyig i practical applicatios hich depeds o the sigal eviromets. It is o that i a tightly-coupled system all GPS tracig chaels are orig idepedetly. I this or e move oe step orard to implemet the deeply-coupled itegratio. he algorithm e proposed uses quaterios as the represetatio o attitude. his or is ased o a sotaredeied GPS receiver e developed. he ormer or o this topic ca e oud i [-3]. I this system a EKF is employed to use the outputs (i.e. code phase error ad requecy error) rom the receiver tracig loops ad Iertial Measuremet Uit (IMU) oservales to ehace GPS tracig perormace hich gives roust ad smoothed avigatio results. II. IMPEMENAION OF A DEEPY-COUPED INEGRAION SYSEM A. System Architecture he realizatio o a deeply-coupled itegratio ca e maily divided ito to categories [4]. Oe ay directly uses the GPS receiver s asead correlator outputs (i.e. Is ad Qs) as the measuremets to use ith the output o IMU. he other ay is ased o the desig o a vector tracig loop hich as irst proposed y Spiler [5]. He used oliear discrimiator outputs as measuremets. I this paper our developmet is ased o the secod approach. he high level architecture o the algorithm is illustrated i Fig. 2. We employ a IMU hich cotais 3-axis gyroscope ad 3-axis accelerometer.

2 RF Frot Ed Acquisitio Fig. 2. Correlator NCO Code/Carrier Geerator IMU (3 Gyro + 3 Acc) I Q Discrimiator rasormed to Code phase ad carrier requecy Code Phase errors & Frequecy errors P.V.t Correctios Measuremets he architecture o the deeply-coupled system. Itegratio Kalma Filter Strap-do Processig I Fig.2 the acquisitio loc (at the let side o the plot) is used to acquire the icomig sigals rom the availale satellites ad to provide code phase ad coarse Doppler requecy to the tracig loop. I the tracig loop the carrier requecy is reied to several Hz precisio. Ulie the traditioal GPS receiver our deeply-coupled system uses a gloal itegratio Kalma ilter or all chaels. For this reaso the chaels are o loger orig idepedetly ut are coupled to a commo avigatio solutio. hus ith the assistace rom the IMU the chaels ca pricipally help each other i the case o GPS sigal atteuatio coditios. I such a deeply-coupled system the essetial relatioships are the coectio etee the code phase error ad the positio cloc error ad the coectio etee the carrier requecy error ad velocity cloc drit. hey have the mathematical relatios as sho i (). Details ca e also oud i [6]. z OS pˆ t z OS t ˆ v d here z ad z are the code phase error [m] ad carrier requecy errors [m/s] at epoch or certai tracig loops; p ˆ ad v ˆ are the estimated positio error ad velocity error at epoch ; OS represets the uit lie o sight vector rom the receiver to the j-th satellite; t is the receiver cloc ias i distace ad t d deotes the cloc drit error [m/s]; ad represet the hite Gaussia oise errors. B. INS Priciple Iertial avigatio is ased o Netoia physics ad is aected y gravity. hat is the oject ill remai i uiorm motio uless distured y a exteral orce. It ivolves a led o iertial measuremets mathematics cotrol system desig ad geodesy [7]. he exteral orce geerates acceleratio o the oject hich ca e measured y the iertial sesor. Ater the itegratio o the measured () acceleratios uder cosideratio o measuremets rom gyroscopes the chage i velocity ad positio ith respect to the iitial coditios ca e determied. A covetioal IMU cosists o three gyroscopes or measurig agular rates ad three accelerometers or measurig acceleratios. hey are mouted i triads so that the sesitive axes o sesors are mutually orthogoal settig up a Cartesia reerece rame. For a strapdo iertial sesor platorm the iertial sesor is rigidly mouted to the structure o the vehicle. he sesor ra data are processed to yield avigatio solutios (i.e. positio velocity ad attitude over time) ad this process is amed strapdo processig. he INS strapdo processig models i cotiuous ad discrete time domais are give i the ext sectio usig quaterios as the represetatio o attitude. C. INS Strapdo ad System Process Models We ill ot use Euler agles as represetatio o attitude due to the iheret prolem o sigularity [7]. I this paper the quaterio vector is deoted as q [ qq ] here q [ q2q3q4]. It is used to represet the rotatio rom the avigatio rame to the ody rame. he attitude dieretial equatio i terms o the quaterio vector q is give i (2). For a detailed derivatio the reader is reerred to [8 9]. 2 ith q Q q Q [ ] here is the agular rate measuremet vector rom the avigatio rame to the ody rame expressed i the ody rame hich is equal to: i i (2) (3) here i is the rotatioal rate vector o the ody rame relative to the iertial rame expressed i the ody rame (i.e. IMU gyroscope ra measuremets); i represets the sum o the rotatio o the Earth ith respect to the iertial rame plus the tur rate o the avigatio rame ith respect to the Earth expressed i the ody rame i.e. i R ie e. Usig a lo-cost MEMS-ased IMU the Earth rotatio is usually uried i sesor errors ad caot e detected. he Coriolis ad cetriugal terms are ot cosidered i the olloig sectios. Moreover or short distace applicatios the trasport rate is egligile. Cosiderig these eects e have. hus (3) turs to e: i (4) i i here i i x i y i z is the gyroscope ra data resolved i the ody rame.

3 Usig (3) ad (4) (2) ca e approximated as: ith Q q Q q 2 i [ ] i [ i] he simpliied strapdo mechaizatio model or the IMU ca e expressed i the avigatio rame as sho i (6) p v v R( q) i g q Q q 2 here p stads or positio ad v is velocity i avigatio rame; g represets gravity idicated i the avigatio rame hich is assumed to e approximately costat i the regio o iterest. he rotatioal trasormatio matrix R ( q ) rom the ody rame to the avigatio rame is expressed usig quaterios: q q2 q3 q4 2( qq 2 3 qq 4) 2( qq 3 qq 2 4) R ( q ) 2( qq 2 3 qq 4) q q2 q3 q4 2( qq 3 4 qq 2) 2( qq 2 4 qq 3) 2( qq 2 qq 3 4) q q2 q3 q 4 (7) he IMU speciic orce ad agular rate measuremet errors (e.g. sesor iases) are modeled as costats superseded y radom al hich are give i (8): ias ias (5) (6) (8) here ad are assumed to e zero mea Gaussia distriuted. It is orth metioig that usig (6) as the GPS/INS system propagatio model the IMU icomig measuremets should e compesated y the curret estimate o the sesor iases eore urther processig i.e. ias i i ias i i ( ad are the remaiig oises hich are assumed to e zero-mea Gaussia distriuted). I the discrete time domai ith a suicietly small time iterval (e.g..s IMU update time) ad or lo dyamic applicatios e have: p p v v v R( q) i g q q Q q 2 ias ias ias ias here is system propagatio time iterval. I (9) a part o the model is oliear e.g. R ( q ) cotais quadratic terms o quaterio elemets. hereore the liearizatio process should e coducted i order to apply Kalma ilterig. A EKF is used i this paper hich is ased o a irst order liearizatio process o the stochastic system model ith the assumptio o Gaussia distriuted oises. Besides i the scope o a deeply-coupled itegratio approach the receiver cloc errors eed to e modeled. he rage-rate equivalet o the cloc drit error is modeled as a costat plus a radom al process hile the rage equivalet o the receiver cloc ias error is the itegral o the cloc drit error. hus the liearized system propagatio model used or GPS/INS deeply-coupled itegratio is ormulated i () at the ottom o the page here error states are employed. (9) here p I I O O O p O3 3 I3 3 F23 R( q) O v v q O43 O4 3 I44 Q O4 3 Ωq 4 4 q ias 2 2 ias O ias 33 O3 3 O3 4 I 33 O ias c t O O O O I c t c t c t ()

4 ad F 23 is computed as: Ω q q q q q q q4 q 3 q I q q q q q q q () F qˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ x q4 y q3z q2 x q3y q4 z q3x q2 y qz qy q4 x q2 z 2q ˆ q ˆ q ˆ q ˆ q ˆ q ˆ q ˆ q ˆ q ˆ q ˆ q ˆ q ˆ q ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ 3x q2 y qz qy q4 x q2 z qx q4 y q3z q ˆ ˆ ˆ 2 x q3y q4 z 23 y 4 x 2 z 3 x 2 y z 2 x 3 y 4 z x 4 y 3 z I (2) ˆ ˆias x ix x ˆ ˆias y iy y ˆias ad ˆ are the IMU ra data compesated y the z iz z curret estimate o the sesor iases expressed i the ody rame. D. System Measuremet Model For the measuremet update e have z OS 3 p z j OSj 3 v z 3 OS t td zj 3 OSj 2 j8 (3) here z ad z are code phase ad carrier requecy discrimiator error outputs. Equatio (3) is the matrix represetatio o (). For the code phase error e employ a delay loc loop (D) discrimiator hich is a o-coheret early mius late evelope ormalized y E+ to remove the amplitude sesitivity []. E 2 E (4) (2) here dot I PI P2 QP QP2 cross I PQP2 I P2 QP. he I P ad I P2 deote the i-phase prompt accumulated correlatio results over time epoch - to ad to +; Q P ad Q P2 deote the quadrature prompt correlatio results over time periods. III. EXPERIMENS A. Experimet Setup he experimet setup is sho i Fig. 3. he simulatio is coducted usig the trajectories geerated rom the IFEN hardare GPS RF sigal simulator NavX -NCS. he high requecy GPS sigals are sampled do coverted ad collected y the hardare rot ed GN3Sv3 rom Sparu at the samplig requecy o Hz. he the itermediate requecy (IF) sigals are hadled over to the itegratio algorithm as the GPS sigal iput or post processig. he IMU data is geerated usig INS oolox 2. (25) or Matla developed y GPSot td. he ree Flight Dyamic Cotrol oolox is used to calculate the iitial attitudes o the platorm ased o the simulated dyamic trajectory. here E IE QE I Q. I E ad I represet sum o the i-phase early ad late correlator outputs respectively etee time epoch ad +; Q E ad Q represet the sum o quadrature early ad late correlatio results etee ad + epoch. For the carrier requecy error e employ a ormalized decisio directed requecy loc loop (F) discrimiator to elimiate the data it trasitio sesitivity []. cross sig( dot) 2 ( t t ) 2 (5) Fig. 3. Simulatio experimet setup. he IMU data is simulated ased o the parameters rom a admar M 2 ex MEMS-IMU. he mai speciicatio parameters are give i ale.

5 ABE I. SPECIFICAION OF ANDMARK M 2 EX MEMS-IMU Gyroscope Bias i-ru Noise(ARW) Scale actor (Agular rates) Staility ( ) ( ) error [ppm] 2 [ /h].35[ /s/ Hz] Accelerometer Bias i-ru Noise(ARW) Scale actor (Speciic orces) Staility ( ) ( ) error [ppm] 2 [μg] 4 [μg/ Hz] B. Straight ie Path I the irst experimet a straight lie trajectory is geerated y the sigal simulator ith costat velocity o m/s toard east. For comparisio puporse red curve o Fig. 4 is the avigatio results rom a deeply-coupled sytem. he lue dots are the stadaloe GPS solutio usig the leastsquares estimator. Fig. 5 shos that ith the aidig rom IMU oservale the tracig loops or ie. I this ay all the chaels are coected to each other y the avigatio ilter ad each chael is ale to correctly trac the GPS satellite sigals ad decode the avigatio data. he similar smoothed ehaviour ca also e oserved i the velocity solutio. Fig. 6 shos that the velocity o the oject is also correctly estimated. he estimated east velocity is aout m/s here i the orth ad up directios the estimates are aroud zero. East [m/s] scalar tracig loop + least squares INS aided tracig loop scalar tracig loop + least squares INS aided tracig loop North [m/s] Up [m] 2 North [m] East [m] 55 Up [m/s] 2 Fig D positio avigatio results. As sho i Fig. 4 the plot is give i ENU (east-orthup) rame. he deeply-coupled system presets much etter avigatio solutio tha the stadaloe GPS solutio. his is ecause that the IMU estimates ca smooth the oisy code phase ad requecy errors hich stailizes the tracig loop. he prompt i-phase compoets rom 6 tracig loops are plotted i Fig. 5 to sho the iteral orig o the tracig. x 4 PRN 26 (I p ) Epoch [ms] x 4 PRN 6 (I p ) Epoch [ms] x 4 PRN 27 (I p ) Epoch [ms] Fig.5. x 4 PRN 7 (I p ) Epoch [ms] x 4 PRN 22 (I p ) Epoch [ms] x 4 PRN 2 (I p ) Epoch [ms] I phase compoet o tracig loops i a deeply-coupled system. Fig Velocity estimatio compariso i ENU avigatio rame. he results o straight lie test sho that the deeplycoupled itegratio system gives smoothed results i costat acceleratio sceario. his is ecause a Kalma ilter is used iside hich uses iormatio rom all the chaels y estimatig the commo state ad the iertial sesors ca provide dyamic measuremets (i this test they are zeros plus sesor errors) hich are used i the system process model. Practically usig true dyamic measuremets turs out to e etter tha usig dyamic models. Because i reality the true dyamic o the platorm may ote ot e descried y ay model due to the lac o eough a priori oledge o the trajectory. For testig the algorithm ith chagig acceleratios a clocise circle path is employed i the ext sectio. C. Clocise Circle Path I this test a clocise circle path is simulated. he platorm lies ith costat orm velocity o 3 m/s. he path radius is m ith circle ceter poit (latitude).88 (logitude) 525 m (height). I this case i the horizotal plae the acceleratios (i.e. the output o x ad y- axis accelerometers) are chagig over time. For the attitude the headig (i.e. the output o gyro-z axis) is chagig costatly over time. he positio ad velocity avigatio results or 2 secods are compared ith GPS aloe solutio

6 (usig a least-squares estimator) i Fig. 7 ad Fig. 8. I oth igures the red curves are the results rom the deeply-coupled itegratio hile the lue curves are the results rom GPS stad-aloe system. he igures sho that ith INS aidig the itegratio solutio is less oisy ad much smoother as compared ith a GPS oly solutio. East [m] Up [m] Fig scalar tracig loop + least squares INS aided tracig loop Up [m] North [m] North [m] Navigatio solutio comparisos i ENU avigatio rame East [m] leaves the outage eviromets. he proo o these eeits goes to the uture or. IV. CONCUSION I this paper e have preseted a implemetatio o a algorithm or a deeply-coupled GPS/INS system. he system models usig quaterios as the represetatio o attitude are give. A exteded Kalma ilter is employed as the tool to use the data rom the GPS receiver tracig loop outputs ad IMU oservales. I our system the separated tracig chaels are coected ith each other due to the act that gloal avigatio ilter estimates the commo states. I this ay the chaels ca help each other i the tracig hich is a ig dierece rom the tightly-coupled system. Besides the IMU ca capture the dyamics o the platorm to improve the avigatio solutio or example i challegig GPS sigal eviromet ad to ehace the system roustess. Usig eve a cheap IMU is etter tha usig dyamic models i the Kalma ilter as i reality. he dyamics o the platorm ca ote ot e accurately descried y ay models. o simulatios have ee made to veriy the approach. ACKNOWEDGMEN he irst author grateully acoledges the support provided y the programme -Multi Modal Sesor Systems or Evirometal Exploratio ad Saety (MOSES) ithi the Ceter or Sesor Systems (ZESS) Uiversity o Siege Germay. East [m/s] North [m/s] Up [m/s] scalar tracig loop + least squares INS aided tracig loop Fig Velocity estimatio compariso i ENU avigatio rame. I this test the advatage o the deeply-coupled system comes rom the act that the IMU ca measure the true dyamics o the platorm ad this iormatio is used i the tracig loops hich ca improve the tracig ehavior. Besides i the case that the platorm ecouters challegig sigal eviromets (e.g. GPS sigals are atteuated or loced) the system ill sho its outperormace y usig the IMU estimates to smooth ad ridge the outage periods hich ill also speed up the reacquisitio o sigals he the vehicle REFERENCES [] Y. Yag J. Zhou ad O. oeld "Quaterio-ased Kalma ilterig o INS/GPS" i 5th IEEE Iteratioal Coerece o Iormatio Fusio (FUSION 22) Sigapore 22 pp [2] Y. Yag J. Zhou ad O. oeld "GPS receiver tracig loop desig ased o a Kalma ilterig approach" i 54th IEEE Iteratioal Symposium EMAR 22 Zadar Croatia 22 pp [3] Y. Yag J. Zhou ad O. oeld "Applyig a ie Doppler acquisitio method o sotare GPS receiver ith ield experimet" i 53th IEEE Iteratioal Symposium EMAR 2 Zadar Croatia 2 pp [4] M. G. Petovello ad G. achapelle "Compariso o vector ased sotare receiver implemetatios ith applicatios to ultra tight GPS/INS itegratio" i ION GNSS 9th IM Fort Worth 26 pp [5] J. J. Spiler "Fudametals o sigal tracig theory" i Gloal Positioig System: heory ad Applicatios. vol. I B. W. Pariso J. J. Spiler P. Axelrad ad P. Ege Eds. Washigto DC: America Istitute o Aeroautics ad Astroautics 996 pp [6] S. Zhao M. u ad Z. Feg "Implemetatio ad perormace assessmet o a vector tracig method ased o a sotare GPS receiver" Joural o Navigatio vol. 64 pp [7] J. Zhou Y. Yag J. Zhag ad E. Eda "Applyig quaterioased usceted particle ilter o INS/GPS ith ield experimets" i ION GNSS 2 Portlad Orego 2 pp [8] J. A. Farrell Aided Navigatio: GPS ith High Rate Sesors. Ne Yor: Mcgra-Hill Proessioal 28. [9] W. R. Hamilto Elemets o Quaterios. odo Eglad: ogmas gree ad Co [] E. D. Kapla ad C. J. Hegarty Uderstadig GPS Priciples ad Applicatios 2d ed.: Artech House 26.