Anda di halaman 1dari 10

Journal of Computational Information Systems 10: 8 (2014) 32553264

Available at http://www.Jofcis.com

Strapdown INS Initial Alignment in Inertial Frame using


H Filter
Fujun PEI ,

Xuan LIU, Li ZHU

School of Electronic Information & Control Engineering, Beijing University of Technology, Beijing
100124, China

Abstract
Initial alignment is the key procedure in the strapdown inertial navigation system (INS), which directly
relates to the working precision of strapdown INS. But the tradition alignment method cant nish
alignment under the dynamic disturbance condition. In this paper, a novel alignment method using H
lter was investigated to solve this problem. In the coarse alignment procedure, the invariance property
of gravity in the inertial frame was used to deal with the disturbances. In the ne alignment procedure,
strapdown INS alignment error model under the dynamic condition was established. Moreover, because
of the uncertainty observation noise in the velocity measurements, the H robust lter was designed to
estimate the misalignment angles accurately. Finally, simulation results demonstrated that the proposed
method can depress the random disturbances more eectively than the traditional alignment method.
Keywords: Strapdown INS; Initial Alignment; Inertial Frame; H Filter

Introduction

The purpose of strapdown INS initial alignment is to determine the coordinate transformation
matrix from body frame to navigation frame. Then misalignment angles can be compensated
based on their estimated values [1]. Since the initial alignment directly relates to the working
precision and start-up time of strapdown INS, it is one of the most important factors in the
operation of strapdown INS [2]. In recent years, a lot of research has been developed for strapdown
INS alignment which contains two phases: coarse alignment and ne alignment.
For the coarse alignment step, the system attitude can be determined directly by using the
known gravity and Earth rate signals in the local level frame and the measurements obtained
by using accelerometers and gyros [3, 4]. However, when marine strapdown INS is under the
dynamic disturbance environment, where the disturbed acceleration and rotation velocity exist,
the static coarse alignment method cannot be used. Therefore, a new coarse alignment method
for the marine strapdown INS using the gravity in the inertial frame as a reference has been

Project supported by the National Science Foundation of China (No. 60975065).


Corresponding author.
Email address: fujunpei@sina.com (Fujun PEI).

15539105 / Copyright 2014 Binary Information Press


DOI: 10.12733/jcis9764
April 15, 2014

3256

F. Pei et al. /Journal of Computational Information Systems 10: 8 (2014) 32553264

discussed. This coarse alignment considers projection of gravity in the inertial frame to calculate
a coarse value of the initial attitude matrix between body initial inertial frame and inertial frame
[5]. It could withstand random movements under the dynamic disturbance condition. And the
results can meet the requirement of coarse alignment for strapdown INS in accuracy [6]. But the
inertial frame coarse alignment method can only provide a fairly good initial condition for the
alignment phase, the ne alignment should be used to compensate the misalignment angles.
As for the ne alignment step, a lot of research has been done on the stationary base algorithms.
It is well known that the Kalman lter (KF) is an eective method for linear systems to distinguish
the real states from noisy information and it is an optimal tracking method when the noise is
subject to Gaussian distribution. In order to solve the strapdown INS alignment problem under
the marine mooring condition, Gao et al proposed the rapid strapdown INS ne alignment method
in [7]. However, when it comes to uncertain disturbances, the Kalman lter becomes disabled. In
recent years, H lter was widely used in initial alignment for strapdown INS instead of Kalman
lter, and have made a lot of research and simulation [8, 9]. Studies show that the advantage
of the H lter in comparison with the Kalman lter is that no statistical assumptions on the
noises are required, and the lter is more robust when the noises are uncertainties in a system
[10, 11, 12].
In order to deal with the lineal and angular disturbances, a novel initial alignment method was
investigated in this paper. On one hand, using of the gravity projection along the inertial frame,
could avoid the eects of disturbed accelerates and angular velocities in alignment process. On
the other hand, compared with the traditional Kalman lter, H robust lter can depress the
random disturbances more eectively. Therefore, the proposed novel initial alignment method
can meet the requirements of strapdown INS alignment under the dynamic disturbance condition.
The remainder of this paper is organized as follows. The algorithm approaches for the inertial
frame coarse alignment is presented in Section 2. Section 3 provides the strapdown INS ne
alignment error model on inertial frame and the mathematical description of H lter is presented
in Section 4. The simulation results are illustrated in Section 5. Finally, the conclusion makes up
in Section 6.

Coarse Alignment in the Inertial Frame

As mentioned above, the traditional coarse alignment cannot provide accurate values for the
initial attitude angles under the dynamic disturbance condition. To tackle these limitations, a
special inertial frame, the initial time strapdown INS body inertial frame (ib0 frame) is dened and
selected as transition reference frames in coarse alignment. The specic denition of coordinate
frames used in this paper are quoted from [13]. Therefore, the initial attitude matrix, which
relates the body frame to the navigation frame, could be described as follows:
cbn (t) = cne cei (t) ciib0 cibb0 (t)
Where cne and cei (t) are as follows:

(1)

cne =

sin
L
0
cos
L

cos L 0 sin L

(2)

F. Pei et al. /Journal of Computational Information Systems 10: 8 (2014) 32553264

cos ie (t t0 )

sin ie (t t0 ) 0

3257

cei (t) =

sin

(t

t
)
cos

(t

t
)
0
ie
0
ie
0

0
0
1

(3)

Where L is latitude, ie is the Earth rotation rate, t0 is the start time of the coarse alignment.
which represents the transform matrix from the body frame b to the base inertial frame ib0 ,
can be computed recurrently with the gyro outputs through the quaternion attitude algorithm.
The matrix initial value for the recurrent computation is unit matrix. Thus the main problem of
the inertial frame alignment is to compute the matrix ciib0 . While the vehicle is mooring on the
sea or aside the dock, liner movements such as surge, sway and heave are exist aroused by the
sway. The accelerometers measured specic force not only contains gravity. The integration of
the accelerometers measured specic force projected in the body inertial frame can be described
as follows:
cibb0

t
Vb ib0 = t0k Cbib0 fb
t
t
t
= t0k Ciib0 Cbi g b dt + t0k Cbib0 abLA dt + t0k Cbib0 abD dt
t
ib0
= Ciib0 t0k g i dt + VLA
+ V ib0

(4)

ib0
Where VLA
is the induced velocity due to the lever-arm and can be calculated using the known
lever-arm as described:
tk
( b
)
ib0
b0
VLA =
aiLA
dt = Cbib0 ib
rb
(5)
t0

Where abD is the disturbing acceleration and V ib0 can be ignored because it is much smaller
than the velocity caused by the gravity. Consequently, Eq. (4) becomes:
d
ib0 = V
ib0 V ib0 = C ib0
Vg
i
LA

tk

g i dt = Ciib0 V i

(6)

t0

Considering,

g cos L cos ie t

.
g i = Cei Cne g n =
g
cos
L
sin

t
ie

g sin L

v i (tk ) =

g cos L
sin (ie tk )
ie
g cos L
(1 cos (ie tk ))
ie

(7)

(8)

tk g sin L
In term of Eq. (6), we calculate V ib0 and V i at tk1 and tk2 respectively. Then,
{

ib0 (tk1 ) = ciib0 i (tk1 )


ib0 (tk2 ) = ciib0 i (tk2 )

(9)

3258

F. Pei et al. /Journal of Computational Information Systems 10: 8 (2014) 32553264

From (9), ciib0 could be calculated by the following Eq. (10):

1
T
T
[v i (tk1 )]
[
v ib0 (tk1 )]


T
T
i

Ciib0 =
[v
(t
)]
[
v ib0 (tk2 )]
k2


T
T
[v i (tk1 ) v i (tk2 )]
[
v ib0 (tk1 ) vib0 (tk2 )]

(10)

After deriving ciib0 , the attitude matrix cbn can be calculated through Eq. (1).

Strapdown INS Alignment Error Model

The coarse alignment method investigated in the Section 2 is eective for strapdown INS in
disturbance environment. But it only accomplished the coarse estimation of attitude matrix from
the body frame to the navigation frame. In this section, the strapdown INS error model in inertial
frame is provided for the ne alignment process.
Use (1) for reference, cbn (t) at the end of the ne alignment process can be represented as
cbn = cne cei cib (t)
cbi (t) can be described as

(11)

Cbi (t) = [Cii (t)]1 Cbi (t)

where,

i
Cii (t) = I i (t) =
z (t)

(12)

iz (t)
1

iy (t)

ix (t)

iy (t)

ix (t)

(13)

The attitude matrix Cbi (0), which relates the body frame to the computed inertial frame at the
beginning of the ne alignment. And the misalignment angles between inertial frame i and the
computed i are
[
]T
i
i
i
i
(t) = x (t) y (t) z (t)
(14)

Using the attitude quaternion algorithm Cbi (t) can be computed. Thus the main problem of
the ne alignment becomes the estimation of misalignment angles. The accelerometers measured
specic force projected in the frame i can be described as follows:

Cbi fb = Cbi [(I + KA )(I + A)(g b + abLA + abD ) + ]

g i + i g i + Cbi + Cbi abLA + Cbi abD

+Cbi (KA

+ A)(g +

abLA

(15)

abD )

Considering, accelerometers constant bias b and observation white noise ab ,


= b + ab

(16)

Cbi fb + g i Cbi abLA = i g i + Cbi b + Cbi ab + ai

(17)

Then, (15) can be written as:

F. Pei et al. /Journal of Computational Information Systems 10: 8 (2014) 32553264

ai = Cbi (KA + A)(g b + abLA + abD )

3259
(18)

Integrating (17) from t1 to t yields,


t i b t i t i b
C f + t1 g t1 Cb aLA d
t1 b
t i
t
t
t
= t1 g i d + t1 Cbi b d + t1 Cbi ab d + t1 ai d

(19)

The measurement vector is:


i
Z=V i = Vfi Vgi VLA
= V i + VW + VDi

Where, Vfi =

(20)

b
i
Vgi = tt0 g i d , VLA
= t1 Cbi abLA d = Cbi (ib
rb ), VDi = t1 ai d ,
t
t
t
i b
i
i
i
(21)
Cbi ab d
g d +
Cb d +
V =

t
i b
t0 Cb f d ,

t1

t1

t1

In order to estimate the misalignment angles by using a H lter, the velocity-error equation
and the misalignment-angle equation on the inertial frame can be written as
V i (t) = g i (t) i (t) + Cbi (t)b + Cbi (t)ab

(22)

i (t) = Cbi (t)b Cbi (t) b


Then the state equation can be represented as

X(t)
= A(t)X(t) + B(t)W
where,

[
X(t) =

Vxi (t), Vyi (t), Vzi (t), ix (t), iy (t), iz (t),

bx , by , bz , bx , by , bz
[
]T
W = abx , aby , abz , wxb , wyb , wzb , 0, 0, 0, 0, 0, 0

033 [g i (t)] 033 Cbi (t)

033
033
Cbi (t) 033

A(t) =
033
033
033

033
033
033
033
033

Cbi (t) 033 033 033

033 Cbi (t) 033 033

B(t)=
033 033 033

033
033
033 033 033

(23)
]T
(24)
(25)

(26)

(27)

Furthermore, the measurement equation can be formulated as


Z(t)=HX(t) + VW + VDi
]
[
H= I33 039 .

(28)
(29)

Where VDi is uncertainty observation noises, consists of disturbing velocity caused by the ships
pitch and roll. Vw is observation white noise.

3260

F. Pei et al. /Journal of Computational Information Systems 10: 8 (2014) 32553264

H Filter Design for Fine Alignment

For the strapdown INS error equation described as Eq. (28), the interference of acceleration is
introduced. VDi is uncertainty observation noises, consists of disturbing velocity caused by the
ships pitch and roll. The Kalman lter is a common way for linear systems when the noise is
subject to Gaussian distribution. However, when it comes to random disturbances, the Kalman
lter becomes not so eective. Because of the uncertainty observation noise, the H robust lter
is designed in the ne alignment procedure. The error model which we discussed above can be
considered as the following state-space system:
XK = K1 XK1 + K1 WK1
(30)

YK = HK XK + VK
ZK = LK XK

LK makes the ratio of estimation error signal energy and disturbance energy less than the
pre-specied number [14]. And ltering error is dened as:
ek = ZK LK XK

(31)

For a xed positive number > 0, the job is nding H suboptimal estimation ZK = Ff (y0 , y1 ,
. . . , yk ) to make ||Tk (Ff )|| < . It can be shown as:
inf

sup

Ff X0 ,W h2 ,V h2

||ek ||22
0 ||2 1 + ||Wk ||22 + ||Vk ||22
||X0 X
P

< 2

(32)

The mathematical equations of H ltering is as follows:


K\K
SK\K = LK X
K+1\K+1 = K+1\K X
K\K + KK+1 (ZK+1 HK+1 K+1\K X
K\K )
X
T
T
KK+1 = PK+1 HK+1
(HK+1 PK+1 HK+1
+ RK+1 )1

PK+1 = K+1\K PK TK+1\K + K TK


[
]
[
]
H
K
1
T
PK TK+1\K
K+1\K PK HK
LTK Re,k
L
[
] [
] K
[
]
RK
0
HK
T
T
Re,k =
+
P
HK LK
K
0 2 I
LK

(33)

By comparing, it can be seen that the structure of Kalman lter and H lter has a lot in
common. In fact, when , H lter will degenerate into Kalman lter. That is to say, if
the norm index is removed, H lter is equivalent to Kalman lter. Therefore in the design of
H lter, is set as small as possible, under the condition that the Riccati equation solution P
is denite positive.

F. Pei et al. /Journal of Computational Information Systems 10: 8 (2014) 32553264

3261

Simulation Results

In this section, the simulation was carried out to test the coarse alignment and the ne alignment
used in our proposed method. The simulation conditions are set as follows: the gyro constant
drift: 0.01/h, the gyro random noise: 0.001/h, the accelerator bias: 1104 g, the accelerator
measurement noise: 1105 g. In these simulations, the ship is assumed to be on the berth and
is rocked by the surf. The roll , pitch and heading , resulting from the ships rocking are
described as follows:
= 30 +5 cos(

2
2
2
t+ ), = 7 cos( t+ ), = 10 cos( t+ )
7
3
5
4
6
7

The velocity caused by heave, surge and sway is as follows:


VDi = ADi Di cos(Di t + Di ).
Where i = x, y, z, ADx = 0.02m, ADy = 0.03m, ADz = 0.3m, Di =
TDz = 8s. Di obeys the uniform distribution on the interval [0, 2].

2
,
TDi

TDx = 7s, TDy = 6s,

Simulation 5.1
The coarse alignment lasts for 120 seconds. The values of tk1 and tk2 in (9) are set to the
50 second and the 120 second respectively. The simulation for the coarse alignment runs for 50
times. The statistics for coarse alignment results are listed in Table 1.
Table 1: Statistics for coarse alignment results
Parameter item(deg)

Max.

Min.

Mean

Pitch error

0.3012

-0.6135

0.0054

Roll error

0.1046

-0.0687

0.0322

Yaw error

1.6502

-0.2894

0.3973

From Table 1, it is clear that the level attitude errors of the coarse alignment are less than
0.3 degrees and the yaw error is less than 1.7 degrees. The coarse alignment guarantees the
subsequent ne alignment. But it only accomplished the coarse estimation of attitude matrix.
So 600 seconds ne alignment was followed. Next, the mean and the maximum of misalignment
angles in Table 1 are used respectively as input for ne alignment to validate the method. The
mean of misalignment angles are used in simulation 5.2 and 5.3, whereas maximum misalignment
angles are used in simulation 5.4.
Simulation 5.2
Consider the measurement noises as white noise or random disturbances respectively, comparisons are made on the H lter and the traditional Kalman lter for the ne alignment. The
results of simulation under white noise condition are shown in Fig. 1. The simulation results show
that the accuracy of H lter is a 1ittle lower than that of Kalman lter under white noise. And
the Kalman lter has better speed of convergence than that of H lter. It can be concluded
from the comparison that Kalman lter is good in dealing system with white noise like static
base. But taking into account the unknown outside interference and the complicated working
environment, the strength of H lter becomes apparent.

3262

F. Pei et al. /Journal of Computational Information Systems 10: 8 (2014) 32553264

Fig. 1: The Results of Kalman lter and H lter with white noise

Fig. 2: The Results of Kalman lter and H lter with mean misalignment angles
Simulation 5.3
When it comes to the random disturbances, the results in Fig. 2 show that the horizon misalignment angles can still be accurately estimated with Kalman lter under random disturbances,
but the speed and accuracy of azimuth angle are all worse. While H lter can still result in a

F. Pei et al. /Journal of Computational Information Systems 10: 8 (2014) 32553264

3263

quick response and a relatively equal accuracy as white noise. According to Fig. 2, convergence
time of level misalignment angle is within 30s, precision for 13 , while azimuth angle convergence
time is 60s, precision about 4 . Azimuth misalignment angle even was emanated using Kalman
lter. In a sense it could be said that H lter is a robust lter and has a better performance
than Kalman lter. Moreover, since the practical noises are usually uncertain, H lter can be
applied more widely than Kalman lter, proved to an available method for initial alignment.
The simulation results in Fig. 1 and Fig. 2 show that the accuracy of H lter is a little
lower than that of Kalman lter under white noise. The advantages of H lter mainly turn out
under random disturbances condition. Next the simulation 5.4 with the maximum initial gures
is made.
Simulation 5.4
The simulation results show that the H lter is ecient with large initial attitude error. The
results are similar to Fig. 2. Compared with the traditional Kalman lter, the H lter can eectively depress the random disturbances in the velocity measurements caused by the ship rocking.
Using H lter can improve system sensitivity to noise and the divergence speed of azimuth.
Taking into account the unknown outside interference and the complicated working environment,
H ltering with more robustness should be applied to initial alignment of strapdown INS.

Fig. 3: The Results of Kalman lter and H lter with maximum misalignment angles

Conclusions

In this paper the gravity in the inertial frame as a reference is used, so as to counteract the
disturbed components. With the deduced strapdown INS model on disturbances base, the relevant
H lter and Kalman lter are designed, and the performance of the lter is compared. The

3264

F. Pei et al. /Journal of Computational Information Systems 10: 8 (2014) 32553264

simulation results show that: 1) the attitude determined by this novel method can meet the
accuracy requirement of coarse alignment and it guarantees the subsequent ne alignment. 2)
compared with the traditional Kalman ltering, the H ltering can eectively deal with the
random lineal and angular disturbances in the velocity measurements caused by the ship rocking.

Acknowledgement
This work was supported by the National Science Foundation of China under Grants 60975065,
and Supported by Program for the Top Young Innovative Talents of Beijing J2002013201301.

References
[1]

Qin Y. Y. Inertial navigation [M]. China: Science Press, 2006.

[2]

Hao Y.L., Mu H.W. Application of MPF-EKF in SINS Initial Alignment with Large Azimuth
Misalignment Angle. Journal of Computational Information Systems, 2012, 8(23): 9705-9712.

[3]

Error analysis of analytic coarse alignment methods. IEEE Transactions on Aerospace and Electronic Systems, 1998, 34(1): 334-337.

[4]

Schimelevich L. and Naor R. New approach to coarse alignment. Position Location and Navigation
Symposium, April 22-26, Atlanta, USA, 1996, pp: 324-327.

[5]

Qin Y. Y, Yan G M, Gu D. Q, Zheng J B. A clever way of SINS coarse alignment despite rocking
ship. Journal of Northwestern Poly-technical University, 2005, 23(5): 681-684.

[6]

Yan G. M. On SINS in-movement initial alignment and some other problems. Ph. D. Thesis,
Northwestern Poly-technical University, China, 2008.

[7]

Gao W., Ben Y., Zhang X., Li Q., Yu F. Rapid ne strapdown INS alignment method under
marine mooring condition. IEEE Transactions on Aerospace and Electronic Systems, 2011, 47(4):
2887-2896.

[8]

Feng S., Yuan X. H Filtering and its application in INS ground alignment. Journal of Nanjing
University of Aeronautics and Astronautics, 1998, 30(4): 383-387.

[9]

Nie L., Wu J., Tian W. H ltering and its application in INS alignment. Journal of China
Inertial Technology, 2003, 11(6): 39-43.

[10] Zhu L., Bao Q., Zhang Y. Comparison on Kalman lter and H lter for initial alignment of
SINS. Journal of China Inertial Technology, 2005, 13(3): 4-9.
[11] Ali J., Ushaq M. A consistent and robust Kalman lter design for in-motion alignment of inertial
navigation system. Measurement, 2009, 42: 577-582.
[12] Lv S., Xie L., Chen J., Yao X. Parameter optimization of H lter in initial alignment of SINS
alignment. Fire Control & Command Control, 2010, 35(6): 67-69.
[13] D. Q. Gu and Naser El-Sheimy, etc. Coarse Alignment for Marine SINS Using Gravity in the
Inertial Frame as a Reference [C]. Position Location and Navigation Symposium. 2008: 961-965.
[14] Huang D. N., Zhang X. T., Liu J. B. SINS initial alignment method using Robust ltering for
Towed Ocean Bottom Magnetometer. Oceans, September 15-18, Quebec, Canada, 2008, pp: 1-6.