A FLEXIBLE APPROACH FOR THE NUMERICAL SOLUTION OF THE INS MECHANIZATION EQUATIONS

Transcription

1 A FLEXIBLE APPROACH FOR THE NUMERICAL SOLUTION OF THE INS MECHANIZATION EQUATIONS José J. Rosales and Ismael Colomina Insiue of Geomaics Generalia de Caalunya & Universia Poliècnica de Caalunya Caselldefels Spain Keywords: Inerial Navigaion Sysems, INS/GNSS inegraion, ordinary differenial equaions, numerical analysis. Absrac This paper describes an alernaive o numerically inegrae he differenial equaions of a srapdown inerial navigaion sysem (INS). The use of predicor-correcor mulisep mehods wih variable sep-size is discussed and a fourh-order mehod of he said ype is derived. The paper sars wih a brief review of he mos popular numerical mehods used in he soluion of he firs order differenial equaions of moion of srapdown inerial navigaion sysems. Then some of heir limiaions in pracical applicaions are discussed; namely ha hey are no well suied o he realiies of INS and Global Navigaion Saellie Sysems (GNSS) daa sreams wih daa gaps, unsynchronized sensors and deviaions from nominal sampling frequencies. Under his circumsances, radiional fixed sep-size algorihms lead o eiher low order mehods or cumbersome algorihmic paches. In he cenral par of he paper i is shown how variable sep-size mulisep mehods inheri all he advanages of single and mulisep mehods while avoiding mos if no all of heir pifalls. Las, a deailed derivaion of a fourh-order predicor-correcor mulisep mehods wih variable sep-size is given. 1 Inroducion The soluion of he differenial equaion model governing he dynamics of a srapdown inerial navigaion sysem canno be described by analyical and closed formulae; hence, i has o be solved by means of numerical algorihms. To he bes nowledge of he auhors, he classical approaches for he numerical inegraion of such models are he ransiion marix, fourh-order Runge-Kua (RK4) or he combined use of one-sep firs-order Euler mehod and RK4. [A numerical mehod is called n h order if is error erm is O(h n+1 ).] The ransiion marix has he main drawbac ha i significanly increases he compuaional burden if a reasonable precision for he soluion is required. Euler mehod has he advanage ha i does no depend on he inegraion sep (he significance of his fac will be perceived laer) bu, on he oher hand, i is of poor accuracy. RK4, however, is accurae enough bu i is based on evaluaions of he field ha defines he differenial equaion. This circumsance inroduces an exra difficuly due o he fac ha an inerial observaion is needed every ime he field defining he srapdown INS model has o be evaluaed. The classical RK4 mehod is defined wih a fixed sep-size, herefore he ime inerval beween wo consecuive inerial observaions mus be consan. Unforunaely, alhough heoreically here is a nominal frequency a which he Inerial Measuremen Uni (IMU) oupus inerial observaions, in pracice he inerial measuremens slighly depar from his nominal value. This can be overcame, for example, by inerpolaing he inerial observaions a he nominal frequency. However, manipulaion of he inerial observaions is no opimal from a numerical poin of view and i is an opion o be avoided if possible. An alernaive o he inerial daa inerpolaion is he use of he Euler mehod. When he difference beween he nominal frequency and he ime span beween wo consecuive inerial measuremens is above cerain hreshold, he inegraion sep can be se as he ime difference beween such measuremens and hen, an Euler sep performed. This avoids he need of inerpolaing, bu inroduces a loss of precision in he navigaion soluion due o he use of a firs-order mehod. Anoher ineresing scenario is he navigaion wih INS aided wih GNSS observaions, a synergy nown as INS/GNSS navigaion. The classical approach o INS/GNSS navigaion is he Kalman filer. The procedure basically is he following: he navigaion soluion is esimaed a each epoch by means of inegraing he INS mechanizaion equaions (see secion 2) unil a GNSS observaion is available. Then, by means of a Kalman filer sep, an opimal 1 sae is esimaed. Afer he filer sep, he navigaion soluion is again esimaed by means of he inegraion of he INS mechanizaion equaions unil he nex GNSS observaion and so forh. This algorihm assumes he GNSS observaion and he sae predicion evenually concur a some epoch; depending on he qualiy of he INS/GNSS componens and on he acquisiion configuraion, his coincidence migh be quie rare. This leads o a daa manipulaion or o algorihm paches such as he described in he previous 1 opimal in he leas-square norm sense.

2 paragraph. I is worh o noe ha he problems relaed wih he inegraion sep-size do no happen when using he Euler mehod alone. The goal of his paper is o design a numerical mehod for he inegraion of ordinary differenial equaions, avoiding all he problems exposed above while preserving he good properies of he hree mehods presened. In oher erms, he goal is o develop a mehod ha does no depend on he inegraion sep-size (as he Euler or he ransiion marix mehods do) wih a reasonable order of accuracy (such as he fourh-order Runge-Kua), considering also he compuaional aspecs. The proposed alernaive is a fourh-order predicor-correcor mulisep mehod wih variable sep-size. In his paper, he main feaures of he Euler, RK4 and he ransiion marix mehods are presened as well as he problems ha arise when hey are implemened in an Inerial Navigaion Sysem. Afer analyzing he performance of hese mehods, he proposed alernaive o overcome he pifalls of hese approaches is presened. 2 The INS mechanizaion equaions and heir inegraion. The sochasic differenial equaion 1 is he mechanizaion model of an srapdown inerial navigaion sysem in an ECEF coordinae sysem. ẋ e = v e + w v v e = Rb(f e b + a b + w f ) 2Ω e iev e + g e (x e ) Ṙb e = Rb e ( Ω b ei + Ω b ib(ωib b + o b + w ω ) ) (1) ȯ b = βo b + w o ȧ b = αa b + w a In his se of equaions, f b and ωib b are, respecively, he linear acceleraion and he angular rae sensed by he IMU. x e, v e, Rb e are he posiion, velociy and he roaion marix ha ransforms he coordinaes in he body-frame o he ECEF-frame respecively; w v, w f, w ω, w o and w a are whie noise generalized processes. Finally, o b and a b are he biases for he angular rae sensors and he linear acceleromeers respecively and α, β > 0 he inverse of he correlaion imes. Oher errors such as scale facors or misalignmens are no considered for he sae of simpliciy. As he objecive of his paper is no o disclose abou he inricacies of his model, he reader is referred o [2] for a complee descripion of he equaion 1 and is represenaion in differen reference frames. The way o numerically solve his sochasic model is o inegrae he associaed differenial equaion, his is, he ordinary differenial equaion resuling from aing apar he funcional and sochasic componens; on he oher hand, he covariance is propagaed according o he saisical properies of he noise. For furher informaion abou he numerical inegraion of sochasic differenial equaions, he reader is referred o [4]. This paper deals wih he numerical inegraion of he associaed ordinary differenial equaion. As already menioned, his differenial equaion does no allow for an analyical closed formula of he soluion, herefore a numerical algorihm has o be used for is inegraion. The nex subsecion explains how (and why) he classical raher, he popular mehods used for he numerical inegraion of equaion 1 fail in many INS/GNSS hybrid sysems. 2.1 The inegraion issue Two consideraions have o be aen ino accoun when saing he problem of he numerical inegraion of equaion 1. The firs concerns o he numerical mehod and he second o he naure of he inerial observaions. The ideal numerical mehod would be a mehod of high accuracy, sep-size independen and fas. Unforunaely, usually high accuracy requiremens increase he compuaional burden, maing he mehod slower. Hence, a compromise beween accuracy and velociy is required. For navigaion purposes, fourh-order mehods are a good compromise and usually hey fulfill he accuracy requiremens. A sep-size independen mehod would no be essenial if he field defining he differenial equaion could be evaluaed a whasoever epoch. For example, if he field had an analyical expression no depending on exernal observaions, his condiion would no be necessary a all. Unforunaely, his is no he case of he INS mechanizaion equaions. The model described in equaion 1 depends on he inerial measuremens linear acceleraions and angular raes provided by he IMU. Again, sep-size independency would no be required if he frequency a which he IMU provides inerial measuremens would be consan (ypically IMUs oupu inerial measuremens beween 50Hz and 400Hz, wha is enough for inegraion purposes). Bu, as said previously, alhough here is a nominal frequency, he inerial observaions slighly depar from i; even worse, daa gaps migh occur. Hence, he mehod becomes sep-size dependen. The nex subsecions presen an overview of hree popular mehods for he numerical inegraion of ordinary differenial equaions: Euler, RK4 and he ransiion marix. The Euler mehod has order one and is sepsize independen and he RK4 is of order four bu sep-size dependen. The impac of such feaures in he

3 inegraion of he INS mechanizaion equaions has been already menioned. The ransiion marix is sep-size independen and migh have arbirary precision. Neverheless, i is expensive from a compuaional poin of view, only holds for linear dynamical sysems and assumes ha he [ime dependen] linear relaionship beween saes and observaions is consan beween wo consecuive epochs. Moreover, he ransiion marix has o be recalculaed each epoch. For he sae of generaliy, insead of he INS mechanizaion model, a more general differenial equaion model will be considered. The saemen of he problem is ha of he classical iniial value problem: given a differenial equaion of he form { ẋ = f(x, u, ) for [0, f ] x( 0 ) = x 0 (2) where x R n is he ime dependen sae vecor, u R m he observaions vecor and he ime, he goal is o numerically compue he soluion. From now on, he noaion will be he following: ˆx sands for he numerical approximaion of he soluion, while x sands for he real soluion, x( ) = x, u( ) = u, f = f(x, u, ), ˆf = f( ˆx, u, ), h = One-sep Euler mehod This mehod requires a low compuaional burden, i is of easy implemenaion and is sep-size independen bu, alas, is of very poor accuracy (O(h 2 )) and highly unsable. However, as i will be seen laer, his mehod migh be very useful for some purposes. Assume ha he iniial value problem 2 has o be numerically solved. By he formal definiion of he derivaive of a funcion, i holds ha x +1 x ẋ = f(x, u, ) h Therefore, isolaing x +1 and considering ha only approximaions of he soluion are used, i can be sraighforwardly seen ha ˆx +1 = ˆx + h f( ˆx, u, ) Noe ha in his case, he inegraion sep h migh no be consan. The inegraion sep can be se as he ime span beween wo consecuive inerial observaions and he field can be evaluaed. As i will be seen in he nex secion, his privileged feaure is no shared wih he RK Fourh-order Runge-Kua mehod The fourh-order Runge-Kua is perhaps he mos nown and widely spread numerical mehod for inegraing ordinary differenial equaions. The general form of an explici Runge-Kua of arbirary order r wih fixed inegraion sep h is ˆx +1 = ˆx + h r j=1 A j j ( ˆx, h, ) 1 ( ˆx, h, ) = f( ˆx, u, ) j ( ˆx, h, ) = f( ˆx + h j 1 s=1 b js s ( ˆx, u, + a j h), u( + a j h), + a j h), j = 2, 3,..., r Noe ha for r = 1, his mehod reduces o he one-sep Euler mehod explained in he previous secion. The classical fourh-order mehod is wih r = 4. The following schema corresponds o i: 1 ˆx +1 = ˆx + h{ } 6 4 where 1 = f( ˆx, u( ), ) 2 = f( ˆx + h( 1 /2), u( + (h/2)), + (h/2)) 3 = f( ˆx + h( 2 /2), u( + (h/2)), + (h/2)) 4 = f( ˆx + h 3, u( + h), + h) The ineresed reader is referred o [1, 3, 5] for furher informaion abou oher paricular feaures of he family of Runge-Kua mehods. Noe ha his mehod requires four evaluaions of he field a differen poins and imes. Noe, as well, ha a higher r requires more evaluaions, increasing his way he compuaional burden. Moreover, fields wih no dependencies on exernal observaion or wih consan ime span beween consecuive observaions are required. Non of his condiions are fulfilled in pracice by inerial navigaion sysems in real condiions.

4 2.1.3 Transiion marix As already said, he ransiion marix mehod suis only for linear dynamical sysems. However, he differenial equaion 2 is no necessary linear. Hence, insead of he differenial equaion 2, he linear model { ẋ () = A()x() + B()u() for [0, f ] x( 0 ) = x 0 will be considered. Noe ha equaion 1 can be expressed in such a fashion. Assume ha he sae a epoch is nown and he soluion a epoch +1 has o be compued. Assume furher he ime span beween and +1 is small; hence, A() and B() can be considered as consan marices, A and B respecively. I can be shown ([4]) ha he soluion can be esimaed as follows: where +1 ˆx +1 = Φ( +1, ) ˆx + Φ( +1, τ)b u(τ)dτ Φ( +1, τ) = e A τ (3) wih τ = +1 τ. From a compuaional poin of view, equaion 3 can be approximaed by means of is expansion in power series up o some degree j. Φ j ( +1, τ) = I + A τ + 1 2! (A τ ) ! (A τ ) j! (A τ ) j (4) The accuracy of he navigaion soluion depends on he approximaion of equaion 3. As more erms of he serie 4 are calculaed, more accurae he soluion is. However, his marix has o be compued a every epoch. Hence, his mehod highly increases he compuaional burden if high accuracy is desired, specially when he sae vecor is of big dimension. The ineresed reader is referred o [2, 4] for more informaion abou he ransiion marix. 3 Mulisep and predicor-correcor mehods The numerical mehods exposed in he previous secions shared a common feaure: he esimaion of he sae a he epoch +1 depended only on he soluion given a epoch. The family of mehods wih such characerisic are called one-sep mehods. Conrary on he one-sep mehods, mulisep mehods use previous saes ˆx, ˆx +1,..., ˆx +r 1, r 2 for he esimaion of he sae a epoch +r, ˆx +r. The order of he mehod is characerized by he number r; if r previous saes are used o compue he soluion, i can be proved ha he mehod has order r. On he oher hand, if r saes are needed o esimae he sae a every epoch, he saes a he r firs epochs are needed in order o sar inegraing. As his informaion is no usually available, he use of a one-sep mehod r 1 imes (for example, Euler mehod) in order o iniialize he muli-sep mehod can be an opion. Assume now ha he iniial value problem 2 has o be solved for epoch +r, and ha he soluions ˆx, ˆx +1,..., ˆx +r 1, r 2 are available. The wo fundamenal ideas behind he mulisep mehods are he fundamenal heorem of calculus and he inerpolaion by polynomials. The fundamenal heorem of calculus saes ha he following equaliy holds for a coninuous funcion f x +r x +r 1 = +r +r 1 f(x, u, s)ds (5) Therefore, he goal is o calculae he righ side of he equaion 5. For his purpose, he funcion f is inerpolaed a he epochs, +1,..., +r by a polynomial of degree r. Le be Ψ r () he Lagrange inerpolaing polynomial of degree r. r Ψ r () = f( ˆx +j, u +j, +j )L +j () where j=0 L +j () = r +j +i +j i=0 i j Hence, isolaing x +r and considering again ha only approximaions of he soluion are nown, i holds ha ˆx +r = ˆx +r 1 + r j=0 +r f( ˆx +j, u +j, +j ) L +j (s)ds +r 1 (6)

5 Assuming ha i+1 i = h, i and by means of he change of variable s = +r 1 + h, he inegral of he righ side can be calculaed and i can be seen ha where ˆx +r = ˆx +r 1 + h β j = 1 0 r i=0 i j r β j f( ˆx +j, u +j, +j ) (7) j=0 s (r j 1) ds i j The paricular case where β r 0 and β 0 = 0 (no inerpolaing in he epoch ) leads o he well nown Adams-Moulon mehods. On he oher hand, in he case where β r = 0 and β 0 0 (no inerpolaing in he epoch +r ) one obains he family of he Adams-Bashforh mehods. As an example, he differen values for β i for he case r = 4 are summarized in able 1. Adams-Moulon Adams-Bashforh β β β β β 4 24 Tab. 1: Values for he Adams-Moulon and Adams-Bashforh formulae for r = 4 Observe ha in he case of he Adams-Moulon mehods, he unnown appears boh in he lef and righ sides of he equaion 7. The mehods wih his characerisic are called implici mehods, in opposiion o he explici mehods, where he unnown can be isolaed in he lef side of he equaion. Euler, RK4 or Adams-Bashforh mehods are examples of explici mehods. Bac o he Adams-Moulon mehod, his implici problem migh be solved by means of he following ieraive scheme: ˆx (i+1) +r = ˆx +r 1 + hβ r f( ˆx (i) +r, u r 1 +r, +r ) + h β j f( ˆx +j, u +j, +j ) (8) for i = 1, 2,... Of course, a good iniial approximaion is essenial for he convergence of he ieraive scheme 8. The explici Adams-Bashforh mehods provide excellen means for esimaing a firs approximaion of he sae. The procedure o follow is, firs, do a predicion sep using he Adams-Bashforh explici mehod in order o compue a seed for he ieraive scheme corresponding o equaion 8; aferwards, feed such equaion wih his esimaion and correc i by means of an Adams-Moulon implici mehod. This sor of procedures are he so-called predicor-correcor mehods. One migh hin ha he more you ierae equaion 8, he bes he approximaion o he soluion; neverheless, generally one ieraion is enough. See [3] for furher deails abou his opic. Now he philosophy behind he predicor-correcor mehods has been explained, bu from he way i has been described i suffers from he same drawbacs of he RK4 mehods. This drawbac is essenially ha he inegraing sep-size is fixed. I has been already commened in previous secions he impac of such consrain. The goal now is go one sep furher and consruc a mehod, based on he philosophy of he mulisep predicor-correcor mehods bu wih independen sep-size, in oher words, from now on he assumpion ha +1 = h, will no be required. The nex secion will explain how o consruc such mehod. I is worh o mae a las remar abou he exposed mehods. From formula 7 one migh be emped o hin ha for an r h order mulisep mehod, r evaluaions of he field are needed. However, a closer loo a formula 7 shows ha for calculaing he soluion a epoch +r, acually jus one evaluaion is needed since he previous ones a epochs +j, j = 0, 1,..., r 2, in he case of he Adams-Bashforh mehods and j = 1, 2..., r 1 for he Adams-Moulon s, were already used for esimaing ˆx +r 1. Hence, he here is no need of recalculaing hem (hey migh be sored in memory, for example), and only f( ˆx +r 1, u +r 1, +r 1 ) for he Adams-Bashforh case and f( ˆx +r, u +r, +r ) for he Adams-Moulon need o be compued. 3.1 A fourh-order variable sep-size mulisep predicor-correcor mehod Recall from he previous secion ha he consrucion of a mulisep mehod essenially consised in calculaing he differen values of β i, i = 0, 1,..., r. This values were he resul of evaluaing he inegral wrien on he j=1

6 righ side of he equaion 6. This calculus was done by means of he change of variable s = +r 1 h. This change of variable was consrained by a fixed inegraion sep h; now, as said before, his consrain does no necessary hold. One of he sraegies for inegraing he polynomials L +j (s) wih a non-consan sep-size h is by means of he change of variables defined by = s +r 1. Noe ha since he inegraion sep is no necessary consan, he recalculaion of he β s a every epoch is unavoidable. Neverheless, his does no imply a dramaic increasing of he compuaion ime since he formulae are quie simple. The modified formulae for he Adams-Bashforh and Adams-Moulon mehods are exposed below. Only he fourh order case (r = 4) is considered. Again, he goal is i o numerically solve he iniial value problem 2 a epoch +1 provided ha he soluions x 3, x 2, x 1, x are nown. Adams-Bashforh formulas General formula: x +1 = x + h (β 3 f + β 2 f 1 + β 1 f 2 + β 0 f 3 ) The formulas for β 0, β 1, β 2 and β 3 are, where β 0 = ζ 12h 1 (h 1 + h 2 )(h 1 + h 2 + h 3 ) β 1 = 12h 1 h 2 (h 2 + h 3 ) h ξ β 2 = 3h2 + 4h (2h 1 + h 2 + h 3 ) + 6h 1 (h 1 + h 2 + h 3 ) h 12h 2 h 3 (h 1 + h 2 ) β 3 = 3h2 + 4h (2h 1 + h 2 ) + 6h 1 (h 1 + h 2 ) h 12h 3 (h 2 + h 3 )(h 1 + h 2 + h 3 ) ζ = 3h 3 + 4h 2 (3h 1 + 2h 2 + h 3 ) ) +6h (h 1 (3h 1 + 4h 2 + 2h 3 ) + h 2 (h 2 + h 3 ) ) +12h 1 (h 1 (h 1 + 2h 2 + h 3 ) + h 2 (h 2 + h 3 ) ξ = 3h 2 + 4h (2h 1 + 2h 2 + h 3 ) +6h 1 (h 1 + 2h 2 + h 3 ) +6h 2 (h 2 + h 3 ) Adams-Moulon formulas General formula: x +1 = x + h (β 4 f +1 + β 3 f + β 2 f 1 + β 1 f 2 ) (9) The formulas for β 4, β 3, β 2 and β 1 are, 4 Applicaions o INS/GNSS inegraion β 1 = 3h2 + 4h (2h 1 + h 2 ) + 6h 1 (h 1 + h 2 ) 12(h + h 1 )(h + h 1 + h 2 ) β 2 = h2 + 2h (2h 1 + h 2 ) + 6h 1 (h 1 + h 2 ) 12h 1 (h 1 + h 2 ) β 3 = 3h 2(2h + h 1 + h 2 ) h 2 12h 1 h 2 (h + h 1 ) h + 2h 1 β 4 = 12h 2 (h 1 + h 2 )(h + h 1 + h 2 ) h2 Wih he excepion of sraegic-grade IMUs, i is well nown ha an INS by iself does no perform well when esimaing precise rajecories during long periods of ime. Free inerial navigaion is affeced by large, imedependen, ime-growing errors; for his reason, he INS is usually aided wih exernal measuremens. Typically, his aid comes from he observaions of one or wo GNSS receivers in order o bound he errors and drifs of he IMU sensors and o calibrae hem. The combined use of INS and GNSS echnologies leads o a echnology synergy widely nown as INS/GNSS navigaion. As he goal of his paper is no o discuss he performance deails of he INS/GNSS sysems, he reader is referred o [2] for furher informaion. This secion is devoed o he applicaions of he above presened numerical algorihm for he INS/GNSS inegraion. Firs, he realiy

7 of he INS/GNSS will be presened and aferwards i will be seen ha he proposed approach adaps o ha realiy and i overcomes he limiaions of he presen algorihms. As menioned in secion 1, he classical approach o he INS/GNSS inegraion is he Kalman filer. For he sae of clariy, a brief descripion is oulined in he following lines. In he Kalman filer approach, he navigaion soluion is essenially calculaed inegraing equaion 1 unil a GNSS observaion comes. When one inerial measuremen and one GNSS observaion occur a he same epoch, he navigaor firs esimaes he sae by means of an inegraion sep of equaion 1 and hen, an opimal sae is calculaed by blending he prediced sae and he GNSS observaion in a Kalman filer. The ineresed reader in he Kalman filer approach o INS/GNSS inegraion and rajecory deerminaion is again referred o [2] for furher deails. The Kalman filer approach assumes ha he clocs of he IMU and he GNSS receiver are synchronized and ha one inerial measuremen and one GNSS observaion concur a he same epoch periodically. This heoreical hypohesis is depiced in figure 1. As seen in picure 1, he wo daa sreams have consan ime spans beween wo consecuive observaions; moreover, here is an epoch a which IMU measuremen and a GNSS observaion coincide (a such epoch he Kalman filer sep is performed). Le f IMU and f GNSS be he IMU frequency and GNSS receiver frequency respecively and IMU and GNSS he epochs of he IMU and GNSS receiver clocs respecively. In his ideal case i holds ha f IMU = n f GNSS for some n N and IMU = GNSS for some. The realiy, however, is no ha simple and, depending on he qualiy of he IMU s cloc and he paricular feaures of he IMU/GNSS assembly, several scenarios migh occur. Figure 2 depics he siuaion where he oupu daa rae of he IMU is consan bu he daa sream is slighly shifed from he sream of he GNSS observaions. In oher words, f IMU = n f GNSS for some n N and IMU = GNSS + 0 for some and fixed 0. Going one sep furher, an even worse scenario can be considered, and his is he case where he frequency of he IMU, alhough being consan, is such ha f IMU n f GNSS, n N. This siuaion is depiced in figure 3. Finally, he wors scenario is he case where he f IMU f and IMU = GNSS + 0, where f is he nominal frequency of he IMU. Figure 4 shows his siuaion. Noe ha here are daa gaps boh in he IMU and GNSS daa sreams. This siuaion is quie common in he INS/GNSS inegraion and will be he one considered in his paper. (If he proposed approach succeeds in overcoming all he inconveniences of his scenario, i will also be able o deal wih he oher ones.) As menioned before, depending on he qualiy of he IMU, he GNSS receiver and he daa acquisiion sysem, he consisency level of he inerial and GNSS daa sreams can be very differen. The wors consisency level is usually found when inegraing low-cos (ypically auomoive grade) IMU wih low-cos GNSS receivers. Therefore, he proposed approach can be used in he currenly emerging fields of auomoive, pedesrian, lighweigh UAV navigaion, ec... In oher words, i is beer o give up any assumpion on daa synchronizaion, specially for IMU observaions. The classical soluion o overcome hese siuaions is o manipulae he daa in order o adap hem o he algorihms. Three soluions migh be considered: resample/inerpolae all IMU observaions (and [GNSS] measuremens, if necessary) o he ideal siuaion. resample/inerpolae [GNSS] measuremens o IMU epochs. resample/inerpolae IMU observaions o [GNSS] measuremens epochs. Neverheless, due o he noisy naure of he inerial observaions, inerpolaion has o be considered a las resor and avoided if possible. I will seen ha he proposed approach compleely avoids he need of inerpolaing inerial (or exernal [GNSS]) measuremens. The inerpolaion of inerial measuremens is also needed when he inerial observaions are no delivered a heir nominal frequency due o he fac ha he RK4 have consan sep-size (Euler mehod is no considered due is poor accuracy). Remember ha he proposed numerical mehod consised in he combinaion of a predicion sep (by means of an explici mehod) wih a correcion sep (an implici mehod). When he navigaion soluion is esimaed by means of he inegraion of he equaion 1, a predicion-correcion sep is made a each epoch considering he inegraion sep as he ime span beween wo consecuive inerial measuremens. These seps are depiced in figures 5 and 6. Assuming ha he sae a ime +1 has o be esimaed, and by means of he nowledge of he saes ˆx 3, ˆx 2, ˆx 1 and ˆx, he Adams-Bashforh [predicion] sep calculaes a fourh-order approximaion of he sae ˆx +1 (figure 5). Then, by means of an Adams-Moulon [correcion] sep, he previous saes ˆx 2, ˆx 1, ˆx and he prediced sae ˆx +1, he sae ˆx +1 is esimaed (figure 6). The INS/GNSS navigaion also benefis from he proposed approach o overcome he wea poins of such echnology. The weaness of he INS/GNSS navigaion (from a compuaional poin of view) is basically he [poenial] poor synchronizaion beween he he INS and GNSS receiver clocs. In case of no having ime synchronizaion in such sense, difficulies o perform he Kalman filer sep arise. Remember ha he Kalman filer approach assumes an inerial observaion and a GNSS observaion o concur periodically a some epoch. This ideal siuaion rarely happens (a leas in low-cos INS/GNSS sysems). Acually, o do he Kalman filer sep a a GNSS epoch, an inerial observaion is no essenial; wha is essenial is an esimaion of he sae a such epoch. In he absence of an inerial observaion a he menioned epoch, he sae can be esimaed by means of an Adams-Bashforh sep. The inegraion sep is se as he ime span beween he GNSS epoch and he

8 inerial measuremen of he previous epoch. This siuaion is graphically represened in figure 7. This provides a fourh-order esimaion of he soluion, and herefore he Kalman filer can be performed. Neverheless, once he Kalman filer sep is done, he navigaor has o coninue inegraing using he INS mechanizaion equaions; hence, an inerial measuremen a he GNSS epoch is required o iniialize he predicor-correcor algorihm. Again, wha is needed is no he inerial measuremen bu he evaluaion of he field a he GNSS epoch. The sraegy o esimae he field a ha poin is o mae use of he Adams-Moulon mehod. Remember ha he Adams-Moulon algorihm is an implici mehod, his is, he unnown is boh in he righ and lef side of he equaion (see equaion 9). However, wha was he unnown (he sae) for correcor is already nown from he predicor. Furhermore, an opimal esimaion has been calculaed by means of a Kalman filer sep. A closer loo a equaion 9 reveals ha he evaluaion of he field a he desired epoch can be easily isolaed. Hence, isolaing he unnown, he esimaed evaluaion of field a he desired epoch is: β 4 ˆf +1 = ˆx +1 ˆx h (β 3 ˆf + β 2 ˆf 1 + β 1 ˆf 2 ) where ˆf +1 is he esimaion of he field s value a he GNSS epoch. This procedure is depiced in he figure 8. This allows for an iniializaion of he predicor-correcor mehod and he equaion 1 can be inegraed unil he nex Kalman filer sep. Noe ha he fourh-order variable sep-size mulisep predicor-correcor mehod does no depend on a nominal frequency. In oher erms, he proposed numerical inegraion scheme adaps iself o he realiies of he INS/GNSS daa sreams, which seems o mae more sense han adaping he daa o he limiaions of he algorihms. 5 Conclusions and ongoing wor The problem of he numerical inegraion of he INS mechanizaion equaions has been presened. The classical mehods Euler, RK4 and ransiion marix have been oulined, heir pifalls exposed and an alernaive o hem described. The alernaive is a fourh-order variable sep-size mulisep predicor-correcor mehod, derived from he classical Adams-Bashforh and Adams-Moulon formulae. I has been also shown ha he RK4 and he Euler mehods are no well suied for he INS and INS/GNSS realiies. Variable sep-size predicor-correcor mulisep mehods are able o overcome his limiaions in a fairly way. The described mehod preserves all he good properies of he Euler mehod and he RK4: i is of fourh order, can adap is inegraion sep o he inerial daa rae and does no dramaically increases he compuaional burden. As said before, i adaps o he INS and GNSS realiies, conrary o he classical approaches ha adap he realiy o hemselves. Furher research includes he validaion of he proposed numerical mehod. Acnowledgemens The research repored in his paper has been funded by he Spanish Minisry of Science and Technology, hrough he OTEA-g projec of he he Spanish Naional Space Research Programme (reference: ESP ). References [1] Engeln-Müllges, G., Uhlig, F., Numerical Algorihms wih C, Spinger-Verlag, [2] Jeeli, C., Inerial Navigaion Sysems wih geodeic applicaions, Waler de Gruyer, [3] Press, W.H. e al., Numerical Recipies in C, Cambridge Universiy Press, [4] Maybec, P.S., Sochasic Models, Esimaion and Conrol. Volume 1, Mahemaics in Science and Engineering, Volume 141-1, [5] Soer, J., Bulirsch, R., Inroducion o Numerical Analysis, Spinger-Verlag, 1992.

9 x GP S o IMU Fig. 1: Ideal IMU/GNSS scenario. x GP S o IMU Fig. 2: Time shif beween he IMU and GNSS daa sreams. x GP S o IMU Fig. 3: IMU and GNSS daa sreams unsynchronizaed by a scale facor and a ime shif. x GP S o IMU Fig. 4: Daa sreams wih gaps and non-consan IMU frequency and daa gaps.

10 predicor {}}{ ˆx 3 ˆx 2 ˆx 1 ˆx ˆx +1 sae Fig. 5: INS inegraion. Predicor sep. correcor {}}{ ˆx 3 ˆx 2 ˆx 1 ˆx ˆx +1 sae Fig. 6: INS inegraion. Correcor sep. predicion {}}{ GNSS PV observaion ˆx 3 ˆx 2 ˆx 1 ˆx ˆx +1 KF sep ˆx +1 sae Fig. 7: INS/GNSS inegraion. Predicion and Kalman filer sep. f f 1 3 f 2 f f +1 field u 3 u 2 u 1 u u +1 GNSS observaion IMU obs. Fig. 8: INS/GNSS inegraion. Iniializaion afer a Kalman filer sep.