# Parameter Identification Method for SINS Initial Alignment under Inertial Frame

Authors: Haijian Xue; Xiaosong Guo; Zhaofa Zhou
Journal: Mathematical Problems in Engineering
 (2016)

---

## Abstract

The performance of a strapdown inertial navigation system (SINS) largely depends on the accuracy and rapidness of the initial alignment. The conventional alignment method with parameter identification has been already applied widely, but it needs to calculate the gyroscope drifts through two-position method; then the time of initial alignment is greatly prolonged. For this issue, a novel self-alignment algorithm by parameter identification method under inertial frame for SINS is proposed in this paper. Firstly, this coarse alignment method using the gravity in the inertial frame as a reference is discussed to overcome the limit of dynamic disturbance on a rocking base and fulfill the requirement for the fine alignment. Secondly, the fine alignment method by parameter identification under inertial frame is formulated. The theoretical analysis results show that the fine alignment model is fully self-aligned with no external reference information and the gyrodrifts can be estimated in real time. The simulation results demonstrate that the proposed method can achieve rapid and highly accurate initial alignment for SINS.


---

## Body

## 1. Introduction

Strapdown inertial navigation system (SINS) has been widely used in aviation, marine, and land vehicle navigation and positioning because of its special advantages, and it necessitates an alignment stage to determine the initial conditions prior to navigation operation [1]. Compared with the initial attitude, initial velocity and position can be easily acquired with reference navigation system, such as Global Position System (GPS) [2]. Thus, the accuracy and stability of initial alignment are critical for high performance SINS [3, 4], and the purpose of initial alignment is to obtain an initial attitude matrix from body frame to navigation frame and set the misalignments to zero. The traditional initial alignment is often divided into two procedures: coarse alignment and fine alignment [5–7]. The coarse alignment is used to resolve large misalignment angle rapidly, and then the fine alignment is used to compensate and correct the misalignment angle further [8].A lot of literature has been devoted to coarse alignment methods. The conventional coarse alignment is based on analytical method which generally uses two feature vectors of the earth: the acceleration of gravity and the angular rate of the earth’s rotation, and it asks for the base in a static case [9, 10]. However, in many cases, the process of the alignment is affected by various interference factors of the base, such as engine vibration, crew motion, and strong flurry. Because the intensity of the interference signal is obviously larger than the angular rate of the earth’s rotation, the earth rotation rate is completely submerged in measurement noise. So the conventional coarse alignment method cannot be utilized. In order to solve the alignment problem on a rocking base, the method is to use the inertial frame as a transitional coordinate to realize the initial alignment by using the gravity acceleration information [11–14].In the fine alignment procedure, gyrocompass alignment, Kalman filter, and parameter identification method are the mostly used techniques to deal with alignment problems in SINS [15–17]. Gyrocompass alignment based on compass effect is an important fine alignment method with the properties of interference suppression and lower computational complexity [18, 19], but it needs to set longer damping time constant and always costs much more time to finish the alignment process [20, 21]. Kalman filter is an optimal linear estimator when measurement noise has a Gaussian distribution with known measurement and process noise variance values [8, 22–25]. However, it is impractical that measurement noise has a Gaussian distribution with known measurement and process noise variance values, and it is very sensitive to the rocking interference and has the disadvantage of large amount of calculation [20]. Parameter identification method based on recursive least square method (RLS) is a very useful fine alignment method, which does not need to know the distribution of measurement noise and has the characteristics of small computational amount [17, 20, 26]. However, all of them need to calculate the gyroscope drifts through two-position method; then the time of initial alignment is greatly prolonged.Inspired by the idea in [17], a novel self-alignment algorithm by parameter identification method under inertial frame for SINS is proposed in this paper, and simulation test verifies the effectiveness of the proposed method. Firstly, similar to [8, 27], utilizing the gravity in the inertial frame as a reference, the coarse alignment method is discussed to overcome the limit of dynamic disturbance on a rocking base. Secondly, the fine alignment model by parameter identification method in the inertial frame is developed, and the theoretical analysis results show that the proposed fine alignment model is fully self-aligned with no external reference information and the gyrodrifts can be estimated in real time.The rest of this paper is organized as follows. Section2 introduces the fundamental principles of coarse alignment method using the gravity information in the inertial frame. The fine alignment method by parameter identification under inertial frame is formulated in Section 3. In Section 4, the validity of the proposed method is further verified by simulation. Finally, some conclusions are made in Section 5.


## 2. Coarse Alignment Method by Using the Gravity Information in the Inertial Frame

### 2.1. Coordinate Frame Definitions

In order to better understand SINS initial alignment, it is necessary to explain the navigation coordinate system, that is, the earth frame (e frame), the inertial frame (i frame), the computed inertial frame (i
′ frame), the navigation frame (n frame), and the body frame (b frame), together with the body inertial frame (i
b
0 frame), which will be introduced in the sequel, and the relationship among the various frames is denoted in Figure 1, where ω
i
e is the angular rate of the earth’s rotation, g is the local gravity acceleration, and t is the time for alignment. And the various frames are defined in detail as follows.Figure 1
The relationship among the various frames.(i)
e
Frame. The earth’s core is the origin and x
e axis points to the intersection of the prime meridian and the equator. z
e axis goes upward along earth polar axis. x
e, y
e, and z
e form a right-hand coordinate frame. e frame moves with the earth in angular rate ω
i
e.(ii)
i
Frame. It is the ideal inertial coordinate frame; at the starting time for the initial alignment, i frame coincides with e frame and is fixed in the inertial space.(iii)
i
′
Frame. It is the computed inertial coordinate frame where there is some error between i frame and i
′ frame owing to the alignment error and the sensor error.(iv)
n
Frame. The origin is the centroid of the carrier. z
n axis goes upward along the local geodetic vertical and y
n axis north (and horizontal) with x
n axis east (and horizontal).(v)
b
Frame. The origin is the centroid of the carrier. x
b axis shifts rightward along the carrier’s transverse axis, y
b forward along the longitudinal axis, and z
b upward along the vertical axis.(vi)
i
b
0
Frame. At the starting time for the initial alignment, i
b
0 frame coincides with b frame and is fixed in the inertial space.


### 2.2. The Coarse Alignment Algorithm in the Inertial Frame

The basic idea using the gravity acceleration information in the inertial frame for the initial alignment is to decomposeC
b
n
(
t
) into several attitude matrices, whose references are all based on the inertial frame [8, 12, 27]. The decomposition method in this paper is as follows:(1)
C
b
n
t
=
C
i
n
t
C
i
b
0
i
C
b
i
b
0
t
.It shows that time-varying matrixC
b
n
(
t
) is composed of three parts. One part is C
i
n
(
t
), which is the transformation matrix between n frame (geographic coordinate frame) and i frame. And it is produced by the earth rotation rate:(2)
C
i
n
t
=
C
e
n
C
i
e
t
=
0
1
0
-
sin
⁡
L
0
cos
⁡
L
cos
⁡
L
0
sin
⁡
L
cos
⁡
ω
i
e
t
-
t
0
sin
⁡
ω
i
e
t
-
t
0
0
-
sin
⁡
ω
i
e
t
-
t
0
cos
⁡
ω
i
e
t
-
t
0
0
0
0
1
=
-
sin
⁡
ω
i
e
t
-
t
0
cos
⁡
ω
i
e
t
-
t
0
0
-
sin
⁡
L
cos
⁡
ω
i
e
t
-
t
0
-
sin
⁡
L
sin
⁡
ω
i
e
t
-
t
0
cos
⁡
L
cos
⁡
L
cos
⁡
ω
i
e
t
-
t
0
cos
⁡
L
sin
⁡
ω
i
e
t
-
t
0
sin
⁡
L
,where t
0 is the starting time for the alignment, t is the time for alignment, and L is the latitude.Another part isC
b
i
b
0
(
t
), which is the transformation matrix between i
b
0 frame and b frame. It can be obtained by the output signal of the gyroscopes at any time:(3)
C
˙
b
i
b
0
t
=
C
b
i
b
0
t
ω
i
b
0
b
b
×
,where C
b
i
b
0
(
t
0
)
=
I and (
ω
i
b
0
b
b
×
) denotes the skew-symmetric matrix of ω
i
b
0
b
b which is measured by the gyroscopes in b frame.Therefore, the mission of the coarse alignment is transformed into the estimation problem ofC
i
b
0
i which is the transformation matrix between i frame and i
b
0 frame, and it is a constant value. Owning to(4)
f
i
b
0
=
C
b
i
b
0
f
b
,
g
i
=
C
n
i
g
n
,where f
b denotes the specific force measured by accelerometers in b frame, g
n
=
0
0
-
g
T is the gravity vector in n frame.Ignoring the influence of the installation error, accelerometer drift, and the disturbing acceleration, the relationship ofg
i and f
i
b
0 is(5)
g
i
=
C
i
b
0
i
f
i
b
0
.Selecting different timest
1 and t
2, C
i
b
0
i can be presented as(6)
C
^
i
b
0
i
=
g
i
t
1
T
g
i
t
2
T
g
i
t
1
×
g
i
t
2
T
-
1
f
i
b
0
t
1
T
f
i
b
0
t
2
T
f
i
b
0
t
1
×
f
i
b
0
t
2
T
,where hat “∧” denotes the calculated value. To improve the measurement accuracy on a rocking base, it can be integrated from (5) that(7)
V
i
t
=
C
i
b
0
i
V
i
b
0
t
,where V
i
(
t
) and V
i
b
0
(
t
) are already expressed in [13] as(8)
V
i
t
=
∫
t
0
t
g
i
d
t
=
∫
t
0
t
C
n
i
g
n
d
t
=
g
cos
⁡
L
sin
⁡
ω
i
e
t
-
t
0
ω
i
e
-
g
cos
⁡
L
cos
⁡
ω
i
e
t
-
t
0
-
1
ω
i
e
t
-
t
0
g
sin
⁡
L
,
(9)
V
i
b
0
t
=
∫
t
0
t
f
i
b
0
d
t
=
∫
t
0
t
C
b
t
i
b
0
f
b
d
t
=
∑
k
=
0
N
-
1
C
b
t
k
i
b
0
∫
t
k
t
k
+
1
C
b
t
b
t
k
f
b
d
t
≈
∑
k
=
0
N
-
1
C
b
t
k
i
b
0
∫
t
k
t
k
+
1
I
+
∫
t
k
t
ω
i
b
0
b
b
×
d
τ
f
b
d
t
=
∑
k
=
0
N
-
1
C
b
t
k
i
b
0
Δ
V
1
+
Δ
V
2
+
1
2
Δ
θ
1
+
Δ
θ
2
×
Δ
V
1
+
Δ
V
2
+
2
3
Δ
θ
1
×
Δ
V
2
+
Δ
V
1
×
Δ
θ
2
,where Δ
V
1 and Δ
V
2 are the first and second samples of the accelerometer-measured incremental velocity and Δ
θ
1 and Δ
θ
2 are the first and second samples of the gyroscope-measured incremental angle, respectively, so that(10)
C
^
i
b
0
i
=
V
i
t
1
T
V
i
t
2
T
V
i
t
1
×
V
i
t
2
T
-
1
V
i
b
0
t
1
T
V
i
b
0
t
2
T
V
i
b
0
t
1
×
V
i
b
0
t
2
T
.This is the practical equation for the coarse alignment.


## 2.1. Coordinate Frame Definitions

In order to better understand SINS initial alignment, it is necessary to explain the navigation coordinate system, that is, the earth frame (e frame), the inertial frame (i frame), the computed inertial frame (i
′ frame), the navigation frame (n frame), and the body frame (b frame), together with the body inertial frame (i
b
0 frame), which will be introduced in the sequel, and the relationship among the various frames is denoted in Figure 1, where ω
i
e is the angular rate of the earth’s rotation, g is the local gravity acceleration, and t is the time for alignment. And the various frames are defined in detail as follows.Figure 1
The relationship among the various frames.(i)
e
Frame. The earth’s core is the origin and x
e axis points to the intersection of the prime meridian and the equator. z
e axis goes upward along earth polar axis. x
e, y
e, and z
e form a right-hand coordinate frame. e frame moves with the earth in angular rate ω
i
e.(ii)
i
Frame. It is the ideal inertial coordinate frame; at the starting time for the initial alignment, i frame coincides with e frame and is fixed in the inertial space.(iii)
i
′
Frame. It is the computed inertial coordinate frame where there is some error between i frame and i
′ frame owing to the alignment error and the sensor error.(iv)
n
Frame. The origin is the centroid of the carrier. z
n axis goes upward along the local geodetic vertical and y
n axis north (and horizontal) with x
n axis east (and horizontal).(v)
b
Frame. The origin is the centroid of the carrier. x
b axis shifts rightward along the carrier’s transverse axis, y
b forward along the longitudinal axis, and z
b upward along the vertical axis.(vi)
i
b
0
Frame. At the starting time for the initial alignment, i
b
0 frame coincides with b frame and is fixed in the inertial space.


## 2.2. The Coarse Alignment Algorithm in the Inertial Frame

The basic idea using the gravity acceleration information in the inertial frame for the initial alignment is to decomposeC
b
n
(
t
) into several attitude matrices, whose references are all based on the inertial frame [8, 12, 27]. The decomposition method in this paper is as follows:(1)
C
b
n
t
=
C
i
n
t
C
i
b
0
i
C
b
i
b
0
t
.It shows that time-varying matrixC
b
n
(
t
) is composed of three parts. One part is C
i
n
(
t
), which is the transformation matrix between n frame (geographic coordinate frame) and i frame. And it is produced by the earth rotation rate:(2)
C
i
n
t
=
C
e
n
C
i
e
t
=
0
1
0
-
sin
⁡
L
0
cos
⁡
L
cos
⁡
L
0
sin
⁡
L
cos
⁡
ω
i
e
t
-
t
0
sin
⁡
ω
i
e
t
-
t
0
0
-
sin
⁡
ω
i
e
t
-
t
0
cos
⁡
ω
i
e
t
-
t
0
0
0
0
1
=
-
sin
⁡
ω
i
e
t
-
t
0
cos
⁡
ω
i
e
t
-
t
0
0
-
sin
⁡
L
cos
⁡
ω
i
e
t
-
t
0
-
sin
⁡
L
sin
⁡
ω
i
e
t
-
t
0
cos
⁡
L
cos
⁡
L
cos
⁡
ω
i
e
t
-
t
0
cos
⁡
L
sin
⁡
ω
i
e
t
-
t
0
sin
⁡
L
,where t
0 is the starting time for the alignment, t is the time for alignment, and L is the latitude.Another part isC
b
i
b
0
(
t
), which is the transformation matrix between i
b
0 frame and b frame. It can be obtained by the output signal of the gyroscopes at any time:(3)
C
˙
b
i
b
0
t
=
C
b
i
b
0
t
ω
i
b
0
b
b
×
,where C
b
i
b
0
(
t
0
)
=
I and (
ω
i
b
0
b
b
×
) denotes the skew-symmetric matrix of ω
i
b
0
b
b which is measured by the gyroscopes in b frame.Therefore, the mission of the coarse alignment is transformed into the estimation problem ofC
i
b
0
i which is the transformation matrix between i frame and i
b
0 frame, and it is a constant value. Owning to(4)
f
i
b
0
=
C
b
i
b
0
f
b
,
g
i
=
C
n
i
g
n
,where f
b denotes the specific force measured by accelerometers in b frame, g
n
=
0
0
-
g
T is the gravity vector in n frame.Ignoring the influence of the installation error, accelerometer drift, and the disturbing acceleration, the relationship ofg
i and f
i
b
0 is(5)
g
i
=
C
i
b
0
i
f
i
b
0
.Selecting different timest
1 and t
2, C
i
b
0
i can be presented as(6)
C
^
i
b
0
i
=
g
i
t
1
T
g
i
t
2
T
g
i
t
1
×
g
i
t
2
T
-
1
f
i
b
0
t
1
T
f
i
b
0
t
2
T
f
i
b
0
t
1
×
f
i
b
0
t
2
T
,where hat “∧” denotes the calculated value. To improve the measurement accuracy on a rocking base, it can be integrated from (5) that(7)
V
i
t
=
C
i
b
0
i
V
i
b
0
t
,where V
i
(
t
) and V
i
b
0
(
t
) are already expressed in [13] as(8)
V
i
t
=
∫
t
0
t
g
i
d
t
=
∫
t
0
t
C
n
i
g
n
d
t
=
g
cos
⁡
L
sin
⁡
ω
i
e
t
-
t
0
ω
i
e
-
g
cos
⁡
L
cos
⁡
ω
i
e
t
-
t
0
-
1
ω
i
e
t
-
t
0
g
sin
⁡
L
,
(9)
V
i
b
0
t
=
∫
t
0
t
f
i
b
0
d
t
=
∫
t
0
t
C
b
t
i
b
0
f
b
d
t
=
∑
k
=
0
N
-
1
C
b
t
k
i
b
0
∫
t
k
t
k
+
1
C
b
t
b
t
k
f
b
d
t
≈
∑
k
=
0
N
-
1
C
b
t
k
i
b
0
∫
t
k
t
k
+
1
I
+
∫
t
k
t
ω
i
b
0
b
b
×
d
τ
f
b
d
t
=
∑
k
=
0
N
-
1
C
b
t
k
i
b
0
Δ
V
1
+
Δ
V
2
+
1
2
Δ
θ
1
+
Δ
θ
2
×
Δ
V
1
+
Δ
V
2
+
2
3
Δ
θ
1
×
Δ
V
2
+
Δ
V
1
×
Δ
θ
2
,where Δ
V
1 and Δ
V
2 are the first and second samples of the accelerometer-measured incremental velocity and Δ
θ
1 and Δ
θ
2 are the first and second samples of the gyroscope-measured incremental angle, respectively, so that(10)
C
^
i
b
0
i
=
V
i
t
1
T
V
i
t
2
T
V
i
t
1
×
V
i
t
2
T
-
1
V
i
b
0
t
1
T
V
i
b
0
t
2
T
V
i
b
0
t
1
×
V
i
b
0
t
2
T
.This is the practical equation for the coarse alignment.


## 3. Fine Alignment by Parameter Identification under Inertial frame

The coarse initial attitude angle can be obtained by the coarse alignment method in Section2. The carrier heading angle error is estimated to be within a few degrees and pitch/roll angle to be within a few tenths of a degree. In this section, the fine alignment by parameter identification in inertial frame is developed.


### 3.1. Analysis of the Attitude Misalignment Angles in the Inertial Frame

Note that there is a big calculation error between estimation valueC
^
i
b
0
i and true value C
i
b
0
i, and C
^
b
i
b
0 calculated by the outputs of gyroscopes also exists error. Therefore, we have(11)
C
b
n
=
C
i
n
C
i
′
i
C
b
i
′
,where C
b
i
′
=
C
^
i
b
0
i
C
^
b
i
b
0, and C
i
′
i which is the transformation matrix from ideal inertial coordinate frame (i frame) to erroneously computed inertial coordinate frame (i
′ frame) can be formulated as(12)
C
i
i
′
=
I
-
Φ
i
×
=
1
ϕ
c
-
ϕ
b
-
ϕ
c
1
ϕ
a
ϕ
b
-
ϕ
a
1
,where Φ
i
=
ϕ
a
ϕ
b
ϕ
c
T is the misalignment angle vector in i frame. The subscripts a, b, c are the three axes of i frame, respectively. And the task of fine alignment is to estimate misalignment angle Φ
i.In ideal conditions, the differential equation ofC
b
i is(13)
C
˙
b
i
=
C
b
i
ω
i
b
b
×
,where C
b
i is the transformation matrix between i frame and b frame. ω
i
b
b denotes the angular rate of b frame with respect to i frame, expressed in b frame.In practice, the differential equation ofC
b
i
′ is(14)
C
˙
b
i
′
=
C
b
i
′
ω
^
i
b
b
×
,where C
b
i
′ is the transformation matrix between i
′ frame and b frame. ω
^
i
b
b measured by gyroscopes in b frame is the computed value of ω
i
b
b. And ω
^
i
b
b can be formulated as(15)
ω
^
i
b
b
=
ω
i
b
b
+
δ
ω
i
b
b
,where δ
ω
i
b
b is the gyroscope error vector in b frame.DifferentiatingC
i
i
′
=
C
b
i
′
C
i
b on both sides yields(16)
C
˙
i
i
′
=
C
˙
b
i
′
C
i
b
+
C
b
i
′
C
˙
i
b
=
C
˙
b
i
′
C
i
b
+
C
i
i
′
C
b
i
C
˙
i
b
.Owing toC
i
b
C
b
i
=
I and C
b
i
C
˙
i
b
=
-
C
˙
b
i
C
i
b, substituting them into (5) and multiplying C
b
i from right on each part yield(17)
C
˙
i
i
′
C
b
i
=
C
˙
b
i
′
-
C
i
i
′
C
˙
b
i
.Substituting (13), (14), and (15) into (17), we have(18)
C
˙
i
i
′
C
b
i
=
C
b
i
′
ω
^
i
b
b
×
-
C
i
i
′
C
b
i
ω
i
b
b
×
=
C
i
i
′
C
b
i
ω
^
i
b
b
×
-
ω
i
b
b
×
=
C
i
i
′
C
b
i
δ
ω
i
b
b
×
.That is,(19)
C
˙
i
i
′
=
C
i
i
′
C
b
i
δ
ω
i
b
b
×
C
i
b
=
C
i
i
′
δ
ω
i
b
b
×
.Substituting (12) into (19) and ignoring the second-order small amount, we have(20)
Φ
˙
i
=
-
δ
ω
i
b
i
=
-
C
b
i
δ
ω
i
b
b
.Ignoring the influence of the installation error and scale factor error, (20) can be rewritten as(21)
Φ
˙
i
=
-
C
b
i
ε
b
=
-
C
n
i
C
b
n
ε
b
,where ε
b is the bias of gyroscope in b frame, and it can be approximately regarded as a random constant vector in the process of alignment. And this is the attitude error equation in i frame.When the carrier shakes modestly,C
b
n can be nearly a constant matrix, so C
b
n
ε
b can be approximately regarded as a random constant vector named as gyroscope’s equivalent constant drift in n frame; that is,(22)
C
b
n
ε
b
=
ε
n
=
ε
E
ε
N
ε
U
T
,where subscripts E, U, N are the three axes of n frame, respectively.The alignment process is usually performed in a few minutes (not more than ten minutes), soω
i
e
t is a very small amount. Then, c
o
s
(
ω
i
e
t
) and s
i
n
(
ω
i
e
t
) can be approximately rewritten as (23)
cos
⁡
ω
i
e
t
≈
1
,
sin
⁡
ω
i
e
t
≈
ω
i
e
t
.To this respect, (23) can be rewritten as(24)
ϕ
˙
a
ϕ
˙
b
ϕ
˙
c
=
-
C
n
i
C
b
n
ε
b
=
-
C
n
i
ε
n
=
C
n
i
ε
E
ε
N
ε
U
=
-
-
sin
⁡
ω
i
e
t
-
sin
⁡
L
cos
⁡
ω
i
e
t
cos
⁡
L
cos
⁡
ω
i
e
t
cos
⁡
ω
i
e
t
-
sin
⁡
L
sin
⁡
ω
i
e
t
cos
⁡
L
sin
⁡
ω
i
e
t
0
cos
⁡
L
sin
⁡
L
ε
E
ε
N
ε
U
≈
-
-
ω
i
e
t
-
sin
⁡
L
cos
⁡
L
1
-
ω
i
e
t
sin
⁡
L
ω
i
e
t
cos
⁡
L
0
cos
⁡
L
sin
⁡
L
ε
E
ε
N
ε
U
=
ε
E
ω
i
e
t
+
ε
N
sin
⁡
L
-
ε
U
cos
⁡
L
-
ε
E
+
ε
N
ω
i
e
t
sin
⁡
L
-
ε
U
ω
i
e
t
cos
⁡
L
-
ε
N
cos
⁡
L
-
ε
U
sin
⁡
L
.Integrating both sides of (24), the misalignment angles changing with the time can be formulated as follows:(25)
ϕ
a
=
ϕ
a
0
+
ε
N
sin
⁡
L
-
ε
U
cos
⁡
L
t
+
t
2
2
ε
E
ω
i
e
,
ϕ
b
=
ϕ
b
0
-
ε
E
t
+
t
2
2
ω
i
e
ε
N
sin
⁡
L
-
ε
U
cos
⁡
L
,
ϕ
c
=
ϕ
c
0
-
ε
N
cos
⁡
L
+
ε
U
sin
⁡
L
t
,where Φ
0
i
=
ϕ
a
0
ϕ
b
0
ϕ
c
0
T is the initial value vector of misalignment angle in i frame.It is easy to see thatϕ
i
0
(
i
=
a
,
b
,
c
) and ε
j
(
j
=
E
,
N
,
U
) are the constant valves; based on them, we can calculate the misalignment angles in real time. In this respect, the fine alignment problem has been transformed into the parameter identification problem for ϕ
i
0 and ε
j.


### 3.2. Estimation of Attitude Misalignment Angles by Parameter Identification Method

On a rocking base, the measured specific force projected from accelerometer ini
′ frame can be written as(26)
f
i
′
=
C
b
i
′
f
^
b
=
C
i
i
′
C
b
i
f
^
b
=
C
i
i
′
C
b
i
-
g
b
+
f
d
b
+
∇
b
=
C
i
i
′
-
g
i
+
f
d
i
+
∇
i
=
1
ϕ
c
-
ϕ
b
-
ϕ
c
1
ϕ
a
ϕ
b
-
ϕ
a
1
-
g
a
+
f
d
a
+
∇
a
-
g
b
+
f
d
b
+
∇
b
-
g
c
+
f
d
c
+
∇
c
.Ignoring the second-order small amount yields(27)
f
a
i
′
=
-
g
b
ϕ
c
+
g
c
ϕ
b
-
g
a
+
f
d
a
+
∇
a
,
f
b
i
′
=
g
a
ϕ
c
-
g
c
ϕ
a
-
g
b
+
f
d
b
+
∇
b
,
f
c
i
′
=
-
g
a
ϕ
b
+
g
b
ϕ
a
-
g
c
+
f
d
c
+
∇
c
,where ∇
b is the accelerometer bias vector in b frame. f
d
b is uncertainty measurement disturbance caused by the carrier’s rocking and swaying in b frame. g
a, g
b, g
c are the three components of the gravity vector in i frame, respectively. ∇
a, ∇
b, ∇
c are the three components of the accelerometer bias in i frame, respectively. According to the time for alignment, g
a
g
b
g
c
T and ∇
a
∇
b
∇
c
T can be calculated, respectively, as follows:(28)
g
a
g
b
g
c
=
C
n
i
g
n
=
-
sin
⁡
ω
i
e
t
-
sin
⁡
L
cos
⁡
ω
i
e
t
cos
⁡
L
cos
⁡
ω
i
e
t
cos
⁡
ω
i
e
t
-
sin
⁡
L
sin
⁡
ω
i
e
t
cos
⁡
L
sin
⁡
ω
i
e
t
0
cos
⁡
L
sin
⁡
L
0
0
-
g
=
-
g
cos
⁡
L
cos
⁡
ω
i
e
t
-
g
cos
⁡
L
sin
⁡
ω
i
e
t
-
g
sin
⁡
L
,
(29)
∇
a
∇
b
∇
c
=
C
n
i
C
b
n
∇
x
∇
y
∇
z
=
C
n
i
∇
E
∇
N
∇
U
=
-
∇
E
sin
⁡
ω
i
e
t
-
∇
N
sin
⁡
L
cos
⁡
ω
i
e
t
+
∇
U
cos
⁡
L
cos
⁡
ω
i
e
t
∇
E
cos
⁡
ω
i
e
t
-
∇
N
sin
⁡
L
sin
⁡
ω
i
e
t
+
∇
U
cos
⁡
L
sin
⁡
ω
i
e
t
∇
N
cos
⁡
L
+
∇
U
sin
⁡
L
,where ∇
E, ∇
N, ∇
U are the three components of the accelerometer equivalent bias in n frame, respectively. Because ω
i
e
t is a small amount, substituting (22) into (28) and (29) yields(30)
g
a
g
b
g
c
=
-
g
cos
⁡
L
-
g
ω
i
e
t
cos
⁡
L
-
g
sin
⁡
L
,
∇
a
∇
b
∇
c
=
-
∇
E
ω
i
e
t
-
∇
N
sin
⁡
L
+
∇
U
cos
⁡
L
∇
E
-
∇
N
ω
i
e
t
sin
⁡
L
+
∇
U
ω
i
e
t
cos
⁡
L
∇
N
cos
⁡
L
+
∇
U
sin
⁡
L
.Substituting (30) into (27), we have(31)
f
a
i
′
=
t
ϕ
c
g
ω
i
e
cos
⁡
L
-
ϕ
b
g
sin
⁡
L
+
g
cos
⁡
L
+
f
d
a
-
∇
E
ω
i
e
t
-
∇
N
sin
⁡
L
+
∇
U
cos
⁡
L
,
f
b
i
′
=
-
ϕ
c
g
cos
⁡
L
+
ϕ
a
g
sin
⁡
L
+
g
ω
i
e
t
cos
⁡
L
+
f
d
b
+
∇
E
-
∇
N
ω
i
e
t
sin
⁡
L
+
∇
U
ω
i
e
t
cos
⁡
L
,
f
c
i
′
=
ϕ
b
g
cos
⁡
L
-
ϕ
a
g
ω
i
e
t
cos
⁡
L
+
g
sin
⁡
L
+
f
d
c
+
∇
N
cos
⁡
L
+
∇
U
sin
⁡
L
.Substituting (24) into (31), we have(32)
f
a
i
′
=
-
t
2
2
g
ω
i
e
ε
N
+
ε
N
cos
2
⁡
L
+
ε
U
sin
⁡
L
cos
⁡
L
+
t
ϕ
c
0
g
ω
i
e
cos
⁡
L
+
ε
E
g
sin
⁡
L
-
∇
E
ω
i
e
+
g
cos
⁡
L
-
ϕ
b
0
g
sin
⁡
L
-
∇
N
sin
⁡
L
+
∇
U
cos
⁡
L
+
f
d
a
,
f
b
i
′
=
t
2
2
ε
E
g
ω
i
e
sin
⁡
L
+
t
g
ε
N
+
g
ω
i
e
cos
⁡
L
-
∇
N
ω
i
e
sin
⁡
L
+
∇
U
ω
i
e
cos
⁡
L
+
-
ϕ
c
0
g
cos
⁡
L
+
ϕ
a
0
g
sin
⁡
L
+
∇
E
+
f
d
b
,
f
c
i
′
=
t
2
2
g
ω
i
e
cos
⁡
L
-
ε
N
sin
⁡
L
+
ε
U
cos
⁡
L
-
t
ϕ
a
0
g
ω
i
e
cos
⁡
L
+
ε
E
cos
⁡
L
+
ϕ
b
0
g
cos
⁡
L
+
g
sin
⁡
L
+
∇
N
cos
⁡
L
+
∇
U
sin
⁡
L
+
f
d
c
.In order to eliminate the alternating specific force interference caused by shaking and improve the measurement accuracy on a rocking base, integrating (32) on both sides yields(33)
V
a
i
′
t
=
t
g
cos
⁡
L
-
ϕ
b
0
g
sin
⁡
L
-
∇
N
sin
⁡
L
+
∇
U
cos
⁡
L
+
t
2
2
ϕ
c
0
g
ω
i
e
cos
⁡
L
+
ε
E
g
sin
⁡
L
-
∇
E
ω
i
e
-
t
3
6
g
ω
i
e
ε
N
+
ε
N
cos
2
⁡
L
+
ε
U
sin
⁡
L
cos
⁡
L
+
V
d
a
,
V
b
i
′
t
=
t
-
ϕ
c
0
g
cos
⁡
L
+
ϕ
a
0
g
sin
⁡
L
+
∇
E
+
t
2
2
g
ε
N
+
g
ω
i
e
cos
⁡
L
-
∇
N
ω
i
e
sin
⁡
L
+
∇
U
ω
i
e
cos
⁡
L
+
t
3
6
ε
E
g
ω
i
e
sin
⁡
L
+
V
d
b
,
V
c
i
′
t
=
t
ϕ
b
0
g
cos
⁡
L
+
g
sin
⁡
L
+
∇
N
cos
⁡
L
+
∇
U
sin
⁡
L
-
t
2
2
g
cos
⁡
L
ϕ
a
0
ω
i
e
+
ε
E
+
t
3
6
g
ω
i
e
cos
⁡
L
-
ε
N
sin
⁡
L
+
ε
U
cos
⁡
L
+
V
d
c
.From (33), it can be observed that initial misalignment angle ϕ
b
0 can be directly estimated according to the first-order terms of V
a
i
′
(
t
) and V
c
i
′
(
t
), and ϕ
c
0 and ϕ
a
0 can be obtained by the quadratic terms of V
a
i
′
(
t
) and V
c
i
′
(
t
), respectively, so the convergence speed of ϕ
b
0 is faster than the convergence speed of ϕ
c
0 and ϕ
a
0. ε
E, ε
N, and ε
U can be estimated according to the third-order terms of V
a
i
′
(
t
), V
b
i
′
(
t
), and V
c
i
′
(
t
); because cubic curve only exhibits a linear characteristic in a short time, the convergence speed of the three initial misalignment angles is faster than the convergence speed of the three gyroscope’s equivalent constant drifts. Therefore, the alignment time with drift measurement and alignment accuracy cannot be taken into account, and alignment time cannot be shortened infinitely.The discrete form of (33) can be formulated as(34)
V
a
i
′
k
=
l
1
a
k
T
s
+
l
2
a
k
T
s
2
+
l
3
a
k
T
s
3
+
V
d
a
,
V
b
i
′
k
=
l
1
b
k
T
s
+
l
2
b
k
T
s
2
+
l
3
b
k
T
s
3
+
V
d
b
,
V
c
i
′
k
=
l
1
c
k
T
s
+
l
2
c
k
T
s
2
+
l
3
c
k
T
s
3
+
V
d
b
,where T
s is the interval of the SINS update which is rather small and k denotes kth time-step and(35)
l
1
a
=
g
cos
⁡
L
-
ϕ
b
0
g
sin
⁡
L
-
∇
N
sin
⁡
L
+
∇
U
cos
⁡
L
,
l
2
a
=
1
2
ϕ
c
0
g
ω
i
e
cos
⁡
L
+
ε
E
g
sin
⁡
L
-
∇
E
ω
i
e
,
l
3
a
=
-
1
6
g
ω
i
e
ε
N
+
ε
N
cos
2
⁡
L
+
ε
U
sin
⁡
L
cos
⁡
L
,
l
1
b
=
-
ϕ
c
0
g
cos
⁡
L
+
ϕ
a
0
g
sin
⁡
L
+
∇
E
,
l
2
b
=
1
2
g
g
ε
N
+
g
ω
i
e
cos
⁡
L
-
∇
N
ω
i
e
sin
⁡
L
+
∇
U
ω
i
e
cos
⁡
L
,
l
3
b
=
1
6
ε
E
g
ω
i
e
sin
⁡
L
,
l
1
c
=
ϕ
b
0
g
cos
⁡
L
+
g
sin
⁡
L
+
∇
N
cos
⁡
L
+
∇
U
sin
⁡
L
,
l
2
c
=
-
1
2
g
cos
⁡
L
ϕ
a
0
ω
i
e
+
ε
E
,
l
3
c
=
1
6
g
ω
i
e
cos
⁡
L
-
ε
N
sin
⁡
L
+
ε
U
cos
⁡
L
.Because the time of alignment is very short,l
i
j
(
i
=
1,2
,
3
;
j
=
a
,
b
,
c
) can be nearly a constant value and (34) is the measurement equation, so l
i
j
(
i
=
1,2
,
3
;
j
=
a
,
b
,
c
) can be derived from the measured values (i.e., V
a
i
′
(
k
), V
b
i
′
(
k
), and V
c
i
′
(
k
)) by using parameter identification method. Define(36)
Θ
a
=
l
1
a
l
2
a
l
3
a
,
Θ
b
=
l
1
b
l
2
b
l
3
b
,
Θ
c
=
l
1
c
l
2
c
l
3
c
.Then, the state equation models of Θ
a, Θ
b, and Θ
c can be, respectively, expressed as follows:(37)
Θ
a
k
+
1
=
Θ
a
k
,
V
a
k
=
H
k
Θ
a
k
+
V
d
a
k
,
(38)
Θ
b
k
+
1
=
Θ
b
k
,
V
b
k
=
H
k
Θ
b
k
+
V
d
b
k
,
(39)
Θ
c
k
+
1
=
Θ
c
k
,
V
c
k
=
H
k
Θ
c
k
+
V
d
c
k
,where H
k
=
k
T
s
(
k
T
s
)
2
(
k
T
s
)
3.Given initial guess of stateΘ
^
i
(
0
) and associate covariance P
i
(
0
), an adaptive recursive weighted least squares algorithm computes a posteriori estimate Θ
^
i
(
k
+
1
), gain matrix K
i
(
k
), and a posteriori covariance P
i
(
k
+
1
) as follows [28]:(40)
Θ
^
i
k
+
1
=
Θ
^
i
k
+
K
i
k
V
i
k
-
H
k
Θ
^
i
k
,
K
i
k
=
P
i
k
H
T
k
H
k
P
i
k
H
T
k
+
Λ
^
i
k
+
1
-
1
,
P
i
k
+
1
=
P
i
k
-
K
i
k
H
k
P
i
k
,
Λ
^
i
k
+
1
=
Λ
^
i
k
+
e
i
k
e
i
T
k
+
Λ
^
i
k
k
+
1
,
e
i
k
=
V
i
k
-
H
k
Θ
^
i
k
,
i
=
a
,
b
,
c
;
k
=
1,2
,
…
,where e
i
(
k
) is called innovation vector. In general, Θ
^
i
(
0
)
=
0, Λ
^
i
(
0
)
=
0.1, P
i
(
0
)
=
I
α and α is a large scalar.We can find that the measurement noise is not used directly in the above algorithm but merged intoe
k. The advantage is that we do not have to know the statistical properties caused by speed disturbance and the gain can be adaptively weighted calculated by the innovation vector. So it will speed up the convergence of the algorithm.After gaining the estimate value ofl
i
j
(
i
=
1,2
,
3
;
j
=
a
,
b
,
c
), substituting them into (35), we have(41)
ε
E
=
6
l
3
b
g
ω
i
e
sin
⁡
L
,
ε
N
=
-
3
g
ω
i
e
l
3
a
+
l
3
c
tan
⁡
L
,
ε
U
=
3
g
ω
i
e
cos
2
⁡
L
2
l
3
c
-
l
3
c
sin
2
⁡
L
-
l
3
a
sin
⁡
L
cos
⁡
L
,
ϕ
a
0
=
-
2
l
2
c
g
ω
i
e
cos
⁡
L
-
ε
E
ω
i
e
,
ϕ
b
0
=
l
1
a
-
g
cos
⁡
L
-
g
sin
⁡
L
,
ϕ
c
0
=
2
l
2
a
-
ε
E
g
sin
⁡
L
g
ω
i
e
cos
⁡
L
.If the gyroscope’s equivalent east constant drift (i.e.,ε
E) is not measured, ϕ
a
0, ϕ
b
0, and ϕ
c
0 can be, respectively, expressed as follows:(42)
ϕ
a
0
≈
-
2
l
2
c
g
ω
i
e
cos
⁡
L
,
ϕ
b
0
=
l
1
a
-
g
cos
⁡
L
-
g
sin
⁡
L
,
ϕ
c
0
≈
2
l
2
a
g
ω
i
e
cos
⁡
L
.And the calculation errors of misalignment angles are formulated as follows:(43)
δ
ϕ
a
0
=
-
ε
E
ω
i
e
,
δ
ϕ
b
0
=
-
∇
N
sin
⁡
L
+
∇
U
cos
⁡
L
g
sin
⁡
L
,
δ
ϕ
c
0
=
-
ε
E
ω
i
e
tan
⁡
L
+
∇
E
g
cos
⁡
L
.After gaining initial misalignment anglesϕ
a
0, ϕ
b
0, and ϕ
c
0, substituting them into (25), we can obtain the misalignment angles in real time. Then, complete the initial alignment according to (11), and the gyrodrifts can be calculated as follows:(44)
ε
x
ε
y
ε
z
=
C
n
b
ε
E
ε
N
ε
U
,where C
n
b is transformation matrix from n frame to b frame and C
n
b
=
C
b
n
T.


## 3.1. Analysis of the Attitude Misalignment Angles in the Inertial Frame

Note that there is a big calculation error between estimation valueC
^
i
b
0
i and true value C
i
b
0
i, and C
^
b
i
b
0 calculated by the outputs of gyroscopes also exists error. Therefore, we have(11)
C
b
n
=
C
i
n
C
i
′
i
C
b
i
′
,where C
b
i
′
=
C
^
i
b
0
i
C
^
b
i
b
0, and C
i
′
i which is the transformation matrix from ideal inertial coordinate frame (i frame) to erroneously computed inertial coordinate frame (i
′ frame) can be formulated as(12)
C
i
i
′
=
I
-
Φ
i
×
=
1
ϕ
c
-
ϕ
b
-
ϕ
c
1
ϕ
a
ϕ
b
-
ϕ
a
1
,where Φ
i
=
ϕ
a
ϕ
b
ϕ
c
T is the misalignment angle vector in i frame. The subscripts a, b, c are the three axes of i frame, respectively. And the task of fine alignment is to estimate misalignment angle Φ
i.In ideal conditions, the differential equation ofC
b
i is(13)
C
˙
b
i
=
C
b
i
ω
i
b
b
×
,where C
b
i is the transformation matrix between i frame and b frame. ω
i
b
b denotes the angular rate of b frame with respect to i frame, expressed in b frame.In practice, the differential equation ofC
b
i
′ is(14)
C
˙
b
i
′
=
C
b
i
′
ω
^
i
b
b
×
,where C
b
i
′ is the transformation matrix between i
′ frame and b frame. ω
^
i
b
b measured by gyroscopes in b frame is the computed value of ω
i
b
b. And ω
^
i
b
b can be formulated as(15)
ω
^
i
b
b
=
ω
i
b
b
+
δ
ω
i
b
b
,where δ
ω
i
b
b is the gyroscope error vector in b frame.DifferentiatingC
i
i
′
=
C
b
i
′
C
i
b on both sides yields(16)
C
˙
i
i
′
=
C
˙
b
i
′
C
i
b
+
C
b
i
′
C
˙
i
b
=
C
˙
b
i
′
C
i
b
+
C
i
i
′
C
b
i
C
˙
i
b
.Owing toC
i
b
C
b
i
=
I and C
b
i
C
˙
i
b
=
-
C
˙
b
i
C
i
b, substituting them into (5) and multiplying C
b
i from right on each part yield(17)
C
˙
i
i
′
C
b
i
=
C
˙
b
i
′
-
C
i
i
′
C
˙
b
i
.Substituting (13), (14), and (15) into (17), we have(18)
C
˙
i
i
′
C
b
i
=
C
b
i
′
ω
^
i
b
b
×
-
C
i
i
′
C
b
i
ω
i
b
b
×
=
C
i
i
′
C
b
i
ω
^
i
b
b
×
-
ω
i
b
b
×
=
C
i
i
′
C
b
i
δ
ω
i
b
b
×
.That is,(19)
C
˙
i
i
′
=
C
i
i
′
C
b
i
δ
ω
i
b
b
×
C
i
b
=
C
i
i
′
δ
ω
i
b
b
×
.Substituting (12) into (19) and ignoring the second-order small amount, we have(20)
Φ
˙
i
=
-
δ
ω
i
b
i
=
-
C
b
i
δ
ω
i
b
b
.Ignoring the influence of the installation error and scale factor error, (20) can be rewritten as(21)
Φ
˙
i
=
-
C
b
i
ε
b
=
-
C
n
i
C
b
n
ε
b
,where ε
b is the bias of gyroscope in b frame, and it can be approximately regarded as a random constant vector in the process of alignment. And this is the attitude error equation in i frame.When the carrier shakes modestly,C
b
n can be nearly a constant matrix, so C
b
n
ε
b can be approximately regarded as a random constant vector named as gyroscope’s equivalent constant drift in n frame; that is,(22)
C
b
n
ε
b
=
ε
n
=
ε
E
ε
N
ε
U
T
,where subscripts E, U, N are the three axes of n frame, respectively.The alignment process is usually performed in a few minutes (not more than ten minutes), soω
i
e
t is a very small amount. Then, c
o
s
(
ω
i
e
t
) and s
i
n
(
ω
i
e
t
) can be approximately rewritten as (23)
cos
⁡
ω
i
e
t
≈
1
,
sin
⁡
ω
i
e
t
≈
ω
i
e
t
.To this respect, (23) can be rewritten as(24)
ϕ
˙
a
ϕ
˙
b
ϕ
˙
c
=
-
C
n
i
C
b
n
ε
b
=
-
C
n
i
ε
n
=
C
n
i
ε
E
ε
N
ε
U
=
-
-
sin
⁡
ω
i
e
t
-
sin
⁡
L
cos
⁡
ω
i
e
t
cos
⁡
L
cos
⁡
ω
i
e
t
cos
⁡
ω
i
e
t
-
sin
⁡
L
sin
⁡
ω
i
e
t
cos
⁡
L
sin
⁡
ω
i
e
t
0
cos
⁡
L
sin
⁡
L
ε
E
ε
N
ε
U
≈
-
-
ω
i
e
t
-
sin
⁡
L
cos
⁡
L
1
-
ω
i
e
t
sin
⁡
L
ω
i
e
t
cos
⁡
L
0
cos
⁡
L
sin
⁡
L
ε
E
ε
N
ε
U
=
ε
E
ω
i
e
t
+
ε
N
sin
⁡
L
-
ε
U
cos
⁡
L
-
ε
E
+
ε
N
ω
i
e
t
sin
⁡
L
-
ε
U
ω
i
e
t
cos
⁡
L
-
ε
N
cos
⁡
L
-
ε
U
sin
⁡
L
.Integrating both sides of (24), the misalignment angles changing with the time can be formulated as follows:(25)
ϕ
a
=
ϕ
a
0
+
ε
N
sin
⁡
L
-
ε
U
cos
⁡
L
t
+
t
2
2
ε
E
ω
i
e
,
ϕ
b
=
ϕ
b
0
-
ε
E
t
+
t
2
2
ω
i
e
ε
N
sin
⁡
L
-
ε
U
cos
⁡
L
,
ϕ
c
=
ϕ
c
0
-
ε
N
cos
⁡
L
+
ε
U
sin
⁡
L
t
,where Φ
0
i
=
ϕ
a
0
ϕ
b
0
ϕ
c
0
T is the initial value vector of misalignment angle in i frame.It is easy to see thatϕ
i
0
(
i
=
a
,
b
,
c
) and ε
j
(
j
=
E
,
N
,
U
) are the constant valves; based on them, we can calculate the misalignment angles in real time. In this respect, the fine alignment problem has been transformed into the parameter identification problem for ϕ
i
0 and ε
j.


## 3.2. Estimation of Attitude Misalignment Angles by Parameter Identification Method

On a rocking base, the measured specific force projected from accelerometer ini
′ frame can be written as(26)
f
i
′
=
C
b
i
′
f
^
b
=
C
i
i
′
C
b
i
f
^
b
=
C
i
i
′
C
b
i
-
g
b
+
f
d
b
+
∇
b
=
C
i
i
′
-
g
i
+
f
d
i
+
∇
i
=
1
ϕ
c
-
ϕ
b
-
ϕ
c
1
ϕ
a
ϕ
b
-
ϕ
a
1
-
g
a
+
f
d
a
+
∇
a
-
g
b
+
f
d
b
+
∇
b
-
g
c
+
f
d
c
+
∇
c
.Ignoring the second-order small amount yields(27)
f
a
i
′
=
-
g
b
ϕ
c
+
g
c
ϕ
b
-
g
a
+
f
d
a
+
∇
a
,
f
b
i
′
=
g
a
ϕ
c
-
g
c
ϕ
a
-
g
b
+
f
d
b
+
∇
b
,
f
c
i
′
=
-
g
a
ϕ
b
+
g
b
ϕ
a
-
g
c
+
f
d
c
+
∇
c
,where ∇
b is the accelerometer bias vector in b frame. f
d
b is uncertainty measurement disturbance caused by the carrier’s rocking and swaying in b frame. g
a, g
b, g
c are the three components of the gravity vector in i frame, respectively. ∇
a, ∇
b, ∇
c are the three components of the accelerometer bias in i frame, respectively. According to the time for alignment, g
a
g
b
g
c
T and ∇
a
∇
b
∇
c
T can be calculated, respectively, as follows:(28)
g
a
g
b
g
c
=
C
n
i
g
n
=
-
sin
⁡
ω
i
e
t
-
sin
⁡
L
cos
⁡
ω
i
e
t
cos
⁡
L
cos
⁡
ω
i
e
t
cos
⁡
ω
i
e
t
-
sin
⁡
L
sin
⁡
ω
i
e
t
cos
⁡
L
sin
⁡
ω
i
e
t
0
cos
⁡
L
sin
⁡
L
0
0
-
g
=
-
g
cos
⁡
L
cos
⁡
ω
i
e
t
-
g
cos
⁡
L
sin
⁡
ω
i
e
t
-
g
sin
⁡
L
,
(29)
∇
a
∇
b
∇
c
=
C
n
i
C
b
n
∇
x
∇
y
∇
z
=
C
n
i
∇
E
∇
N
∇
U
=
-
∇
E
sin
⁡
ω
i
e
t
-
∇
N
sin
⁡
L
cos
⁡
ω
i
e
t
+
∇
U
cos
⁡
L
cos
⁡
ω
i
e
t
∇
E
cos
⁡
ω
i
e
t
-
∇
N
sin
⁡
L
sin
⁡
ω
i
e
t
+
∇
U
cos
⁡
L
sin
⁡
ω
i
e
t
∇
N
cos
⁡
L
+
∇
U
sin
⁡
L
,where ∇
E, ∇
N, ∇
U are the three components of the accelerometer equivalent bias in n frame, respectively. Because ω
i
e
t is a small amount, substituting (22) into (28) and (29) yields(30)
g
a
g
b
g
c
=
-
g
cos
⁡
L
-
g
ω
i
e
t
cos
⁡
L
-
g
sin
⁡
L
,
∇
a
∇
b
∇
c
=
-
∇
E
ω
i
e
t
-
∇
N
sin
⁡
L
+
∇
U
cos
⁡
L
∇
E
-
∇
N
ω
i
e
t
sin
⁡
L
+
∇
U
ω
i
e
t
cos
⁡
L
∇
N
cos
⁡
L
+
∇
U
sin
⁡
L
.Substituting (30) into (27), we have(31)
f
a
i
′
=
t
ϕ
c
g
ω
i
e
cos
⁡
L
-
ϕ
b
g
sin
⁡
L
+
g
cos
⁡
L
+
f
d
a
-
∇
E
ω
i
e
t
-
∇
N
sin
⁡
L
+
∇
U
cos
⁡
L
,
f
b
i
′
=
-
ϕ
c
g
cos
⁡
L
+
ϕ
a
g
sin
⁡
L
+
g
ω
i
e
t
cos
⁡
L
+
f
d
b
+
∇
E
-
∇
N
ω
i
e
t
sin
⁡
L
+
∇
U
ω
i
e
t
cos
⁡
L
,
f
c
i
′
=
ϕ
b
g
cos
⁡
L
-
ϕ
a
g
ω
i
e
t
cos
⁡
L
+
g
sin
⁡
L
+
f
d
c
+
∇
N
cos
⁡
L
+
∇
U
sin
⁡
L
.Substituting (24) into (31), we have(32)
f
a
i
′
=
-
t
2
2
g
ω
i
e
ε
N
+
ε
N
cos
2
⁡
L
+
ε
U
sin
⁡
L
cos
⁡
L
+
t
ϕ
c
0
g
ω
i
e
cos
⁡
L
+
ε
E
g
sin
⁡
L
-
∇
E
ω
i
e
+
g
cos
⁡
L
-
ϕ
b
0
g
sin
⁡
L
-
∇
N
sin
⁡
L
+
∇
U
cos
⁡
L
+
f
d
a
,
f
b
i
′
=
t
2
2
ε
E
g
ω
i
e
sin
⁡
L
+
t
g
ε
N
+
g
ω
i
e
cos
⁡
L
-
∇
N
ω
i
e
sin
⁡
L
+
∇
U
ω
i
e
cos
⁡
L
+
-
ϕ
c
0
g
cos
⁡
L
+
ϕ
a
0
g
sin
⁡
L
+
∇
E
+
f
d
b
,
f
c
i
′
=
t
2
2
g
ω
i
e
cos
⁡
L
-
ε
N
sin
⁡
L
+
ε
U
cos
⁡
L
-
t
ϕ
a
0
g
ω
i
e
cos
⁡
L
+
ε
E
cos
⁡
L
+
ϕ
b
0
g
cos
⁡
L
+
g
sin
⁡
L
+
∇
N
cos
⁡
L
+
∇
U
sin
⁡
L
+
f
d
c
.In order to eliminate the alternating specific force interference caused by shaking and improve the measurement accuracy on a rocking base, integrating (32) on both sides yields(33)
V
a
i
′
t
=
t
g
cos
⁡
L
-
ϕ
b
0
g
sin
⁡
L
-
∇
N
sin
⁡
L
+
∇
U
cos
⁡
L
+
t
2
2
ϕ
c
0
g
ω
i
e
cos
⁡
L
+
ε
E
g
sin
⁡
L
-
∇
E
ω
i
e
-
t
3
6
g
ω
i
e
ε
N
+
ε
N
cos
2
⁡
L
+
ε
U
sin
⁡
L
cos
⁡
L
+
V
d
a
,
V
b
i
′
t
=
t
-
ϕ
c
0
g
cos
⁡
L
+
ϕ
a
0
g
sin
⁡
L
+
∇
E
+
t
2
2
g
ε
N
+
g
ω
i
e
cos
⁡
L
-
∇
N
ω
i
e
sin
⁡
L
+
∇
U
ω
i
e
cos
⁡
L
+
t
3
6
ε
E
g
ω
i
e
sin
⁡
L
+
V
d
b
,
V
c
i
′
t
=
t
ϕ
b
0
g
cos
⁡
L
+
g
sin
⁡
L
+
∇
N
cos
⁡
L
+
∇
U
sin
⁡
L
-
t
2
2
g
cos
⁡
L
ϕ
a
0
ω
i
e
+
ε
E
+
t
3
6
g
ω
i
e
cos
⁡
L
-
ε
N
sin
⁡
L
+
ε
U
cos
⁡
L
+
V
d
c
.From (33), it can be observed that initial misalignment angle ϕ
b
0 can be directly estimated according to the first-order terms of V
a
i
′
(
t
) and V
c
i
′
(
t
), and ϕ
c
0 and ϕ
a
0 can be obtained by the quadratic terms of V
a
i
′
(
t
) and V
c
i
′
(
t
), respectively, so the convergence speed of ϕ
b
0 is faster than the convergence speed of ϕ
c
0 and ϕ
a
0. ε
E, ε
N, and ε
U can be estimated according to the third-order terms of V
a
i
′
(
t
), V
b
i
′
(
t
), and V
c
i
′
(
t
); because cubic curve only exhibits a linear characteristic in a short time, the convergence speed of the three initial misalignment angles is faster than the convergence speed of the three gyroscope’s equivalent constant drifts. Therefore, the alignment time with drift measurement and alignment accuracy cannot be taken into account, and alignment time cannot be shortened infinitely.The discrete form of (33) can be formulated as(34)
V
a
i
′
k
=
l
1
a
k
T
s
+
l
2
a
k
T
s
2
+
l
3
a
k
T
s
3
+
V
d
a
,
V
b
i
′
k
=
l
1
b
k
T
s
+
l
2
b
k
T
s
2
+
l
3
b
k
T
s
3
+
V
d
b
,
V
c
i
′
k
=
l
1
c
k
T
s
+
l
2
c
k
T
s
2
+
l
3
c
k
T
s
3
+
V
d
b
,where T
s is the interval of the SINS update which is rather small and k denotes kth time-step and(35)
l
1
a
=
g
cos
⁡
L
-
ϕ
b
0
g
sin
⁡
L
-
∇
N
sin
⁡
L
+
∇
U
cos
⁡
L
,
l
2
a
=
1
2
ϕ
c
0
g
ω
i
e
cos
⁡
L
+
ε
E
g
sin
⁡
L
-
∇
E
ω
i
e
,
l
3
a
=
-
1
6
g
ω
i
e
ε
N
+
ε
N
cos
2
⁡
L
+
ε
U
sin
⁡
L
cos
⁡
L
,
l
1
b
=
-
ϕ
c
0
g
cos
⁡
L
+
ϕ
a
0
g
sin
⁡
L
+
∇
E
,
l
2
b
=
1
2
g
g
ε
N
+
g
ω
i
e
cos
⁡
L
-
∇
N
ω
i
e
sin
⁡
L
+
∇
U
ω
i
e
cos
⁡
L
,
l
3
b
=
1
6
ε
E
g
ω
i
e
sin
⁡
L
,
l
1
c
=
ϕ
b
0
g
cos
⁡
L
+
g
sin
⁡
L
+
∇
N
cos
⁡
L
+
∇
U
sin
⁡
L
,
l
2
c
=
-
1
2
g
cos
⁡
L
ϕ
a
0
ω
i
e
+
ε
E
,
l
3
c
=
1
6
g
ω
i
e
cos
⁡
L
-
ε
N
sin
⁡
L
+
ε
U
cos
⁡
L
.Because the time of alignment is very short,l
i
j
(
i
=
1,2
,
3
;
j
=
a
,
b
,
c
) can be nearly a constant value and (34) is the measurement equation, so l
i
j
(
i
=
1,2
,
3
;
j
=
a
,
b
,
c
) can be derived from the measured values (i.e., V
a
i
′
(
k
), V
b
i
′
(
k
), and V
c
i
′
(
k
)) by using parameter identification method. Define(36)
Θ
a
=
l
1
a
l
2
a
l
3
a
,
Θ
b
=
l
1
b
l
2
b
l
3
b
,
Θ
c
=
l
1
c
l
2
c
l
3
c
.Then, the state equation models of Θ
a, Θ
b, and Θ
c can be, respectively, expressed as follows:(37)
Θ
a
k
+
1
=
Θ
a
k
,
V
a
k
=
H
k
Θ
a
k
+
V
d
a
k
,
(38)
Θ
b
k
+
1
=
Θ
b
k
,
V
b
k
=
H
k
Θ
b
k
+
V
d
b
k
,
(39)
Θ
c
k
+
1
=
Θ
c
k
,
V
c
k
=
H
k
Θ
c
k
+
V
d
c
k
,where H
k
=
k
T
s
(
k
T
s
)
2
(
k
T
s
)
3.Given initial guess of stateΘ
^
i
(
0
) and associate covariance P
i
(
0
), an adaptive recursive weighted least squares algorithm computes a posteriori estimate Θ
^
i
(
k
+
1
), gain matrix K
i
(
k
), and a posteriori covariance P
i
(
k
+
1
) as follows [28]:(40)
Θ
^
i
k
+
1
=
Θ
^
i
k
+
K
i
k
V
i
k
-
H
k
Θ
^
i
k
,
K
i
k
=
P
i
k
H
T
k
H
k
P
i
k
H
T
k
+
Λ
^
i
k
+
1
-
1
,
P
i
k
+
1
=
P
i
k
-
K
i
k
H
k
P
i
k
,
Λ
^
i
k
+
1
=
Λ
^
i
k
+
e
i
k
e
i
T
k
+
Λ
^
i
k
k
+
1
,
e
i
k
=
V
i
k
-
H
k
Θ
^
i
k
,
i
=
a
,
b
,
c
;
k
=
1,2
,
…
,where e
i
(
k
) is called innovation vector. In general, Θ
^
i
(
0
)
=
0, Λ
^
i
(
0
)
=
0.1, P
i
(
0
)
=
I
α and α is a large scalar.We can find that the measurement noise is not used directly in the above algorithm but merged intoe
k. The advantage is that we do not have to know the statistical properties caused by speed disturbance and the gain can be adaptively weighted calculated by the innovation vector. So it will speed up the convergence of the algorithm.After gaining the estimate value ofl
i
j
(
i
=
1,2
,
3
;
j
=
a
,
b
,
c
), substituting them into (35), we have(41)
ε
E
=
6
l
3
b
g
ω
i
e
sin
⁡
L
,
ε
N
=
-
3
g
ω
i
e
l
3
a
+
l
3
c
tan
⁡
L
,
ε
U
=
3
g
ω
i
e
cos
2
⁡
L
2
l
3
c
-
l
3
c
sin
2
⁡
L
-
l
3
a
sin
⁡
L
cos
⁡
L
,
ϕ
a
0
=
-
2
l
2
c
g
ω
i
e
cos
⁡
L
-
ε
E
ω
i
e
,
ϕ
b
0
=
l
1
a
-
g
cos
⁡
L
-
g
sin
⁡
L
,
ϕ
c
0
=
2
l
2
a
-
ε
E
g
sin
⁡
L
g
ω
i
e
cos
⁡
L
.If the gyroscope’s equivalent east constant drift (i.e.,ε
E) is not measured, ϕ
a
0, ϕ
b
0, and ϕ
c
0 can be, respectively, expressed as follows:(42)
ϕ
a
0
≈
-
2
l
2
c
g
ω
i
e
cos
⁡
L
,
ϕ
b
0
=
l
1
a
-
g
cos
⁡
L
-
g
sin
⁡
L
,
ϕ
c
0
≈
2
l
2
a
g
ω
i
e
cos
⁡
L
.And the calculation errors of misalignment angles are formulated as follows:(43)
δ
ϕ
a
0
=
-
ε
E
ω
i
e
,
δ
ϕ
b
0
=
-
∇
N
sin
⁡
L
+
∇
U
cos
⁡
L
g
sin
⁡
L
,
δ
ϕ
c
0
=
-
ε
E
ω
i
e
tan
⁡
L
+
∇
E
g
cos
⁡
L
.After gaining initial misalignment anglesϕ
a
0, ϕ
b
0, and ϕ
c
0, substituting them into (25), we can obtain the misalignment angles in real time. Then, complete the initial alignment according to (11), and the gyrodrifts can be calculated as follows:(44)
ε
x
ε
y
ε
z
=
C
n
b
ε
E
ε
N
ε
U
,where C
n
b is transformation matrix from n frame to b frame and C
n
b
=
C
b
n
T.


## 4. Simulation and Analysis

The parameters of the simulation are set as follows.The carrier is rocked by the wind. Pitchθ, roll γ, and heading ψ resulting from the carrier rocking are changed periodically and can be described as follows:(45)
θ
=
1
∘
cos
⁡
2
π
5
t
+
π
4
,
γ
=
1
∘
cos
⁡
2
π
6
t
+
π
7
,
ψ
=
3
0
°
+
30
′
cos
⁡
2
π
7
t
+
π
3
.The velocity caused by surge, sway, and heave is as follows:(46)
V
i
=
A
D
i
2
π
T
D
i
c
o
s
2
π
T
D
i
t
+
φ
D
i
,
i
=
x
,
y
,
z
,where subscript x, y, z are the three axes of b frame, respectively. A
D
x
=
0.02 m, A
D
y
=
0.03 m, and A
D
z
=
0.04 m. T
D
x
=
7 s, T
D
y
=
6 s, and T
D
z
=
5 s. φ
D
i obeys the uniform distribution on interval [
0
,
2
π
].The inertial measurement unit (IMU) is composed of medium precision sensors and the errors are set as follows: the gyroconstant drift is 0.01°/h; the gyrorandom noise is 0.01°/h; the accelerator bias is 1 × 10−4 g; and the accelerator measurement noise is 1 × 10−4 g. SINS location is as follows: north latitude is 40° and east longitude is 118°.Simulation 1.
The coarse alignment lasts 120 s. The values oft
1 and t
2 in (10) are set to 60 s and 120 s, respectively. The simulation for the coarse alignment runs 50 times. The pitch, roll, and heading errors at the end of the coarse alignments are shown in Figure 2 and the statistics for coarse alignment simulation result are shown in Table 1.Table 1
Statistics for coarse alignment simulation results.
Pitch error (°)
Roll error (°)
Heading error (°)
Mean
−0.0189
−0.2011
−0.3048
Max.
0.0045
−0.1015
1.4320
Min.
−0.0429
−0.3348
−1.1483
Std.
0.0136
0.0491
0.5902Figure 2
The simulation results for coarse alignment.From Figure2 and Table 1, it is obvious that the level attitude errors of the coarse alignment are less than 0.4 degrees and the heading attitude error is less than 1.5 degrees. The attitude errors calculated by the proposed coarse alignment algorithm can fulfill the requirement for the fine alignment. Next, the maximum of misalignment angles in Table 1 is used as input for fine alignment to validate the proposed parameter identification under inertial frame in Simulation 2.Simulation 2.
The simulation for fine alignment by parameter identification under inertial frame lasts 600 s and runs 6 times. The statistics for fine alignment simulation result are shown in Table2 and the estimate errors of misalignment angles (one of them) are shown in Figure 3.Table 2
Statistics for fine alignment simulation results.
Number
δ
ϕ
a (′)
δ
ϕ
b (′)
δ
ϕ
c (′)
1
1.863
0.471
1.535
2
1.781
0.325
1.774
3
1.818
0.453
2.024
4
1.870
0.337
2.251
5
1.752
0.301
1.663
6
1.774
0.374
1.486
Mean
1.810
0.377
1.789
Std.
0.0489
0.0703
0.2970Figure 3
Estimate errors of misalignment angles by parameter identification under inertial frame.According to Figure3 and Table 2, it is clear that δ
ϕ
b converges almost instantaneously with a high-precision (better than 0.5 arcmin). Specifically, δ
ϕ
a and δ
ϕ
c take longer time than δ
ϕ
b to converge; they stabilize at 2 arcmin in 200 seconds and the results confirm the former analysis in Section 3.2.Simulation 3.
In this simulation, the gyrodrifts are estimated in real time and the simulation lasts for 1500 s. The simulation results are shown in Figure4. The simulation results demonstrate that all the three gyrodrifts are observable and they converge with good results at the time of 300 s.Figure 4
Estimation of gyrodrifts.


## 5. Conclusions

The accuracy and applicability of SINS largely depended on the precision and swiftness of the alignment. This paper proposed a novel self-alignment algorithm by parameter identification under inertial frame for SINS. Firstly, the coarse alignment method by using the gravity information in the inertial frame is introduced. Secondly, the fine alignment method by parameter identification under inertial frame is formulated. The theoretical analysis results show that the fine alignment model is fully self-aligned with no external reference information and the gyrodrifts can be estimated in real time. Simulation results proved the accuracy and validity of the proposed method for SINS self-alignment, and the estimation results of the misalignment angles and gyrodrifts can approach the theoretical analysis results.



---

*Source: 5301242-2016-08-11.xml*