A Robust Self-Alignment Method for Ship s Strapdown INS Under Mooring Conditions

Transcription

1 Sensors 3, 3, ; do:.339/s3783 Artcle OPEN ACCESS sensors ISSN A Roust Self-Algnment Method for Shp s Strapdown INS Under Moorng Condtons Feng Sun, Hayu Lan,, *, Chunyang Yu, Naser El-Shemy, Guangtao Zhou, Tong Cao and Hang Lu 3 3 Marne Navgaton Research Insttute, College of Automaton, Harn Engneerng Unversty, Harn 5, Chna; E-Mals: sunfeng47@6.com (F.S.); chunyangy@yahoo.cn (C.Y.); zhougt@hreu.edu.cn (G.Z.); caotongheu@63.com (T.C.) Department of Geomatcs Engneerng, 5 Unversty Drve NW, Unversty of Calgary, Calgary, AB TN N4, Canada; E-Mal: elshemy@ucalgary.ca Natonal Space Scence Center, Chnese Academy of Scences (CAS), Bejng 9, Chna; E-Mal: luhang_heu@6.com * Author to whom correspondence should e addressed; E-Mal: lanhayu@6.com; Tel.: ; Fax: Receved: Aprl 3; n revsed form: 7 June 3 / Accepted: 9 June 3 / Pulshed: 5 June 3 Astract: Strapdown nertal navgaton systems (INS) need an algnment process to determne the ntal atttude matrx etween the ody frame and the navgaton frame. The conventonal algnment process s to compute the ntal atttude matrx usng the gravty and Earth rotatonal rate measurements. However, under moorng condtons, the nertal measurement unt (IMU) employed n a shp s strapdown INS often suffers from oth the ntrnsc sensor nose components and the external dsturance components caused y the motons of the sea waves and wnd waves, so a rapd and precse algnment of a shp s strapdown INS wthout any auxlary nformaton s hard to acheve. A roust soluton s gven n ths paper to solve ths prolem. The nertal frame ased algnment method s utlzed to adapt the moorng condton, most of the perodcal low-frequency external dsturance components could e removed y the mathematcal ntegraton and averagng characterstc of ths method. A novel preflter named hdden Markov model ased Kalman flter (HMM-KF) s proposed to remove the relatvely hgh-frequency error components. Dfferent from the dgtal flters, the HMM-KF arely cause tme-delay prolem. The turntale, moorng and sea experments favoraly valdate the rapdness and accuracy of the proposed self-algnment method and the good de-nosng performance of HMM-KF.

2 Sensors 3, 3 84 Keywords: nertal navgaton systems (INS); algnment; moorng condton; nertal measurement unt (IMU); nertal frame; hdden Markov model; Kalman flter. Introducton Algnment s the essental procedure for strapdown nertal navgaton systems (INS). The purpose of algnment s to estalsh the ntal atttude transformaton matrx etween the ody frame and navgaton frame []. Before the start of navgaton, accurate algnment s crucal for strapdown INS f precse navgaton results, namely poston, velocty and atttude (PVA), are to e acheved, especally for the shp s strapdown INS whch may e requred to provde PVA nformaton of a shp wth relatvely hgh precson over perods of days, weeks, or even longer [,3]. Normally, algnment technques can e categorzed nto two types ased on the condtons of strapdown INS: statonary algnment and n-moton algnment [4 6]. The essence of statonary algnment s to compute the ntal atttude matrx usng two non-collnear vectors, namely the gravty and the earth rotatonal rate measurements from accelerometers and gyroscopes respectvely [7,8]. Nowadays, the statonary algnment has een studed and developed very well [9 ]. In-moton algnment means algnment on a movng or shakng platform, n ths case, dfferent from statonary algnment, the aove mentoned gravty and earth rotatonal rate measurements cannot e accurately measured due to the external moton dynamcs of the vehcles. Sometmes, there s not enough tme for strapdown INS to get algnment process done on a statc platform at the startng pont [3 5]. And also strapdown INS appled n shps or weapon systems of vessels have to move n response to the moton of the sea and wnd waves oth under moorng and voyagng condtons [6 8]. Moreover, after startng the navgaton calculaton procedure, re-algnment process s necessary for strapdown INS on the movng platform, as the errors of navgaton soluton could gradually grow up due to the poor ntal algnment naccuraces and the growng sensor errors [9 ]. Accordngly, t s essental to explore the relale n-moton algnment schemes for strapdown INS. The algnment of shp s strapdown INS contans moorng algnment n docks and algnment at sea ased on the condtons of the shp [7]. As for algnment at sea, the auxlary external nformaton should e used, such as poston nformaton provded y the gloal postonng system (GPS) [ 8], velocty reference gven y the Doppler velocty log (DVL) [9 3], etc. Ths artcle wll not dscuss the methods of algnment at sea. Moorng algnment elongs to n-moton algnment. As mentoned aove that under moorng condton, the shp has to wthstand external random movements whch are manly caused y the sea wnd and sea waves, and mostly the frequences of these external movements are hgher than /5 Hz [3]. So the prmary prolem of shp s strapdown INS algnment under moorng condton s that the gravty and the earth rotatonal rate measurements wll e dstured y lneal and angular movements of the shp especally for the acceleratons measurements, thus resultng n the algnment accuracy fallng and tme ncreasng [6 8]. Also, as state-of-the-art nertal measurement unt (IMU) employed n a strapdown INS often suffers from ntrnsc sensor nose, especally for the low-end tactcal grade and low-cost IMU, thus the algnment angles more or less may not e computed accurately and rapdly enough [,]. Fgure shows the frequency spectrum

3 Sensors 3, 3 85 of the z axs raw acceleraton of a shp s strapdown INS under moorng condton, The INS contans a self-made medum level IMU, and ts samplng frequency s Hz (the relatve coordnate frames defnton and expermental detals wll e addressed further n ths artcle). It can e depcted that the raw measurements of z axs accelerometer are manly mxed wth oth the ntrnsc sensor nose components (n roadand-frequency) and the relatvely low-frequency error components whch are mostly caused y the external dsturance. Besdes, other nertal sensors outputs of ths IMU have the same frequency spectrum characterstc as the outputs of the z axs accelerometer [7]. Fgure. The z axs acceleraton frequency spectrum under moorng condton. Frequency spectrum of z axs acceleraton (m/s ) external dstruance n low-frequency ntrnsc sensor nose n roadand-frequency Frequency (Hz) In ths work, we concentrate on the nvestgaton of moorng algnment method for shp s strapdown INS. As s evdent from the aove dscussons, how to extract the useful gravty and angular rate of earth rotaton measurements from the IMU outputs s the prmary ojectve. Two useless error components whch are lended n the useful measurements need to e removed as much as possle efore or durng the algnment process:. Intrnsc sensor nose.. External random dsturance. For solvng ths prolem, studes on algnment of shp s strapdown INS under moorng condtons have employed varous approaches. The conventonal Kalman flter methods or other optmal estmaton methods are utlzed n [3,4 6] to estmate the msalgnment angles. However, as the estmaton process s affected y the external dstured movements of the shp, the algnment duraton wll e eyond mnutes []. In some studes, the pre-flters are desgned to remove or attenuate the nfluence of external dstured movements and sensor nose. Wavelet de-nosng technque s used n [,] to remove the hgh-frequency sensors nose components. Lan et al. [33,34] frstly gve the dea of utlzng the fnte mpulse response (FIR) dgtal flters to attenuate dsturng acceleratons, and they clam ths method s sutale for large ntal msalgnment angles cases. Smlarly, Zhao [35,36], Zhang [37] and L, et al. [38] utlze the cascaded FIR flters. However, as proved y Zhang n [37], these FIR flters should e desgned wth very large orders to meet the de-nosng requrement, thus resultng n the ncreasng of oth the computaton urden and algnment tme. In [39], we adopt the nfnte mpulse response (IIR) dgtal low-pass flter to process the gyroscope and accelerometer outputs. The proposed IIR flter can reduce well the nfluence caused y the dstured movements of the shp wth very small flter orders. Nevertheless, one major drawack of all

4 Sensors 3, 3 86 these dgtal flters s that they wll more or less suffer from tme-delay prolems n real-tme mplementatons. Lü et al. [4,4] propose a novel preftler whch comnes a Kalman flter and a IIR dgtal flter to flter the outer dsturances and the sensor nose n a real-tme way, dfferent from the Wavelet de-nosng technques, whch have lock processng structures, the computaton tme of the freflter s much shorter than the Wavelet methodology. Gaffe [4] and Napoltano [43] gve the dea of nertal frame ased algnment (IFBA) scheme whch uses the projecton of gravty n the nertal frame to calculate the atttude matrx etween nertal frame and ody frame. Ths scheme can attenuate the dstured movements, and quckly otan the ntal atttude matrx. Varous authors have presented solutons usng ths method [44 5]. As analyzed n [49], most of the perodcal and lowfrequency random dsturance can e removed y the mathematcal ntegraton and average characterstc of ths method, however, the relatvely hgh-frequency and non-perodcal components would stay all through the algnment process. The Hdden Markov model (HMM) s a powerful statstcal tool for the modellng of sequence data [5]. It has een orgnally appled to stochastc sgnal processng, and nowadays has een wdely used n speech recognton [5,53], human movement analyss [54], etc. Enlghtened y Lü et al. [4,4] and the theory of HMM [5], we consder the useful IMU outputs for algnment as a HMM, vz., the vald measurements of IMU used for INS algnment (the local gravty and earth rotatonal rate) are hdden n the IMU s raw outputs whch nclude ntrnsc sensor nose and external dsturance. Based on ths, we propose a two-dmensonal hdden Markov model ased Kalman flter (HMM-KF) to pre-process the IMU output sgnals, most of the error components referred aove could e fltered out y the proposed HMM-KF, then the useful data for INS algnment can e otaned. Dfferent from the pre-flters n [7,,,33 39], the man advantage of the proposed HMM-KF s that the processng of sequental data wll arely cause tme-delay prolem. After pre-processng the IMU outputs, useful nput measurements are used n the followng algnment process whch can e dvded nto two steps.e., coarse algnment and fne algnment [5]. Due to the enefts that the IFBA method can counteract and average the low-frequency perodcal dstured acceleratons, n our approach, we adopt the IFBA method, oth n the coarse and fne algnment processes. The remander of ths paper s organzed as follows: Secton addresses the coordnate frames used n ths paper. Secton 3 descres the modfed IFBA method for shp s INS under moorng condton. Secton 4 ntroduces the prncple of the proposed two-dmensonal HMM-KF, ts usefulness for de-nosng the error components of the IMU outputs s evaluated y experments. Secton 5 detals the connecton etween HMM-KF and low-pass dgtal flter, whch explans why the HMM-KF can remove the hgh-frequency nose components, and tme-delay performance comparson wll e shown usng the HMM-KF as compared wth the correspondng low-pass dgtal flter. Expermental results that valdate the proposed approach are presented and dscussed n Secton 6. Fnally, conclusons are gven n Secton 7.. Frames Defntons The dfferent coordnate frames used n ths paper are defned as follows and llustrated n Fgure (a). Each frame s an orthogonal, rght-handed coordnate frame.

5 Sensors 3, 3 87 () The e frame: Earth-fxed frame, wth ts z axs parallelng the earth s rotaton axs, x and y axes are fxed and parallel equatoral plane. () The frame: Inertal frame, stalzed n nertal space, at the egnnng of nertal algnment, t parallels earth frame. (3) The n frame: Navgaton frame, used for navgaton and atttude representaton, n ths work, we choose the local level coordnate frame as the n frame, ts x, y and z axs pont to local east, north and upward respectvely. (4) The frame: Body frame, IMU sensor coordnate frame wth ts orgn at the centrod of IMU, three axes parallel nertal sensors nput axes. (5) The frame: Body nertal frame, at the egnnng of nertal algnment procedure, t s formed y fxng the frame n the nertal space. Fgure. (a) The dfferent coordnate frames; () Concal movements of the gravty vector n nertal frame. z z e y n ω e y z y z x z n o x o λ ω t e g o x n y e x y x e (a) () 3. Modfed Inertal Frame ased Algnment Method 3.. Prolem Formulaton The tradtonal analytcal algnment of strapdown INS s to calculate the ntal value of atttude matrx C n etween the frame and the n frame y usng the followng equaton [3]: C n T T [ g ] [ f ] = [ ωe] [ ω ] n n T [ ] T [ ] g ωe f ω n n T T n T n T where g = [ g ] and ωe = [ ωe cos L ω e sn L] are the projectons of the local gravty vector g and the turn rate of the earth ω e n the n frame respectvely, L s the local lattude, f and ω oth can e easly extracted from the measurements of IMU (see e.g., [3] page 5 57 for detals) are the projectons of g and ω e on the frame respectvely. Ths analytcal method s condtonal upon the precondton that the strapdown INS s on a statc platform durng the algnment process. As for shp s ()

6 Sensors 3, 3 88 strapdown INS under moorng condton, due to the external movements of the shp, the IMU outputs are mxed wth external random dsturance, so the algnment method mentoned aove s napplcale. 3.. Inertal Frame ased Algnment As we know that the pure gravty vector can form a cone n the nertal frame due to the rotaton of the Earth, see Fgure (). So the projectons of gravty vector n the nertal frame change slowly wth a perod of 4 hours, namely the Earth s rotaton perod. When the shp s strapdown INS s under moorng condtons, most of the dstured angular rate and acceleraton components vary perodcally and n relatvely low frequences [44]. By projectng the measurements of accelerometers and gyroscopes nto the nertal frame, the perodc dstured components can e averaged and counteracted, the pure gravty vector and earth rotatonal rate vector reman [45,46]. Gven the partcularty of shp s strapdown INS under moorng condton and y makng full useful of the nformaton mentoned aove, here we gve the dervaton of the analytcal nertal frame ased algnment method for shp s strapdown INS. Ths method contans coarse and fne algnment process, and oth are derved n the nertal frame Coarse Algnment In order to fulfll the algnment process, the ody nertal frame (the frame) s ntroduced frstly, whch s formed y fxng the frame n the nertal space at the egnnng of the algnment (t ), namely C ( t ) = I. Then after the start of algnment, the frame s fxed n the nertal space. So the algnment matrx etween the frame and the n frame can e expressed y: C = C C C () n n where C represents the atttude transformaton matrx of the strapdown INS ody frame ( frame) relatve to the frame, and can e nstantaneously updated y usng the followng expresson [3]: [ C = C ω ] (3) where ω s the skew symmetrc matrx of the vector ω [,, ] T = ωx ωy ωz measured y the gyroscopes represents the turn rate of the frame wth respect to the frame [3]. Notce that the external dstured angular velocty δω s and the random error of gyroscopes are ncluded n ω. Also, the matrx C n can e updated y a functon of local lattude L and the tme t as follows: [ ωe t t ] [ ωe t t ] [ ω ] [ ω ] [ ω ] [ ω ] sn ( ) cos ( ) n C = sn Lcos e( t t ) sn Lsn e( t t ) cos L cos Lcos e( t t) cos Lsn e ( t t) sn L As the moorng algnment s generally mplemented n the specfc docks, the local lattude L n Equaton (4) s a known quantty. C s a constant matrx representng the transformaton matrx etween the frame and the frame, the detaled dervaton of C can e seen n Appendx A. (4)

7 Sensors 3, 3 89 Fnally, susttutng for n C, C and C respectvely from Equatons (3), (4) and (A8) n Equaton () gves the coarse algnment result C n. Worth notng that n Equaton (A4), the accelerometer sensor nose component s gnored, and n Equaton (3), the gyroscope outputs ω used to update C contan sensor nose. Moreover, after the ntegraton of Equaton (A5), the non-perodc dsturance components reman, so the further fne algnment stage s ndspensale Fne Algnment We use the standard Kalman flter to estmate the msalgnment angles n the fne algnment procedure, the state and measurement equatons of the Kalman flter are oth estalshed n the nertal frame. The velocty-error dfferental equaton and the msalgnment angles equaton n the nertal frame could e wrtten as: δv = g ϕ + C as + C nose (5) ϕ = Cεas Cεnose where δv = [ δv ] T x δvy δvz s the velocty-error vector n nertal frame. g s gven n Equaton (5), ϕ = [ ϕ ] T x ϕy ϕz denotes the msalgnment angle vector. nose and ε nose are Gaussan whte noses related to the velocty errors and the msalgnment angles respectvely. as s the random acceleraton as vector, ε as s the random gyroscope drft vector, oth are assumed n gauss dstruton, namely as =, ε as =. The detaled dervaton procedure can e seen n [45]. Equaton (5) can e regarded as the standard lnear system drven y the Gaussan whte noses. Then the state equaton of the fne algnment Kalman flter can e represented as: 33 [ g ] 33 C C [ ] nose C C [ nose] 3 X ε = X (6) T T T T T where X = [ δv ] [ ϕ ] [ εas ] [ as ] s the state vector. The measurement equaton s formulated as: ˆ Vx V x ˆ Z = Vy Vy = [ I ] X + N3 (7) ˆ Vz Vz N where 3 s the measurement nose vector, V = [ V ] T x Vy Vz gven n Equaton (A) s the ntegraton of the gravty n the nertal frame, ˆ ˆ [ ˆ ˆ T V = V V V ] s the velocty of the strapdown INS calculated n the nertal frame, and s gven y: ˆ V C f dt x y z = (8)

8 Sensors 3, 3 8 Equatons (6) and (7) consttute the state and measurement model respectvely. Through usng the dscrete Kalman flter, the convergng values of the msalgnment angles could e estmated, then the coarse atttude matrx derved n Equaton () wll e corrected. Although all the remaned components depcted n the last secton are n small quanttes, however, the algnment speed slows down due to these error components, also the precson of algnment results are stll more or less affected y these components [7]. If precse and rapd algnment results are to e acheved, these error components should e pre-fltered as much as possle. In next secton, a novel technque s proposed to solve ths prolem. 4. Hdden Markov Model Based Kalman Flter In an ordnary Markov model, the states are drectly vsle to the oservatons. However, n a hdden Markov model (HMM), the states are not drectly vsle, ut the outputs dependent on the states are vsle. Each state has a proalty dstruton over the possle output sequence. Therefore the output sequence generated y the HMM gves some nformaton aout the sequence of the states. Fgure 3 shows the topology structure of the HMM. Fgure 3. Topology structure of the hdden Markov model. F q q q qk q k + O O Ok O k + H t t t tk t k + In Fgure 3, {O k } s the oservaton sequence at tme step t k, {q k } n the shadow crcle denotes the hdden state sequence, and q s the ntal value of the state. H s the transton matrx etween the hdden state {q k } and oservaton {O k }, F s the transton matrx etween the states q, oth H and F are governed y proalty dstrutons. 4.. HMM of IMU Outputs We defne X k (k =,, ) as the IMU deal output sequence (a Markov chan) whch s vald for INS algnment. When the shp s under moorng condton, X k s dstured y the addtve sensor nose and random movements of the shp, so the resultant output sequence s Z k. From the precedng dscusson we know that X k s hdden n Z k (partcularly when X k = Z k, the state s no longer hdden) then the HMM can e formulated as follows: X k+ = FX k + μ k+ (9) Zk+ = HX k + ν k+ ()

9 Sensors 3, 3 8 where Equatons (9) and () are dscrete n the tme, n the state space and the measurement space N M respectvely, X R, Z R, the matrces F and H consst of transton proaltes and have elements F j, H j, satsfyng: N Fj =, j= M Hj =, Fj, Hj () j= In ths study the IMU outputs are modeled as a two dmensonal HMM. And ased on the characterstcs of IMU outputs, Equatons (9) and () can respectvely e expanded nto [55]: Xk+ Xk μk+ = + () Xk Xk μk Z X ν = + k+ k k+ Z k X k ν k We term μ k the drvng noses and ν k the measurement nose, they are assumed to e zero mean Gaussan whte nose, satsfyng: T { }, {, } { } T { }, {, } { } T cov{ μ, ν j} = E{ μν j} = E μ = cov μ μ = E μ μ = Qδ j j j E ν = cov ν ν = E νν = Rδ j j j (3) 4.. Kalman Flter Based on HMM After dervng the dscrete HMM, the next step s to estmate the state sequence X k from the measurement sequence Z k usng some optmal algorthms. An optmal soluton s through comnng the standard Kalman flter wth HMM to remove the nose and other perturaton components [5]. The standard Kalman flter equatons are descred n [56], n each flterng step, the Kalman gan K s calculated and used to correct the propagated state when a measurement s avalale. In ths specfc applcaton, the dynamcs matrx F and measurement matrx H of HMM can e regarded as constant matrces, so the Kalman gan K wll quckly tend to e a constant, represented y K. Then the Kalman flter equatons ased on the two dmensonal HMM are smplfed as follows: Computng the state predcton: Xˆ Xˆ k+ / k k = Fkk, ˆ ˆ Xk/ k Xk (4) Updatng the state estmate: Xˆ ˆ ˆ k+ Xk+ / k Xk+ Xk+ / k = + K Hk ˆ ˆ ˆ X / k X X k k k Xk/ k (5) Smoothng the two state estmate for removng the flterng rpples: X = ( Xˆ + Xˆ ) (6) k+ k+ k

10 Sensors 3, 3 8 where ~ * X s the IMU output at tme step k +, k+ X s the smoothed estmate at tme step k +. k HMM-KF Implementaton Experments The proposed HMM-KF algorthm was appled to the real data collected from a self-made medumaccuracy IMU. The IMU conssts of three nterferometrc fer optc gyroscopes (IFOG) and three accelerometers mounted n three mutually orthogonal drectons. The specfcatons of the self-made IMU are shown n Tale. Durng the experments, the IMU was on a moorng shp, so the error components of the IMU outputs under ths condton manly contan the external dsturance and sensor nose. The HMM-KF parameters of accelerometer outputs were ntally set as follows: a 3 3 = {.3,.3 } 4 4, R {, } a dag = g g Q dag g g where Q a and R a are the process nose covarance and measurement error covarance of the flter, g s the local gravty. Smlarly, the HMM-KF ntal parameters of gyroscope Q g and R g are: g 5 5 = {.5 ( / ),.5 ( / )} 5 5, R { 5 ( / ), 5 ( / )} g dag = h h Q dag h h Tale. Specfcatons of the IFOG ased IMU. Parameters Gyroscope Accelerometer Constant as. o /h 4 g Random nose.5 o /h 4 g/ Hz Dynamc range ± o /s ±3g The choces of the parameters Q a, R a, Q g, R g wll e dscussed n the followng secton. Fgures 4 and 5 respectvely show the accelerometer and gyroscope outputs n tme-doman wth and wthout usng the proposed HMM-KF. Tale and Tale 3 lst the standard devaton (STD) of the error components of each nertal sensor for oth the orgnal and the fltered outputs. Fgure 4. Tme-doman analyss of the accelerometer outputs wth and wthout HMM-KF (a) x axs accelerometer; () y axs accelerometer; (c) z axs accelerometer. f x (m/s ) x Wthout HMM-KF Wth HMM-KF Tme/s (a)

11 Sensors 3, 3 83 Fgure 4. Cont. 4 x -3 Wthout HMM-KF Wth HMM-KF f y (m/s ) Tme/s 9.89 Wthout HMM-KF Wth HMM-KF 9.88 () f z (m/s ) Tme/s (c) Fgure 5. Tme-doman analyss of the gyroscope outputs wth and wthout HMM-KF. (a) x axs gyroscope; () y axs gyroscope; (c) z axs gyroscope. Wthout HMM-KF Wth HMM-KF ω x ( /h) - ω y ( /h) Tme/s (a) Tme/s () Wthout HMM-KF Wth HMM-KF

12 Sensors 3, 3 84 Fgure 5. Cont. 4 3 Wthout HMM-KF Wth HMM-KF ω z ( /h) Tme/s (c) Tale. STD of accelerometer outputs wth and wthout HMM-KF. Methods f x [m/s ] f y [m/s ] f z [m/s ] STD wthout HMM-KF.3.9. STD wth HMM-KF.85e e e-4 Tale 3. STD of gyroscope outputs wth and wthout HMM-KF. Methods ω x [ o /h] ω y [ o /h] ω z [ o /h] STD wthout HMM-KF STD wth HMM-KF It s ovous from Fgures 4 and 5 that most of the nose components are removed. Tales and 3 provde the comparson of the standard devaton of the accelerometer and gyroscope outputs wth and wthout usng the proposed HMM-KF. The two tales ndcate that sgnfcant reductons n nose components are acheved through usng the HMM-KF. Fgures 6 and 7 respectvely analyze the accelerometer and gyroscope outputs n frequency-doman wth and wthout the HMM-KF. It can e depcted that the HMM-KF can provde ovous attenuaton of nose components wth specfc frequency ands for oth the accelerometer and gyroscope outputs. Fgure 6. Frequency-doman analyss of the accelerometer outputs wth and wthout HMM-KF (a) x axs accelerometer; () y axs accelerometer; (c) z axs accelerometer. Normalsed frequency spectrum of f x x Frequency (Hz) (a) Wthout HMM-KF Wth HMM-KF vertcal amplfcaton

13 Sensors 3, 3 85 Fgure 6. Cont. Normalsed frequency spectrum of f y Normalsed frequency spectrum of f z Frequency (Hz) x -3 () (c) Wthout HMM-KF Wth HMM-KF x -3 vertcal amplfcaton Wthout HMM-KF Wth HMM-KF vertcal amplfcaton Frequency (Hz) Fgure 7. Frequency-doman analyss of the gyroscope outputs wth and wthout HMM-KF (a) x axs gyroscope; () y axs gyroscope; (c) z axs gyroscope. Normalsed frequency spectrum of ω x Normalsed frequency spectrum of ω y Frequency (Hz) x -3 (a) () Wthout HMM-KF Wth HMM-KF x -3 vertcal amplfcaton Wthout HMM-KF Wth HMM-KF vertcal amplfcaton Frequency (Hz)

14 Sensors 3, 3 86 Fgure 7. Cont. Normalsed frequency spectrum of ω z x -3 Wthout HMM-KF Wth HMM-KF vertcal amplfcaton Frequency (Hz) (c) 5. Analyses of HMM-KF In ths secton, some dscussons on the prncple of the proposed HMM-KF are ntroduced, whch elaorate the de-nosng characterstc of the HMM-KF. By mathematcal dervaton, we found the flterng property of the HMM-KF, as t can e changed to the form of dgtal flter s dfference equaton. Then experments were conducted to compare the HMM-KF wth the correspondng dgtal flter. Results clearly show that the proposed HMM-KF has etter real-tme characterstc as compared wth the correspondng dgtal flter, whle the dgtal flter gets ovous sgnal tme delay(s) under the same flterng effect. 5.. Connectons etween Dgtal Flter and the HMM-KF Susttutng Equaton (4) n Equaton (5), we have: ( ) Xˆ ˆ k = I K Hk Fk, k Xk + KZk (7) Susttutng Equatons () and (3) n Equaton (7), expandng t yelds: Xk+ Xk Zk+ K K X = k + X k Z k (8) Smplfyng t, one gets: ( ) ( ) X = K X + K Z X = K X + K Z k+ k k+ k k k (9) Assume that x = Z, y = X, then the Equaton (9) can e descred as the form: ( ) ( ) ( ) ( ) y k K y k K x k + = + + () We nfer from Equaton () that the value of y at tme k + only depends on the current nput x(k + ) and the pror output y(k). Ths propagaton model s the standard recursve dgtal flter s dfference equaton [57]. So Equaton () formulates that *the HMM-KF has the dgtal flter s characterstcs, whch explans why HMM-KF can remove the nose and error components of the IMU

15 Sensors 3, 3 87 outputs. Moreover, the connecton etween HMM-KF gan K and the cutoff frequency f c of the correspondng dgtal flter s as follows: K ( K) fc = arccos () π / f ( K ) ( ) s where f s s the samplng frequency, and the detaled dervaton procedures are gven n Appendx B. Through usng Equaton (), the HMM-KF and the correspondng dgtal flter can e mutually transformed. Commonly, efore usng dgtal flters for sgnals processng, the sgnals frequency spectrum or power spectrum should e analysed n advance, ased on ths, the flter s cutoff frequency f c can e determned. In contrast to the use of dgtal flters, the ntalzaton of HMM-KF s relatvely easer. As the choces of the ntal measurement error covarance matrx R and the process nose covarance matrx Q are less determnstc n the actual mplementaton of Kalman flter [56], ths s also sutale for the proposed HMM-KF. But once Q and R are determned, HMM-KF gan K wll stalze quckly and then reman constant. To our knowledge, the dfferent ntal condtons of Q and R for the HMM- KF do not nfluence the flters performances clearly, ths wll e evaluated and dscussed next n the experments part of ths secton. 5.. Comparsons of the HMM-KF and the Correspondng Dgtal Flters Once Q and R are determned off-lne, we can get the determnstc HMM-KF gan K, then the correspondng dgtal flter s cutoff frequency f c can e determned y Equaton (). Two dfferent dgtal flters have een wdely used: IIR (Infnte Impulse Response) flters and FIR (Fnte Impulse Response) flters. In general, IIR flters could e approxmated y a prescred frequency response wth relatvely fewer multplcatons and lower computaton urdens than FIR flters, ecause that the FIR flters need much hgher orders than the correspondng IIR flters to meet the same flterng requrements. However, the FIR flters are capale of workng wth a strct lnear-phase,.e., the tme delay etween the nputs and outputs of FIR flters can e exactly known [57]. For ths reason, IIR flters are more relale n the applcatons that do not need real-tme requrements and are n low computaton altes, whle the FIR flters are always employed n systems whch need to know the accurate flterng tme delay. Here n our study, oth the correspondng IIR and FIR low-pass dgtal flters were desgned and analyzed to compare the real-tme altes and the flterng performances wth the proposed HMM-KF. The comparson experments were conducted through usng the same IMU data as depcted n Secton 4.3. In ths work, we use the MATLAB/Flter Desgn & Analyss Tool to desgn the two dgtal flters. The samplng frequency of the IMU s Hz, then F s equals Hz. For the IIR flters, we choose the Butterworth low-pass dgtal flters and specfy the flter order N IIR as 5 and respectvely. For the FIR flters, the transton-and s set as [f c.5 Hz, f c +.5 Hz], the pass-and attenuaton A p s db, whle the stop-and attenuaton A s equals 4 db, 6 db respectvely. Worth notng that n order to reduce the FIR flters orders for the convenence of gvng the tme-delay comparsons, we ntentonally set the transton-and of the FIR flters relatvely n wdth.

16 Sensors 3, 3 88 Assume that the dfferent ntal parameters Q a, R a, Q g, R g of HMM-KF are expressed y: g a 3 3 = {, } 4 4, R {, } a dag r = g r g Q dag q g q g 5 5 = { ( / ), ( / )} 5 5, R { ( / ), ( / )} g dag n = h n h Q dag m h m h where q, r, m and n are varales, Tale 4 gves the dfferent values of K, f c and N FIR (mnmum order of FIR n dfferent stop-and attenuaton A p ) for q remans.5, r = 5,,,,5; and for r remans, q =.,.5,.. Smlarly, Tale 5 shows the correspondng values of K, f c and N FIR, when m and n respectvely reman constants or change as varales. Fgure 8 provdes the HMM-KF flterng results of the z axs acceleraton usng the dfferent parameters q and r n Tale 4. Tale 4. Values of K, f c and N FIR wth dfferent q and r. Parameters K f c [Hz] N FIR [Mnmum order] A s = 4 db A s = 6 db r = 5, q = r =,, q = r 3 =,5, q = q =., r = q =.5, r = q 3 =., r = Tale 5. Values of K, f c and N FIR wth dfferent m and n. Parameters K f c [Hz] N FIR [Mnmum order] A s = 4 db A s = 6 db n =, m = n = 5, m = n 3 =,, m = m =.5, n = m =.5, n = m 3 =.5, n = Fgure 8. Comparson of the HMM-KF flterng results usng the dfferent parameters n Tale 4. (a) q remans.5, r = 5,,,,5; () r remans, q =.,.5, Wth HMM-KF, r=5, q=.5 Wth HMM-KF, r=, q=.5 Wth HMM-KF, r=5, q=.5 f z (m/s ) Tme /s (a)

17 Sensors 3, 3 89 Fgure 8. Cont Wth HMM-KF, q=., r= Wth HMM-KF, q=.5, r= Wth HMM-KF, q=., r= 9.87 f z (m/s ) Tme /s () Fgure 9 compares the group tme delay and the flterng results of the IIR flters usng the dfferent parameter f c and flter order N IIR. Fgure 9. (a) Group tme delay of IIR flters wth dfferent f c and orders n road-and frequences; () IIR flterng results of the z axs acceleraton wth dfferent f c and orders. Group delay (n samples) Horzontal Zoom X:.75 Y: 93 X:.4 Y: X:.48 Y: X:.4578 Y: 67.3 X:.9583 Y: 98.8 X:.994 Y: Lowpass Butterworth IIR: fc=.9836, N IIR =5 Lowpass Butterworth IIR: fc=.9836, N IIR = Lowpass Butterworth IIR: fc=.494, N IIR =5 Lowpass Butterworth IIR: fc=.494, N IIR = Lowpass Butterworth IIR: fc=.39, N IIR =5 Lowpass Butterworth IIR: fc=.39, N IIR = Frequency (Hz) (a) f z (m/s ) Tme/s () vertcal zoom Lowpass Butterworth IIR: fc=.9836, N IIR =5 Lowpass Butterworth IIR: fc=.9836, N IIR = Lowpass Butterworth IIR: fc=.494, N IIR =5 Lowpass Butterworth IIR: fc=.494, N IIR = Lowpass Butterworth IIR: fc=.39, N IIR =5 Lowpass Butterworth IIR: fc=.39, N IIR =

18 Sensors 3, 3 8 Fgure gves the exact group tme delay and the flterng results of the FIR flters usng the parameters f c and N FIR n Tale 4. Tales 6 and 7 present the results of the tme delay (n samples) after usng the three dfferent approaches to process the z axs accelerometer outputs and z axs gyroscope outputs respectvely. Fgure. (a) Group tme delay of FIR flters wth dfferent f c and A s n road-and frequences; () FIR flterng results of the z axs acceleraton wth dfferent f c and A s. 5 5 X:. Y: X: 7.59 Y: 6 X: 3.34 Y: 8 Group delay (n samples) X:.994 Y: 839 X: Y: 856 X:.79 Y: 863 Lowpass Cheyshev FIR: fc=.9836, N FIR =678, Ap= 4dB Lowpass Cheyshev FIR: fc=.9836, N FIR =55, Ap= 6dB Lowpass Cheyshev FIR: fc=.494, N FIR =7, Ap= 4dB Lowpass Cheyshev FIR: fc=.494, N FIR =39, Ap= 6dB Lowpass Cheyshev FIR: fc=.39, N FIR =76, Ap= 4dB Lowpass Cheyshev FIR: fc=.39, N FIR =49, Ap= 6dB Frequency (Hz) (a) f z (m/s ) Tme/s () vertcal zoom Lowpass Cheyshev FIR: fc=.9836, N FIR =678, Ap=4dB Lowpass Cheyshev FIR: fc=.9836, N FIR =55, Ap=6dB Lowpass Cheyshev FIR: fc=.494, N FIR =7, Ap=4dB Lowpass Cheyshev FIR: fc=.494, N FIR =39, Ap=6dB Lowpass Cheyshev FIR: fc=.39, N FIR =76, Ap=4dB Lowpass Cheyshev FIR: fc=.39, N FIR =49, Ap=6dB Tale 6. Comparson of the tme delay (n samples) of the z axs acceleraton usng the three flters. Parameters HMM-KF [samples] 5 order IIR [Max. samples] order IIR [Max. samples] FIR A s = 4 db [samples] r = 5, q = r =,, q = r 3 =,5, q = q =., r = q =.5, r = q 3 =., r =

19 Sensors 3, 3 8 Tale 7. Comparson of the tme delay (n samples) of the z axs angular rate usng the three flters. Parameters HMM-KF [samples] 5 order IIR [Max. samples] order IIR [Max. samples] FIR A s = 4 db [samples] n =, m = n = 5, m = n 3 =,, m = m =.5, n = m =.5, n = m 3 =.5, n = Wthout reference to the tme-delay ssue, t can e seen n Fgure 8 and the vertcal-zoomed pctures of oth Fgures 9() and () that the de-nosng performance of the proposed HMM-KF has almost the same level as compared wth the other two dgtal flter approaches. And the de-nosng performance of the three methods can e acheved and gradually ncreased through decreasng oth the values of HMM-KF gan K and the dgtal flter s cutoff frequency f c. However, ncreasng the de-nosng performance of the three flters wll more or less cause the tme-delay prolem. And there are sgnfcant dfferences n the tme-delay performance of the three dfferent approaches. The smaller the values of K and f c are, the larger the tme delay of the three flters wll get, ovously for the IIR and FIR dgtal flters, slghtly for the proposed HMM-KF, that can e seen n Fgures 7, and summarzed n Tales 6 and 7. Some conclusons on the tme-delay ssue of the three approaches are gven as follows:. As aove mentoned that the nput-output of the IIR flters do not satsfy lnear-phase, so the tme delay of the IIR flters cannot e exactly calculated and gven. Also, t can e seen n Fgure 9(a) that the lended sgnal components wth frequences around cutoff frequency f c suffer from relatvely larger tme delay than that wth frequences not close to f c.. As for the FIR dgtal flters, t s shown n Tales 4 and 5 that even wth relatvely wde transton-ands, the mnmum orders of the FIR flters are much hgher than the correspondng IIR flters. Moreover, the larger the values of A p are, the larger the orders of the FIR flters wll e, thus resultng n ovous tme delay, whch can e seen n Fgure (). However, as shown n Fgure (a) that for each specfc FIR flter, the tme delay could reman a certan constant after processng sgnals wth any frequency ands. And, the tme delay of the specfc FIR flter can e exactly calculated usng the followng equaton [57]: T delay N FIR = () F 3. As for the HMM-KF, t can e depcted n Fgures 8 and 9 (drawng the z axs acceleraton denosng results) that effects of lowerng the values of q and meanwhle keepng the values of r n a constant wll arely cause the tme-delay prolem. Conversely, the HMM-KF could suffer from ovous tme-delay prolem to a certan extent when q remans.5, r vares n 5,, and,5. s

20 Sensors 3, 3 8 So we can expermentally conclude that under the same de-nosng performance, the proposed HMM-KF has etter real-tme altes than the dgtal flters. And how to make the HMM-KF work approprately depends on the adjustng of the values of parameters Q and R. As dfferent IMU have dfferent nose levels, the optmzaton of choosng the values of Q and R cannot e mathematcally solved. But t s no dout that the HMM-KF parameters Q and R mpact not only the de-nosng performance of the IMU outputs, ut also the flterng response speed, namely the tme-delay level. The de-nosng performance ncreases together wth the values of parameters R, whle the tme delay ncrease slghtly for the HMM-KF. Conversely, the de-nosng performance ncreases together wth the decrease of the values of Q, whle the tme delay arely ncrease for HMM-KF. So n real-tme mplementatons, n order to augment the flterng performance at no cost of ncreasng the tme delay, the parameter Q should e kept n relatvely small values, and through adjustng the values of R to meet the good de-nosng performance. In addton, the conclusons addressed aove are equvalent to every channel of the nertal sensors,.e., the three accelerometers and the three gyroscopes, as the relevant experments have een conducted through usng the measurements of all these sensors. Under statc condtons, the tme-delay prolem does not affect the INS algnment results very much, whereas under moorng or voyagng condtons, as the INS are sometmes n the dynamc modes, errors caused y the tme delay could accumulate gradually all through the algnment process [6]. Ths s wll e valdated and dscussed n Secton 6. Frstly, two dfferent algnment mechansms usng the proposed HMM-KF, FIR and IIR flters wll e gven and dscussed n the followng secton Algnment Mechansms Usng Dfferent Flters In Secton 5., the tme-delay performance of the proposed HMMKF and dgtal flters were compared and dscussed. In order to cope wth the tme-delay ssue n the algnment process when usng the dfferent pre-flters, two algnment mechansms are gven n ths secton. Accordng to references [4] and [45] that the nertal frame ased algnment method can well attenuate the angular-moton dsturances, and also for avodng the stuaton that the tme delay of the accelerometer s dgtal flter may not e dentcal to that of the gyroscope s dgtal flter, n ths way, the dgtal flters are employed only to remove the lnear-moton dsturances and thus extractng the pure gravty for the INS algnment. The flowchart of the dgtal flter aded nertal frame ased algnment mechansm can e seen n detal n Fgure (a). As specfed n ref. [33], the dgtal flters should e adopted after the ntegraton of specfc force f, then the output of the dgtal flter V at current tme step t corresponds to the value V (t T delay ) at a former tme step t T delay wth the tme delay T delay. In order to elmnate the effect of the tme delay T delay and stll get the correct value of derved from Equaton (A8), equally, the value of V calculated usng Equaton (A) at current tme step t should e represented y V (t T delay ) at tme step t T delay. For each specfc FIR flter, as T delay can e accurately calculated usng Equaton (), the algnment results wll not e nfluenced y the FIR flters very much, however the algnment duraton and computaton urden can e greatly ncreased, ecause as nterpreted n Secton 5., T delay s always n a very large value. For the IIR dgtal flters, T delay cannot e exactly gven, ut relatvely s n small value, so sometmes the tme delay of IIR flters used n the applcaton of INS algnment s gnored, such as the schemes gven y Sun, et al. n [7] and Yan, et al. n [44]. C

21 Sensors 3, 3 83 Fgure. Flowcharts of the dgtal flter aded and the HMM-KF aded nertal frame ased algnment mechansms. (a) Dgtal flter aded; () HMM-KF aded. K_ gyro. K_ acc. ω f ω f ˆ ˆ = [ ] C C ω ˆ C fˆ = Cˆ f V ˆ ω ˆ ˆ = [ ] C C ω ˆ f ˆ C L ˆ ˆ n C C V ( t T ) delay ˆn ˆ = ˆ ˆ n C C C C V ( t T delay ) ˆ C t = ˆ ˆ k t Vˆ C f dt V ˆn ˆ = ˆ ˆ n C C C C ˆ C L ˆ n C (a) () Dfferent from the dgtal flter aded nertal frame ased algnment mechansm, the HMM-KF aded mechansm does not have to consder the tme-delay ssue, whch can e avoded through adjustng the ntal parameters Q and R, oth for the outputs of the gyroscope and accelerometer. So the HMM-KF can favoraly and properly e used as the optmal pre-flters to pre-process the gyroscope outputs and accelerometer outputs efore and durng the algnment procedures, the detals of the HMM-KF aded mechansm can e seen n Fgure (). 6. Algnment Experments and Performance Evaluaton In order to evaluate the performance of the proposed new roust self-algnment method for the shp s strapdown INS under moorng condton, n ths secton, several experments under dfferent crcumstances were conducted. The turntale experments were mplemented n the la, as the test condton was relatvely deal and uncomplex, the external dsturances were constraned n perodcal forms, so only the coarse algnment process was conducted. In Secton 6., oth the coarse and fne algnment procedures were conducted to test the roust algnment method n the shp s moorng experments. At last, the data from a sea experment was used to valdate the IFOG onlne de-nosng performance of the proposed HMM-KF n the navgaton calculaton stage. 6.. Turntale Coarse Algnment Experments We fxed the IFOG-IMU ased strapdown INS whch s produced y our laoratory on the SGT-3 three-axs turntale to mplement the coarse algnment experments. The IMU, strapdown INS and the turntale can e seen n Fgure. The specfcatons of the SGT-3 turntale are shown n Tale 8. The IMU s three output axes (x, y, z ) paralleled to the turntale s nsde frame, mddle frame and

22 Sensors 3, 3 84 outsde frame respectvely. At the start of each experment, the three frames of the turntale ntally turned to a statc poston for seconds,.e., ptch angle, roll angle, yaw angle 8, the start tme of algnment t was chosen at the th second. Then all the three frames of the turntale started to sway wth 5 n magntude and 4 seconds n perod. The total swayng tme was seconds. For each experment, t k was set at the th second when the turntale enters the swayng mode. After fnshng the swayng form, the turntale turned to the statc mode agan wth the ntal poston for seconds, t k was set at the th second n the statc tme, the ptch, roll and yaw angles of the turntale at ths tme were consdered as references (the true values) to compare the results of the dfferent algnment schemes. Durng the algnment process, a specfc lever-arm etween the strapdown INS and the turntale s frames should not e gnored. When the turntale was n swayng mode, the strapdown INS could accordngly experence a velocty component due to ths lever-arm. And ths velocty component can e regarded as the dsturng velocty n perod form of 4 seconds too. Fgure. (a) Self-made IFOG-IMU and ts ody frame; () IFOG-IMU ased strapdown INS; (c) STG-3 turntale and the experment scene. z x y (a) () (c) Tale 8. Specfcatons of the SGT-3 turntale. Parameters Precson Poston resoluton ±3 ( = 6 =36 ) Poston range ±36 Angular rate resoluton. /h Angular rate range ±5 /s

23 Sensors 3, 3 85 Ffty experments were conducted. Durng the turntale experments, the frequency of the dsturance could e regarded as.5 Hz (/4 Hz), we set the IIR flter cutoff frequency f c as. Hz wth order N IIR = 5, the transton-and of the FIR flter was [. Hz,. Hz], stop-and attenuaton A p = 4 db, the order of FIR N FIR = 48, T delay of the FIR was 7.9 seconds, and the parameters of the HMM-KF Q a, R a, Q g, R g were optmally set as follows: a 3 3 = {.,. } 4 4, R {, } a dag = g g Q dag g g g 5 5 = {.5 ( / ),.5 ( / )} 5 5, R { 5 ( / ), 5 ( / )} g dag = h h Q dag h h Fgure 3 provdes the msalgnment angles of ptch error φ e, roll error φ n and yaw error φ u of the 5 coarse algnment results usng four dfferent algnment schemes,.e., (a) algnment wthout any preflters, only the nertal frame ased algnment (IFBA) technque was adopted; () IFBA + HMM-KF aded algnment scheme; (c) IFBA + FIR aded algnment scheme; (d) IFBA + IIR aded algnment scheme. In accordance to Fgure 3, Tale 9 shows the mean and STD values of the msalgnment angles usng the four approaches. Fgure 3 and Tale 9 clearly ndcate that all the four methods employng the IFBA technque can well accomplsh the algnment process and get reasonale algnment results, ths demonstrates the effectveness of the IFBA for the n-moton or moorng algnment applcatons of strapdown INS. Whle the algnment methods wth preflters (HMM-KF, FIR and IIR flters) could more or less mprove the algnment results than that wthout any preflters. However, among the three freflter aded algnment methodologes, the HMM-KF aded scheme can acheve the optmal algnment precsons, all the three msalgnment angles are smaller than that of the IIR aded scheme, especally for the yaw msalgnment angle φ u, precson ncreased y more than.. That s ecause dfferent from the other two dgtal flter aded methods, the proposed HMM-KF could flter out the relatvely hgh-frequency sensor noses and external dsturances oth for the measurements of gyroscope and accelerometer. Fgure 3. 5 tmes turntale coarse algnment experments results. (a) Msalgnment wthout any preflters; () Msalgnment wth HMM-KF; (c) Msalgnment wth FIR; (d) Msalgnment wth IIR (N IIR = 3). Roll error (degree) Ptch error (degree) x -3 Msalgnment wthout any preflters Roll error (degree) Ptch error (degree) x x -3 Msalgnment wth HMM-KF Yaw error (degree) Yaw error (degree) (a) ()

24 Sensors 3, 3 86 Fgure 3. Cont. Roll error (degree) Ptch error (degree) x -3 Msalgnment wth FIR x Roll error (degree) Ptch error (degree) x -3 Msalgnment wth IIR Yaw error (degree) Yaw error (degree) (c) (d) Tale 9. Mean and STD of the msalgnment angles usng the four algnment schemes. Algnment Schemes φ e [degree] φ n [degree] φ u [degree] Mean wthout Preflters Mean wth HMM-KF Mean wth FIR Mean wth IIR (N IIR = 3) STD wthout Preflters e-4.58 STD wth HMM-KF e-4.99 STD wth FIR e-4. STD wth IIR (N IIR = 3) e-4.86 Although the precson of the FIR aded method has almost the same level compared wth that of the HMM-KF aded scheme. The computaton tme s ovously shorter for the HMM-KF aded scheme than that of the FIR aded method. In the 5 experments wth the algnment program wrtten n C++ language, tested on an Intel Core Due.94 GHz CPU, the average tme for each FIR aded method s 4.3 seconds and.69 seconds for each HMM-KF aded method mplementaton. As the experments were conducted on the turntale, the external dsturances were constraned relatvely n long perodcal tme (4 seconds), then the cutoff frequency f c was chosen smaller than /4 Hz, namely,. Hz n the experments. However, as mentoned n the Introducton part, n practce, the frequences of the external dsturances are mostly hgher than /5 Hz durng the moorng algnment. So accordngly, f c should e set smaller than /5 Hz, thus the orders and tme delay of the dgtal flters wll ncrease, and the correspondng results and the nfluence wll e gven and dscussed n detal n the followng moorng experment secton. 6.. Moorng Experment The moorng experment was conducted n the East Sea of Chna to valdate the proposed algnment method. In ths experment, the shp was under moorng condton. A self-made IFOG ased IMU was used for the experment, the atttude reference was gven y the PHINS from the company IXSEA,

25 Sensors 3, 3 87 detals of ths system are descred n [58]. The performance of the PHINS n GPS aded mode s as follows: oth ptch and roll errors are less than., headng error s less than.. The self-made IMU, PHINS and nformaton control panel are shown n Fgure 4. The fne algnment procedure was performed to estmate the msalgnment angles y usng a software package whch s compled y the Marne Navgaton Research Insttute of Harn Engneerng Unversty. We carred out the algnment experments three tmes. Durng the moorng experments, the cutoff frequency f c was set as.3 Hz, the IIR order N IIR = 5, the FIR flter transton-and was [. Hz,.5 Hz], stop-and attenuaton A p = 4 db, then the order of FIR N FIR = 3544, T delay of the FIR was 7.7 seconds, and accordngly the parameters of the HMM-KF Q a, R a, Q g, R g were optmal chosen as follows: a 3 3 = {.3,.3 } 4 4, R {, } a dag = g g Q dag g g g 5 5 = {.5 ( / ),.5 ( / )} 5 5, R { 5 ( / ), 5 ( / )} g dag = h h Q dag h h Fgure 4. (a) Self-made strapdown INS, PHINS; () Informaton control panel. (a) () The coarse and fne algnment tme were set as seconds and 6 mnutes, respectvely. The algnment results utlzng the aove mentoned four schemes are depcted n hstogram form n Fgure 5. In accordance wth Fgure 5, the mean and STD values of the msalgnment angles are shown n Tale. It s clear from Fgure 5 and Tale that the overall trends of the algnment results usng the four schemes could also preferaly valdate the conclusons n Secton 6.. The msalgnment angles of ptch errors and roll errors wth HMM-KF are less than., and yaw errors are less than.6. Although the algnment results are less accurate than the results from the turntale experments, whle as the IMU was n moong dynamc mode, these algnment results all the same prmely fulfll the requrement and the accuracy of the shp s strapdown INS algnment under moorng condton. Moreover, compared wth other three algnment schemes, the proposed HMM-KF aded scheme can not only help the Kalman flter n the fne algnment stage to converge faster, ut also help to get more relale msalgnment angle estmates, especally for the yaw angle, that can e seen n Fgure 6.