Analysis of Integrated Navigation with GPS Source Variability

Transcription

1 Analyss of Integrated Navgaton wth GS Source Varablty Mahmoud Efatmaneshn School of Surveyng & SIS, Unversty of New South Wales, Sydney, Australa Yong L School of Surveyng & SIS, Unversty of New South Wales, Sydney, Australa Allson Kealy Deartment of Geomatcs, Unversty of Melbourne, Australa Andrew G Demster School of Surveyng & SIS, Unversty of New South Wales, Sydney, Australa Gudance and steerng control systems that are now n wdesread use by farmers for loughng and cultvatng the land wth unrecedented accuracy, requre centmetre-level oston as well as atttude nformaton accurate to a few degrees. Addtonally, oston and atttude udate rate of no less than z s requred for successful gudance n agrcultural envronments. Gudance and steerng control systems rely artcularly on headng and roll measurements to steer the tractor on arallel straght lnes or curves nown as lough lnes. At UNSW, an Atttude and eadng Reference System nown as AhrsKF has been develoed for a new generaton of gudance and steerng control system (oerated commercally by Leca Geosystems as mojork ). A Kalman flter (KF) s the most common estmaton engne for ntegrated navgaton systems. he AhrsKF system s based on a closed loo KF. It uses stradown Inertal Navgaton System (INS) comutaton equatons n the navgaton coordnate system (n-frame). he hardware system maes use of automotvegrade MEMS nertal sensors. In AhrsKF, the GNSS solutons of oston and velocty are fed nto the Kalman Flter for state error estmaton of the INS measurements []. A dual frequency survey grade GS/GLONASS recever rovdes oston nformaton. An alternatve oston nformaton source to RK (Real me Knematc) s used n AhrsKF. hs s a ostonng algorthm that benefts from a relatve seudorange/delta-hase (D) flter (commercally now as GLDE) leadng to an enhanced soluton of meter accuracy over mnutes []. he D flter rovdes a fltered oston and velocty soluton based on assumed vehcle dynamcs, and reles heavly on Doler measurements. he D s not regarded as the man oston nut for the mojork system. he reason the D algorthm s used n the mojork system s to rovde stable oston nuts when the lne of sght to the RK based staton s obstructed. In ths aer t s shown that the atttude solutons from AhrsKF are more accurate when D, whch rovdes much less accurate ostons than RK, s used. For ntegratng the D soluton nto the AhrsKF, frst both D and RK solutons are sequentally fed nto the AhrsKF. hen the ntegraton uses a Multle Model Kalman Flter (MMKF) that enables the AhrsKF to accommodate both GS nut sources. he sequental and MMKF algorthms atttude solutons are tested wth reference to mult-antenna GS or long boom atttude solutons.

2 AhrsKF ardware System he gudance and steerng control system sensors consst of three rate gyros, a three as accelerometer, and a -as magnetometer. In addton to these sensors, the system ncludes two GS recevers a dual frequency L/L GS/GLONASS recever and a lowcost L-only sngle frequency GS recever whch roduces a two as atttude soluton used for estmatng the bases of the nertal sensors. he nertal and GS data are then rocessed by an embedded 4Mz owerc rocessor. he secfcatons of the MEMS gyros n the gudance and steerng control system are lsted n able. able. echncal secfcatons of MEMS gyros []. Characterstcs Secfcatons Gyro Senstvty Error 6% Gyro Lnearty Gyro Bas Varaton at Constant emerature Gyro Bas Stablty over one hour.%.4 /S.4 /S In ths gudance and steerng control system the temerature-comensated nertal data are fed nto the ntegraton Kalman flter. In AhrsKF software the GS oston and velocty are used to derve the oston and velocty errors of the INS n a closed loo. hs means that the error corrected INS oston and velocty solutons are ntegrated wth the net eoch GS soluton. he AhrsKF state vector s []: X [,,, V, V, V, L,, h,,,,,, ] E N U E N U y z y z () he frst three states are atttude errors, then velocty errors, and oston errors. he fnal 6 states are sensor error terms (resectvely related to three gyros and three accelerometers), whch are modeled as a random wal n the AhrsKF. he measurement equatons for velocty and oston are []: vie vge Zv ( t) v v IN GN vx( t) Vv ( t) viu v GU LIE LGE Z ( t) IN GN X( t) V ( t) hiu h GU () When the seed of the vehcle s hgh enough, e.g. m/s, the GS velocty can be used to derve the headng at an accuracy of about.57deg (assumng a GS velocty error of cm/s). he headng derved from the GS velocty can then be used to correct the INS headng n the ntegraton Kalman flter []:

3 M u G INS V t Z ) ( () So the equaton of measurement s wrtten n a matr form as below: V X I I Z Z Z Z v (4) he system model n the Kalman flter s: Gw F t ) ( (5) where F s the system model matr descrbed n [4] and G s the system nose dstrbuton matr and w s the system nose vector. Matr w contans the varances of accelerometers and gyros. F and G w must be dscretzed for dscrete systems. Matrces Φ and Q are the result of dscretzaton rocess and are used n the Kalman Flter State vector estmaton equatons through tme roagaton: (6) Q ) ( R K ) ( z K he eected values of the correctons are zero n closed loo flters, thus. he covarance of the measurement nose n AhrsKf s descrbed []: π c b a... I R (7) where a, b, and c are the standard devaton of the D GS ostons, wth the adoted values lsted n able. able. Standard devaton of the oston n AhrsKF []. Mode/coeffcent a b c Carrer hase (RK)... seudorange/hase Dfference (D).

4 4 est and Setu A test was conducted at Boonah, at Leca s test farm near Brsbane, Australa, on July. In the test, multle GS recevers (used to derve the atttude soluton as the reference to evaluate the AhrsKF soluton) and the mojork were mounted on the tractor. he nstallaton of the GS antennas s llustrated n Fgure. he longest baselne s m, from whch the headng and roll can be derved wth an accuracy of.56deg (mm/m=.56deg by assumng mm accuracy of the double-dfferenced carrer hase measurements). he forward comonent of the baselnes s 7mm, and the derved tch has an accuracy of.5deg (=mm/7mm), thus the boom GS-derved atttude s accurate enough to evaluate the AhrsKF soluton. Detals of the GS atttude algorthm used for ths test can be found n [4]. here was no obstructon to the RK base staton lne of sght durng the test. Fgure. Leca s autonomous farmng tractor (left), and nstallaton scheme for the GS antennas (rght) he tractor was drven at a seed of -5m/s along straght lnes, and about m/s at turns. he feld s relatvely flat, wth both the roll and tch angles beng wthn ±deg ( ). he ArsKf and mult-antenna GS atttude solutons are dected n Fgure, where the AhrsKF soluton s lotted as the blue lne and the boom GS soluton n red. It can be seen that the AhrsKF soluton concdes wth the GS soluton and correctly tracs the changes of all atttude angles. It can also be seen that the AhrsKF soluton s noser than the GS soluton. he AhrsKF soluton s obtaned by ost rocessng the collected data durng the test. Only yaw and roll oututs are lotted here snce the auto steerng system only uses the yaw and roll measurements.

5 5 Fgure. he trajectory of the tractor durng the test Fgure. Fgure 4 and able show that AhrsKF can roduce less jttery (nosy) and smoother atttude solutons when t s suled wth D oston, velocty and headng solutons relatve to when t s oeratng wth RK solutons. he table shows the statc results of the atttude error relatve to the long boom atttude soluton. Generally seang the SD of atttude error for both roll and yaw are lower for the D-drven soluton. he sudden hes n the errors are related to tme lags when the tractor has shar turns.

6 6 able. Statc results for the comarson between ost rocessed AhrsKF solutons wth dfferent nformaton sources and long boom atttude solutons Wth RK Wth D Roll Yaw Roll Yaw Mean (Degree) SD (Degree) Fgure 4. he comarson between roll (left) and yaw (rght) errors for ost rocessed AhrsKF soluton wth RK and D ostons relatve to mult-antenna atttude soluton. he reason for the better erformance of AhrsKF wth D relatve to RK rests n the mechansm by whch D solutons are derved and also n the ecellent qualty of velocty measurements. he D soluton otmzes the absolute ostonng accuracy of the GS code observaton and leverages the relatve stablty of the GS carrer hase and Doler observatons []. hus, the D flter roduces a very smooth soluton wth consstent rather than absolute oston accuracy. here s tycally less than cm dfference from eoch to eoch []. Fgure 5 shows the velocty trajectory of RK and D solutons. It s clear that the velocty trajectory of D s smoother and has less jtter or nose. he hgh qualty of velocty measurement reflects ecellently on the erformance of AhrsKF. hs s because the northng and eastng veloctes are used to derve the headng (or yaw) as a measurement nut (see Equaton ) to the AhrsKF at each eoch.

7 7 Fgure 5. he velocty trajectory for RK soluton (left) and D soluton (rght). he D {ths s the frst tem you ve mentoned Glde use ether that, or D or f you use both use them both earler} trajectory s vsbly smoother. Multle Model Kalman Flter A Multle Model Kalman Flter (MMKF) s a way to combne several arallel KF solutons together and to from a flter ban. hs s also nown as a federated KF. Each KF denoted by an nde, s based on ndeendent nformaton from other flters, e.g. dfferent measurement sources, dfferent R or Q matrces or even dfferent ntal matrces. he MMKF s the default canddate for mang use of both the D and RK solutons n AhrsKF smultaneously and n arallel to each other. he MMKF enables ntegratng the solutons of two KFs, one oerated by RK and the other oerated by D n a sngle estmator. he smlest ways to erform ths are one of the followng [5]:. Weghted f: weghted averagng of the solutons and ther rocess covarance noses.. Best f: accets the soluton wth the hghest robablty and rejects others. In ether case the weghts are calculated by [5]:, ( ) m,,,, R,, l,, j e z, R z,,,,, (8)

8 8 where m s the number of comonents of the measurement vector, l s the number of flter models and the measurement nnovaton s generally defned as: z z (9) he best f flter smly chooses the soluton (hyothess) wth the hghest weght. he weghted f rocess can be based on redetermned fed weghts. In the weghted f MMKF wth adatve weghts, the flter model or hyothess wth the smallest normalzed measurement nnovaton s allocated the largest robablty, and s regarded as most consstent wth the measurement stream. Over tme the robablty of the best hyothess wll aroach unty, whle others aroach zero. If that s the case the flter cannot go bac to the adatve mode. o avod ths, the weghts must be reset to an ntal value after a fed nterval of eochs f the zerong haens. he overall state estmate and error covarance are obtaned as follows [5]: K,,, l R,,,, K, z,,, l,,,,, () Smultaneous RK and D For the urose of mang smultaneous use of RK and D solutons n AhrsKF, three algorthms were used. Frst, ntegratng the D soluton nto the AhrsKF, the D and RK solutons can be fed nto AhrsKF sequentally. hs means that f at eoch, the AhrsKF s fed by D soluton then the net eoch t s fed by the RK soluton. Note that both frequences of the RK/GS and the D were set at z. he other two aroaches to RK/D smultaneous utlzaton n AhrsKF nclude weghted-f MMKF and best-f MMKF. All three algorthms results were obtaned by ost rocessng usng D and RK data from the same test already descrbed. he long boom GS soluton was agan used as the reference and the statc results are shown n table 4, as well as Fgure 6a and 6b. he qualty of the sequental algorthm soluton s eactly between that of AhrsKF wth RK and AhrsKF wth D, n terms of both the error mean and the error SD. On the contrary, the MKF wth weghted f technque leads to no better erformance than AhrsKF wth RK or wth D. owever, the best erformances for both roll and yaw and n terms of both error mean and error SD are acheved by the best f MKF. hs may be due to the fact that the D ostons are not stable over longer erods than m, suggestng that RK hels n stablzng the overall soluton.

9 9 able 4. Statc results for the comarson between dfferent algorthms based on the ost rocessed AhrsKF solutons smultaneously fed wth both GS/RK and D wth long boom GS atttude soluton. Error Mean (Degree) Error SD (Degree) Sequental RK and D MMKF weghted f MMKF best f Roll Yaw Roll Yaw Roll Yaw Fgure 6. he roll and yaw errors for sequental algorthm (a and b), weghted f MKF (c and d) and best f MKF (e and f).

10 Concluson GNSS/INS (Global Navgaton Satellte System/Inertal Navgaton System) systems have found wdesread use n ndustry, esecally n automated agrculture. Automated agrculture requres hgh frequency, recse, steady and smooth atttude solutons. hese together wth strngent cost requrements, necesstate smart ntegraton algorthms. Gven that navgaton solutons can be derved from dfferent nformaton sources, combnng several nformaton sources can result n smoother atttude solutons relatve to when a sngle nformaton source s utlzed. A Multle Model Kalman Flter (MMKF) s used for ths urose. he results show that the fne attrbutes of both nformaton sources, namely RK and D, are adequately catured by best f MKF. References [] Y. L, et al., "Atttude determnaton by ntegraton of MEMS nertal sensors and GS for autonomous agrculture alcatons," GS Solutons,. -,. [] NovAtel. (8, Alcaton Note on seudorange/delta-hase (D) and GLDE Flters. Avalable: htt:// [] Y. Geng, et al., "Develong a Low-Cost MEMS IMU/DGS Integrated System for Robust Machne Automaton," n Fort Worth, eas, 7, [4] Y. L, et al., "Calbrated MEMS Inertal Sensors wth GS for a recse Atttude eadng Reference System on Autonomous Farmng ractors," n ION-GNSS, GA, Savannah, 9. [5]. D. Groves, rncles of GNSS, Inertal, and Multsensor Integrated Navgaton Systems. London: Artech ouse, 8.