ARTICLE IN PRESS

Journal of the Franklin Institute 346 (2009) 10211037 www.elsevier.com/locate/jfranklin

New techniques for initial alignment of strapdown inertial navigation system

Shaolin Lua,, Ling Xieb, Jiabin Chenb
a

Department of Precision Instruments and Mechanology, Tsinghua University, Beijing 100084, China b School 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 prelter, which consists of an IIR lter and a Kalman lter using hidden Markov model, is designed to attenuate the inuence of sensor noise and outer disturbance. Navigation algorithm in alignment is modied to feedback continuously for the closed-loop system. It is shown that the initial estimated variance setting of azimuth angle error can inuence the speed of initial alignment signicantly. At the beginning of alignment, Kalman lter 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 veried by simulation and experiment. & 2009 The Franklin Institute. Published by Elsevier Ltd. All rights reserved.
Keywords: Strapdown inertial navigation system; Initial alignment; Kalman lter

1. Introduction Initial alignment is the rst work stage of strapdown inertial navigation system (SINS). The initial azimuth accuracy is one of the main factors which inuence the navigation
Corresponding author.

E-mail address: leo20980@sohu.com (S. Lu). 0016-0032/$32.00 & 2009 The Franklin Institute. Published by Elsevier Ltd. All rights reserved. doi:10.1016/j.jfranklin.2009.09.003

ARTICLE IN PRESS
1022 S. Lu et al. / Journal of the Franklin Institute 346 (2009) 10211037

accuracy of SINS. For this reason, initial alignment is always an important issue in navigation eld [120]. 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] utilized wavelet de-noising method in IMU alignment. In this paper, a new method is proposed to reduce the inuence brought by the mixture of the sensor noise and the outer disturbance. The new prelter consists of an IIR lter and a Kalman lter using hidden Markov model (HMM). Different from wavelet method which processes the signal in the timefrequency domain, our method works in the frequency and time domain subsequently. Firstly, it lters the high-frequency noise in the frequency domain by the lowpass lter. And then, it works in the time domain by the steady Kalman lter. Wavelet de-noising method has a block 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 data like wavelet. Beneting from this merit, the structure of the initial alignment program can be simplied. With this prelter, the large noise problem of SINS on a vehicle with the engine on can be solved well and easily. Navigation algorithm has been studied very well [21,22]. Here, it will be modied for the closed-loop Kalman lter in alignment. With it, SINS can work like the gimbal system in the alignment stage. Moreover, since the modication is very slight, it will not bring too much computation burden. When this navigation algorithm is implemented, the error model needs to be modied accordingly. Error models of SINS have been lucubrated [9,23]. In this paper, only the cangle error model is considered and modied. Using the conventional Kalman lter design method, the precise azimuth alignment suffers a low convergent speed of the azimuth angle. Observability analysis of cangle error model with velocity measurement was made by Bar-Itzhack et al. [9] and Jiang [12]. It is widely accepted that since the azimuth angle error is not completely observable it will cost Kalman lter 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 lter works with a proper setting of this variance, the azimuth angle error will decrease soon. Also it will be shown that the low signal to noise ratio of gyroscope makes this phenomenon happen. The reason why the azimuth angle gets convergent very slow with an ordinary variance setting is not that the system is not completely observable. The reason is that the earth angular rate is too slow and the sensor noise is too large. Generally, the rst 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 ne alignment. However, in practice the accuracy provided by coarse alignment may be very low. Under this condition, the precise alignment may be accomplished with a large azimuth error. This problem is called the large azimuth error problem. To solve this problem, the modied cangle model [11,24] is proposed. Nonlinear ltering is used to accomplish the initial alignment with a large azimuth error [16]. Here, it will be shown that using our solution this problem can be solved well with the cangle model by a closed-loop Kalman lter. The modied cangle model or nonlinear model is not necessary. If the azimuth angle error is large, for example 1801, the correction speed of the angle error can be accelerated by the large azimuth variance to make the angle error decrease quickly, which makes the cangle model proper for initial alignment.

ARTICLE IN PRESS
S. Lu et al. / Journal of the Franklin Institute 346 (2009) 10211037 1023

The organization of this paper proceeds as follows. In the next section, our solution for initial alignment of SINS will be introduced briey. In Section 3, a new prelter will be proposed to attenuate the sensor noise in real environment. Navigation algorithm in alignment and error model will be modied for the feedback of Kalman lter in Section 4. In Section 5, the variance tuning technique for improving the convergent speed of the azimuth angle error will be introduced. As presented in Section 5, the main factors, which inuence the speed of convergence, are obtained to solve the large azimuth angle error problem. The experiment results on a land vehicle are presented in Section 6. 2. The closed loop solution for SINS alignment using optimal ltering Different from the conventional two-stage solution, the whole procedure of our solution consists of three stages: coarse alignment, precise leveling and precise azimuth alignment. Coarse alignment can be accomplished by least square method [15,17]. Since our solution can work with a large azimuth angle error, this stage will cost a very short time. In our program, it is set as 10 s. In the precise leveling stage, the system can be leveled by gyrocompassing or Kalman lter very soon [114]. In our solution, gyrocompassing is used for its high speed and it costs 10 s too. In this paper, it is assumed that the precise azimuth alignment is performed after the system has been leveled well. The whole procedure cost 4 min. The precise azimuth alignment is accomplished by a closed-loop solution. It solves the problems as follows: (1) Preltering 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 navigation algorithm runs, the output of IMU will be preprocessed by IIR lowpass lters and

Fig. 1. The scheme of precise azimuth alignment.

ARTICLE IN PRESS
1024 S. Lu et al. / Journal of the Franklin Institute 346 (2009) 10211037

two-state Kalman lters. The velocity calculated by navigation algorithm is used as the observation of the closed-loop Kalman lter. The estimation of the closed-loop Kalman lter is utilized as the feedback to stabilize the output of SINS navigation algorithm. 3. A new prelter for strapdown inertial navigation system in alignment In this section, a new prelter is proposed for SINS in alignment. It possesses of a realtime 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 prelter facilitate the programmer to design an alignment software with a simpler structure. It consists of an IIR lowpass lter and a two-state Kalman lter with HMM. The IIR lter will depress the high frequency disturbance. The output of the IIR lter is modeled using HMM. Then, the two-state Kalman lter will be used to attenuate the energy of the sensor noise further. Coarsely speaking, this prelter works in the frequency domain and time domain subsequently. Wavelet tries to analyze the signal as precisely as possible, while the prelter we designed concentrate on the useful signal. In the frequency domain, it lters the noise which frequency is higher than the true signal. In the time domain, since it estimates the signal by a Kalman lter using HMM, the amplitude of the noise is reduced further. Here a ring laser gyroscope (RLG) SINS is considered. 3.1. IIR lowpass lter 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 be augmented further. The IIR lter can restrain the high frequency disturbance. However, in alignment its cutoff frequency cannot be set too low. Otherwise, useful information will be rejected and the ltered result may be biased. In our system, a 4-order IIR lter with 10 Hz cutoff frequency is designed. sk b0 rk b1 rk 1 b2 rk 2 b3 rk 3 b4 rk 4 a1 sk 1 a2 sk 2 a3 sk 3 a4 sk 4 1

where rk is the sensor output of k, sk is the ltered result of k and the coefcients bi ; ai ; i 0; . . . ; 4 can be calculated using different methods. See [26] for details. Here, these coefcients are chosen as follows: b0 0:0048423; 0:0048243; 0:1873795 b1 0:0192974; b2 0:0289461;b3 0:0192974; b4 a1 2:3695130;a2 2:3139884; a3 1:0546654; a4

For different systems, the cutoff frequency can be changed to satisfy the practical need. Accordingly, the coefcients should be redesigned, too. 3.2. Steady-state Kalman lter Besides the IIR lter, a steady-state Kalman lter with HMM is used to attenuate the energy of signal noise further. In alignment, the sensor output ltered by the lowpass lter can be seen as random walk.

ARTICLE IN PRESS
S. Lu et al. / Journal of the Franklin Institute 346 (2009) 10211037 1025

According to the theory of HMM, the true output of sensors are hidden in the output of the lowpass lter. The discrete HMM can be formulated as follows [27,28]: Xk1 AX k Zk Sk1 CX k nk where X 2 RN , S 2 RM and the entries satisfy
N X j1 M X j1

2 3

Aij 1;

Aij Z0

Cij 1;

Cij Z0

For this application it can be modeled as follows [2729]: " # " #" # A11 A12 xk xk1 Zk xk xk1 A21 A22 The measurement model can be given as follows: " # " #" # xk sk1 C11 C12 nk sk C21 C22 xk1

where Zk $N0; U and nk $N0; V represent the process noise and the measurement noise respectively. To make the output sequence smooth, the estimated covariance of measurement noise can be set much bigger than that of the process noise [2932]. In practice, the noise covariance matrix in continuous time form for the lter of accelerometer is set as " # 0:01 mg2 0 8 Ua 0 0:01 mg2 " Va 2 mg2 0 0 2 mg2 # 9

The noise covariance matrix for gyroscope in continuous time form is set as " # 0:2=h2 0 Ug 0 0:2=h2 " Vg 20:57=h2 0 0 20:57=h2 #

10

11

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

ARTICLE IN PRESS
1026 S. Lu et al. / Journal of the Franklin Institute 346 (2009) 10211037

the steady two-state Kalman lter works as follows: State prediction " # " #" # ^ s k1jk ^ sk A11 A12 ^ s kjk ^ s k1 A21 A22 Measurement update " # " # " # " #! ^ ^ s k1jk s k1jk ~ ^ s k1 s k1 Koff ^ ^ s kjk s kjk ^ ~ sk sk

12

13

where Koff is the steady-state Kalman gain computed ofine. The prelter described by Eqs. (1), (12) and (13) needs only 30 ops to lter a sensor output. Thus, it is very suitable for the online implementation. Another merit of this method is that it smooths the previous output when it lters the current output. This is one of the reasons why the noise variance is attenuated to a very low level.

103 618 412 206 0 206 412 618 824 1030 0 10 20 30 t (s) 8.24 Angular velocity (deg/h) 9.27 10.3 11.33 12.36 13.39 14.42 0 10 20 30 t (s) 40 50 60 40 50 60 Angular velocity (deg/h) Angular velocity (deg/h) 82.4 61.8 41.2 20.6 0 20.6 41.2 61.8 0 10 20 30 t (s) 40 50 60

Fig. 2. The preltering results of a gyroscope in alignment: (a) the output of a gyroscope in alignment, (b) the output of a gyroscope ltered by an IIR lowpass lter and (c) the output of a gyroscope ltered by an IIR lowpass lter and a Kalman lter.

ARTICLE IN PRESS
S. Lu et al. / Journal of the Franklin Institute 346 (2009) 10211037 1027

0 5 Specific force (mg) 10 15 20 25 30 0 10 20 30 t (s) 8.45 Specific force (mg) 8.5 8.55 8.6 8.65 8.7 8.75 0 10 20 40 50 60 Specific force (mg)

4 6 8 10 12 14 0 10 20 30 t (s) 40 50 60

30 t (s)

40

50

60

Fig. 3. The preltering results of an accelerometer in alignment: (a) the output of an accelerometer in alignment, (b) the output of an accelerometer ltered by an IIR lowpass lter and (c) the output of an accelerometer ltered by an IIR lowpass lter and a Kalman lter.

3.3. Experiment results In Figs. 2 and 3, the output of a gyroscope and an accelerometer of IMU placed on a vehicle with engine on are illustrated. The variance of the original gyroscope output is 3001/h. After it passes through the IIR lter, the variance decreases to 201/h. If it is ltered by an IIR lter and a two-state Kalman lter, the variance becomes 11/h. The variance of the original accelerometer is 11 mg. The IIR lter attenuates its variance to 2.5 mg. An IIR lter and a two-state Kalman lter can make its variance fall to 40 mg. It can be seen that with this new prelter initial alignment will be accomplished in a relatively benign environment.

4. Modied navigation algorithm and SINS error model In this section, navigation algorithm and SINS error model will be modied for the closed-loop Kalman lter. Then, the closed-loop Kalman lter will work not only as an estimator but also as an observer.

ARTICLE IN PRESS
1028 S. Lu et al. / Journal of the Franklin Institute 346 (2009) 10211037

4.1. Modied navigation algorithm in alignment In this paper, only continuous implementation is considered. Detailed discrete implementation is given in [21,22]. Here, navigation algorithm is implemented in local geography level frame (North, East, Down). In alignment, only attitude and velocity need be updated. The navigation algorithm for alignment is given as follows: Attitude rate
n n _n C b Cb ob OCb ib

14

n where Cb represents the direction cosine matrix from the body frame to the navigation frame, O means the angular earth rate, ob means the spatial rate of vehicle in body frame ib which is measured by strapdown gyroscopes, and the cross-product form of a vector w is the skew-symmetric matrix: 2 3 0 wz wy 6 0 wx 7 15 w 4 wz 5 wy wx 0

Acceleration transformation
n an C b a b SF SF

16

where ab means the body-axis specic force sensed by strapdown accelerometers. SF Velocity rate gn gn OORn P _ v n an gn 2O vn SF P 17 18

where gn represents the mass attraction gravitational acceleration in navigation frame and gn means the plumb-bob gravity in navigation frame. P In alignment, the angle error can be corrected as follows [17]:
n ^ Hacek; C n I cCb b

19

^ where c is the cangle error estimated by Kalman lter. To implement this correction in a continuous form, here Eq. (14) is modied as
n n ^ _n C b Cb ob O c=tCb ib

20

where t is the period of Kalman lter. To correct the velocity error, Eq. (18) is changed to _ v n an gn 2O vn d^ =t v SF P 21

When Eqs. (20) and (21) are used to update attitude and velocity, navigation errors will be corrected continuously. It should be noted that this modication will not bring too much computation burden. Only ve additions are needed to append to the conventional navigation algorithm [21,22].

ARTICLE IN PRESS
S. Lu et al. / Journal of the Franklin Institute 346 (2009) 10211037 1029

4.2. SINS error model in alignment The error model is modied according to the navigation algorithm. Different SINS error models in alignment were proposed [9,11,16,23,24]. Here only cangle error model is considered [9,23]. Sensor biases are modeled in navigation frame and assumed to be constant. When the modied navigation algorithm above is implemented, the cangle error model in alignment can be modied as follows: 3 2 2 32 dv 3 d_ N v 0 2OD 0 g 0 1 0 0 0 0 N 7 6 6 76 dvE 7 v 7 6 d_ E 7 6 2OD 0 g 0 0 0 1 0 0 0 76 7 7 6 76 _ 6 cN 7 6 0 6 0 0 0 1 0 0 76 c N 7 0 0 OD 7 6 7 6 6 76 7 6 _ 7 6 6 cE 7 6 0 0 OD 0 ON 0 0 0 1 0 76 cE 7 76 7 6 7 6 76 _ 6c 7 6 0 0 0 0 0 0 1 76 cD 7 0 0 ON 7 6 D7 6 76 22 7 Bu 6 _ 76 6 rN 7 6 0 0 0 0 0 0 0 0 0 0 76 r N 7 76 7 6 7 6 7 6 rE 7 6 0 _ 0 0 0 0 0 0 0 0 0 76 r E 7 7 6 7 6 76 7 6 7 6 6 6 eN 7 6 0 _ 0 0 0 0 0 0 0 0 0 7 6 eN 7 76 7 6 7 6 7 6 eE 7 4 0 0 0 0 0 0 0 0 0 0 5 6 eE 7 5 4 _ 5 4 _ eD 0 0 0 0 0 0 0 0 0 0 eD where r means the accelerometer bias, e represents the gyro drift. Eq. (22) can be written as _ x Fx Bu The process noise of this dynamic system is q, and q$N0; Q; Q diagq2 ; q2 ; q2 ; q2 ; q2 ; 0; 0; 0; 0; 0 a a g g g 23

qa represents the random noise of accelerometer and qg means the random noise of ^ gyroscope. B I1010 ; u G x. Since B is an identity matrix, there is much exibility in designing G. Considering sensor biases are unobservable, in our solution only angle and velocity errors are corrected. Thus, G diagGv ; Gv ; GN ; GE ; GD ; 0; 0; 0; 0; 0. At the same time, navigation algorithm has to be modied to reect u. The measurement model can be listed as follows [9]: " #   d~ N v 1 0 0 0 0 0 0 0 0 0 0 0 x 24 d~ E v 0 1 0 0 0 0 0 0 0 0 0 0 which can be written as z Hx 25

the measurement noise is r, and r$N0; R; R diagRv ; Rv . Rv can be set as a very small constant. Utilizing the model described by Eqs. (22) and (24), the closed-loop Kalman lter can be implemented [2732]. In state prediction, the control input must be considered ^ ^ ^ xk 1jk Fxk Bu eF t xk Bu 26

ARTICLE IN PRESS
1030 S. Lu et al. / Journal of the Franklin Institute 346 (2009) 10211037

Other steps are the same as the standard Kalman lter [2733]. If no control input is used in navigation algorithm, Bu will be withdrawn from Eq. (26). Then, an open-loop solution is gotten. 5. Azimuth angle error variance tuning for precise azimuth alignment In all open literature [120], 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 of its estimated state variance should be set as 12 . In this section, it will be shown that this value 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 pointed out that the reason why the azimuth angle error gets convergent slowly with a small variance setting lies in the special structure of the system not in its observability. The large azimuth angle error problem is a difcult problem in alignment [11,16,24]. It will be demonstrated that with a right setting of the estimated variance the closed-loop solution can solve this problem very well. 5.1. Variance tuning for the precise azimuth alignment A ten-state closed-loop Kalman lter is implemented to accomplish the precise alignment. The period of Kalman lter is 1 s. According to our experience, all of Gi are set as 1. SINS works at Beijing which latitude is 39:96. It is assumed that the system has been 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 and random biases of each accelerometer are 50 mg and 2 mg. Now, simulation results will show the ability of the system to correct the initial azimuth error. Through them, the inuence of the initial variance setting on the convergent speed will be illustrated clearly. Different initial errors, 0; 71; 72; 75; 710, are introduced to the azimuth angle. The covariance matrices are set as follows: P diag0:3 m=s2 ; 0:3 m=s2 ; 12 ; 12 ; PD 2 ; 50 mg2 ; 50 mg2 ; 0:005=h2 ; 0:005=h2 ; 0:005=h2 Q diag2 mg2 ; 2 mg2 ; 2=h2 ; 2=h2 ; 2=h2 ; 0; 0; 0; 0; 0 R diag0:0025 m=s2 ; 0:0025 m=s2 P2 , D 27 28 29

The fth diagonal element of P, is an important parameter for the closed-loop Kalman lter in initial alignment. Different setting of PD will make the azimuth angle have different convergent speed. Fig. 4 illustrates the inuence of PD on the convergence of the azimuth angle. It can be seen that for SINS in alignment Kalman lter will be too optimistic if the initial covariance of azimuth angle is set normally. It must be set as a big value, otherwise the estimation curve will get convergent at a very low speed. 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
S. Lu et al. / Journal of the Franklin Institute 346 (2009) 10211037 1031

10 8 6 4 2 0 2 4 6 8 10 0 100 200 300 t (s) 12 Azimuth angle (degree) 10 8 6 4 2 0 1 0 100 200

10 degree 20 degree

10 8 6 4 2 0 2 4 6 8 10 0 100 200 300 t (s) 400 500 600

Azimuth angle (degree)

400

500

600

30 degree 45 degree 80 degree

Azimuth angle (degree)

300 t (s)

400

500

600

Fig. 4. The inuence 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 .

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

5.2. Low signal to noise ratio of gyroscope In the previous subsection, it has been shown that the initial setting of the estimated azimuth variance decides the convergent speed. In this subsection, it will be illustrated that this phenomenon comes from the low signal to noise ration (SNR), not from the observability of the system. Consider an ideal SINS in simulation which has no turn-on gyroscope drift and accelerometer bias. The other parameters used are the same as those of the system in last subsection. Under this condition, the system model will degenerate to

ARTICLE IN PRESS
1032 S. Lu et al. / Journal of the Franklin Institute 346 (2009) 10211037

165 Azimuth angle (degree) Azimuth angle (degree) 0 100 200 300 t (s) 166 Azimuth angle (degree) 164 162 160
10degree 20degree 30degree

165 162 160 158 156 154 152 150 148 145 400 500 600 0 100 200 300 t (s) 400 500 600

162 160 158 156 154 152 150 148 145

45degree

158
80degree

156 154 0 100 200 300 t (s) 400 500 600

Fig. 5. The inuence 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 .

a ve-state model. 2 0 6 d_ 7 6 2O D 6 vE 7 6 6 _ 7 6 6 cN 7 6 0 6 7 6 6c 7 6 0 4 _E 5 4 _ cD 0 It can be written as _ x 5 F5 x5 B5 u5 31 d_ N v 3 2 2OD 0 0 0 0 0 g 0 OD 0 g 0 OD 0 ON 32 3 dvN 0 0 76 dvE 7 76 7 76 7 0 7 6 c N 7 B5 u 5 76 7 ON 76 cE 7 54 5 cD 0

30

^ where as the ten-state system described by Eq. (22), B5 I55 ; u5 G5 x; G5 diagGv ; Gv ; GN ; GE ; GD . These values of G are set as those in last subsection. The process noise of the dynamic system is q5 , and q5 $N0; Q5 ; Q5 diagq2 ; q2 ; q2 ; q2 ; q2 a a g g g diag2 mg2 ; 2 mg2 ; 2=h2 ; 2=h2 ; 2=h2 .

ARTICLE IN PRESS
S. Lu et al. / Journal of the Franklin Institute 346 (2009) 10211037 1033

The measurement model is " # 

dvN

3 32

d~ N v d~ E v

1 0 0 1

0 0

0 0

6 7 6 dvE 7 7 0 6 6 cN 7 6 7 0 6 7 4 cE 5 cD

which can be written as z H5 x5 the measurement noise is the same as the ten-state system. The observability matrix Q11 is constructed as follows: 3 2 H5 7 6 6 H5 F 5 7 7 Q5 6 6 ... 7 5 4 4 H5 F 5 33

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 as 2 and the estimated covariance matrix is initialized as diag0:3 m=s2 ; 0:3 m=s2 ; 12 ; 12 ; 22 . Assumed that the system has been leveled, perform the precise azimuth

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

ARTICLE IN PRESS
1034 S. Lu et al. / Journal of the Franklin Institute 346 (2009) 10211037

2.5 2 Azimuth angle (degree) 1.5 1 0.5 0 0.5 0 100 200 300 t (s) 400 500 600
centuplicate noise variance decuple noise variance original noise variance

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

200 Azimuth angle (degree) Azimuth angle (degree) 150 100 50 0 50 0 50 100 t (s) 150 200

200 150 100 50 0 50 0 50 100 t (s) 150 200

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

alignment. As can be seen from Fig. 6, the convergent speed of the azimuth angle increases as the earth angular rate increases. The low earth angular rate in real world makes the SNR very low. It cannot work well with an ordinary azimuth angle variance setting. This conclusion can be veried vice versa. Now, keep the earth angular rate unchanged and change the noise variance of sensors. Let the random noise of the gyroscope and accelerometer respectively be 0:02=h2 and 20 mg2 . Perform the precise azimuth alignment with 2 azimuth angle error and PD 2. Increase the standard variance of sensor noise to their decuple and centuplicate. Repeat the simulation and Fig. 7 can be gotten. It can be concluded from this gure that the variance of the sensor noise has a very strong inuence on the convergent speed of the azimuth angle. For this reason, a prelter is necessary for SINS in alignment.

ARTICLE IN PRESS
S. Lu et al. / Journal of the Franklin Institute 346 (2009) 10211037 1035

280 260 240 220 200 180 160 140 120 100 80 0 50 100 t (s) 150 200

280 260 240 220 200 180 160 140 120 100 80 0 50 100 t (s) 150 200

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

According to the results above, the reason why the azimuth angle gets convergent very slow is not that the system is not completely observable. The reason is that the earth angular rate is too slow and the sensor noise is too large. In practice, the estimated variance of the azimuth angle must be set as a big value and a prelter must be used to depress 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 lter with the cangle error model will be of no use when the initial azimuth angle error is large. To solve this problem, different methods, such as nonlinear ltering [16,19] and modied cangle error model [11], were proposed. Here it will be shown by using the closed-loop Kalman lter with a right setting of the initial azimuth variance this problem can be solved very well. The precise alignment may be performed with a large azimuth angle error, even 180! With this technique, coarse alignment 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 closedloop Kalman lter and the open-loop one respectively to perform the precise azimuth alignment. The initial value of the estimated azimuth angle variance is set as 12002 . It can be seen from Figs. 8 and 9 that the simulation results are the same with the experiment results. The open-loop Kalman lter cannot work when the initial azimuth angle error is big. It is very hard for the open-loop Kalman lter to get convergent. On the contrary, the closed-loop Kalman lter works very well even when the initial azimuth angle error is 180. This means that the large azimuth angle error problem in initial alignment can be solved well by using the closed-loop Kalman lter. 6. Initial azimuth alignment results on a land vehicle To verify the effectiveness of our approach, initial alignment is performed on a land vehicle. It is repeated 8 times (4 times with engine on and 4 times off) respectively in four different directions. The alignment results of the azimuth angle are presented in Table 1.

Azimuth angle (degree)

Azimuth angle (degree)

ARTICLE IN PRESS
1036 S. Lu et al. / Journal of the Franklin Institute 346 (2009) 10211037 Table 1 Alignment results of a RLG SINS on a land vehicle. n 1 2 3 4 5 6 7 8 Position 1 (deg) 81.394 81.398 81.400 81.410 81.403 81.407 81.406 81.410 Position 2 (deg) 25.234 25.228 25.226 25.248 25.233 25.253 25.259 25.243 Position 3 (deg) 79.815 79.804 79.795 79.782 79.792 79.799 79.807 79.789 Position 4 (deg) 79.998 80.014 80.037 80.002 79.978 79.985 80.053 79.994

The rst four lines are the results with the engine off and the last four lines are the results with 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 with the engine on is 0:016. Thus, it can be seen that the prelter works well no matter the engine is off or on. Also it can be known that since the large azimuth angle error problem is solved well, the inaccurate coarse alignment will not inuence the accuracy of the nal results. The speed of initial alignment is improved by our solution. The accuracy of the results with our solution is satisfactory. The azimuth angle error can be less than 1 mrad in 4 min. To summarize, our solution to the initial alignment works efciently in practice. 7. Conclusion In this paper, a new solution for initial alignment of strapdown inertial navigation system is proposed. In the new solution, a novel prelter with low computational complexity is designed to suppress the sensor noise in real environment. Also is shown that the initial estimated variance of the azimuth angle error is the key factor which inuences the convergent speed of the azimuth angle error. By tuning it, the azimuth angle error will get convergent very soon. At the same time, the large azimuth angle error problem can be solved well by the new solution. The feasibility of our solution is veried by simulation and experiment. 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) 237245.

ARTICLE IN PRESS
S. Lu et al. / Journal of the Franklin Institute 346 (2009) 10211037 1037 [10] M.S. Grewal, V.D. Henderson, R.S. Miyasako, Application of Kalman ltering to the calibration and alignment of inertial navigation systems, IEEE Transactions on Automatic Control 36 (1) (1991) 413. [11] T.M. Pham, Kalman lter mechanization for INS airstart, IEEE AES Systems Magazine (January 1992) 310. [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) 9296. [13] J. Aranda, J.M. De La Cruz, S. Dormido, P. Ruiperez, R. Hernandez, Reduced-order Kalman lter for alignment, Cybernetics and Systems 25 (1) (1994) 116. [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) 15011505. [15] L. Schimelevich, R. Naor, New approach to coarse alignment, in: IEEE Position, Location and Navigation Symposium, 1996, pp. 324327. [16] S.P. Dmitriyev, O.A. Stepanov, S.V. Shepel, Nonlinear ltering methods applications in INS alignment, IEEE Transactions on Aerospace and Electronic Systems 33 (1) (1997) 260271. [17] Y.F. Jiang, Error analysis of analytic coarse alignment methods, IEEE Transactions on Aerospace and Electronic Systems 34 (1) (1998) 334337. [18] N. El-Sheimy, S. Nassar, and Noureldin A., Wavelet de-noising for IMU alignment, IEEE Aerospace and Electronic Systems Magazine (2004) 3238. [19] H.S. Ahn, C.H. Won, Fast alignment using rotation vector and adaptive Kalman lter, IEEE Transactions on Aerospace and Electronic Systems 42 (1) (2006) 7082. [20] S.L. Lu, L. Xie, J.B. Chen, Reduced order Kalman lter for RLG SINS initial alignment, in: 2008 Chinese Control and Decision Conference, pp. 36753680. [21] P.G. Savage, Strapdown inertial navigation integration algorithm design part 1: attitude algorithms, Journal of Guidance, Control and Dynamics 21 (1) (1998) 1928. [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) 208221. [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) 539542. [24] B.M. Scherzinger, D.B. Reid, Inertial navigation error models for large heading uncertainty, in: IEEE Position, Location and Navigation Symposium, 1996, pp. 477484. [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. Moulines, T. Ryd n, Inference in Hidden Markov Models, Springer, Berlin, 2005. e e [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 ltering: 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 ltering and prediction probelms, ASME Journal of Basic Engineering 82 (1960) 3445.