TELKOM NIKA Indonesia n  Journal of  Electrical En gineering   Vol. 13, No. 2, Februa ry 20 15, pp. 292 ~ 299   DOI: 10.115 9 1 /telkomni ka. v 13i2.687 6          292     Re cei v ed O c t ober 2 0 , 201 4; Revi se d Novem b e r  19, 2014; Accept ed De cem b e r  12, 2014   Robust SINS/GNSS Integration Method for High  Dynamic Applications       Falin Wu 1 , Heng y a ng Zhao 1 *, Yan Zhao 1 , Haibo Z hong 2   1 School of Instrumentati on Sci ence a nd Opto -e lectro nics En gin eeri ng, Bei h ang U n ivers i t y ,    Beiji ng 1 0 0 191 , China   2 Space Star T e chno log y  C o . L t d, Beijin g, 100 086, Ch in a   *Corres p o ndi n g  author, e-ma i l : heng ya n g17 8@1 63.com       A b st r a ct   As high dy namic movement is   always accompanied  by color ed no ise which lacks of  m a them atic al  mo de l, traditi o nal K a l m an fi lterin g b a sed  o n  an  assu mp ti o n  of w h ite Ga u ssian  no ise  al w a ys faces ser i ous   diver genc e. T o  en hanc e th e p e rformanc e i n   hig h  dy na mi c  e n viro nment w i t h  u n certai n co l o red  no ise, a  ki nd   of  rob u st  filteri ng base d  on H - infinity  t e chn o l ogy is  d e vel o p ed.  State mod e l of  the  al gorit hm is  deriv ed  fr o m   SINS error  pro pag atio n. Both  pos ition  a nd  velocity  errors   are  used  as t h me asure m en ts. A simul a tio n   system  which  includes a tri- axial tu rnt able and a GNSS  signal sim u lator  is used t o  verif y  the integration  desi gn  un der  h i gh  dyn a m ic  en viron m e n t. Si mulati on r e su lts  prove d  th at b o th the  acc u racy  an d r obustn es s   of the integr ati on des ig n hav e  been i m prove d  sign ificantly.     Ke y w ords : SINS/GNSS inte gratio n, hig h  d y na mic, H-infi n i ty Kalman filte r ing     Copy right  ©  2015 In stitu t e o f  Ad van ced  En g i n eerin g and  Scien ce. All  rig h t s reser ve d .       1. Introduc tion  Strapdo wn In ertial  Navigati on System  (S INS)  is a  kind  of dea d reckoning  method  based   on Inertial M easurement Unit (IMU) [1]. As t he navigation error will get accumulated, the   precisi on of IMU determines the navigation effe ct mainly. Global Navigation Satellite System  (G NSS)  is a kind of  high p r eci s io po siti oning sol u tio n   whi c h co uld   provide po sition  an velo city  informatio n b y  a sin g le  re ceiver [2]. But the  u s ag e  of GNSS i s   limited be ca u s e of th sig nal  attenuation in  urban  canyo n  or situation  with st ron g  electroma gne tic in terference. Furthermore,   the lo w o u tp ut rate  of  G N SS limits its u s a g e  in  high  dynami c  e n viro nme n t. SINS/GNSS  integratio n mi xes thei r a d vant age and  disa dvantag e s  respe c tively whe r e th Kalman filteri n g   (KF) is   c o mmonly us ed [3, 4].  High  dynami c  m o vement  is  always  a c compa n ied   with u n certai n colored  no ise [5].  Ho wever, tra d itional Kalm an filter reli es  on the  a s sumption th at both pro c ess noi se a nd  measurement  noise are uncorrel at ed white Gaussian  noise whi c will  result in the divergent of  navigation re sults. Te chn o logie s  su ch  as  robu st filtering ha s b een develo p ed to enhan ce  navigation p e r forma n ce un der hi gh dyn a mic e n viron m ent  [6, 7].  H-infinity filtering i s  a ki nd  of  robu st filterin g technol ogy  which  could  retard f ilteri ng diverg ence effectively  in colo red no ise   environ ment  [8]. There  is no  req u ire m ent of  noi se pri o ri  kno w led ge  but j u st h a ving fi nite  boun ded  ene rgie s. H-infini ty filtering ba sed  on tra d itional Kalm an  filtering do es  not de stroy t he  linearity and  only need the  transfo rmatio n of the covariance matrix cal c ulatio n.  In this pap er, a kind of  H-infinity Kal m an  filter al gorithm fo r SINS/GNSS integrate d   navigation  sy stem i s  d e si g ned. Th e p r o c e s s mod e l i s  b a sed o n  S I NS erro r p r o pagatio n. Errors  of both  po sition a nd vel o ci ty are  use d  a s  m easurem ents . H-infinity  filteri n g   w a s  us e d  to  en h anc sy st em  rob u s t n e ss.  A t  la st ,  a com p a r ison  of  H-i n finity Kalman  filtering wit h  the traditio nal  Kalman filteri ng ha s bee n dra w n u s ing  a hard w a r e - in -the–lo op si m u lation sy ste m     2. Integra t ion Design   Structu r e of the integration  des ig n is sho w n in Figu re  1.      Evaluation Warning : The document was created with Spire.PDF for Python.
TELKOM NIKA   ISSN:  2302-4 046     Rob u st SINS/GNSS Integration Method  for  High  Dyna m i c Applicati ons (Falin  Wu)  293     Figure 1. Integration  stru ct ure       SINS algorith m  is appli ed  based on gy roscop e ang ul ar velo city  b ib  and accel e ro m e te r   specific force  b f  which a c qui red from IMU. Errors  between SINS an d GNSS re ce iver outputs  are  used as  measurement s of the  filter.  Filter estimation output will modify the SINS outputs  whi c u s e d  as  the   final outputs and feedba ck  to  t he next SINS iteration at  the same ti me.  Be s i d e  th es e,  L λ  and  h  are  latitude, lon g itude a nd  al titude with  L  and  h  a s  the  er rors e/ n/ u v  are v e locitie s  in  ge ogra phi coo r dinate  (i.e., east, no rth an d up ) with   e/ n / u v  as the   er rors ,  and    a r craft attitude (i.e., y a w, pit c an d roll ) from  t  frame (T rue   frame   or  navigation frame)  coo r din a te to  b  fram e (Body fram e or craft frame) coordina te.  e/ n / u Φ  are  misalig nment  angle s  from   t  frame to  c  frame (Com p u ter fram e)  coordi nate [9].  x /y / z v  are  velocitie s  in e a rth fixed coo r dinate a s  pa rts of GNSS receive r  outpu ts.    2.1. Process  Model   Process mod e l of the SINS/GNSS integration i s    XF X G W &  (1)     whe r X  is the estimation  state variable  with cova rian ce matrix  P   22 2 2 2 2 2 2 2 2 2 2 2 2 2 d i a g ,  , ,   ,   ,   en u e n u x y z x y z T en u e n u x y z x y z ΦΦ Φ vv v L h ΦΦ Φ vv v L h     X P  (2)     Whe r 2 e/ n/ u Φ 2 e/ n/ u v  an 2 / L /h  are varia n ce of mi sali gnment an gl es, velocity errors an d   positio n e r rors.  x /y / z  and   x /y / z  are gyrosco p e  drift a n d  a c cele rom e ter  bias at  b  frame   coo r din a te wi th variance  2 x/ y / z  and  2 x/ y / z W  is additive ze ro mea n  whit e Gau ssi an n o ise of the IM U with covari ance matrix  Q    2 2 222 2 d i a g , ,  , ,   gx g y g z ax ay az gx gy gz ax ay az      T W Q=  (3)     W h er g x/ g y / g z  and   ax / a y / az  are gy ro sco pe a n d  a ccel e rom e ter noi se  process  with va rian ce   2 g x/ g y / g z  and  2 ax / a y / az Process noi se input matrix   G  is given as:       33 33 33 3 3 33 33 33 33 33 33 T       0000 I G 000 I 0    (4)   Evaluation Warning : The document was created with Spire.PDF for Python.
                               ISSN: 23 02-4 046                     TELKOM NI KA  Vol. 13, No. 2, Februa ry 2015 :  292 – 299   294 State tran sition mat r ix  F  co uld be  extra c t ed fro m  p r op agation  of mi salig nment  a ngle s velocity errors, positio n erro rs a nd IMU  errors whi c can b e  expre s sed a s  belo w  [10].    2 ( s in ta n ) ( c o s ) ( s in ta n ) + s in (c o s ) t a n (c o s s e c ) ee n ei e n i e u e nn m en e ei e e u i e n nm n en e e ui e e n i e nm n n vv v LL L Rh Rh R h vv v LL L L Rh R h Rh vv v v LL L L L Rh R h Rh Rh           u & & &  (5)      2 2 (t a n ) (2 s i n t a n ) ( 2 c o s ) ta n (2 c o s s e c 2 s i n ) 2( si n t an ) nu en u u n e mm ee ie n i e u nn en e u en ie n i e u e n n eu n nu e e u i e e n nm vv vf f L v Rh Rh vv LL v L v Rh R h vv v v vv L Lv L L v L h Rh Rh vv v vf f L L v v Rh R h R            & &  2 2 ta n (2 c o s s e c ) 2 2( cos ) 2 s in 2 s in u m ee u e ie e n nm n en ue n n e i e e u nm ie e i e e u v h vv v + v L LL v L h Rh R h Rh vv vf f L v v Rh R h Lv L L v h          &  (6)       se c s e c ta n nn mm n ee e nn m n u vv L= h Rh Rh R h vv v λ =L L L L h Rh Rh R h Rh h= v       & & &  (7)     x / y /z g x /g y / g z x /y / z a x / a y / a z   & &  (8)     Whe r e/ n / u f e/ n/ u  and  e/ n / u  denote the  specifi c  force,  gyrosco pe d r i ft and accele rometer  bias  at  t  frame, res p ec tively.  ie  is the rotation al angul ar ve locity of the earth.  m R  and  n R  u s ed  above are ra dius of me ridi an plan e and  prime  verti c al  plane which can b e  cal c ul ated as:       2 2 12 3 s i n 1s i n e m e n R R ee L R R eL   (9)     Whe r e R  repre s ent s the radi us of the ea rth and  e  is  the earth ec centrity.          Evaluation Warning : The document was created with Spire.PDF for Python.
TELKOM NIKA   ISSN:  2302-4 046     Rob u st SINS/GNSS Integration Method  for  High  Dyna m i c Applicati ons (Falin  Wu)  295 2.2. Measure ment Model   Measurement  model is b a se d on po sition error a nd velocity error at geo grap hical  coo r din a te which  can b e  e x presse d as:      ZH X V  (10 )     Whe r Z  is the measu r em en t given as:              Z    T en u T e ,  S I NS e ,   G N SS n,  SI NS n,   GN SS u ,  SINS u,   GNSS SI N S GN SS SI N S GNSS SI N S GNSS vv v L h vv v v v v L L h h  (11 )     The  GNSS  receive r  h a s velocity o u tput s at  ea rth E C EF  (Earth   Cente r ed  Earth Fixed)  c o or d i na te  a s   x /y / z ,  G N S S v  which h a ve to be tran sferred into geo graphi cal coord i nate as  e / n / u,  G N SS v   cos s in 0 22 co s s i n c o s c o s      s i n 22 2 2 2 sin s in sin c os c o s 22 2 2 2                           e,  GN S S n,  G N SS u,  G N SS λλ v vL λ L λ L v L λ L λ L         x,  GN S S y,  G N S S z,  G N S S v v v  (12 )     The mea s u r e m ent matrix is given a s   33 33 3 3 3 3 33 33 33 33 3 3 33      0I 0 0 0 H 00 I 0 0  (13 )     V  is ze ro mea n  white Gau s si an noi se with  covari an ce m a trix  R      22 22 2 2 2 2 2 2 2 2 d i a g , , ,  d i a g en u h h h h v en u h h v h h v v v v L h v v v pe pe p vv v L h v v v p e p e p RR RR        v TT V R (14 )     Whe r e/ n/ u v  are GNSS velocity measurement  noise s at ge ogra phi cal co ordin a te whi c h can al so  be expresse d  as ho ri zontal  and verti c al  noise  h v  and  v v  w i th var i ances  of  2 h v and  2 v v . / L /h   are  GNSS  g eographi cal  p o sition  mea s urem ent n o ises  t r an sferre d from  ho rizo ntal an d verti c al  noise  h p  and  v p  with variances  of  2 h p  and  2 v p   2.3. H-infinity  Kalman Filter   Discretizatio n  of the process model a nd  measurement  model is represe n ted a s   11 / kk k k k k kk k k    XX W ZH X V   (15 )     A ssu me  K F T  is the cal c ulatio n perio d of the Kalman filter.   and   are give n as:     2 2 KF KF KF T TT  Ι F GF     (16)    Evaluation Warning : The document was created with Spire.PDF for Python.
                               ISSN: 23 02-4 046                     TELKOM NI KA  Vol. 13, No. 2, Februa ry 2015 :  292 – 299   296 A ssu me  ˆ k X  is t he  state e s timation  with v a rian ce   k P , the recursio n alg o rithm of th H- infinity filtering can b e  expressed a s :       ,1 2 1 11 / 1 / 1 / , 1 / 1 11 1 1 1 1 11 / 1 1 1 1 / 0 0 ˆˆ ˆ k TT ek k k k k k TT T T T k k k k kk k k kk k k k k kk k TT kk k k k k k k kk k k k k kk -                     ek H R RP H L IL H PP Q P H L R P L KP H H P H R XX K Z H X    (17)    Whe r k L I  an  1 21 max e i g T kk k   PH H ma x e i g M  mean s th e m a ximum ei ge nvalu e   of matrix  M 1  is a paramete r  use d  to gu arante e  the positiven ess  of  k P  and control the   tradeoff between ro bu stne ss a nd preci s i on [11]. The value of   is set  as 10 in this  system.       4. Validation and Analy s is  To verify the  integration  desi gn, a h a r dw are-in -the -loop  sim u lati on sy stem h a s b een  develop ed [1 2]. The  syste m  in clude s a   IMU, a  G N S S  re ceive r , a  trajecto ry  sim u lator, a  tri - axial  turntable, a G N SS sign al si mulator a nd a  nav igation compute r  as  shown in Figu re 2.          Figure 2. Simulation sy ste m  stru cture           Figure 3. Time synchro n ization diag ram   Evaluation Warning : The document was created with Spire.PDF for Python.
TELKOM NIKA   ISSN:  2302-4 046     Rob u st SINS/GNSS Integration Method  for  High  Dyna m i c Applicati ons (Falin  Wu)  297 The sy stem  is built o n  ann ular fi ber- opti c  co mmuni cation  netwo rk to  ensure   instanta neity [13]. The IMU co ntain s  a  tri-axia l fiber-optic  gyro scope an d a tri - axial pen dul ous  integratin g a c celeromete r with  output  freq uen cy  of 100 Hz. T he G N SS  receive r   outp u ts  navigation data and PPS (Pulses Pe r Second) [14] at frequency  of 10Hz. The  accelerom e ter  outputs de sired a c cele rati on u nde r ext e rnal  current  inje ction  whi c co mes form thre e el ect r ic  isolate d  cha n nels on the  DC po we r bo ard [15].  Impulse counte r  board is use d  to receive  and  transfo rm co ntinuou s imp u lse s  co me from the IMU [16]. The navigation com puter is u s ed  to   receive and  synchroni ze d a ta as sho w n  in Figure 3 a nd implem ent  the integratio n algorith m The b a si c SI NS alg o rithm  will  be  done  firstly  when   the IMU data  ha s b een  a c qui red.   Assu me the   SINS has out put of  , SI N S IM U m xt  and  ,1 SI N S I M U m xt  at time  , I MU m t  and   ,1 IM U m t . S o   navigation re sults at time  , P PS k t  is availabl e by applying bin o mial  interpol ation [17]. GNSS data at  time  , P PS k t  could b e  recko ned b y  the followin g  operation.         ,1 , ,, , , ,1 , ,, ,, , S I N S IM U m S I N S IM U m S I NS PPS k S I N S I M U m PPS k I M U m IM U m I M U m SI N S SI N S I M U n S I N S P P S k G N S S I M U n G N S S P P S k S I N S G NS S G NS S k S I NS tt tt t t tt tt tt t    xx xx xx x xx x x x    (18)    Initial value of matrix 0 ˆ X  and  0 P , along side  with the matrix  Q  and  R   are set a s  belo w .              0 22 2 0 2 2 222 2 2 ˆ 0 . 0 0 1 0 . 0 0 1 0 . 0 0 1 0000 0 0                0. 05 0. 05 0 . 0 5 50 5 0 5 0 di ag 0. 00 1 ,  0 . 0 0 1 ,   0. 001 ,                     0 . 0 1 ,   0. 0 1 ,   0. 0 1 ,   1 ,   1 ,   1 ,                       0 . 0 1 , T ee ms ms ms m h h h ggg m s ms ms m R m R m h  X P ooo o o oo o oo o o                 22 222 222 222 222 2 2 2   0 . 0 1 0 . 0 1 1 0 , 1 0 , 1 0 dia g 0.01 ,   0.01 ,   0 . 0 1 ,  1 0 ,  1 0 ,  1 0 d i ag 0 . 0 1 ,   0. 0 1 ,  0. 0 1 ,  0 . 1 ,   0. 1 ,  0. 1 ee h h ggg h h h ggg m s ms ms m R m R m   Q R oo ooo    (19)    Comp arative experim ents  of H-infinity Kalm an filteri ng and tra d itional Kalman  filtering  unde r high  dynamic e n v ironme n t have been  c ondu cted u s ing the sim u lation sy ste m Comp ari s o n s of attitude error, velo city erro r and p o siti on error a r shown in Figu re 4.  Attitude erro r mainly ca used by colored  noise of IMU unde r hig h  d y namic e n vironment.  Comp ari s o n  of attitude errors in dicates  that the  H-infi nity filtering coul d provide  high ro bu stne ss  to some exte nt. IMU noise  and GNSS receive r  noi se  affect velocity and positio n errors main ly,  expeci a lly the  up  axis  re sul t s a s  the  hei g h t ch ann el of  both SINS  an d G N SS hav e lo w p r e c isi o n.  Comp ari s o n  of velocity erro r and p o sition erro in dicate that H-infinity filtering ha s bet ter  inhibition  of u p  velocity an d altitude e r ror dive rge n cy along side with  mod e st improvem ent at  both velocity and po sition  errors.    Evaluation Warning : The document was created with Spire.PDF for Python.
                               ISSN: 23 02-4 046                     TELKOM NI KA  Vol. 13, No. 2, Februa ry 2015 :  292 – 299   298   (a) Attitude error    (b) Velo city  erro r   (c ) Position e rro r     Figure 4. Nav i gation erro r compa r ison       5. Conclusio n   This  pap er  p r esents a  ki nd of  H-i n fin i ty Kalman filtering  algo rithm for SI NS/GNSS  integratio m e thod und er high dynami c   enviro n me nt. Both de sign s of th e p r o c ess mo del  a nd  the me asure m ent mo del  are  linea r,  which  ap plys  to  Ka lman  filte r in g or  an o t h e r  lin e a r  fu s i on  techn o logie s .  A brief desig n of H-infinity filter ing deriv ed from tradit i onal Kalman  filtering is u s ed  to enhan ce  the robu stne ss p e rfo r ma nce. Co mpa r ative experi m ents un de r high dyna mic  environ ment based on a hard w a r e - in-t he-lo op sim u lation system  proved that  the H-infinity  Kalman filteri ng ha s better  robu stne ss  than tradition al Kalman filteri ng.      Ackn o w l e dg ements   The re se arch  is partial su pporte d by t he Ope n  Re sea r ch Fun d  of the Academy of  Satellite Applicatio n un der g r ant  No.20 14_ CX JJ-DH_ 07,  and the Sc ientific Re se arch  Found ation fo r the Retu rne d  Oversea s  Chine s Sch o l a rs, State Ed ucatio n Minist ry, China.       Referen ces   [1]  T i tterton DH, W e ston JL.  Strapd ow n Inerti al Nav i gati on  T e chno logy . T he Institutio n of Electrica l   Engi neers. 2 0 0 4 Evaluation Warning : The document was created with Spire.PDF for Python.
TELKOM NIKA   ISSN:  2302-4 046     Rob u st SINS/GNSS Integration Method  for  High  Dyna m i c Applicati ons (Falin  Wu)  299 [2]  Kapl an ED, H egart y   CJ. Un derstan din g  GPS: Principl es  and App licati ons.  ARTECH  HOUSE, INC 200 6.  [3]  Grew al M, Andre w s A.  Ka l m a n  F ilteri ng: T heory a nd P r actice Usi ng  MAT L AB . Wiley - IEEE Press .   200 8.  [4]  Groves PD. Principles  of GNSS, Inertial,  a nd Mu ltise n sor  Integrat ed Navigation S y stems.  ARTECH  HOUSE, IN C. 200 8.  [5]  Qi W-J, Heil on gjia ng  U, Zha n g  P, Ni e G-H,  De n g  Z - L. R o b u st W e ig hted  Measur ement  F u sion  Kalm a n   Predictors  w i t h  Uncerta i Noise V a ria n c es.  T E LKOMNIKA Indon es ian Jo urna l of Electrica l   Engi neer in g . 2014; 12( 6); 469 2-47 04.   [6]  Xi L, So h YC.  Ro bust K a lma n   filter ing for  uncertain s y stems.  Systems  &  Co ntrol  Letter s . 199 4; 2 2 (2);   123- 129.   [7]  Xi e L, So h YC,  De Souz a CE.  Robust K a lma n   filterin g for u n certai n discr e t e-time s y stem s.  Automatic  Control, IEEE  Transactions on . 1994; 3 9 (6); 131 0-13 14.   [8]  Simon D. Optimal Stat e Estimation: Kalman, H , and No n line a r Appr oac hes. Ne w  York:  W ile y .  20 06.   [9]  Benso n  DO. A Compar iso n  o f   T w o Ap pro a c hes to  Pur e -In e rtial a nd  Dop p ler-In e rtial Err o r Anal ys is.   Aerospace and Electronic Sys t em s, IEEE Transactions on . 1 975; AES-1 1 (4 ); 447-45 5.  [10]  Z hou W ,  D a li a n  Un ivers i t y  Of T ,  Ma H, W u  S,  Meng  N. IN S/GPS Integrat ed N a vi gatio T e chnolog y f o r   H y pers onic UA V.  T E LKOMNIKA Indon esia n Journ a l of Elec trical Eng i ne eri n g . 201 4; 12(1) ; 398-40 5.  [11]  Li W ,  Jia Y.  H-infin i t y  filteri ng for  a cl as s  of no nli n e a discrete-tim e  s y stems  base d  on  unsc ented   transform.  Sign al Process i ng 201 0; 90(1 2 ); 3301- 330 7.   [12]  Lei  HR, C h e n  S, Cha n g  Y,  W ang  L. Res earch  on  the  Hard w a re-i n-th e-Lo op S i mul a tion for  Hig h   D y namic  SINS /GNSS Integra t ed N a vig a tio n  S y stem.  Adva nced  Materi als  Res earch . 20 13;  8 46- 847 ;   378- 382.   [13]  Z hao K-R, Xu S, Ye Q, Li Y.  Desig n  an d rea l i z a t io n of flight  simu latio n  sys tem b a sed o n   Virtual R eal ity   techno lo gy . Co ntrol a nd D e cis i on  Confer enc e (CCD C), 2 0 1 1  Ch ines e. Mia n y an g, Chi na.  201 1; 43 61- 436 4.  [14]  Niu  X ,  Yan K,  Zhang T ,  Zhang Q, Zhang  H, Li u J. Qualit y   evaluat ion of  the pulse  per second (PPS)   signals from commercial GNS S  receivers.  GPS Solutions . 201 4; 1-10.   [15]  Li G, Mu J,  Xu H. Meas ure m ent stud y fo r accel e romet e r bas ed  on i n jecti on c u rren t.  Journal o f   Astronautic Me trology a nd Me asure m ent . 20 00; 20; 6-7.   [16]  Xi e W ,  Cai Y, Yao J, Xin  C .   Design and im plem entation of t he LIMU puls e  counting system . IE T   Confer ence Pr ocee din g s. Xia m en, Chi na. 20 12; 778- 78 1.  [17]  Z hang  P, Gu J,  Mili os EE, H u ynh P.  Navi gati o n w i th IMU/GPS/digita l co mp ass w i th u n sce nted K a l m a n   filter . Mechatr onics and A u tomation, IEEE Internatio nal  Confer ence. Niagara Falls, Ont., Canada.   200 5; 3; 1497- 150 2.         Evaluation Warning : The document was created with Spire.PDF for Python.