TELKOM NIKA Indonesia n  Journal of  Electrical En gineering   Vol. 14, No. 3, June 20 15, pp. 543 ~ 5 5 6   DOI: 10.115 9 1 /telkomni ka. v 14i3.806 9        543     Re cei v ed Fe brua ry 24, 20 15; Re vised  Ma y 13, 20 15 ; Accepte d  May 28, 20 15   The Adaptive F e deral Unscented Particle Filt er  Algorithm with Applications in All-Attitude Integrated  BDS/INS Navigation System      Ping Luo, Li-jie Yuan*, Liang-xue Hua ng, Xian-fei  Li  Ke y   Lab orator y of Industrial In ternet of T h ings& Net w o r k Co ntrol, MOE, Chong qin g  Un iver sit y  of Posts an d  T e lecommunic a tions, Ch on gq ing, Ch ina, 4 0 0 065    *Corres p o ndi n g  suthor, e-mai l :  y u anl iji e20 15 @16 3 .com       A b st r a ct   In order to  i m pr ove th e at titude acc u rac y , the t hesis  establ ishes t h e al l-attitud e  i n tegrate d   BDS/INS nav ig ation  n onl ine a r  system  mo del  bas ed  on  th e   positi on, v e loc i ty, attitude  by  add ing  the  BD S ’ s   attitude meas u r ement infor m ation   int o   th e me asur e m ent   equ atio n of  th e trad ition a l  B D S/INS inte gr ated   navigation nonlinea r system  m o del. Considering  the pr oblem that  the dy nam i c  nav igation syst em m o del  is  difficult to  acc u rately  descr ib e the c o mp lex  navi gat i on  en viron m e n t, the  thesis  i m prov es the  dyn a m i c   character i stics of the infor m ati on di stri buti on  of the feder al  fi lter alg o rith m w h ich co ul d time ly chan ge  base d   on the  ei genv a l ues r a tio of  ea ch subsyste m ’s error  vari anc e matrix. Then,  the  ad aptiv e feder al u n sce nte d   particl e filter (AF U PF ) is propose d . T he si mu lati on sh ow s that the pro pose d  al gorith m  co uld effecti v el y   w eaken th e i m p a ct o n  the  system  accura cy of the  inac curate hig h -dy n a m ic mod e l, and i m pr ove  t h e   ada ptab ility, the fault tolera nc e and th e a ccu racy, especi a ll y the attitude a ccuracy.    Ke y w ords :   BDS/INS integ r ated nav ig ati on syste m , attit ude u pdate,  adaptiv e info rmati on d i strib u tion ,   AFUPF    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   The BDS/INS integrate d  navigation  system ha s b e com e  a n  i m porta nt dev elopme n t  dire ction of the navigat io n  system, which could  overcome th e sh ortco m ing s  of  each  other,  and   have hig her navigation  accuracy  th a n  ea ch  syst em to work  alone.  With  the continu ous  developm ent  of satellite att i tude dete r mi nation te chn o l ogy, BDS no t only provid es the  po sition   informatio n a nd velo city i n formatio n, b u t also give s accu ra cy at titude inform ation [1] Error!   Refere nce s o urce not  found.  for the carri er. The f u lly integr ate d  BDS/INS navigation sy stem  whi c h  refle c ted the  BDS  attitude information in  the  integrated  n a vigation  system mea s u r e m ent   model  ha s b e com e  the f easi b le  soluti ons to  imp r o v e the a c curacy of attitud e , whi c h fu rther  increa se s the  obse r vability of the model [2].  In pra c tical  appli c ation s ,  the integrat ed navi gatio n system typically u s e s  indire ctly  kalma n  filter (KF) algo rithm to simplif y proce d u r e s  and improv e perfo rman ce, be cau s e  the   system’ s  e rro r is  studie d  a nd mod e led i n  advan c e . A t  pres ent, the domes tic  and  fo r e ign  e x pe r t s  also  study the appli c ation s  of the indirectly KF  in the BDS/INS integrate d  na vigation syst em.   Ho wever, the  BDS’s pse u d o  rang e-e r ror  is wo rst  and t he model isn’ t accu rate so  that it does no t  meet the ba sic p r emi s e  of the i ndirect KF, thus affects the filt er accuracy of  the INS’s data   integratio n.  The the s i s   gets th e no nlinea r m o d e l of the  all - attitude inte grated  BDS/ INS  navigation  system [3-5] by the  dire ct m e thod [6]: est ablishe s  the  state equ atio n based o n  the   mech ani cs  chore o g r aphy  equation s   and attitude  erro r equ ation in INS; establi s he s the   measurement  equatio n ba sed  on the  p o sition m e ss age, velo city messag e of t he BDS a nd  the   attitude error  messag e of the BDS/INS.  The i n form ation fu sion  h a s t w o  ways:  centralize d  filter  and   distrib u ted  filter. Th e  distrib u ted fe deral  kalman  filter (FKF ) based  o n  t he KF a nd  the inform ation di stributio n  techn o logy h a s be en ta ke n se riou sly b e ca use of  the parallel dat a pro c e s sing,  flexible desi gn,  small am ount  of calcul ation ,  better fault-tolera nt perfo rmance.  Ho wever, the  use of the su bfilter of the  FKF must mee t  certain ide a l  conditio n s:   1. Dynamic m odel of the sy stem is a c curate and line a r;  Evaluation Warning : The document was created with Spire.PDF for Python.
                             ISSN: 23 02-4 046                     TELKOM NI KA  Vol. 14, No. 3, June 20 15 :  543 – 55 6  544 2. Noise is un related to the  white noi se  a nd ha s the kn own  statistica l prope rtie s;  3. Ch oo se th e ap pro p riate  initial value   of the  state v a riabl es an d  the noi se  variance mat r ix i n   orde r to en su re the unbi ased estimatio n  re sult s and p r event the filter divergen ce .  Therefore, th e un scented  particl e filter  (UPF ) [7, 8] is u s ed in  su bfilter and th e federal   unsce nted pa rticle filter (F UPF) [9] is  constitu ted in t he thesi s . Th e UPF is the  non-li nea r no n- gau ssi an filter algo rithm an d has b een u s ed in ma ny  fields  su ch a s  navigation  system and ta rg et  tracking. Th e  UPF dire ctl y  estimates t he pa ra mete rs of the n a v igation syst em, avoide s the   lineari z atio n of the nonlin ear  state equ ation and e n s ures th e hig h  pre c i s ion o f  the navigation   sy st em.    After analy s is, it is not  difficult to find  th at the info rm ation di strib u tion ha s  alwa ys be en  the key to d e sig n ing a n d  achievin g th e FKF, the  d i fferent value s  have diffe rent federal filter   cha r a c teri stics. In a c tual   appli c ation, d ue to  th e  ch ange  of the   syst em  environment  or ot her  factors, the  perfo rman ce  of each su bsyste m of  the integ r ated  navigation  system may  also   cha nge. The  pred efined inf o rmatio n distribution  co efficient rem a ini ng unchan ge d durin g filtering  doe s not accurately refle c t  the actual sit uation an d affects the e s ti mated ac cu ra cy  of  t he sy st em.  Mean while, t a kin g  into a c count that  each comp o nent of the  state vecto r   has th e diffe rent   observability and co nverg ence, t he feedba ck con s i derin g the st ate variable s  as a whol e  i s   extremely unreasona ble.   B e cau s e  t h e  v a rian ce  m a t r ix  of  ea ch  su bf ilter of  the FKF  could  refle c t the  state   estimation a c curacy in re al -time. Based  on the  point, a new ad apti v e federal un scented pa rti c le   filter (AFUPF ) is propo se d  in the thesi s , w hose information dist rib u tion co effici en co uld  timely   cha ng ba se d  on the eige nval ue s ratio  of each  sub s ystem’ s error va rian ce m a trix   a nd bett e r  satisf y the d y nam ic state  chan ge s u nder  hi gh -d ynam ic envi r o n m ent so that  improve the  estimation a c curacy of the  federal filter.   The rest  of th e thesi s  i s  o r gani zed  as fo llows : In Sect ion 2, the  est ablishment  of the all- attitude integ r ated BDS/INS navigation  system m ode l is de scribe d. In Section  3, the AFUPF   algorith m  is d i scusse d. In Section 4, experim ent s results and an al ysis are put. Finally, in Section  5, con c lu sion s of the wo rk  are ma de.       2. Establishment of the All-atti tude In te grate d  BDS/INS Nav i gation Sy stem  Model   2.1. The Co mposition of the BDS/INS Integra t ed Nav i gation Sy stem   In the thesi s , the com p o s ition of the BDS/IN S integra t ed navigatio n system i s   shown in   Figure 1. Th e  informatio n fusio n  sy stem  has t w o  navigation  sen s o r s which a r e B D S an d INS  as  the com m on  referen c e system, whi c h  would  co ns t i tute three  subfilters, n a m ely: the po sition  subfilter, the  velocity subfil ter and the a ttitude s ubfilter [10-1 2 ] an d the main filter. The three   subfilters  use  the UPF  alg o rithm,  an d t heir i n formati on is se nt to  the main filt er to h a ve the   fusion  in  ord e r  to  get the  n a vigation  parameters  and   the be st attitu de e s timation  as the  output  of  the navigatio n system.       1 , g g PX 2 , g g PX 3 , g g PX 11 ,X P 33 ,X P 22 ,X P , m m PX , gg PX , f a     Figure 1. Structure of all - attitude  integrated BDS/INS navigation  system   Evaluation Warning : The document was created with Spire.PDF for Python.
TELKOM NIKA   ISSN:  2302-4 046     The Adapti v e  Federal Un scente d  Particl e  Filter  Algori t hm  with Application s  in… (Ping Luo )  545 2.2. The Mod e l of the BDS/INS Nav i gation Sy stem  The all-attitude integrated  BDS/INS navigati on syst em model e s tablish ed by the dire ct  method is a d opted in the thesi s , whi c h  con s id ers  the  navigation p a ram e ters of the INS as the   state vari able ,  establi s he s  the mea s u r e m ent equ atio n ba sed  on t he po sition m e ssag e, velo city  messag e of the BDS an d the attitude error  me ssage of the BDS/INS, and deal s with  the   modified AF UPF in o r de r to get the  navigati on  para m eter  e s timate s a s   the output of  the  BDS/INS integrated n a viga tion system.     2.2.1. The State Equa tion  of the Integr ated  Nav i gation Sy stem  The thesi s   con s id ers the navigation  c oordinate  system to  be the ge ogra phi cal  coo r din a te system, estab lishe s the  state  equatio n  base d  on t he me cha n ics ch oreog ra phy  equatio ns  an d the INS’ s a ttitude error  e quation  co ns i derin g that th e existen c e  o f  the inclin ation   error of the platform coul d  result in oth e r  axial gene rating spe c ific force com p onent s acco rding  to the literatu r e [6], and e s tabli s he s th e ran dom  co nstant mo del  for the accel e rom e ter’ s of fset  and gyro scop e’s offset. Th erefo r e, the state variable s  used in  simul a tion are:      T en u e n u e n u e n u x h a a add d                                                                   (1)        Whe r e, the  st ates a r e th e  velocity, posit ion, pla tform  attitude error,  accele rom e ter’s bia s e s  a nd  gyroscope’ s range drift.  The state e q u a tion of the BDS/INS  integrated navigati on syste m  is:     , x tF x t u t                                                                                                                         (2)    Discretize fo rmula (2 ) with  the fourth-ord er Ru nge -Kut ta method:      k k k u x f x , 1 `                                                                                                                                    (3)    Whe r e,  f  is th e no nlinea r f unctio n ;  k u  is the n o ise  vari able s  of  syst em; varia n ce  matrix of  the system n o ise i s   k Q .    2.2.2. The Observ a tion Equation o f  th e Integra t ed  Nav i gation Sy stem  The velo city for the ea st (x), north (y ),  and altitude  (z), po sition s  of the BDS a nd the  differen c e of  the attitude b e twee n the INS and B D S  are  sele cted  as the o b se rved varia b le s,  namely:    [] T kE B N B U B B B B yl h      The an gle-error of the  st at e equatio n  of the integrated  navig ation syste m  is the   mathemati c al  platform mi salig nment a ngle wh ich descri b e s  the relation shi p  betwe en the   mathemati c al   platform co ordin a te syst em  an d the  geog rap h ical  coo r di nate  system, b u t  the   differen c e  of  the  observ ed attitude   betwe en th e  INS a nd B D S d e scri be s the  relatio n shi p   betwe en  the  carrie r coo r di nate system and  th e  ge og raphi c co ordi nate system. In  ord e r  to   m a ke   them unified i n  the phy sica l sen s e, the r e sh oul d  be  essentially th e relatio n ship  betwe en the m ,  namely bb t pt P CC C . Where,  p  is the pl atform coo r di nate sy stem,   b is the  ca rri e r  coordinate  sy st em,   t  is th e geo graphi cal co ordinate  system. Ba se d  on th e relati onship, we  could o b tain a n   attitude subfil ter [11, 12] of the federal  filter, who s e ob servatio n mat r ix is:    3 cos c os c o s s i n 0 1 sin c os 0 cos sin s in sin c os c o s h                                                                        (4)  Evaluation Warning : The document was created with Spire.PDF for Python.
                             ISSN: 23 02-4 046                     TELKOM NI KA  Vol. 14, No. 3, June 20 15 :  543 – 55 6  546 In the way, the attitude m easure m ent i n fo rmatio n which  com e s f r om the multi-antenn a  Beidou recei v er on the  basi s  of carrier p h a s e d i fference technolo g y is e n tered i n to the  observation  e quation of th e tradition al BDS/IN S integrated  navig ation nonli n e a r sy stem m odel,  the purp o se of which i s  to corre c t the mathem atical model error of the BDS/INS integrated   navigation  system and im p r ov e the attitude accu ra cy.  The ob se rvation equ ation o f  the in tegrate d  navigation  system i s :     13 3 3 3 3 6 11 1 3 3 2 3 3 3 6 1 1 33 33 3 3 6 000 ,0 0 0 00 0 kk k k k h yh x h x h                                                      (5)    Whe r e,  1 ( 111 ) hd i a g ;  2 11 1 hd i a g ; 1 k y  is the ob se rved variabl e;   h  is linear  function;  k x  is the state vector value at the  k  time;  1 k v  is obse r vation  noise, the varian ce   matrix of the measurement  noise i s   k R .      3. The AF UPF Algorithm   3.1. The Uns cented Parti c le Filter   In the B D S/INS inte grate d  navig ation  system, th e p a rticle  filter (PF) [13]  coul d solve th e   nonlin ear  no ngau ssian p r oblem. Th e  PF relie s on  the impo rtan ce  sampli ng  and the  pro p o sal  distrib u tion  which  could  ap proximate  the  po sterio r  dist ribution  rea s o nably. Becau s e it i s  ha rd t o   desi gn  su ch  prop osal di stribution  so tha t  the PF  gen erally  sele cte s  the p r io ri di stributio n a s  t he  prop osal di stribution,  whi c h woul d ign o r e the ob se rvation inform ation of the  curre n t mom ent,  and ma ke th e state e s tim a tion serio u sl y depen d on  the syste m ’ s  mod e l. If the mod e l is  not  accurate o r  t he me asure m ent noi se  in cre a ses s udd enly, the way co uld n o t effectively re pre s en t   the re al di stri bution of th e  pro bability d ensity  fun c tio n . Mean while , unde r the d i stributio n, th e   weig hts a r e  calcul ated  with out co nsi d e r ing the i n fluen ce of th e mo del noi se,  wh ich  woul d aff e ct  the filtering a c cura cy.   Therefore,  R. Merwe  pro poses the  UPF  [7] which  uses the  UKF app roxim a tion to  gene rate the  propo sal di stribution for the PF. Bu t the UPF doe s not satisfy the fault-tolerant  requi rem ent of BDS/INS system.    3.2. The Principle of the  Federal  Kal m an Filter   The fed e ral kalman filte r  [1 4] ba sed  on t he the o ry  of t he d e centrali zed  filter i s  a   kind  of  two-stage   filter stru ctu r e, who s e   ba si c i dea i s  to  pa ra llel de al with   each  subfilter and  obtai n th eir  estimation  i x  and  varian ce  matrix  1, , i Pi n n , then sen d  them int o  the main filter to get  the peri odi c fusio n  processing a nd obt ain the glob a l  optimal esti mation  g x  and the varian ce   matrix  g P , and t hen  se nd th e  g x  and  the  enla r ged va rian ce   matrix 1 ig P   into ea ch su bfilter  to  reset each su bfilter, that is:       1 1 1 1 ii g ii g ig n i i P kP k Qk Q k xk x k                                                                                                                   (6)    Each  subfilt er corre c tes the time and the ob se rvation 1 i yk  and gets the   estimation   1 i xk  and the varian ce matrix  1 i Pk . Accordi ng to the following fo rmula, we   can o b tain th e global o p timal estimatio n  and  the vari ance matrix o f  the main filter.   Evaluation Warning : The document was created with Spire.PDF for Python.
TELKOM NIKA   ISSN:  2302-4 046     The Adapti v e  Federal Un scente d  Particl e  Filter  Algori t hm  with Application s  in… (Ping Luo )  547     11 1 1 1 11 11 k + 1 1 n gi i n gg i i i Pk P k xk P k P x k                                                                     (7)    Whe r e,  x k  is th e state va riab les of  system ;  1 yk  is the o b se rved vari able;  Pk  is the  mean squa re  erro r matrix (MSE) for the  k  time;  Qk  is the noise matrix for the  k  time;  i  is  the informatio n distrib u tion  coeffici ent;  i  mean s the  ith  subfilter;  g  means  the main filter.    3.3. The Info rmation Dis t ribution Me t hod of th e F e deral Filter  Algorithm   The  key to  resea r ch an d de sign  the  federal  filter is to  determine the i n formatio n   distrib u tion coefficient [14 ,  15] which  dire ctly  affects the pre c i s ion and the  fault toleran c e  perfo rman ce.   After  a   lot o f  simulation, the global  estimation of th e federal filter has the be st   accuracy whe n  the informat ion distri butio n coeffici ent is  0, 1 , 1 , 2, , mi nn i n n   .  But in the  a c tual  high  d y namic navi gation  enviro n ment, the   perfo rman ce  and  the   estimated  qu ality of each  subfilter is  co nstantly c han ging. Th erefo r e, it is h ope d that the ov eral l   perfo rman ce  of the filter can alway s  b e  clo s e  to the optimal su bsyste m, in other words,  the   informatio n  d i stributio n co efficient can  follow  th e  pe rforma nce of  the  subfilter to ma ke tim e ly  cha nge s [15].  Acco rdi ng to  the inform atio n dist ributio n  way,  the e ssence of the  feder al filter  al gorithm  [14] is to magnify the m ean squa re  error mat r ix (MSE) 1 i times, and a ssi gn t hem to the  different su bfilters. The KF  could autom atically  make  the use of different weig hts acco rdin g to  the me rits  of  the qu ality of inform ation,  the hig her th e information  quality of th e subfilter i s ,  the  smalle r the in formation di st ribution  coeffi cient is,  an d then the hig h e r  the utilizatio n of the fusio n   informatio n of the subfilter i s .  We  coul d det ermin e  the inf o rmatio n dist ributi on coefficient of each subfilter a c cording to  the varia n ce  matrix which  contai ns the  estimate d  e r ror m e ssa ge.  Acco rdi ng to   this m e thod, t he  highe r the  preci s ion  of the  filter is, the  smaller th e inf o rmatio n di stribution  coeffi cient i s , whi c h is   equivalent to  expandin g  the role  of se nso r  with  hi g her a c curacy . The adapti v e federal filter  algorith m  pro posed in the thesi s  is a s  fol l ows:  Schu r de com positio n of  i P .       T ii PU U                                                                                                                                         (8)    Therefore, th e informatio n di strib u tion coefficient of the  i  subfilter i s :      1 ,, 1 2 12 11 1 , ,n n ; 1 , ,m m ,, , nn ij i ij i j i i ii i i m m im m ij di a g                         (9)    Whe r e,   ,1 , 2 , ,, , ii i i m m diag   ， ,1 , 2 , ,, , ii i m m  is  the eig enval ues of i P ;  U is the  unitary matrix;  nn is the total  numbe r of the su bfilters,  mm is the dime nsio n of the varian ce   matrix of the subfilter,  i is the assig n me nt matrix, which is d e term ined by the prop ortio n  of   each re spe c ti ve comp onen t , ij on the diago nal of the varian ce ma t r ix of each  subf ilter to the  Evaluation Warning : The document was created with Spire.PDF for Python.
                             ISSN: 23 02-4 046                     TELKOM NI KA  Vol. 14, No. 3, June 20 15 :  543 – 55 6  548 sum of e a ch  respe c tive comp one nt , 1 nn ij i on  the diago na l of the variance matrix  of each  subfilter,  whi c h i s   used t o  ma ke th e j compon ent of  the i subfilter  have its o w n  inform atio n   distrib u tion coefficient ij , avoid treatin g  the feature s ,  the estim a tion accu ra cy and the   conve r ge nce  velocity of  each  comp o nent in   the  same   way, there b y am pli f ying the  sa me  multiples  so a s  to re sult in the pathol ogi ca l ca se s, and  improve calculation a c curacy.  Therefore:       1 11 1 2 1 1 1 21 2 2 2 1 2 1 12 11 1 11 1 1 1 2 11 11 1 22 1 2 2 2 2 2 11 1 12 gg g m m i gg g m m i ii g g m m g m m g mmmm im m ig ig ig m m ig ig ig m m im m g m m im m g m m im m g PP P PP P Pk P k PP P PP P PP P PP P                 mmm m                            (10)             Whe r e, kn own that  11 12 ii   and  = T gg Pk P k , we coul d kn ow   T ii Pk P k , whic h  mean s that the varian ce  matrix doe s not satisf y the requi rem e n t  of the symmetry, which coul d   destroy the converg e n c e  and sta b ilit y of the filter, resultin g in lo wer e s timatio n  accuracy, e v e n   diverge n t, losing the true  meanin g  of t he alg o rithm.  Therefore, in  orde r to e n sure the  sym m etry  of the varian ce matrix of  the  subfilte r  of the FKF, the pap er  imp r oves i n form ation dist ribut ion   method de scribed ab ove.  Firs t, s p lit the information dis t ribution c o effic i ent. Set up:    1 2 i i i im m B                                                                                                                       (11)             Therefore:     11 1 2 22 ii i i ii ii i im m im m i m m BB                                                (12)             Get the new f o rmul a, that is:       11 11 1 22 = ii ii ii g i g im m i m m Pk B P k B Pk                                              (13)  Evaluation Warning : The document was created with Spire.PDF for Python.
TELKOM NIKA   ISSN:  2302-4 046     The Adapti v e  Federal Un scente d  Particl e  Filter  Algori t hm  with Application s  in… (Ping Luo )  549 Thus:        1 11 1 11 1 1 12 1 2 1 1 21 2 1 2 2 2 2 2 2 11 2 2 ii i g i gi i g i i g m m i i m m gi i g i i g m m i i m m g m m i mm i g m m i m m i g mmm m i mm i m m Pk P k B P k B PP P PP P PP P                               (14)             Whe r e, T ii Pk P k , which mean s th a t  the varian ce matrix satisfies th e req u irem ent of  symmetry. T herefo r ,  we  can g e t the  a daptive info rmation di stri b u tion  coe ffici ent expresse d in  vec t or form. With the adaptive alloc a tion s t rategy, we could  re set the state e s timation   i x k ,  the varian ce  matrix i Pk , and the varian ce m a trix of the proce s s noi se i Qk  for ea ch subfil ter   in accordan ce with the formula (1 5).           11 11 1 1 ii g i ii g i ig n i i Pk Pk Qk Qk xk x k                                                                                       (15)            3.4. The Improv ed Adaptiv e  Federal UPF Algorith m   In this work,  the AFUPF is pro p o s ed to ov ercome  the FKF’s informatio n dist ribution  limitation an d  sati sfy the fa ult-tolerant re quire ment of  the BDS/INS  system. Fi gure 1  sho w s th e   stru cture of t he AF UPF a l gorithm. Ba sed o n  th e  al l-attitude inte grated  BDS/I N S navig atio n  system comp leted with the  nonlinea r st ate E quation  (3) a nd linea r obse r vation  equatio n (5)  o f   the all-attitud e  integrate d  BDS/INS navigati on syste m , the impro v ed AFUPF algorith m  can  be  adopte d  to so lve the proble m  of informati on fu sio n , whi c h could b e  concl ude d as f o llows:   (1) Initiali ze  the state  pa ramete rs: 00 0 0 ,, , g gg n xP Q ,and dist ribute t hem for ea ch su bfilter  according to the formul a (6 ).   For  1, , , nn n   For  1, , N , i    Sample the  i th particl e fro m  the prior  0 p x in the  n th subfilte r  and set up:     0|0 00 00 ˆ [0 0 ] , 0 0 00 k TT kk k P xx P Q R                                                                                                (16)    Comp ute  21 x n  sig m a points a n d  their wei ght s:    0| 0 00 0 | 0 , x xP P          Evaluation Warning : The document was created with Spire.PDF for Python.
                             ISSN: 23 02-4 046                     TELKOM NI KA  Vol. 14, No. 3, June 20 15 :  543 – 55 6  550   00 0 0 0 ,, ii i kx x j j xx x n P x n P                                                   (17)        0 2 0 ,0 1, 0 12 , 1 , 2 , 2 1 m x c x cm jj j x Wj n Wj n WW W n j n                                                                           (18)    Whe r e,  nn  is the numbe r of the su bfilters,  and  N  is the numbe r of the sampl ed pa rti c le s,  x n is the dimen s i on of state vector.     (2)  Cal c ulate d  usin g the UPF [9] for each subfilter at t he  TT k , , 1 time.  Importan c e  sampling  step  of the UPF in the  n th subfilte r  at the  TT k , , 1 time.  For  1, , , kT T    For   1, , , nn n   For  1, , N , i  update the parti cle s  with the UKF [16-18] in the  n th s ubfilter in time k .      (a) Calculati ng sigm a poi nts       11 1 1 1 1 ˆˆ ˆ ,, ii i i i i kk k k k k j j xx x n P x n P                    (19)     (b) Updatin g time      ,1 , 1 ii jk k j k xf x                                                                                                               (20)    2 1 ,1 0 2 11 | 1 ,1 ,1 1 0 n i mi kk jj k k j n T ii ci i kk kk kk j j k k j k k k j xW x PW x x x x Q                                              (21)     ,1 , | 1 ii jk k j k k zh x                                                                                                             (22)     2 1 ,1 0 n i mi kk j jk k j zW z                                                                                                      (23)     (c)  Upd a ting  measurement  with latest ob servatio n     2 11 ,1 ,1 0 2 1 1 ,1 ,1 0 kk kk n T ii ci i kk kk z zj j k k j k k k j n T ii ci i kk kk xz j j k k j k k j P W zz zz R PW x x z z                                                                             (24)    Evaluation Warning : The document was created with Spire.PDF for Python.
TELKOM NIKA   ISSN:  2302-4 046     The Adapti v e  Federal Un scente d  Particl e  Filter  Algori t hm  with Application s  in… (Ping Luo )  551 1 k k k k z z z x k P P K                                                                                                                       (25)    1 1 ˆ ii i kk kk kk k xx K z z                                                                                          (26)  T k z z k i k k i k K P K P P k k 1 |                                                                                                   (27)    (d) Sam p ling    Sample:     1 ˆ ,, ii ii kk k k k qx x z N x P                                                                                        (28)    For  1, , N , i  evaluate and no rmali z e the importa nce  weig hts:       1 1 0 1 , , ii i n kk k k ii ii i kk kk k ii i kk k pz x p x x ww ww w qx x z                                      (29)    (3)  Re sampli ng of the subf ilter  (a) Evaluatin g eff N      2 1 1 eff N i k i Nw                                                                                                          (30)    (b) Re sidu al  resam p ling wh en  ef f t h NN , for  1, , iN , s e t:    11 ,, 1 N N ii i kk k ii xx N                                                                                             (31)    (c) Subfilter’ s  output   The po steri o r distributio n is approxim ate d  as follo ws.        i k k N i i k k k x x z x p 1 : 1                                                                                            (32)    One obtai ne s the following  estimate a s :     1 ˆ N ii kk k i x x                                                                                                                         (33)    (4) Ada p tive informatio n di stributio n mat r ix and total output  Fusin g  re sult s of all subfilters to gen era t e the adaptive information  distrib u tion m a trix  of each  subfil ter acco rdin g to the formul a (9) a nd the  final total estimation re sult  according to  the formula (7).       4. Res Simulation res u lts   In this se ctio n the nume r i c al results o b tai ned by a pplying si mul a ted data to  the filters  are  presente d . The r e  are  thre e different cases u s ed in the t e s t ing in  order to verify  the   Evaluation Warning : The document was created with Spire.PDF for Python.
                             ISSN: 23 02-4 046                     TELKOM NI KA  Vol. 14, No. 3, June 20 15 :  543 – 55 6  552 perfo rman ce of the imp r oved AF UPF algo rithm   with the  a ll-attitude int egrate d  BDS/INS  navigation  system, whi c h a r e:   Ca se 1: th e  traditional  FUPF algo rithm  based  on th e BDS/INS i n tegrate d  n a v igation   system mo de l establi s he d by the direct   method a c cording to the literatu r e [3];  Ca se 2: the  traditional  FUPF algorith m  bas ed o n  th e all-attitude  integrate d  B D S/INS  navigation  system mod e l e s tabli s he d by  the dire ct method in the the s is;   Ca se 3: the improve d  AFUPF algo rith m base d  on  the all-attitud e  integrate d  BDS/INS  navigation  system mod e l e s tabli s he d by  the dire ct method in the the s is.     Simulation compute r  is: Pentium (R) D ual-Co r e  E5200 CP U;  2.50GHz freque ncy;  2.00GB RAM ;  Windo ws X P  Profession al OS. The  Matlab 7.1 si mulator  run s   in a Monte Carlo   fashio n. In the sim u lation,  the sa mpling   freque ncy  of I M U i s  10 Hz, the up date  cycle of the  BDS’s  data is 1 s , and the sim u lation time is 500s. Set u p : the rand o m  con s tant  of gyroscop e  is   0. 1 / h , the driving white noi se o f  the first- ord e r Markov proce s s of gyroscope is  0. 1 / h , the   white n o ise of  gyrosco pe i s   0. 05 / h , the relate d  time is 60 s; t he drivin g whi t e noise of th e firs t- orde r Ma rkov process of g y roscop e is  g 4 10 2 ,  the related ti me is 60 s; the position, ve locity  and attitude p r eci s io n of the BDS re ceivers  whi c h  is u s ed in the  system is 10m, 0 . 2 m/s and 1’.   The re sult s from ea ch case are  comp ared in term s o f  trajectory, velocity, positi on and   attitude error.  The co mpa r i s on s of traje c tory unde r  different filter al gorithm a r e  showed in Fig u re  2. The e r ror  curve s   of the  positio n e s ti mation in  th e  three  ca se s  are  sh owed i n  Figu re  3 to  5.  The e r ror curves of th e vel o city e s timation in  the th re e cases are  showed i n  Fig u re  6 to  8. T h e   error curve s   of the attitude estimation in  the  three cases are sh owe d  in Figure 9 to 10.         (a)  Comp ari s on of estimat ed and real  trajecto ry   (b)  Comp ari s on of trajecto ry in height-lat itude  plane   (c) Com p a r ison of trajecto ry in height-lo ngitude  plane   (d)  Comp ari s on of trajecto ry in latitude- longitud e  pla n e     Figure 2. Co mpari s o n  of trajecto ry unde r different filter algo rithm   46. 78 46. 8 46. 82 46. 84 46. 86 46. 88 46 . 9 8 8. 2 8. 4 8. 6 3000 3200 3400 3600 3800 4000 4200 4400 4600 4800   L o n g i t ude ( ) L a ti tu d e ( )   He i g h t ( m ) T h e t r u e  t r aj ec t o r y Th e  c a s e  1 Th e  c a s e  2 Th e  c a s e  3 8. 0 5 8. 1 8. 1 5 8. 2 8. 2 5 8. 3 8. 3 5 8. 4 8. 4 5 8. 5 300 0 320 0 340 0 360 0 380 0 400 0 420 0 440 0 460 0 480 0 L ong i t ud e ( ) H e ight ( m )     T h e t r u e   t r aj ec t o r y Th e  c a se  1 Th e  c a se  2 Th e  c a se  3 8. 1 6 8. 1 7 8. 1 8 8. 19 41 00 41 50 42 00 42 50     8. 34 8. 35 8. 3 6 8. 37 8. 38 3 350 3 400 3 450     46. 78 46. 8 46. 82 46. 84 46. 86 46. 88 46. 9 3000 3200 3400 3600 3800 4000 4200 4400 4600 4800 L ongi t ude ( ) ） He i g ht ( m 46. 815 46. 82 46. 825 46. 83 4100 4150 4200 4250     46. 865 46. 87 46. 875 3350 3400 3450 T h e  tr ue  tr a j e c t o r y Th e  c a s e  1 Th e  c a s e  2 Th e  c a s e  3 46 . 7 8 46 . 8 46 . 8 2 46. 84 46 . 8 6 46 . 8 8 46 . 9 8. 05 8. 1 8. 15 8. 2 8. 25 8. 3 8. 35 8. 4 8. 45 8. 5 Lo n g i t u d e ( ) L a ti tud e ( ) 46 . 8 1 4 6 46 . 8 1 4 8 46 . 8 1 5 8. 1 4 7 8. 1 4 8 8. 1 4 9     46 . 8 79 8 46 . 8 8 46. 88 02 8. 3 9 8. 3 9 1 8. 3 9 2 46 . 8 5 9 6 46 . 8 59 8 46 . 8 6 46 . 8 60 2 8. 31 6 8 . 31 65 8. 31 7 8 . 31 75 8. 31 8 T h e  tr u e  tr a j e c tor y Th e  c a se  1 Th e  c a se  2 Th e  c a se  3 Evaluation Warning : The document was created with Spire.PDF for Python.