ARTICLE IN PRESS

Journal of the Franklin Institute 346 (2009) 1021–1037

0016-0032/$3

doi:10.1016/j

�CorrespoE-mail ad

www.elsevier.com/locate/jfranklin

New techniques for initial alignment of strapdowninertial navigation system

Shaolin Lua,�, Ling Xieb, Jiabin Chenb

aDepartment of Precision Instruments and Mechanology, Tsinghua University, Beijing 100084, ChinabSchool of Automation, Beijing Institute of Technology, Beijing 100081, China

Received 19 September 2008; received in revised form 26 August 2009; accepted 15 September 2009

Abstract

Some new techniques for initial alignment of strapdown inertial navigation system are proposed in

this paper. A new solution for the precise azimuth alignment is given in detail. A new prefilter, which

consists of an IIR filter and a Kalman filter using hidden Markov model, is designed to attenuate the

influence of sensor noise and outer disturbance. Navigation algorithm in alignment is modified to

feedback continuously for the closed-loop system. It is shown that the initial estimated variance

setting of azimuth angle error can influence the speed of initial alignment significantly. At the

beginning of alignment, Kalman filter must make a very conservative guess at the initial value of

azimuth angle error to get a high convergent speed of the azimuth angle. It is pointed out that the low

signal to noise ratio makes the ordinary setting of the estimated azimuth variance slow down the

convergent speed of the azimuth angle. Also is shown that the large azimuth angle error problem can

be solved well by our solution. The feasibility of these new techniques is verified by simulation and

experiment.

& 2009 The Franklin Institute. Published by Elsevier Ltd. All rights reserved.

Keywords: Strapdown inertial navigation system; Initial alignment; Kalman filter

1. Introduction

Initial alignment is the first work stage of strapdown inertial navigation system (SINS).The initial azimuth accuracy is one of the main factors which influence the navigation

2.00 & 2009 The Franklin Institute. Published by Elsevier Ltd. All rights reserved.

.jfranklin.2009.09.003

nding author.

dress: leo20980@sohu.com (S. Lu).

ARTICLE IN PRESSS. Lu et al. / Journal of the Franklin Institute 346 (2009) 1021–10371022

accuracy of SINS. For this reason, initial alignment is always an important issue innavigation field [1–20].In real environment, the output of inertial measurement unit (IMU) often suffers from

large sensor noise. This will slow down the speed of alignment. El-sheimy et al. [18] utilizedwavelet de-noising method in IMU alignment. In this paper, a new method is proposed toreduce the influence brought by the mixture of the sensor noise and the outer disturbance.The new prefilter consists of an IIR filter and a Kalman filter using hidden Markov model(HMM). Different from wavelet method which processes the signal in the time–frequencydomain, our method works in the frequency and time domain subsequently. Firstly, itfilters the high-frequency noise in the frequency domain by the lowpass filter. And then, itworks in the time domain by the steady Kalman filter. Wavelet de-noising method has ablock processing structure, while the new method possesses of a sequential structure. Thus,the signal can be processed in a realtime way. It does not need to save big-block signal datalike wavelet. Benefiting from this merit, the structure of the initial alignment program canbe simplified. With this prefilter, the large noise problem of SINS on a vehicle with theengine on can be solved well and easily.Navigation algorithm has been studied very well [21,22]. Here, it will be modified for

the closed-loop Kalman filter in alignment. With it, SINS can work like the gimbalsystem in the alignment stage. Moreover, since the modification is very slight, it will notbring too much computation burden. When this navigation algorithm is implemented,the error model needs to be modified accordingly. Error models of SINS have beenlucubrated [9,23]. In this paper, only the c�angle error model is considered andmodified.Using the conventional Kalman filter design method, the precise azimuth alignment

suffers a low convergent speed of the azimuth angle. Observability analysis of c�angleerror model with velocity measurement was made by Bar-Itzhack et al. [9] and Jiang [12]. Itis widely accepted that since the azimuth angle error is not completely observable it willcost Kalman filter a very long time to estimate the azimuth angle error.In this paper, it will be pointed out that in alignment the estimated variance of the

azimuth error should be initialized as a large value. If Kalman filter works with a propersetting of this variance, the azimuth angle error will decrease soon. Also it will be shownthat the low signal to noise ratio of gyroscope makes this phenomenon happen. The reasonwhy the azimuth angle gets convergent very slow with an ordinary variance setting is notthat the system is not completely observable. The reason is that the earth angular rate istoo slow and the sensor noise is too large.Generally, the first stage of SINS initial alignment is coarse alignment [15,17]. Coarse

alignment is a very short procedure which purpose is to provide a good condition for finealignment. However, in practice the accuracy provided by coarse alignment may be verylow. Under this condition, the precise alignment may be accomplished with a large azimutherror. This problem is called the large azimuth error problem. To solve this problem, themodified c�angle model [11,24] is proposed. Nonlinear filtering is used to accomplish theinitial alignment with a large azimuth error [16]. Here, it will be shown that using oursolution this problem can be solved well with the c�angle model by a closed-loop Kalmanfilter. The modified c�angle model or nonlinear model is not necessary. If the azimuthangle error is large, for example 1801, the correction speed of the angle error can beaccelerated by the large azimuth variance to make the angle error decrease quickly, whichmakes the c�angle model proper for initial alignment.

ARTICLE IN PRESSS. Lu et al. / Journal of the Franklin Institute 346 (2009) 1021–1037 1023

The organization of this paper proceeds as follows. In the next section, our solution forinitial alignment of SINS will be introduced briefly. In Section 3, a new prefilter will beproposed to attenuate the sensor noise in real environment. Navigation algorithm inalignment and error model will be modified for the feedback of Kalman filter in Section 4.In Section 5, the variance tuning technique for improving the convergent speed of theazimuth angle error will be introduced. As presented in Section 5, the main factors, whichinfluence the speed of convergence, are obtained to solve the large azimuth angle errorproblem. The experiment results on a land vehicle are presented in Section 6.

2. The closed loop solution for SINS alignment using optimal filtering

Different from the conventional two-stage solution, the whole procedure of our solutionconsists of three stages: coarse alignment, precise leveling and precise azimuth alignment.Coarse alignment can be accomplished by least square method [15,17]. Since our solutioncan work with a large azimuth angle error, this stage will cost a very short time. In ourprogram, it is set as 10 s. In the precise leveling stage, the system can be leveled bygyrocompassing or Kalman filter very soon [1–14]. In our solution, gyrocompassing is usedfor its high speed and it costs 10 s too. In this paper, it is assumed that the precise azimuthalignment is performed after the system has been leveled well. The whole procedure cost4min.

The precise azimuth alignment is accomplished by a closed-loop solution. It solves theproblems as follows:

(1)

Prefiltering large sensor noise. (2) The convergent speed of the azimuth angle. (3) The large azimuth angle error problem.

In Fig. 1, the scheme of the precise azimuth alignment is illustrated. Before navigationalgorithm runs, the output of IMU will be preprocessed by IIR lowpass filters and

Fig. 1. The scheme of precise azimuth alignment.

ARTICLE IN PRESSS. Lu et al. / Journal of the Franklin Institute 346 (2009) 1021–10371024

two-state Kalman filters. The velocity calculated by navigation algorithm is used as theobservation of the closed-loop Kalman filter. The estimation of the closed-loop Kalmanfilter is utilized as the feedback to stabilize the output of SINS navigation algorithm.

3. A new prefilter for strapdown inertial navigation system in alignment

In this section, a new prefilter is proposed for SINS in alignment. It possesses of a real-time recursive structure which is different from the block processing structure of wavelet[18,25]. Moreover, its computational complexity is very low. The merits of this prefilterfacilitate the programmer to design an alignment software with a simpler structure. Itconsists of an IIR lowpass filter and a two-state Kalman filter with HMM. The IIR filterwill depress the high frequency disturbance. The output of the IIR filter is modeled usingHMM. Then, the two-state Kalman filter will be used to attenuate the energy of the sensornoise further. Coarsely speaking, this prefilter works in the frequency domain and timedomain subsequently. Wavelet tries to analyze the signal as precisely as possible, while theprefilter we designed concentrate on the useful signal. In the frequency domain, it filters thenoise which frequency is higher than the true signal. In the time domain, since it estimatesthe signal by a Kalman filter using HMM, the amplitude of the noise is reduced further.Here a ring laser gyroscope (RLG) SINS is considered.

3.1. IIR lowpass filter

The sensor output of RLG SINS will be disturbed by the dithering motion of RLG.When the system is placed on a vehicle with engine on, the noise variance will beaugmented further. The IIR filter can restrain the high frequency disturbance. However, inalignment its cutoff frequency cannot be set too low. Otherwise, useful information will berejected and the filtered result may be biased. In our system, a 4-order IIR filter with 10Hzcutoff frequency is designed.

sðkÞ ¼ b0rðkÞ þ b1rðk � 1Þ þ b2rðk � 2Þ þ b3rðk � 3Þ þ b4rðk � 4Þ � a1sðk � 1Þ� a2sðk � 2Þ � a3sðk � 3Þ � a4sðk � 4Þ ð1Þ

where rðkÞ is the sensor output of k, sðkÞ is the filtered result of k and the coefficientsbi; ai; i ¼ 0; . . . ; 4 can be calculated using different methods. See [26] for details. Here, thesecoefficients are chosen as follows:

b0 ¼ 0:0048423; b1 ¼ 0:0192974; b2 ¼ 0:0289461;b3 ¼ 0:0192974; b4

¼ 0:0048243; a1 ¼ �2:3695130;a2 ¼ 2:3139884; a3 ¼ �1:0546654; a4

¼ 0:1873795

For different systems, the cutoff frequency can be changed to satisfy the practical need.Accordingly, the coefficients should be redesigned, too.

3.2. Steady-state Kalman filter

Besides the IIR filter, a steady-state Kalman filter with HMM is used to attenuate theenergy of signal noise further. In alignment, the sensor output filtered by the lowpass filtercan be seen as random walk.

ARTICLE IN PRESSS. Lu et al. / Journal of the Franklin Institute 346 (2009) 1021–1037 1025

According to the theory of HMM, the true output of sensors are hidden in the output ofthe lowpass filter. The discrete HMM can be formulated as follows [27,28]:

Xkþ1 ¼ AX k þ Zk ð2Þ

Skþ1 ¼ CX k þ nk ð3Þ

where X 2 RN , S 2 RM and the entries satisfy

XN

j¼1

Aij ¼ 1; AijZ0 ð4Þ

XMj¼1

Cij ¼ 1; CijZ0 ð5Þ

For this application it can be modeled as follows [27–29]:

xkþ1

xk

" #¼

A11 A12

A21 A22

" #xk

xk�1

" #þ Zk ð6Þ

The measurement model can be given as follows:

skþ1

sk

" #¼

C11 C12

C21 C22

" #xk

xk�1

" #þ nk ð7Þ

where Zk�Nð0;UÞ and nk�Nð0;VÞ represent the process noise and the measurement noiserespectively. To make the output sequence smooth, the estimated covariance ofmeasurement noise can be set much bigger than that of the process noise [29–32]. Inpractice, the noise covariance matrix in continuous time form for the filter of accelerometeris set as

Ua ¼0:01 ðmgÞ2 0

0 0:01 ðmgÞ2

" #ð8Þ

Va ¼2 ðmgÞ2 0

0 2 ðmgÞ2

" #ð9Þ

The noise covariance matrix for gyroscope in continuous time form is set as

Ug ¼ð0:2=hÞ2 0

0 ð0:2=hÞ2

" #ð10Þ

Vg ¼ð20:57=hÞ2 0

0 ð20:57=hÞ2

" #ð11Þ

The steady-state Kalman gain is computed offline previously. Then, Riccati equationneed not be computed online, which lightens the computational burden. In alignment,

ARTICLE IN PRESSS. Lu et al. / Journal of the Franklin Institute 346 (2009) 1021–10371026

the steady two-state Kalman filter works as follows:State prediction

skþ1jk

skjk

" #¼

A11 A12

A21 A22

" #sk

sk�1

" #ð12Þ

Measurement update

skþ1

sk

" #¼

skþ1jk

skjk

" #þ Koff

~skþ1

~sk

" #�

skþ1jk

skjk

" # !ð13Þ

where Koff is the steady-state Kalman gain computed offline. The prefilter described byEqs. (1), (12) and (13) needs only 30 flops to filter a sensor output. Thus, it is very suitablefor the online implementation. Another merit of this method is that it smooths theprevious output when it filters the current output. This is one of the reasons why the noisevariance is attenuated to a very low level.

0 10 20 30 40 50 60

−1030−824−618−412−206

0206412618

t (s)

Ang

ular

vel

ocity

(deg

/h)

0 10 20 30 40 50 60−61.8

−41.2

−20.6

0

20.6

41.2

61.8

82.4

103

t (s)

Ang

ular

vel

ocity

(deg

/h)

0 10 20 30 40 50 60

8.24

9.27

10.3

11.33

12.36

13.39

14.42

t (s)

Ang

ular

vel

ocity

(deg

/h)

Fig. 2. The prefiltering results of a gyroscope in alignment: (a) the output of a gyroscope in alignment, (b) the

output of a gyroscope filtered by an IIR lowpass filter and (c) the output of a gyroscope filtered by an IIR lowpass

filter and a Kalman filter.

ARTICLE IN PRESS

0 10 20 30 40 50 60−30

−25

−20

−15

−10

−5

0

t (s)

Spe

cific

forc

e (m

g)

0 10 20 30 40 50 60−14

−12

−10

−8

−6

−4

t (s)

Spe

cific

forc

e (m

g)

0 10 20 30 40 50 60−8.75

−8.7

−8.65

−8.6

−8.55

−8.5

−8.45

t (s)

Spe

cific

forc

e (m

g)

Fig. 3. The prefiltering results of an accelerometer in alignment: (a) the output of an accelerometer in alignment,

(b) the output of an accelerometer filtered by an IIR lowpass filter and (c) the output of an accelerometer filtered

by an IIR lowpass filter and a Kalman filter.

S. Lu et al. / Journal of the Franklin Institute 346 (2009) 1021–1037 1027

3.3. Experiment results

In Figs. 2 and 3, the output of a gyroscope and an accelerometer of IMU placed on avehicle with engine on are illustrated. The variance of the original gyroscope output is3001/h. After it passes through the IIR filter, the variance decreases to 201/h. If it is filteredby an IIR filter and a two-state Kalman filter, the variance becomes 11/h. The variance ofthe original accelerometer is 11mg. The IIR filter attenuates its variance to 2.5mg. An IIRfilter and a two-state Kalman filter can make its variance fall to 40mg.

It can be seen that with this new prefilter initial alignment will be accomplished in arelatively benign environment.

4. Modified navigation algorithm and SINS error model

In this section, navigation algorithm and SINS error model will be modified for theclosed-loop Kalman filter. Then, the closed-loop Kalman filter will work not only as anestimator but also as an observer.

ARTICLE IN PRESSS. Lu et al. / Journal of the Franklin Institute 346 (2009) 1021–10371028

4.1. Modified navigation algorithm in alignment

In this paper, only continuous implementation is considered. Detailed discreteimplementation is given in [21,22]. Here, navigation algorithm is implemented in localgeography level frame (North, East, Down). In alignment, only attitude and velocity needbe updated. The navigation algorithm for alignment is given as follows:Attitude rate

_Cn

b ¼ Cnbðo

bib�Þ � ðO�ÞC

nb ð14Þ

where Cnb represents the direction cosine matrix from the body frame to the navigation

frame, O means the angular earth rate, obib means the spatial rate of vehicle in body frame

which is measured by strapdown gyroscopes, and the cross-product form of a vector w isthe skew-symmetric matrix:

ðw�Þ ¼

0 �wz wy

wz 0 �wx

�wy wx 0

264

375 ð15Þ

Acceleration transformation

anSF ¼ Cn

babSF ð16Þ

where abSF means the body-axis specific force sensed by strapdown accelerometers.

Velocity rate

gnP ¼ gn � ðO�ÞðO�ÞRn ð17Þ

_vn ¼ anSF þ gn

P � 2O� vn ð18Þ

where gn represents the mass attraction gravitational acceleration in navigation frame andgn

P means the plumb-bob gravity in navigation frame.In alignment, the angle error can be corrected as follows [1–7]:

Hacek;C nb ¼ ½I þ ðc�Þ�C

nb ð19Þ

where c is the c�angle error estimated by Kalman filter. To implement this correction in acontinuous form, here Eq. (14) is modified as

_Cn

b ¼ Cnbðo

bib�Þ � ððO� c=tÞ�ÞCn

b ð20Þ

where t is the period of Kalman filter.To correct the velocity error, Eq. (18) is changed to

_vn ¼ anSF þ gn

P � 2O� vn � dv=t ð21Þ

When Eqs. (20) and (21) are used to update attitude and velocity, navigation errors willbe corrected continuously. It should be noted that this modification will not bring toomuch computation burden. Only five additions are needed to append to the conventionalnavigation algorithm [21,22].

ARTICLE IN PRESSS. Lu et al. / Journal of the Franklin Institute 346 (2009) 1021–1037 1029

4.2. SINS error model in alignment

The error model is modified according to the navigation algorithm. Different SINS errormodels in alignment were proposed [9,11,16,23,24]. Here only c�angle error model isconsidered [9,23]. Sensor biases are modeled in navigation frame and assumed to beconstant. When the modified navigation algorithm above is implemented, the c�angleerror model in alignment can be modified as follows:

d_vN

d_vE

_cN

_cE

_cD

_rN

_rE

_eN

_eE

_eD

266666666666666666664

377777777777777777775

¼

0 2OD 0 g 0 1 0 0 0 0

�2OD 0 �g 0 0 0 1 0 0 0

0 0 0 OD 0 0 0 1 0 0

0 0 �OD 0 ON 0 0 0 1 0

0 0 0 ON 0 0 0 0 0 1

0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0

26666666666666666664

37777777777777777775

dvN

dvE

cN

cE

cD

rN

rE

eN

eE

eD

266666666666666666664

377777777777777777775

þ Bu ð22Þ

where r means the accelerometer bias, e represents the gyro drift. Eq. (22) can be writtenas

_x ¼ Fxþ Bu ð23Þ

The process noise of this dynamic system is q, and

q�Nð0;QÞ; Q ¼ diag½q2a; q

2a; q

2g; q

2g; q

2g; 0; 0; 0; 0; 0�

qa represents the random noise of accelerometer and qg means the random noise ofgyroscope. B ¼ I10�10; u ¼ Gx. Since B is an identity matrix, there is much flexibility indesigning G. Considering sensor biases are unobservable, in our solution only angle andvelocity errors are corrected. Thus, G ¼ diag½Gv;Gv;GN ;GE ;GD; 0; 0; 0; 0; 0�. At the sametime, navigation algorithm has to be modified to reflect u.

The measurement model can be listed as follows [9]:

d~vN

d~vE

" #¼

1 0 0 0 0 0 0 0 0 0 0 0

0 1 0 0 0 0 0 0 0 0 0 0

� �x ð24Þ

which can be written as

z ¼ Hx ð25Þ

the measurement noise is r, and r�Nð0;RÞ;R ¼ diag½Rv;Rv�. Rv can be set as a very smallconstant.

Utilizing the model described by Eqs. (22) and (24), the closed-loop Kalman filter can beimplemented [27–32]. In state prediction, the control input must be considered

xðk þ 1jkÞ ¼ FxðkÞ þ Bu ¼ eFtxðkÞ þ Bu ð26Þ

ARTICLE IN PRESSS. Lu et al. / Journal of the Franklin Institute 346 (2009) 1021–10371030

Other steps are the same as the standard Kalman filter [27–33]. If no control input is usedin navigation algorithm, Bu will be withdrawn from Eq. (26). Then, an open-loop solutionis gotten.

5. Azimuth angle error variance tuning for precise azimuth alignment

In all open literature [1–20], the azimuth angle error variance is set as a very small value.Assumed that the initial azimuth angle error is 1, it is well accepted that the initial value ofits estimated state variance should be set as ð1Þ2. In this section, it will be shown that thisvalue should be set much bigger than the square of azimuth angle error. Under this setting,the convergent speed of the azimuth angle error will be satisfying. Also it will be pointedout that the reason why the azimuth angle error gets convergent slowly with a smallvariance setting lies in the special structure of the system not in its observability. The largeazimuth angle error problem is a difficult problem in alignment [11,16,24]. It will bedemonstrated that with a right setting of the estimated variance the closed-loop solutioncan solve this problem very well.

5.1. Variance tuning for the precise azimuth alignment

A ten-state closed-loop Kalman filter is implemented to accomplish the precisealignment. The period of Kalman filter is 1 s. According to our experience, all of Gi areset as 1. SINS works at Beijing which latitude is 39:96. It is assumed that the system hasbeen leveled well. The system is placed on a table in lab.The navigation system considered in this section is an RLG SINS. The turn-on drift of

each gyroscope are 0.0051/h. The random noise of each gyroscope is 21/h. The turn-on andrandom biases of each accelerometer are 50mg and 2mg.Now, simulation results will show the ability of the system to correct the initial azimuth

error. Through them, the influence of the initial variance setting on the convergent speedwill be illustrated clearly. Different initial errors, 0;71;72;75;710, are introduced tothe azimuth angle. The covariance matrices are set as follows:

P ¼ diag½ð0:3m=sÞ2; ð0:3m=sÞ2; ð1Þ2; ð1Þ2; ðPDÞ2; ð50 mgÞ2; ð50mgÞ2; ð0:005=hÞ2;

ð0:005=hÞ2; ð0:005=hÞ2� ð27Þ

Q ¼ diag½ð2mgÞ2; ð2mgÞ2; ð2=hÞ2; ð2=hÞ2; ð2=hÞ2; 0; 0; 0; 0; 0� ð28Þ

R ¼ diag½ð0:0025m=sÞ2; ð0:0025m=sÞ2� ð29Þ

The fifth diagonal element of P, P2D, is an important parameter for the closed-loop

Kalman filter in initial alignment. Different setting of PD will make the azimuth anglehave different convergent speed. Fig. 4 illustrates the influence of PD on theconvergence of the azimuth angle. It can be seen that for SINS in alignment Kalmanfilter will be too optimistic if the initial covariance of azimuth angle is set normally. Itmust be set as a big value, otherwise the estimation curve will get convergent at a very lowspeed.The real system shows the same conclusion as the system in simulation. As can be

seen from Fig. 5, similar results as those in simulation are gotten. It should be noted

ARTICLE IN PRESS

0 100 200 300 400 500 600

−10−8−6−4−2

02468

10

t (s)

Azi

mut

h an

gle

(deg

ree)

0 100 200 300 400 500 600−1

0

2

4

6

8

10

12

t (s)

Azi

mut

h an

gle

(deg

ree) 10 degree

20 degree

30 degree

45 degree

80 degree

0 100 200 300 400 500 600

−10−8−6−4−2

02468

10

t (s)

Azi

mut

h an

gle

(deg

ree)

Fig. 4. The influence of PD on the precise azimuth alignment in simulation results: (a) simulation of the precise

azimuth alignment procedure with PD ¼ 10, (b) simulation of the precise azimuth alignment procedure with

PD ¼ 80 and (c) simulation of the precise azimuth alignment procedure with different PD.

S. Lu et al. / Journal of the Franklin Institute 346 (2009) 1021–1037 1031

that the curves in experiment are smoother than those in simulation. The reason liesin the sensor noise. The noise sequence used in simulation is whiter than that of a realsystem.

5.2. Low signal to noise ratio of gyroscope

In the previous subsection, it has been shown that the initial setting of the estimatedazimuth variance decides the convergent speed. In this subsection, it will be illustrated thatthis phenomenon comes from the low signal to noise ration (SNR), not from theobservability of the system.

Consider an ideal SINS in simulation which has no turn-on gyroscopedrift and accelerometer bias. The other parameters used are the same as those ofthe system in last subsection. Under this condition, the system model will degenerate to

ARTICLE IN PRESS

0 100 200 300 400 500 600145

148150152154156158160162

165

t (s)

Azi

mut

h an

gle

(deg

ree)

0 100 200 300 400 500 600

145

148150152154156158160162

165

t (s)

Azi

mut

h an

gle

(deg

ree)

0 100 200 300 400 500 600154

156

158

160

162

164

166

t (s)

Azi

mut

h an

gle

(deg

ree)

10degree

20degree

30degree

45degree

80degree

Fig. 5. The influence of PD on the precise azimuth alignment in experiment results: (a) the precise azimuth

alignment with PD ¼ 10, (b) the precise azimuth alignment with PD ¼ 80 and (c) the precise azimuth alignment

with different PD.

S. Lu et al. / Journal of the Franklin Institute 346 (2009) 1021–10371032

a five-state model.

d_vN

d_vE

_cN

_cE

_cD

26666664

37777775¼

0 2OD 0 g 0

�2OD 0 �g 0 0

0 0 0 OD 0

0 0 �OD 0 ON

0 0 0 ON 0

26666664

37777775

dvN

dvE

cN

cE

cD

26666664

37777775þ B5u5 ð30Þ

It can be written as

_x5 ¼ F5x5 þ B5u5 ð31Þ

where as the ten-state system described by Eq. (22), B5 ¼ I5�5; u5 ¼ G5x;G5 ¼

diag½Gv;Gv;GN ;GE ;GD�. These values of G are set as those in last subsection. The processnoise of the dynamic system is q5, and q5�Nð0;Q5Þ;Q5 ¼ diag½q2

a; q2a; q

2g; q

2g; q

2g�

¼ diag½ð2mgÞ2; ð2mgÞ2; ð2=hÞ2; ð2=hÞ2; ð2=hÞ2�.

ARTICLE IN PRESSS. Lu et al. / Journal of the Franklin Institute 346 (2009) 1021–1037 1033

The measurement model is

d~vN

d~vE

" #¼

1 0 0 0 0

0 1 0 0 0

� �dvN

dvE

cN

cE

cD

26666664

37777775

ð32Þ

which can be written as

z ¼ H5x5 ð33Þ

the measurement noise is the same as the ten-state system.The observability matrix Q11 is constructed as follows:

Q5 ¼

H5

H5F5

. . .

H5F45

266664

377775 ð34Þ

then, it can be demonstrated that Q5 is of full rank. Thus, the system described by Eqs. (30)and (32) is completely observable.

Now let the earth angular rate be O, 10O, 100O. The initial azimuth angle error is set as2 and the estimated covariance matrix is initialized as diag½0:3m=sÞ2; ð0:3m=sÞ2;ð1Þ2; ð1Þ2; ð2Þ2�. Assumed that the system has been leveled, perform the precise azimuth

Fig. 6. The influence of earth angular rate on the convergent speed of azimuth angle in simulation.

ARTICLE IN PRESS

0 100 200 300 400 500 600−0.5

0

0.5

1

1.5

2

2.5

t (s)

Azi

mut

h an

gle

(deg

ree)

centuplicate noise variance decuple noise variance original noise variance

Fig. 7. The influence of sensor noise variance on the convergent speed of azimuth angle in simulation.

0 50 100 150 200−50

0

50

100

150

200

t (s)

Azi

mut

h an

gle

(deg

ree)

0 50 100 150 200−50

0

50

100

150

200

t (s)

Azi

mut

h an

gle

(deg

ree)

Fig. 8. Comparison of different precise azimuth alignment methods in simulation: (a) the closed-loop Kalman

filter and (b) the open-loop Kalman filter.

S. Lu et al. / Journal of the Franklin Institute 346 (2009) 1021–10371034

alignment. As can be seen from Fig. 6, the convergent speed of the azimuth angle increasesas the earth angular rate increases. The low earth angular rate in real world makes theSNR very low. It cannot work well with an ordinary azimuth angle variance setting.This conclusion can be verified vice versa. Now, keep the earth angular rate unchanged

and change the noise variance of sensors. Let the random noise of the gyroscope andaccelerometer respectively be ð0:02=hÞ2 and ð20mgÞ2. Perform the precise azimuthalignment with 2 azimuth angle error and PD ¼ 2. Increase the standard variance ofsensor noise to their decuple and centuplicate. Repeat the simulation and Fig. 7 can begotten. It can be concluded from this figure that the variance of the sensor noise has a verystrong influence on the convergent speed of the azimuth angle. For this reason, a prefilter isnecessary for SINS in alignment.

ARTICLE IN PRESS

0 50 100 150 20080

100120140160180200220240260280

t (s)

Azi

mut

h an

gle

(deg

ree)

0 50 100 150 20080

100120140160180200220240260280

Azi

mut

h an

gle

(deg

ree)

t (s)

Fig. 9. Comparison of different precise azimuth alignment methods in experiment: (a) the closed-loop Kalman

filter and (b) the open-loop Kalman filter.

S. Lu et al. / Journal of the Franklin Institute 346 (2009) 1021–1037 1035

According to the results above, the reason why the azimuth angle gets convergent veryslow is not that the system is not completely observable. The reason is that the earthangular rate is too slow and the sensor noise is too large. In practice, the estimatedvariance of the azimuth angle must be set as a big value and a prefilter must be used todepress the sensor noise. If not, its convergent speed will be very low.

5.3. The large azimuth angle error problem

It is well known that the open-loop Kalman filter with the c�angle error model will beof no use when the initial azimuth angle error is large. To solve this problem, differentmethods, such as nonlinear filtering [16,19] and modified c�angle error model [11], wereproposed. Here it will be shown by using the closed-loop Kalman filter with a right settingof the initial azimuth variance this problem can be solved very well. The precise alignmentmay be performed with a large azimuth angle error, even 180! With this technique, coarsealignment does not need too much time. In our system, coarse alignment only costs 10 s.

The systems used in simulation and experiment are the same as those in Section 5.1.Different initial errors, 1, 45, 90, 180, are introduced to the azimuth angle. Use the closed-loop Kalman filter and the open-loop one respectively to perform the precise azimuthalignment. The initial value of the estimated azimuth angle variance is set as ð1200Þ2.

It can be seen from Figs. 8 and 9 that the simulation results are the same with theexperiment results. The open-loop Kalman filter cannot work when the initial azimuthangle error is big. It is very hard for the open-loop Kalman filter to get convergent. On thecontrary, the closed-loop Kalman filter works very well even when the initial azimuth angleerror is 180. This means that the large azimuth angle error problem in initial alignment canbe solved well by using the closed-loop Kalman filter.

6. Initial azimuth alignment results on a land vehicle

To verify the effectiveness of our approach, initial alignment is performed on a landvehicle. It is repeated 8 times (4 times with engine on and 4 times off) respectively in fourdifferent directions. The alignment results of the azimuth angle are presented in Table 1.

ARTICLE IN PRESS

Table 1

Alignment results of a RLG SINS on a land vehicle.

n Position 1 (deg) Position 2 (deg) Position 3 (deg) Position 4 (deg)

1 81.394 �25.234 �79.815 79.998

2 81.398 �25.228 �79.804 80.014

3 81.400 �25.226 �79.795 80.037

4 81.410 �25.248 �79.782 80.002

5 81.403 �25.233 �79.792 79.978

6 81.407 �25.253 �79.799 79.985

7 81.406 �25.259 �79.807 80.053

8 81.410 �25.243 �79.789 79.994

S. Lu et al. / Journal of the Franklin Institute 346 (2009) 1021–10371036

The first four lines are the results with the engine off and the last four lines are the resultswith the engine on. It can be seen that there are almost no differences in the two situations.In fact, RMS of the alignment results with the engine off is 0:011, while that of those withthe engine on is 0:016. Thus, it can be seen that the prefilter works well no matter theengine is off or on. Also it can be known that since the large azimuth angle error problem issolved well, the inaccurate coarse alignment will not influence the accuracy of the finalresults. The speed of initial alignment is improved by our solution. The accuracy of theresults with our solution is satisfactory. The azimuth angle error can be less than 1mrad in4min. To summarize, our solution to the initial alignment works efficiently in practice.

7. Conclusion

In this paper, a new solution for initial alignment of strapdown inertial navigationsystem is proposed. In the new solution, a novel prefilter with low computationalcomplexity is designed to suppress the sensor noise in real environment. Also is shown thatthe initial estimated variance of the azimuth angle error is the key factor which influencesthe convergent speed of the azimuth angle error. By tuning it, the azimuth angle error willget convergent very soon. At the same time, the large azimuth angle error problem can besolved well by the new solution. The feasibility of our solution is verified by simulation andexperiment.

References

[1] G.R. Pitman, Inertial Navigation, Wiley, New York, London, 1962.

[2] M.S. Grewal, L.R. Weill, A.P. Andrews, Global Positioning Systems, Inertial Navigation and Integration,

Wiley-Interscience, New York, 2001.

[3] D.H. Titterton, J.L. Weston, Strapdown Inertial Navigation Technology, second ed., IEE, 2004.

[4] R.M. Rogers, Applied Mathematics in Integrated Navigation Systems, second ed., AIAA, New York, 2003.

[5] O.S. Salychev, Applied Inertial Navigation: Problems and Solutions, BMSTU Press, 2004.

[6] C. Jekeli, Inertial Navigation Systems with Geodetic Applications, Walter de Gruyter, Berlin, New York,

2001.

[7] J. Farrell, M. Barth, The Global Position System and Inertial Navigation, McGraw-Hill, New York, 1998.

[8] S. Ye, B. Stieler, Simulation on Gyrocompassing Alignment and Calibration of Strapdown Inertial

Navigation System in a Swaying Vehicle, Braunschweig, 1984.

[9] I.Y. Bar-Itzhack, N. Berman, Control theoretic approach to inertial navigation systems, Journal of

Guidance, Control and Dynamics 11 (3) (1988) 237–245.

ARTICLE IN PRESSS. Lu et al. / Journal of the Franklin Institute 346 (2009) 1021–1037 1037

[10] M.S. Grewal, V.D. Henderson, R.S. Miyasako, Application of Kalman filtering to the calibration and

alignment of inertial navigation systems, IEEE Transactions on Automatic Control 36 (1) (1991) 4–13.

[11] T.M. Pham, Kalman filter mechanization for INS airstart, IEEE AES Systems Magazine (January 1992)

3–10.

[12] Y.F. Jiang, Y.P. Lin, Error estimation of INS ground alignment through observability analysis, IEEE

Transactions on Aerospace and Electronic Systems 28 (1) (1992) 92–96.

[13] J. Aranda, J.M. De La Cruz, S. Dormido, P. Ruiperez, R. Hernandez, Reduced-order Kalman filter for

alignment, Cybernetics and Systems 25 (1) (1994) 1–16.

[14] J.C. Fang, D.J. Wan, A fast initial alignment method for strapdown inertial navigation system on stationary

base, IEEE Transactions on Aerospace and Electronic Systems 32 (4) (1996) 1501–1505.

[15] L. Schimelevich, R. Naor, New approach to coarse alignment, in: IEEE Position, Location and Navigation

Symposium, 1996, pp. 324–327.

[16] S.P. Dmitriyev, O.A. Stepanov, S.V. Shepel, Nonlinear filtering methods applications in INS alignment,

IEEE Transactions on Aerospace and Electronic Systems 33 (1) (1997) 260–271.

[17] Y.F. Jiang, Error analysis of analytic coarse alignment methods, IEEE Transactions on Aerospace and

Electronic Systems 34 (1) (1998) 334–337.

[18] N. El-Sheimy, S. Nassar, and Noureldin A., Wavelet de-noising for IMU alignment, IEEE Aerospace and

Electronic Systems Magazine (2004) 32–38.

[19] H.S. Ahn, C.H. Won, Fast alignment using rotation vector and adaptive Kalman filter, IEEE Transactions

on Aerospace and Electronic Systems 42 (1) (2006) 70–82.

[20] S.L. Lu, L. Xie, J.B. Chen, Reduced order Kalman filter for RLG SINS initial alignment, in: 2008 Chinese

Control and Decision Conference, pp. 3675–3680.

[21] P.G. Savage, Strapdown inertial navigation integration algorithm design part 1: attitude algorithms, Journal

of Guidance, Control and Dynamics 21 (1) (1998) 19–28.

[22] P.G. Savage, Strapdown inertial navigation integration algorithm design part 2: veloctiy and position

algorithms, Journal of Guidance, Control and Dynamics 21 (2) (1998) 208–221.

[23] A. Weinred, I.Y. Bar-Itzhack, The psi-angle error equation in strapdown inertial navigation systems, IEEE

Transactions on Aerospace and Electronic Systems 14 (3) (1978) 539–542.

[24] B.M. Scherzinger, D.B. Reid, Inertial navigation error models for large heading uncertainty, in: IEEE

Position, Location and Navigation Symposium, 1996, pp. 477–484.

[25] G. Strang, T. Nguyen, Wavelets and Filter Banks, Wellesley-Cambridge Press, 1996.

[26] J.G. Proakis, D.G. Manolakis, Digital Signal Processing: Principles, Algorithms, and Applications, third ed.,

Prentice-Hall, Englewood Cliffs, NJ, 1996.

[27] R.J. Elliott, L. Aggoun, J.B. Moore, Hidden Markov Models, Springer, Berlin, 1995.

[28] O. Capp�e, E. Moulines, T. Ryd�en, Inference in Hidden Markov Models, Springer, Berlin, 2005.

[29] C.K. Chui, G. Chen, Kalman Filtering with Real Time Applications, third ed., Springer, Berlin, 1999.

[30] M.S. Grewal, A.P. Andrews, Kalman filtering: Theory and Practice Using MATLAB, Wiley, New York,

2001.

[31] D. Simon, Optimal State Estimation: Kalman, H1, and Nonlinear Approaches, Wiley, New York, 2006.

[32] P. Zarchan, H. Musoff, Fundamentals of Kalman Filtering: A Practical Approach, second ed., AIAA, 2005.

[33] R.E. Kalman, A new approach to linear filtering and prediction probelms, ASME Journal of Basic

Engineering 82 (1960) 34–45.