Date post: | 13-Oct-2014 |
Category: |
Documents |
Upload: | pham-van-tang |
View: | 148 times |
Download: | 8 times |
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: [email protected] (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.