# Moving State Marine SINS Initial Alignment Based on High Degree CKF

Authors: Yong-Gang Zhang; Yu-Long Huang; Zhe-Min Wu; Ning Li
Journal: Mathematical Problems in Engineering
 (2014)

---

## Abstract

A new moving state marine initial alignment method of strap-down inertial navigation system (SINS) is proposed based on high-degree cubature Kalman filter (CKF), which can capture higher order Taylor expansion terms of nonlinear alignment model than the existing third-degree CKF, unscented Kalman filter and central difference Kalman filter, and improve the accuracy of initial alignment under large heading misalignment angle condition. Simulation results show the efficiency and advantage of the proposed initial alignment method as compared with existing initial alignment methods for the moving state SINS initial alignment with large heading misalignment angle.


---

## Body

## 1. Introduction

It is well known that the attitude update of strap-down inertial navigation system (SINS) is achieved based on numerical integration [1]. Therefore, it is necessary to know initial navigation parameters including position, velocity, and attitude for navigation calculation. The procedure of estimating initial navigation parameters is initial alignment, and the accuracy of estimation of these initial navigation parameters, especially the estimation accuracy of attitude, is very important to subsequent navigation operation, since the initial attitude errors (or misalignment angles) will seriously degrade the performance of SINS and cause positioning and attitude errors [2]. Thus, it is important to estimate initial attitude and reduce misalignment angles. Initial alignment of SINS is usually accomplished in stationary mode [3]. However, a moving state initial alignment is necessary to maintain high navigation accuracy. Generally, after initial alignment, the resulting navigation state errors grow up because of the initialization errors and cumulative sensor inaccuracies [4]. Consequently, in large navigation errors, due to the growing sensor error and the poor orientation, SINS often requires to be realigned, and the initialization needs the ship to stop at the initial position for at least 5 to 10 minutes [4, 5]. However, it is inconvenient and impractical that there is not enough time to stop at the initial position. Therefore, a moving state initial alignment of SINS is necessary to enable the ship to start instantly [5]. Besides, in some applications, such as carrier-launched aircraft, it is necessary to achieve an accurate moving state (or in-motion) initial alignment of host SINS [6]. As the host carrier may be sailing while aligning the SINS of a carrier-launched aircraft, aiding information from host SINS will be used to accomplish the alignment, then a moving state alignment should be implemented to realign SINS for vessel in sail [6].In moving state initial alignment of SINS, heading misalignment angle may be large since there is no reference to indicate current heading, especially for integrated alignment, and error model of SINS with large heading misalignment angle is nonlinear, which means linear estimation methods are not suitable for SINS initial alignment with large heading misalignment angle [7]. In order to solve the problem of moving state initial alignment with large heading misalignment angle, Kong et al. proposed an initial alignment method based on extended Kalman filter (EKF) [8]. However, it has low alignment accuracy and slow alignment speed. In order to improve the alignment accuracy and alignment speed, Zhou et al. proposed an initial alignment method based on unscented Kalman filter (UKF), which can at least capture the posterior mean and covariance to the second order of the Taylor series of any nonlinearity [9]. To improve the computational efficiency of UKF method, Chang et al. proposed an initial alignment method based on marginalized UKF [10]. To further improve the accuracy of UKF method, Long et al. proposed an initial alignment method based on central difference Kalman filter (CDKF), which can provide better covariance estimation than UKF [11]. To improve the numerical stability of UKF method, Sun proposed an initial alignment method based on cubature Kalman filter (CKF) [12], which is a special case of UKF with better numerical stability [13].However, all moving state initial alignment methods mentioned above have limited alignment accuracy and alignment speed because they cannot capture the fifth order Taylor expansion terms of nonlinear alignment model. In order to improve alignment accuracy and alignment speed, a new moving state initial alignment method based on the fifth-degree CKF (5th-CKF) is proposed in this paper. For moving state initial alignment of SINS with large heading misalignment angle, the 5th-CKF addresses the strong nonlinearity problem better than existing methods because it can capture the fifth order Taylor expansion terms of nonlinear alignment model. As will be seen in our simulation results, the proposed initial alignment method outperforms existing initial alignment methods in terms of alignment accuracy and alignment speed.The remainder of this paper is organized as follows. The nonlinear error model of moving state marine SINS initial alignment is presented in Section2. The 5th-CKF method is formulated in Section 3. Section 4 focuses on the application of the 5th-CKF to the nonlinear estimation problem of moving state initial alignment of SINS and compares the proposed initial alignment method with existing initial alignment methods for the moving state SINS initial alignment with large heading misalignment angle. Concluding remarks are drawn in Section 5.


## 2. Marine SINS Initial Alignment Nonlinear Error Model

Initial alignment is a process to precisely determine initial values of strap-down matrix between the vehicle’s body frame and the reference frame so that the navigation computer can start with exact initial conditions. Initial alignment is a key technique in SINS. The alignment accuracy and alignment speed will influence the performance of SINS navigation. Next we will firstly introduce nomenclature used in inertial technology and then formulate marine SINS nonlinear error model in moving state initial alignment.


### 2.1. SINS Nonlinear Error Model for Moving State Marine Initial Alignment

In this paper, we choosex
=
[
δ
φ
δ
λ
δ
v
x
δ
v
y
ϕ
x
ϕ
y
ϕ
z
∇
x
b
∇
y
b
ε
x
b
ε
y
b
ε
z
b
]
T as state vector in initial alignment, where δ
φ and δ
λ are errors of latitude and longitude (note we ignore the altitude error for marine application), δ
v
x and δ
v
y are velocity errors in east and north directions, ϕ
x, ϕ
y, and ϕ
z are rolling, pitching, and heading misalignment angles, respectively, ∇
x
b and ∇
y
b are constant bias of specific force in b frame, and ε
x
b, ε
y
b, and ε
z
b are constant drifts of gyro in b frame. If we denote vectors δ
v
n
=
[
δ
v
x
δ
v
y
δ
v
z
]
T, ϕ
=
[
ϕ
x
ϕ
y
ϕ
z
]
T, ∇
b
=
[
∇
x
b
∇
y
b
∇
z
b
]
T, and ε
b
=
[
ε
x
b
ε
y
b
ε
z
b
]
T, we will have the following SINS nonlinear error model for moving state marine initial alignment [10]:
(1)
δ
φ
˙
=
δ
v
y
R
m
,
δ
λ
˙
=
δ
v
x
sec
φ
R
n
+
δ
φ
v
x
sec
φ
tan
φ
R
n
,
δ
v
˙
n
=
[
I
-
(
C
n
n
′
)
T
]
C
b
n
′
f
^
sf
b
+
(
C
n
n
′
)
T
C
b
n
′
δ
f
sf
b
-
(
2
ω
^
i
e
n
+
ω
^
e
n
n
)
×
δ
v
n
-
(
2
δ
ω
i
e
n
+
δ
ω
e
n
n
)
×
(
v
^
n
-
δ
v
n
)
,
ϕ
˙
=
C
w
-
1
[
(
I
-
C
n
n
′
)
ω
^
i
n
n
+
C
n
n
′
δ
ω
i
n
n
-
C
b
n
′
δ
ω
i
b
b
]
,
∇
˙
b
=
0
,
ε
˙
b
=
0
,
with
(2)
C
w
-
1
=
1
cos
⁡
ϕ
x
[
cos
⁡
ϕ
y
cos
⁡
ϕ
x
0
sin
ϕ
y
cos
⁡
ϕ
x
sin
ϕ
y
sin
ϕ
x
cos
⁡
ϕ
x
-
cos
⁡
ϕ
y
sin
ϕ
x
-
sin
ϕ
y
0
cos
⁡
ϕ
y
]
,
where R
m is the meridian radius of curvature and R
n is the transverse radius of curvature, φ is the computed geographic latitude, and C
n
n
′ is the transformation matrix from true navigation frame (n frame) to erroneously computed navigation frame (n
’ frame) which is formulated as(3)
C
n
n
′
=
[
cos
⁡
ϕ
y
cos
⁡
ϕ
z
-
sin
ϕ
y
sin
ϕ
x
sin
ϕ
z
cos
⁡
ϕ
y
sin
ϕ
z
+
sin
ϕ
y
sin
ϕ
x
cos
⁡
ϕ
z
-
sin
ϕ
y
cos
⁡
ϕ
x
-
cos
⁡
ϕ
x
sin
ϕ
z
cos
⁡
ϕ
x
cos
⁡
ϕ
z
sin
ϕ
x
sin
ϕ
y
cos
⁡
ϕ
z
+
cos
⁡
ϕ
y
sin
ϕ
x
sin
ϕ
z
sin
ϕ
y
sin
ϕ
z
-
cos
⁡
ϕ
y
sin
ϕ
x
cos
⁡
ϕ
z
cos
⁡
ϕ
y
cos
⁡
ϕ
x
]
,where C
b
n
′ is the computed attitude matrix, f
^
sf
b is the specific force measured by accelerometers in the body frame, and δ
f
sf
b is the specific force error vector in the body frame, which can be formulated as
(4)
δ
f
sf
b
=
w
a
b
+
∇
b
,
where w
a
b
=
[
w
a
x
b
w
a
y
b
w
a
z
b
]
T is a random white noise vector in the body frame and ∇
b
=
[
∇
x
b
∇
y
b
∇
z
b
]
T is a constant bias vector in the body frame. δ
ω
i
b
b is the gyro error vector in the body frame, which can be formulated as
(5)
δ
w
i
b
b
=
w
g
b
+
ε
b
,
where w
g
b
=
[
w
g
x
b
w
g
y
b
w
g
z
b
]
T is a random white noise vector in the body frame and ε
b
=
[
ε
x
b
ε
y
b
ε
z
b
]
T is a constant drift vector in the body frame. ω
i
e
n is the angular rate of the earth frame with respect to the inertial frame. ω
^
i
e
n is the computed value of ω
i
e
n, and δ
ω
i
e
n is the computational error of ω
i
e
n. ω
i
e
n, δ
ω
i
e
n, and ω
^
i
e
n can be formulated as follows:
(6)
ω
i
e
n
=
[
0
ω
i
e
cos
⁡
φ
ω
i
e
sin
φ
]
T
,
δ
ω
i
e
n
=
[
0
-
ω
i
e
sin
φ
δ
φ
ω
i
e
cos
⁡
φ
δ
φ
]
T
,
ω
^
i
e
n
=
ω
i
e
n
+
δ
ω
i
e
n
,
where ω
e
n
n is the angular rate of the navigation frame with respect to the earth frame. ω
^
e
n
n is the computed value of ω
e
n
n, and δ
ω
e
n
n is the computational error of ω
e
n
n. ω
e
n
n, δ
ω
e
n
n, and ω
^
e
n
n can be formulated as
(7)
ω
e
n
n
=
[
-
v
y
R
m
v
x
R
n
v
x
tan
φ
R
n
]
T
,
δ
ω
e
n
n
=
[
-
δ
v
y
R
m
δ
v
x
R
n
δ
v
x
tan
φ
R
n
+
v
x
sec
φ
2
δ
φ
R
n
]
T
,
ω
^
e
n
n
=
ω
e
n
n
+
δ
ω
e
n
n
,
where v
x, v
y, v
z are true velocity values in east, north, and up direction.According to the definitions ofω
i
n
n, δ
ω
i
n
n, and ω
^
i
n
n, they can be formulated as
(8)
ω
i
n
n
=
ω
i
e
n
+
ω
e
n
n
,
δ
ω
i
n
n
=
δ
ω
i
e
n
+
δ
ω
e
n
n
,
ω
^
i
n
n
=
ω
i
n
n
+
δ
ω
i
n
n
.We choose the velocity and position differences between SINS and external sensors, such as GPS or other higher accuracy SINSs as measurement vectorz, which can be formulated as
(9)
z
=
[
φ
INS
-
φ
ref
λ
INS
-
λ
ref
v
INS
,
x
-
v
ref
,
x
v
INS
,
y
-
v
ref
,
y
]
,
where φ
ref, λ
ref, v
ref
,
x, and v
ref
,
y are measured latitude, longitude, velocity in east, and north directions, respectively.Note that the process model of moving state initial alignment introduced in (1) is a continuous model and we must transform it into discrete form. Given the sample time T, the propagations of position error, velocity error, and misalignment angles are discretized by using the fourth-degree Runge-Kutta method, and all the parts related to noise are discretized by using first-degree Runge-Kutta method. Based on (1) and (9), the discrete state equation and observation equation for state estimation can be formulated as
(10)
x
k
=
f
(
x
k
-
1
)
+
W
k
-
1
,
z
k
=
h
(
x
k
)
+
V
k
,
where h
(
x
k
)
=
[
I
4
×
4
0
4
×
8
]
x
k, W
k
-
1 is the Gaussian random process noise with mean 0 and covariance Q
k and V
k is the Gaussian random measurement noise with mean 0 and covariance R
k. Equation (10) formulates the nonlinear error model for moving state marine SINS initial alignment.It is clear to see from (1) that the state equation of the error model of moving state marine SINS initial alignment is typically nonlinear. Thus, nonlinear filtering algorithms are necessary to estimate the state vector from which misalignment angles can be obtained to finish initial alignment. Next we will introduce high degree CKF method.


## 2.1. SINS Nonlinear Error Model for Moving State Marine Initial Alignment

In this paper, we choosex
=
[
δ
φ
δ
λ
δ
v
x
δ
v
y
ϕ
x
ϕ
y
ϕ
z
∇
x
b
∇
y
b
ε
x
b
ε
y
b
ε
z
b
]
T as state vector in initial alignment, where δ
φ and δ
λ are errors of latitude and longitude (note we ignore the altitude error for marine application), δ
v
x and δ
v
y are velocity errors in east and north directions, ϕ
x, ϕ
y, and ϕ
z are rolling, pitching, and heading misalignment angles, respectively, ∇
x
b and ∇
y
b are constant bias of specific force in b frame, and ε
x
b, ε
y
b, and ε
z
b are constant drifts of gyro in b frame. If we denote vectors δ
v
n
=
[
δ
v
x
δ
v
y
δ
v
z
]
T, ϕ
=
[
ϕ
x
ϕ
y
ϕ
z
]
T, ∇
b
=
[
∇
x
b
∇
y
b
∇
z
b
]
T, and ε
b
=
[
ε
x
b
ε
y
b
ε
z
b
]
T, we will have the following SINS nonlinear error model for moving state marine initial alignment [10]:
(1)
δ
φ
˙
=
δ
v
y
R
m
,
δ
λ
˙
=
δ
v
x
sec
φ
R
n
+
δ
φ
v
x
sec
φ
tan
φ
R
n
,
δ
v
˙
n
=
[
I
-
(
C
n
n
′
)
T
]
C
b
n
′
f
^
sf
b
+
(
C
n
n
′
)
T
C
b
n
′
δ
f
sf
b
-
(
2
ω
^
i
e
n
+
ω
^
e
n
n
)
×
δ
v
n
-
(
2
δ
ω
i
e
n
+
δ
ω
e
n
n
)
×
(
v
^
n
-
δ
v
n
)
,
ϕ
˙
=
C
w
-
1
[
(
I
-
C
n
n
′
)
ω
^
i
n
n
+
C
n
n
′
δ
ω
i
n
n
-
C
b
n
′
δ
ω
i
b
b
]
,
∇
˙
b
=
0
,
ε
˙
b
=
0
,
with
(2)
C
w
-
1
=
1
cos
⁡
ϕ
x
[
cos
⁡
ϕ
y
cos
⁡
ϕ
x
0
sin
ϕ
y
cos
⁡
ϕ
x
sin
ϕ
y
sin
ϕ
x
cos
⁡
ϕ
x
-
cos
⁡
ϕ
y
sin
ϕ
x
-
sin
ϕ
y
0
cos
⁡
ϕ
y
]
,
where R
m is the meridian radius of curvature and R
n is the transverse radius of curvature, φ is the computed geographic latitude, and C
n
n
′ is the transformation matrix from true navigation frame (n frame) to erroneously computed navigation frame (n
’ frame) which is formulated as(3)
C
n
n
′
=
[
cos
⁡
ϕ
y
cos
⁡
ϕ
z
-
sin
ϕ
y
sin
ϕ
x
sin
ϕ
z
cos
⁡
ϕ
y
sin
ϕ
z
+
sin
ϕ
y
sin
ϕ
x
cos
⁡
ϕ
z
-
sin
ϕ
y
cos
⁡
ϕ
x
-
cos
⁡
ϕ
x
sin
ϕ
z
cos
⁡
ϕ
x
cos
⁡
ϕ
z
sin
ϕ
x
sin
ϕ
y
cos
⁡
ϕ
z
+
cos
⁡
ϕ
y
sin
ϕ
x
sin
ϕ
z
sin
ϕ
y
sin
ϕ
z
-
cos
⁡
ϕ
y
sin
ϕ
x
cos
⁡
ϕ
z
cos
⁡
ϕ
y
cos
⁡
ϕ
x
]
,where C
b
n
′ is the computed attitude matrix, f
^
sf
b is the specific force measured by accelerometers in the body frame, and δ
f
sf
b is the specific force error vector in the body frame, which can be formulated as
(4)
δ
f
sf
b
=
w
a
b
+
∇
b
,
where w
a
b
=
[
w
a
x
b
w
a
y
b
w
a
z
b
]
T is a random white noise vector in the body frame and ∇
b
=
[
∇
x
b
∇
y
b
∇
z
b
]
T is a constant bias vector in the body frame. δ
ω
i
b
b is the gyro error vector in the body frame, which can be formulated as
(5)
δ
w
i
b
b
=
w
g
b
+
ε
b
,
where w
g
b
=
[
w
g
x
b
w
g
y
b
w
g
z
b
]
T is a random white noise vector in the body frame and ε
b
=
[
ε
x
b
ε
y
b
ε
z
b
]
T is a constant drift vector in the body frame. ω
i
e
n is the angular rate of the earth frame with respect to the inertial frame. ω
^
i
e
n is the computed value of ω
i
e
n, and δ
ω
i
e
n is the computational error of ω
i
e
n. ω
i
e
n, δ
ω
i
e
n, and ω
^
i
e
n can be formulated as follows:
(6)
ω
i
e
n
=
[
0
ω
i
e
cos
⁡
φ
ω
i
e
sin
φ
]
T
,
δ
ω
i
e
n
=
[
0
-
ω
i
e
sin
φ
δ
φ
ω
i
e
cos
⁡
φ
δ
φ
]
T
,
ω
^
i
e
n
=
ω
i
e
n
+
δ
ω
i
e
n
,
where ω
e
n
n is the angular rate of the navigation frame with respect to the earth frame. ω
^
e
n
n is the computed value of ω
e
n
n, and δ
ω
e
n
n is the computational error of ω
e
n
n. ω
e
n
n, δ
ω
e
n
n, and ω
^
e
n
n can be formulated as
(7)
ω
e
n
n
=
[
-
v
y
R
m
v
x
R
n
v
x
tan
φ
R
n
]
T
,
δ
ω
e
n
n
=
[
-
δ
v
y
R
m
δ
v
x
R
n
δ
v
x
tan
φ
R
n
+
v
x
sec
φ
2
δ
φ
R
n
]
T
,
ω
^
e
n
n
=
ω
e
n
n
+
δ
ω
e
n
n
,
where v
x, v
y, v
z are true velocity values in east, north, and up direction.According to the definitions ofω
i
n
n, δ
ω
i
n
n, and ω
^
i
n
n, they can be formulated as
(8)
ω
i
n
n
=
ω
i
e
n
+
ω
e
n
n
,
δ
ω
i
n
n
=
δ
ω
i
e
n
+
δ
ω
e
n
n
,
ω
^
i
n
n
=
ω
i
n
n
+
δ
ω
i
n
n
.We choose the velocity and position differences between SINS and external sensors, such as GPS or other higher accuracy SINSs as measurement vectorz, which can be formulated as
(9)
z
=
[
φ
INS
-
φ
ref
λ
INS
-
λ
ref
v
INS
,
x
-
v
ref
,
x
v
INS
,
y
-
v
ref
,
y
]
,
where φ
ref, λ
ref, v
ref
,
x, and v
ref
,
y are measured latitude, longitude, velocity in east, and north directions, respectively.Note that the process model of moving state initial alignment introduced in (1) is a continuous model and we must transform it into discrete form. Given the sample time T, the propagations of position error, velocity error, and misalignment angles are discretized by using the fourth-degree Runge-Kutta method, and all the parts related to noise are discretized by using first-degree Runge-Kutta method. Based on (1) and (9), the discrete state equation and observation equation for state estimation can be formulated as
(10)
x
k
=
f
(
x
k
-
1
)
+
W
k
-
1
,
z
k
=
h
(
x
k
)
+
V
k
,
where h
(
x
k
)
=
[
I
4
×
4
0
4
×
8
]
x
k, W
k
-
1 is the Gaussian random process noise with mean 0 and covariance Q
k and V
k is the Gaussian random measurement noise with mean 0 and covariance R
k. Equation (10) formulates the nonlinear error model for moving state marine SINS initial alignment.It is clear to see from (1) that the state equation of the error model of moving state marine SINS initial alignment is typically nonlinear. Thus, nonlinear filtering algorithms are necessary to estimate the state vector from which misalignment angles can be obtained to finish initial alignment. Next we will introduce high degree CKF method.


## 3. High Degree CKF

### 3.1. Brief Introduction of CKF

The heart of Gaussian filter is to compute multidimensional Gaussian-weighted integral [13, 14]. Different Gaussian approximate filters can be obtained when different integral rules are used. The third-degree CKF (3rd-CKF) is obtained when the third-degree spherical-radial cubature rule is used, and the third-degree spherical-radial cubature rule can be formulated as [13]
(11)
∫
R
n
g
(
x
)
N
(
x
;
x
^
,
P
x
)
d
x
=
1
2
n
∑
j
=
1
n
[
g
(
n
P
x
e
j
+
x
^
)
+
g
(
-
n
P
x
e
j
+
x
^
)
]
,
where x is an n-dimensional Gaussian random vector with mean x
^ and covariance P
x and P
x is the square root matrix of P
x; that is, P
x
P
x
T
=
P
x, and e
j
=
[
0,0
,
…
,
1
j
,
…
,
0
]
T denotes a unit vector to the direction of coordinate axis j.The heart of the 3rd-CKF is the third-degree spherical-radial cubature rule in (11), which makes it possible to numerically compute multivariate moment integrals encountered in nonlinear Bayesian filter. The 3rd-CKF provides a systematic solution for high-dimensional nonlinear filtering problems. In addition, the 3rd-CKF is more stable and more principled in mathematics than sigma point approaches [13]. However, the accuracy of the 3rd-CKF is limit. To improve the accuracy of the 3rd-CKF, the 5th-CKF is proposed, which can capture higher order Taylor expansion terms of nonlinear function than the 3rd-CKF, thus higher accuracy can be obtained [14]. Next we will introduce the 5th-CKF method.


### 3.2. 5th-CKF Method

CKF is a recursive filtering method. We assume the posterior probability density ofx
k
-
1 has been already known in the previous update p
(
x
k
-
1
)
=
N
(
x
^
k
-
1
∣
k
-
1
,
P
k
-
1
∣
k
-
1
). Firstly we calculate the Cholesky decomposition of P
k
-
1
∣
k
-
1 as follows:
(12)
P
k
-
1
∣
k
-
1
=
S
k
-
1
∣
k
-
1
S
k
-
1
∣
k
-
1
T
.
The first class cubature-point and its weight are calculated as follows:
(13)
X
0
i
,
k
-
1
∣
k
-
1
=
x
^
k
-
1
∣
k
-
1
,
w
0
=
2
n
+
2
.
The second class cubature-points and their weights are calculated as follows:
(14)
X
1
i
,
k
-
1
∣
k
-
1
=
(
n
+
2
)
S
k
-
1
∣
k
-
1
e
i
+
x
^
k
-
1
∣
k
-
1
,
X
2
i
,
k
-
1
∣
k
-
1
=
-
(
n
+
2
)
S
k
-
1
∣
k
-
1
e
i
+
x
^
k
-
1
∣
k
-
1
,
w
1
=
4
-
n
2
(
n
+
2
)
2
,
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
(
i
=
1,2
,
…
,
n
)
,
where e
i denotes a unit vector to the direction of coordinate axis i.The third class cubature-points and their weights are calculated as follows:(15)
X
3
i
,
k
-
1
∣
k
-
1
=
(
n
+
2
)
S
k
-
1
∣
k
-
1
s
i
+
+
x
^
k
-
1
∣
k
-
1
,
X
4
i
,
k
-
1
∣
k
-
1
=
-
(
n
+
2
)
S
k
-
1
∣
k
-
1
s
i
+
+
x
^
k
-
1
∣
k
-
1
,
X
5
i
,
k
-
1
∣
k
-
1
=
(
n
+
2
)
S
k
-
1
∣
k
-
1
s
i
-
+
x
^
k
-
1
∣
k
-
1
,
X
6
i
,
k
-
1
∣
k
-
1
=
-
(
n
+
2
)
S
k
-
1
∣
k
-
1
s
i
-
+
x
^
k
-
1
∣
k
-
1
,
w
2
=
1
(
n
+
2
)
2
,
h
h
h
h
h
h
h
h
h
h
h
h
h
h
(
i
=
1,2
,
…
,
n
(
n
-
1
)
2
)
,
where
(16)
s
i
+
=
{
1
2
(
e
j
+
e
l
)
:
j
<
l
,
j
,
l
=
1,2
,
…
,
n
}
,
s
i
-
=
{
1
2
(
e
j
-
e
l
)
:
j
<
l
,
j
,
l
=
1,2
,
…
,
n
}
.
Sample points are obtained by propagating the above cubature-points through state equation in (10) as follows:
(17)
X
0
i
,
k
∣
k
-
1
*
=
f
(
X
0
i
,
k
-
1
∣
k
-
1
)
,
X
1
i
,
k
∣
k
-
1
*
=
f
(
X
1
i
,
k
-
1
∣
k
-
1
)
,
X
2
i
,
k
∣
k
-
1
*
=
f
(
X
2
i
,
k
-
1
∣
k
-
1
)
,
X
3
i
,
k
∣
k
-
1
*
=
f
(
X
3
i
,
k
-
1
∣
k
-
1
)
,
X
4
i
,
k
∣
k
-
1
*
=
f
(
X
4
i
,
k
-
1
∣
k
-
1
)
,
X
5
i
,
k
∣
k
-
1
*
=
f
(
X
5
i
,
k
-
1
∣
k
-
1
)
,
X
6
i
,
k
∣
k
-
1
*
=
f
(
X
6
i
,
k
-
1
∣
k
-
1
)
.
One-step state prediction x
^
k
∣
k
-
1 is then obtained as weighted linear combination of sample points
(18)
x
^
k
∣
k
-
1
=
w
0
X
0
i
,
k
∣
k
-
1
*
+
w
1
∑
j
=
1
n
(
X
1
i
,
k
∣
k
-
1
*
+
X
2
i
,
k
∣
k
-
1
*
)
+
w
2
∑
j
=
1
n
(
n
-
1
)
/
2
(
X
3
i
,
k
∣
k
-
1
*
+
X
4
i
,
k
∣
k
-
1
*
+
X
5
i
,
k
∣
k
-
1
*
+
X
6
i
,
k
∣
k
-
1
*
)
.
One-step state prediction error covariance P
k
∣
k
-
1 is updated as follows:
(19)
P
k
∣
k
-
1
=
w
0
X
0
i
,
k
∣
k
-
1
*
X
0
i
,
k
∣
k
-
1
*
T
+
w
1
∑
j
=
1
n
(
X
1
i
,
k
∣
k
-
1
*
X
1
i
,
k
∣
k
-
1
*
T
+
X
2
i
,
k
∣
k
-
1
*
X
2
i
,
k
∣
k
-
1
*
T
)
+
w
2
∑
j
=
1
n
(
n
-
1
)
/
2
(
X
3
i
,
k
∣
k
-
1
*
X
3
i
,
k
∣
k
-
1
*
T
+
X
4
i
,
k
∣
k
-
1
*
X
4
i
,
k
∣
k
-
1
*
T
m
m
m
m
m
m
m
+
X
5
i
,
k
∣
k
-
1
*
X
5
i
,
k
∣
k
-
1
*
T
+
X
6
i
,
k
∣
k
-
1
*
X
6
i
,
k
∣
k
-
1
*
T
)
-
x
^
k
∣
k
-
1
x
^
k
∣
k
-
1
T
+
Q
k
-
1
.
Next the measurement update is performed. Cholesky decomposition of P
k
∣
k
-
1 is performed firstly:
(20)
P
k
∣
k
-
1
=
S
k
∣
k
-
1
S
k
∣
k
-
1
T
.
The first class cubature-point and its weight are calculated as follows:
(21)
X
0
i
,
k
∣
k
-
1
=
x
^
k
∣
k
-
1
,
w
0
=
2
n
+
2
.
Then the second class cubature-points and their weights are calculated as follows:
(22)
X
1
i
,
k
∣
k
-
1
=
(
n
+
2
)
S
k
∣
k
-
1
e
i
+
x
^
k
∣
k
-
1
,
X
2
i
,
k
∣
k
-
1
=
-
(
n
+
2
)
S
k
∣
k
-
1
e
i
+
x
^
k
∣
k
-
1
,
w
1
=
4
-
n
2
(
n
+
2
)
2
,
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
(
i
=
1,2
,
…
,
n
)
.
The third class cubature-points and their weights are calculated as follows:
(23)
X
3
i
,
k
∣
k
-
1
=
(
n
+
2
)
S
k
∣
k
-
1
s
i
+
+
x
^
k
∣
k
-
1
,
X
4
i
,
k
∣
k
-
1
=
-
(
n
+
2
)
S
k
∣
k
-
1
s
i
+
+
x
^
k
∣
k
-
1
,
X
3
i
,
k
∣
k
-
1
=
(
n
+
2
)
S
k
∣
k
-
1
s
i
-
+
x
^
k
∣
k
-
1
,
X
4
i
,
k
∣
k
-
1
=
-
(
n
+
2
)
S
k
∣
k
-
1
s
i
-
+
x
^
k
∣
k
-
1
,
w
2
=
1
(
n
+
2
)
2
,
h
h
h
h
h
h
h
h
h
h
(
i
=
1,2
,
…
,
n
(
n
-
1
)
2
)
.
Sample points are obtained by propagating the above cubature-points through observation equation as follows:
(24)
Z
0
i
,
k
∣
k
-
1
=
h
(
X
0
i
,
k
∣
k
-
1
)
,
Z
1
i
,
k
∣
k
-
1
=
h
(
X
1
i
,
k
∣
k
-
1
)
,
Z
2
i
,
k
∣
k
-
1
=
h
(
X
2
i
,
k
∣
k
-
1
)
Z
3
i
,
k
∣
k
-
1
=
h
(
X
3
i
,
k
∣
k
-
1
)
,
Z
4
i
,
k
∣
k
-
1
=
h
(
X
4
i
,
k
∣
k
-
1
)
,
Z
5
i
,
k
∣
k
-
1
=
h
(
X
5
i
,
k
∣
k
-
1
)
,
Z
6
i
,
k
∣
k
-
1
=
h
(
X
6
i
,
k
∣
k
-
1
)
.
One-step measurement prediction z
^
k
∣
k
-
1 is then obtained as weighted linear combination of sample points:
(25)
z
^
k
∣
k
-
1
=
w
0
Z
0
i
,
k
∣
k
-
1
+
w
1
∑
j
=
1
n
(
Z
1
i
,
k
∣
k
-
1
+
Z
2
i
,
k
∣
k
-
1
)
+
w
2
∑
j
=
1
n
(
n
-
1
)
/
2
(
Z
3
i
,
k
∣
k
-
1
+
Z
4
i
,
k
∣
k
-
1
+
Z
5
i
,
k
∣
k
-
1
+
Z
6
i
,
k
∣
k
-
1
)
.
Autocorrelation covariance matrix P
z
z
,
k
∣
k
-
1 is obtained as follows:
(26)
P
z
z
,
k
∣
k
-
1
=
w
0
Z
0
i
,
k
∣
k
-
1
Z
0
i
,
k
∣
k
-
1
T
+
w
1
∑
j
=
1
n
(
Z
1
i
,
k
∣
k
-
1
Z
1
i
,
k
∣
k
-
1
T
+
Z
2
i
,
k
∣
k
-
1
Z
2
i
,
k
∣
k
-
1
T
)
+
w
2
∑
j
=
1
n
(
n
-
1
)
/
2
(
Z
3
i
,
k
∣
k
-
1
Z
3
i
,
k
∣
k
-
1
T
+
Z
4
i
,
k
∣
k
-
1
Z
4
i
,
k
∣
k
-
1
T
mmmmmm
+
Z
5
i
,
k
∣
k
-
1
Z
5
i
,
k
∣
k
-
1
T
+
Z
6
i
,
k
∣
k
-
1
Z
6
i
,
k
∣
k
-
1
T
)
-
z
^
k
∣
k
-
1
z
^
k
∣
k
-
1
T
+
R
k
.
Cross-correlation covariance matrix P
x
z
,
k
∣
k
-
1 is calculated as follows:
(27)
P
x
z
,
k
∣
k
-
1
=
w
0
X
0
i
,
k
∣
k
-
1
Z
0
i
,
k
∣
k
-
1
T
+
w
1
∑
j
=
1
n
(
X
1
i
,
k
∣
k
-
1
Z
1
i
,
k
∣
k
-
1
T
+
X
2
i
,
k
∣
k
-
1
Z
2
i
,
k
∣
k
-
1
T
)
+
w
2
∑
j
=
1
n
(
n
-
1
)
/
2
(
X
3
i
,
k
∣
k
-
1
Z
3
i
,
k
∣
k
-
1
T
+
X
4
i
,
k
∣
k
-
1
Z
4
i
,
k
∣
k
-
1
T
mmmmmm
+
X
5
i
,
k
∣
k
-
1
Z
5
i
,
k
∣
k
-
1
T
+
X
6
i
,
k
∣
k
-
1
Z
6
i
,
k
∣
k
-
1
T
)
-
x
^
k
∣
k
-
1
z
^
k
∣
k
-
1
T
.
The Kalman filter gain is calculated as follows:
(28)
K
k
=
P
x
z
,
k
∣
k
-
1
P
z
z
,
k
∣
k
-
1
-
1
.
State estimation x
^
k
∣
k is calculated as follows:
(29)
x
^
k
∣
k
=
x
^
k
∣
k
-
1
+
K
k
(
z
k
-
z
^
k
∣
k
-
1
)
.
The state estimation error covariance P
k
∣
k is calculated as follows:
(30)
P
k
∣
k
=
P
k
∣
k
-
1
-
K
k
P
z
z
,
k
∣
k
-
1
K
k
T
.x
^
k
∣
k and P
k
∣
k will be used in the next iteration. From the estimated state vector x
^
k
∣
k we can obtain estimated misalignment angles ϕ
^
=
[
ϕ
^
x
ϕ
^
y
ϕ
^
z
]
T, with which the strap-down matrix between vehicle’s body frame and the reference frame C
b
n can be determined, and the navigation computer can start with exact initial conditions. P
k
∣
k can be used to evaluate the accuracy of estimation. Next simulations will be performed to show the advantage of the proposed initial alignment method based on the 5th-CKF as compared with existing methods in marine initial alignment.


## 3.1. Brief Introduction of CKF

The heart of Gaussian filter is to compute multidimensional Gaussian-weighted integral [13, 14]. Different Gaussian approximate filters can be obtained when different integral rules are used. The third-degree CKF (3rd-CKF) is obtained when the third-degree spherical-radial cubature rule is used, and the third-degree spherical-radial cubature rule can be formulated as [13]
(11)
∫
R
n
g
(
x
)
N
(
x
;
x
^
,
P
x
)
d
x
=
1
2
n
∑
j
=
1
n
[
g
(
n
P
x
e
j
+
x
^
)
+
g
(
-
n
P
x
e
j
+
x
^
)
]
,
where x is an n-dimensional Gaussian random vector with mean x
^ and covariance P
x and P
x is the square root matrix of P
x; that is, P
x
P
x
T
=
P
x, and e
j
=
[
0,0
,
…
,
1
j
,
…
,
0
]
T denotes a unit vector to the direction of coordinate axis j.The heart of the 3rd-CKF is the third-degree spherical-radial cubature rule in (11), which makes it possible to numerically compute multivariate moment integrals encountered in nonlinear Bayesian filter. The 3rd-CKF provides a systematic solution for high-dimensional nonlinear filtering problems. In addition, the 3rd-CKF is more stable and more principled in mathematics than sigma point approaches [13]. However, the accuracy of the 3rd-CKF is limit. To improve the accuracy of the 3rd-CKF, the 5th-CKF is proposed, which can capture higher order Taylor expansion terms of nonlinear function than the 3rd-CKF, thus higher accuracy can be obtained [14]. Next we will introduce the 5th-CKF method.


## 3.2. 5th-CKF Method

CKF is a recursive filtering method. We assume the posterior probability density ofx
k
-
1 has been already known in the previous update p
(
x
k
-
1
)
=
N
(
x
^
k
-
1
∣
k
-
1
,
P
k
-
1
∣
k
-
1
). Firstly we calculate the Cholesky decomposition of P
k
-
1
∣
k
-
1 as follows:
(12)
P
k
-
1
∣
k
-
1
=
S
k
-
1
∣
k
-
1
S
k
-
1
∣
k
-
1
T
.
The first class cubature-point and its weight are calculated as follows:
(13)
X
0
i
,
k
-
1
∣
k
-
1
=
x
^
k
-
1
∣
k
-
1
,
w
0
=
2
n
+
2
.
The second class cubature-points and their weights are calculated as follows:
(14)
X
1
i
,
k
-
1
∣
k
-
1
=
(
n
+
2
)
S
k
-
1
∣
k
-
1
e
i
+
x
^
k
-
1
∣
k
-
1
,
X
2
i
,
k
-
1
∣
k
-
1
=
-
(
n
+
2
)
S
k
-
1
∣
k
-
1
e
i
+
x
^
k
-
1
∣
k
-
1
,
w
1
=
4
-
n
2
(
n
+
2
)
2
,
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
(
i
=
1,2
,
…
,
n
)
,
where e
i denotes a unit vector to the direction of coordinate axis i.The third class cubature-points and their weights are calculated as follows:(15)
X
3
i
,
k
-
1
∣
k
-
1
=
(
n
+
2
)
S
k
-
1
∣
k
-
1
s
i
+
+
x
^
k
-
1
∣
k
-
1
,
X
4
i
,
k
-
1
∣
k
-
1
=
-
(
n
+
2
)
S
k
-
1
∣
k
-
1
s
i
+
+
x
^
k
-
1
∣
k
-
1
,
X
5
i
,
k
-
1
∣
k
-
1
=
(
n
+
2
)
S
k
-
1
∣
k
-
1
s
i
-
+
x
^
k
-
1
∣
k
-
1
,
X
6
i
,
k
-
1
∣
k
-
1
=
-
(
n
+
2
)
S
k
-
1
∣
k
-
1
s
i
-
+
x
^
k
-
1
∣
k
-
1
,
w
2
=
1
(
n
+
2
)
2
,
h
h
h
h
h
h
h
h
h
h
h
h
h
h
(
i
=
1,2
,
…
,
n
(
n
-
1
)
2
)
,
where
(16)
s
i
+
=
{
1
2
(
e
j
+
e
l
)
:
j
<
l
,
j
,
l
=
1,2
,
…
,
n
}
,
s
i
-
=
{
1
2
(
e
j
-
e
l
)
:
j
<
l
,
j
,
l
=
1,2
,
…
,
n
}
.
Sample points are obtained by propagating the above cubature-points through state equation in (10) as follows:
(17)
X
0
i
,
k
∣
k
-
1
*
=
f
(
X
0
i
,
k
-
1
∣
k
-
1
)
,
X
1
i
,
k
∣
k
-
1
*
=
f
(
X
1
i
,
k
-
1
∣
k
-
1
)
,
X
2
i
,
k
∣
k
-
1
*
=
f
(
X
2
i
,
k
-
1
∣
k
-
1
)
,
X
3
i
,
k
∣
k
-
1
*
=
f
(
X
3
i
,
k
-
1
∣
k
-
1
)
,
X
4
i
,
k
∣
k
-
1
*
=
f
(
X
4
i
,
k
-
1
∣
k
-
1
)
,
X
5
i
,
k
∣
k
-
1
*
=
f
(
X
5
i
,
k
-
1
∣
k
-
1
)
,
X
6
i
,
k
∣
k
-
1
*
=
f
(
X
6
i
,
k
-
1
∣
k
-
1
)
.
One-step state prediction x
^
k
∣
k
-
1 is then obtained as weighted linear combination of sample points
(18)
x
^
k
∣
k
-
1
=
w
0
X
0
i
,
k
∣
k
-
1
*
+
w
1
∑
j
=
1
n
(
X
1
i
,
k
∣
k
-
1
*
+
X
2
i
,
k
∣
k
-
1
*
)
+
w
2
∑
j
=
1
n
(
n
-
1
)
/
2
(
X
3
i
,
k
∣
k
-
1
*
+
X
4
i
,
k
∣
k
-
1
*
+
X
5
i
,
k
∣
k
-
1
*
+
X
6
i
,
k
∣
k
-
1
*
)
.
One-step state prediction error covariance P
k
∣
k
-
1 is updated as follows:
(19)
P
k
∣
k
-
1
=
w
0
X
0
i
,
k
∣
k
-
1
*
X
0
i
,
k
∣
k
-
1
*
T
+
w
1
∑
j
=
1
n
(
X
1
i
,
k
∣
k
-
1
*
X
1
i
,
k
∣
k
-
1
*
T
+
X
2
i
,
k
∣
k
-
1
*
X
2
i
,
k
∣
k
-
1
*
T
)
+
w
2
∑
j
=
1
n
(
n
-
1
)
/
2
(
X
3
i
,
k
∣
k
-
1
*
X
3
i
,
k
∣
k
-
1
*
T
+
X
4
i
,
k
∣
k
-
1
*
X
4
i
,
k
∣
k
-
1
*
T
m
m
m
m
m
m
m
+
X
5
i
,
k
∣
k
-
1
*
X
5
i
,
k
∣
k
-
1
*
T
+
X
6
i
,
k
∣
k
-
1
*
X
6
i
,
k
∣
k
-
1
*
T
)
-
x
^
k
∣
k
-
1
x
^
k
∣
k
-
1
T
+
Q
k
-
1
.
Next the measurement update is performed. Cholesky decomposition of P
k
∣
k
-
1 is performed firstly:
(20)
P
k
∣
k
-
1
=
S
k
∣
k
-
1
S
k
∣
k
-
1
T
.
The first class cubature-point and its weight are calculated as follows:
(21)
X
0
i
,
k
∣
k
-
1
=
x
^
k
∣
k
-
1
,
w
0
=
2
n
+
2
.
Then the second class cubature-points and their weights are calculated as follows:
(22)
X
1
i
,
k
∣
k
-
1
=
(
n
+
2
)
S
k
∣
k
-
1
e
i
+
x
^
k
∣
k
-
1
,
X
2
i
,
k
∣
k
-
1
=
-
(
n
+
2
)
S
k
∣
k
-
1
e
i
+
x
^
k
∣
k
-
1
,
w
1
=
4
-
n
2
(
n
+
2
)
2
,
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
(
i
=
1,2
,
…
,
n
)
.
The third class cubature-points and their weights are calculated as follows:
(23)
X
3
i
,
k
∣
k
-
1
=
(
n
+
2
)
S
k
∣
k
-
1
s
i
+
+
x
^
k
∣
k
-
1
,
X
4
i
,
k
∣
k
-
1
=
-
(
n
+
2
)
S
k
∣
k
-
1
s
i
+
+
x
^
k
∣
k
-
1
,
X
3
i
,
k
∣
k
-
1
=
(
n
+
2
)
S
k
∣
k
-
1
s
i
-
+
x
^
k
∣
k
-
1
,
X
4
i
,
k
∣
k
-
1
=
-
(
n
+
2
)
S
k
∣
k
-
1
s
i
-
+
x
^
k
∣
k
-
1
,
w
2
=
1
(
n
+
2
)
2
,
h
h
h
h
h
h
h
h
h
h
(
i
=
1,2
,
…
,
n
(
n
-
1
)
2
)
.
Sample points are obtained by propagating the above cubature-points through observation equation as follows:
(24)
Z
0
i
,
k
∣
k
-
1
=
h
(
X
0
i
,
k
∣
k
-
1
)
,
Z
1
i
,
k
∣
k
-
1
=
h
(
X
1
i
,
k
∣
k
-
1
)
,
Z
2
i
,
k
∣
k
-
1
=
h
(
X
2
i
,
k
∣
k
-
1
)
Z
3
i
,
k
∣
k
-
1
=
h
(
X
3
i
,
k
∣
k
-
1
)
,
Z
4
i
,
k
∣
k
-
1
=
h
(
X
4
i
,
k
∣
k
-
1
)
,
Z
5
i
,
k
∣
k
-
1
=
h
(
X
5
i
,
k
∣
k
-
1
)
,
Z
6
i
,
k
∣
k
-
1
=
h
(
X
6
i
,
k
∣
k
-
1
)
.
One-step measurement prediction z
^
k
∣
k
-
1 is then obtained as weighted linear combination of sample points:
(25)
z
^
k
∣
k
-
1
=
w
0
Z
0
i
,
k
∣
k
-
1
+
w
1
∑
j
=
1
n
(
Z
1
i
,
k
∣
k
-
1
+
Z
2
i
,
k
∣
k
-
1
)
+
w
2
∑
j
=
1
n
(
n
-
1
)
/
2
(
Z
3
i
,
k
∣
k
-
1
+
Z
4
i
,
k
∣
k
-
1
+
Z
5
i
,
k
∣
k
-
1
+
Z
6
i
,
k
∣
k
-
1
)
.
Autocorrelation covariance matrix P
z
z
,
k
∣
k
-
1 is obtained as follows:
(26)
P
z
z
,
k
∣
k
-
1
=
w
0
Z
0
i
,
k
∣
k
-
1
Z
0
i
,
k
∣
k
-
1
T
+
w
1
∑
j
=
1
n
(
Z
1
i
,
k
∣
k
-
1
Z
1
i
,
k
∣
k
-
1
T
+
Z
2
i
,
k
∣
k
-
1
Z
2
i
,
k
∣
k
-
1
T
)
+
w
2
∑
j
=
1
n
(
n
-
1
)
/
2
(
Z
3
i
,
k
∣
k
-
1
Z
3
i
,
k
∣
k
-
1
T
+
Z
4
i
,
k
∣
k
-
1
Z
4
i
,
k
∣
k
-
1
T
mmmmmm
+
Z
5
i
,
k
∣
k
-
1
Z
5
i
,
k
∣
k
-
1
T
+
Z
6
i
,
k
∣
k
-
1
Z
6
i
,
k
∣
k
-
1
T
)
-
z
^
k
∣
k
-
1
z
^
k
∣
k
-
1
T
+
R
k
.
Cross-correlation covariance matrix P
x
z
,
k
∣
k
-
1 is calculated as follows:
(27)
P
x
z
,
k
∣
k
-
1
=
w
0
X
0
i
,
k
∣
k
-
1
Z
0
i
,
k
∣
k
-
1
T
+
w
1
∑
j
=
1
n
(
X
1
i
,
k
∣
k
-
1
Z
1
i
,
k
∣
k
-
1
T
+
X
2
i
,
k
∣
k
-
1
Z
2
i
,
k
∣
k
-
1
T
)
+
w
2
∑
j
=
1
n
(
n
-
1
)
/
2
(
X
3
i
,
k
∣
k
-
1
Z
3
i
,
k
∣
k
-
1
T
+
X
4
i
,
k
∣
k
-
1
Z
4
i
,
k
∣
k
-
1
T
mmmmmm
+
X
5
i
,
k
∣
k
-
1
Z
5
i
,
k
∣
k
-
1
T
+
X
6
i
,
k
∣
k
-
1
Z
6
i
,
k
∣
k
-
1
T
)
-
x
^
k
∣
k
-
1
z
^
k
∣
k
-
1
T
.
The Kalman filter gain is calculated as follows:
(28)
K
k
=
P
x
z
,
k
∣
k
-
1
P
z
z
,
k
∣
k
-
1
-
1
.
State estimation x
^
k
∣
k is calculated as follows:
(29)
x
^
k
∣
k
=
x
^
k
∣
k
-
1
+
K
k
(
z
k
-
z
^
k
∣
k
-
1
)
.
The state estimation error covariance P
k
∣
k is calculated as follows:
(30)
P
k
∣
k
=
P
k
∣
k
-
1
-
K
k
P
z
z
,
k
∣
k
-
1
K
k
T
.x
^
k
∣
k and P
k
∣
k will be used in the next iteration. From the estimated state vector x
^
k
∣
k we can obtain estimated misalignment angles ϕ
^
=
[
ϕ
^
x
ϕ
^
y
ϕ
^
z
]
T, with which the strap-down matrix between vehicle’s body frame and the reference frame C
b
n can be determined, and the navigation computer can start with exact initial conditions. P
k
∣
k can be used to evaluate the accuracy of estimation. Next simulations will be performed to show the advantage of the proposed initial alignment method based on the 5th-CKF as compared with existing methods in marine initial alignment.


## 4. Simulations

Three simulations are performed with different parameter sets under different moving states of ship. In the first simulation, the ship is on the mooring. In the second simulation, the ship sails with constant speedv
x
=
2 m/s and v
y
=
2 m/s. In the third simulation, the ship accelerates with a
x
=
0.02
m
/
s
2 and a
y
=
0.02
m
/
s
2 and initial velocity of v
x
=
2 m/s and v
y
=
2 m/s. In addition, initial values of process noise covariance matrix and state and measurement noise covariance matrix in simulations are set as diag
⁡
{
0
2
×
1
(
0.00
1
°
/
h
)
2
(
0.00
1
°
/
h
)
2
(
1
μ
g
)
2
(
1
μ
g
)
2
(
1
μ
g
)
2
0
5
×
1
}, 0
12
×
1, diag
⁡
{
(
0.1
m
/
s
)
2
(
0.1
m
/
s
)
2
(
10
/
Re
)
2
(
10
/
Re
)
2
}, respectively. Other parameters used in simulations are shown in Table 1.Table 1
Parameters used for simulations.
Swing amplitude (deg)
Swing period (s)
Initial error values of pitch and roll (deg)
Initial error values of heading (deg)
White noise bias stability of gyro (deg/h)
Bias of gyro (deg/h)
White noise error of accelerometer (μg)
Bias error of accelerometer (μg)
Roll
Pitch
Yaw
10
8
10
6
1
30
0.003
0.01
3.16
10To compare the performance of existing initial alignment methods based on the 3rd-CKF, UKF, CDKF, and the proposed initial alignment method based on the 5th-CKF, we choose the absolute value of estimation error of misalignment angles as performance metric. For a fair comparison, we make 500 independent Monte Carlo runs. Simulation results of existing methods and the proposed method are shown in Figures1, 2, and 3 and Tables 2, 3, and 4, which corresponds to simulation 1, simulation 2, and simulation 3, respectively. Besides, a comparison of computational complexity between the proposed method and existing methods is shown in Table 5.Table 2
Absolute value of steady state estimation error of misalignment angles when the ship is on the mooring with heading misalignment angle of 30°.
Initial alignment methods
3rd-CKF
UKF
CDKF
5th-CKF
Absolute value of steady state estimation error of rolling misalignment (arc mins)
0.75
0.45
0.36
0.15
Absolute value of steady state estimation error of pitching misalignment (arc mins)
0.58
0.37
0.32
0.16
Absolute value of steady state estimation error of heading misalignment (arc mins)
16
5
3.5
2.45Table 3
Absolute value of steady state estimation error of misalignment angles when the ship sails with constant speedv
x
=
2 m/s and v
y
=
2 m/s and heading misalignment angle of 30°.
Initial alignment methods
3rd-CKF
UKF
CDKF
5th-CKF
Absolute value of steady state estimation error of rolling misalignment (arc mins)
0.78
0.47
0.4
0.16
Absolute value of steady state estimation error of pitching misalignment (arc mins)
0.6
0.4
0.34
0.18
Absolute value of steady state estimation error of heading misalignment (arc mins)
16
5.3
4.1
2.5Table 4
Absolute value of steady state estimation error of misalignment angles when the ship accelerates with heading misalignment angle of 30°.
Initial alignment methods
3rd-CKF
UKF
CDKF
5th-CKF
Absolute value of steady state estimation error of rolling misalignment (arc mins)
0.64
0.33
0.25
0.09
Absolute value of steady state estimation error of pitching misalignment (arc mins)
0.63
0.4
0.35
0.15
Absolute value of steady state estimation error of heading misalignment (arc mins)
14
4.0
3.0
2.2Table 5
Comparison of computational complexity.
Initial alignment methods
3rd-CKF
UKF
CDKF
5th-CKF
Computational complexity
O(n
3) (n
=
12)
O(n
3) (n
=
12)
O(n
3) (n
=
12)
O(n
4) (n
=
12)Figure 1
Absolute value of estimation error of misalignment angles based on existing methods and the proposed method when the ship is on the mooring with heading misalignment angle of 30°.Figure 2
Absolute value of estimation error of misalignment angles based on existing methods and the proposed method when the ship sails with constant speedv
x
=
2 m/s and v
y
=
2 m/s and heading misalignment angle of 30°.Figure 3
Absolute value of estimation error of misalignment angles based on existing methods and the proposed method when the ship accelerates with heading misalignment angle of 30°.It is seen from Figures1–3 that the proposed initial alignment method has faster alignment speed than existing initial alignment methods under large heading misalignment angle conditions. From Tables 2–4, we also can see that the proposed initial alignment method outperforms existing initial alignment methods in terms of alignment accuracy under large heading misalignment angle conditions. As shown in Table 5, although the proposed initial alignment method has higher computational complexity than existing initial alignment methods, its computation requirement is acceptable for practical marine navigation application.Theoretically, as discussed in Section2, the initial alignment model is nonlinear for the case of large heading misalignment angle, and all nonlinear filtering algorithms only can achieve suboptimal estimation of initial misalignment angles. However, the 5th-CKF can capture higher order Taylor expansion terms of nonlinear initial alignment model than the 3rd-CKF, UKF, and CDKF. Thus, the proposed initial alignment method based on the 5th-CKF is superior to existing methods based on the 3rd-CKF, UKF, and CDKF in terms of alignment accuracy and alignment speed under large heading misalignment angle. Theoretical analysis agrees with simulation results.


## 5. Conclusion

In this paper, a new moving state initial alignment method is proposed based on the 5th-CKF. Three simulations are performed for marine SINS initial alignment under different conditions, including mooring, moving with constant speed, and moving with constant acceleration. Simulation results show that the proposed marine SINS initial alignment method is superior to existing methods in terms of alignment accuracy and alignment speed for the moving state SINS initial alignment with large heading misalignment angle. It is more suitable for applications where fast and accurate alignment is necessary.



---

*Source: 546107-2014-06-24.xml*