An Improved Inertial Frame Alignment Algorithm Based on Horizontal Alignment Information for Marine SINS

Abstract

In this paper, an improved inertial frame alignment algorithm for a marine SINS under mooring conditions is proposed, which significantly improves accuracy. Since the horizontal alignment is easy to complete, and a characteristic of gravity is that its component in the horizontal plane is zero, we use a clever method to improve the conventional inertial alignment algorithm. Firstly, a large misalignment angle model and a dimensionality reduction Gauss-Hermite filter are employed to establish the fine horizontal reference frame. Based on this, the projection of the gravity in the body inertial coordinate frame can be calculated easily. Then, the initial alignment algorithm is accomplished through an inertial frame alignment algorithm. The simulation and experiment results show that the improved initial alignment algorithm performs better than the conventional inertial alignment algorithm, and meets the accuracy requirements of a medium-accuracy marine SINS.

1. Introduction

A strapdown inertial navigation system (SINS) is a dead-reckoning navigation system, and the initial alignment is an essential procedure for a SINS, since it directly affects the precision of navigation parameters (position, velocity, and attitude) [1,2]. The main purpose of the initial alignment is to determine the initial strapdown attitude matrix between the body frame and navigation frame, and its accuracy is especially important for a marine SINS, which usually has to work for a long time [1,2,3,4].

Generally, the alignment process can be divided into two phases, the coarse-alignment phase and the fine-alignment phase [4,5,6,7,8]. The coarse-alignment phase is required to estimate the ship’s heading within a few degrees and pitch/roll within a few tenths of a degree in order to allow the fine-alignment filter to operate within its linear region [5,6]. The typical coarse-alignment method is analytic coarse-alignment. However, it is unable to handle the in-motion alignment problem [3,4,5,7]. In order to overcome the difficulties when a marine SINS is under mooring conditions, many methods have been developed and analyzed. Reference [3] proposed an improved alignment based on gravity in an inertial frame, and velocity is used in the calculation to reduce the influence of disturbance acceleration. In mooring conditions, due to the presence of the disturbed acceleration and angular velocities, accurate gravity and Earth rate are difficult to obtain directly, which finally leads to the low precision of the coarse alignment. Since the signal-to-noise ratios of gyros’ and accelerometers’ output are poor and the frequency bands of disturbed signals are wide, it is unable to separate the pure, useful signals from the interference signals measured by gyros and accelerometers [6,7,9]. In order to remove the high frequency noise, reference [7] used an IIR digital low-pass filter to process the gyro and accelerometer measurements. Although there have been various methods presented so as to obtain the purer Earth rate and gravity signals, the precision is still not high enough.

While in the fine-alignment phase, usually the standard Kalman filter or the compass loop method can be implemented based on the coarse-alignment result, and under the assumption of a small misalignment angle linear error model [1,2]. However, with the increase of SINS application technology and the development of nonlinear filtering estimation technology, its error models are no longer confined to linear models, and new error models are constantly emerging. Therefore, nonlinear filters are used for alignment [10,11,12,13,14,15,16]. However, no matter what method is employed, the heading misalignment angle will usually converge over 10 min under mooring conditions, which is slower than the horizontal misalignment angle (within 2 min only) and does not meet the demand for a quick start.

It is well known that the horizontal components of gravity projection in the horizontal coordinate frame are zero and the horizontal alignment is rapid. If an accurate horizontal coordinate frame is established, the interference caused by sway can be easily isolated. Based on this idea, we proposed an improved alignment scheme in [9]. Then, according to the characteristics of system structure, we used a dimensionality reduction Gauss-Hermite filter (GHF) algorithm to establish the accurate horizontal coordinate frame [17]. We specify the details of the algorithm and supplement error analysis in this paper. Because of the low precision of the traditional inertial frame alignment algorithm in the mooring environment, it is usually employed as a coarse alignment for a marine SINS. We use a clever method to improve the traditional inertial alignment algorithm in this paper, and improve the performance of the traditional inertial frame alignment algorithm.

Compared with the commonly used nonlinear filtering algorithms such as EKF [18,19], UKF [20] and CKF [21,22], GHQF [23] has the advantages of incomparable precision and stability. However, this approach is infeasible for high-dimensional systems since the computation burden increases exponentially with the index of dimension. This results in the “curse of dimensionality”. Fortunately, only misalignment angles suffer from nonlinearity in initial alignment, so that we can apply a dimension reduction nonlinear filter to carry out the alignment.

The remainder of this paper is organized as follows. Firstly, reference frames and parameter definitions are addressed in Section 2. The algorithmic principle for traditional alignment in the inertial frame is presented in Section 3. Section 4 details how to establish an accurate horizontal reference frame and Section 5 details how to accomplish the alignment. Then, the simulation results that validate the proposed approach are presented in Section 6. Section 7 presents the experimental results. Finally, the conclusions are presented in Section 8.

2. Reference Frames and Parameter Definitions

The reference frames are defined in Table 1 and the parameters are defined in Table 2.

Table 1. The reference frames definitions used in this paper.

Reference Frame	Definition
n-frame	Navigation reference frame which is the local horizontal reference frame. Its axes are aligned with east–north–up (ENU) geodetic axes.
h-frame	Horizontal reference frame. Its $z_h$ axis is aligned with $z_n$ axis, but the horizontal axes are arbitrary in the horizontal plane.
e-frame	Earth-centered Earth-fixed orthogonal reference frame. Its xe axis points to the local longitude.
b-frame	Body reference frame aligned with inertial measurement unit (IMU) axes.
i-frame	Earth-centered inertial fixed (ECIF) orthogonal reference frame. The axes are fixed with e-frame at the beginning of the alignment process.
$i_{b0}$-frame	Body inertial reference frame. It is formed by fixing the axes of b-frame in the inertial space at the beginning of the alignment process.

Table 1. The reference frames definitions used in this paper.

Reference Frame	Definition
n-frame	Navigation reference frame which is the local horizontal reference frame. Its axes are aligned with east–north–up (ENU) geodetic axes.
h-frame	Horizontal reference frame. Its $z_h$ axis is aligned with $z_n$ axis, but the horizontal axes are arbitrary in the horizontal plane.
e-frame	Earth-centered Earth-fixed orthogonal reference frame. Its xe axis points to the local longitude.
b-frame	Body reference frame aligned with inertial measurement unit (IMU) axes.
i-frame	Earth-centered inertial fixed (ECIF) orthogonal reference frame. The axes are fixed with e-frame at the beginning of the alignment process.
$i_{b0}$-frame	Body inertial reference frame. It is formed by fixing the axes of b-frame in the inertial space at the beginning of the alignment process.

The relationship of the frames mentioned in Table 1 is shown in Figure 1.

Figure 1. The relationship of the frames.

Figure 1. The relationship of the frames.

Table 2. The parameters.

Parameter	Definition
$C_{p1}^{p2}$	Transform matrix from $p1$ frame to $p2$ frame
${\mathrm{\omega }}_{ie}$	Angular rate of Earth rotation
$g$	Gravity
$L$	Latitude
$\text{V }$	Velocity
$\mathrm{\epsilon }$	Gyro drift
$\nabla$	Acceleration bias
$\varphi$	Misalignment angle
$f$	Specific Force

Table 2. The parameters.

Parameter	Definition
$C_{p1}^{p2}$	Transform matrix from $p1$ frame to $p2$ frame
${\mathrm{\omega }}_{ie}$	Angular rate of Earth rotation
$g$	Gravity
$L$	Latitude
$\text{V }$	Velocity
$\mathrm{\epsilon }$	Gyro drift
$\nabla$	Acceleration bias
$\varphi$	Misalignment angle
$f$	Specific Force

3. The Algorithmic Scheme for an Alignment Algorithm in an Inertial Frame

The alignment algorithm in an inertial frame is based on the consideration that the Earth rate is constant in a body inertial reference frame, and we can get north from the projection of the gravity in the inertial reference frame which defines a cone whose main axis is the rotational axis of the Earth [3,7].

The traditional alignment algorithm in an inertial frame is presented in [3,4,5,7]. It is usually decomposing the strapdown matrix $C_b^n(t)$ (which represents the orientation of the b frame relative to the $n$ frame) as per Equation (1):

$$C_b^n(t)=C_e^n{C}_i^e(t)C_{i_{b0}}^i{C}_b^{i_{b0}}(t)$$

(1)

Under mooring conditions, $C_e^n$ is a function of latitude $L$, and $C_i^e(t)$ is a function of time $t$.

$$C_e^n=\left[\begin{array}{ccc}0& 1& 0\\ -\sin L& 0& \cos L\\ \cos L& 0& \sin L\end{array}\right]$$

(2)

$$C_i^e(t)=\left[\begin{array}{ccc}\cos ({\mathrm{\omega }}_{ie}t)& \sin ({\mathrm{\omega }}_{ie}t)& 0\\ -\sin ({\mathrm{\omega }}_{ie}t)& \cos ({\mathrm{\omega }}_{ie}t)& 0\\ 0& 0& 1\end{array}\right]$$

(3)

where $C_b^{i_{b0}}(t)$ can be updated by the gyro output (its initial value is a unit matrix):

$${\dot{\text{C}}}_b^{i_{b0}}\left(t\right)=C_b^{i_{b0}}\left(t\right)\left[\left({\omega }_{i_{b0}}^b\right)\times \right]$$

(4)

where $\left[\left({\omega }_{i_{b0}}^b\right)\times \right]$ is the skew symmetric matrix of the vector ${\omega }_{i_{b0}}^b$ measured by the gyroscopes representing the angular rate of $b$ -frame with respect to $i_{b0}$-frame, and $C_{i_{b0}}^i$ is calculated as:

$$C_{i_{b0}}^i={\left[\begin{array}{c}{\left[g^i(t_1)\right]}^T\\ {\left[g^i(t_2)\right]}^T\\ {\left[g^i(t_1)\times g^i(t_2)\right]}^T\end{array}\right]}^{-1}\left[\begin{array}{c}{\left[g^{i_{b0}}(t_1)\right]}^T\\ {\left[g^{i_{b0}}(t_2)\right]}^T\\ {\left[g^{i_{b0}}(t_1)\times g^{i_{b0}}(t_2)\right]}^T\end{array}\right]$$

(5)

where, $g^i$ and $g^{i_{b0}}$ represent the projections of the gravity in the $i$ frame and the $i_{b0}$ frame, respectively. Since $g^n$ is known, the projection in the inertial frame can be calculated as:

$$g^i=C_e^i(t)C_n^e{g}^n$$

(6)

In order to restrain the interference of disturbing acceleration, we usually use the following equation instead of Equation (5):

$$C_{i_{b0}}^i={\left[\begin{array}{c}{\left[V^i(t_1)\right]}^T\\ {\left[V^i(t_2)\right]}^T\\ {\left[V^i(t_1)\times V^i(t_2)\right]}^T\end{array}\right]}^{-1}\left[\begin{array}{c}{\left[V^{i_{b0}}(t_1)\right]}^T\\ {\left[V^{i_{b0}}(t_2)\right]}^T\\ {\left[V^{i_{b0}}(t_1)\times V^{i_{b0}}(t_2)\right]}^T\end{array}\right]$$

(7)

where $V^i(t_j)={\displaystyle \int g^i(t_j)}dt$, $V^{i_{b0}}(t_j)={\displaystyle \int g^{i_{b0}}(t_j)}dt\text{=}{\displaystyle \int {\widehat{\text{C}}}_b^{i_{b0}}{f}^b(t_j)}dt$, ($j=1,2$).

Taking into account that $C_e^n$, $C_i^e(t)$, $C_b^{i_{b0}}(t)$, and $g^i$ can all be calculated by the parameters known or measured, how to extract the pure gravity projection in the $i_{b0}$ frame from the output of accelerometers is the essential operation we have to carry out.

Figure 2 illustrates the algorithmic scheme.

Figure 2. Flow chart of alignment algorithm in an inertial frame.

Figure 2. Flow chart of alignment algorithm in an inertial frame.

4. The Establishment of a Horizontal Reference Frame

In Appendix A, we analyze the propagation of errors, and find it has the same precision as other methods. Therefore, if we can isolate the interference errors, the inertial frame alignment algorithm can be used for fine alignment.

In Section 3, we have pointed out that the essential and difficult process is to extract the pure gravity from measuring signals which are often interfered with by environmental disturbance. In order to obtain the accurate gravity projection in the $i_{b0}$ frame, we proposed a clever solution by using a feature of gravity. It is easy to obtain the pure gravity once an accurate horizontal reference frame is established, since the projection of gravity in the h-frame has nothing to do with the heading, and it is also relatively easy to establish the h-frame. In this section, we will show how to establish an accurate horizontal reference frame.

4.1. Nonlinear Error Model of a SINS

Traditional linear differential equations are based on the assumption that the misalignment angles are small. However, for a small misalignment angle model, a coarse alignment is necessary. To improve the accuracy and reduce the time, the nonlinear error model of large misalignment angle for a SINS described in [24] is adopted in this paper (see Section 2.1 in [24]).

Attitude error equation:

$$\dot{\mathrm{\varphi }}=C_{\omega }^{-1}\left[\left(I-C_n^{n^{\prime }}\right){\widehat{\mathrm{\omega }}}_{in}^n+C_n^{n^{\prime }}\delta {\omega }_{in}^n-C_b^{n^{\prime }}\delta {\omega }_{ib}^b\right]$$

(8)

Velocity error equation:

$$\delta {\dot{\text{v}}}^n=\left[I-{\left(C_n^{n^{\prime }}\right)}^T\right]C_b^{n^{\prime }}{\widehat{\text{f}}}^b+C_b^n{\nabla }^b-\left(2\delta {\omega }_{ie}^n+\delta {\omega }_{en}^n\right)\times \left({\widehat{v}}^n-\delta v^n\right)-\left(2{\widehat{\mathrm{\omega }}}_{ie}^n+{\widehat{\mathrm{\omega }}}_{en}^n\right)\times \delta v^n+C_b^n{w}_a^b$$

(9)

Position error equations:

$$\{\begin{array}{c}\delta \dot{\text{L}}=\frac{\delta v_N^n}{R_M}\\ \delta \dot{\mathrm{\lambda }}=\frac{\sec L}{R_N}\delta v_E^n+\frac{v_E^n\sec L\tan L}{R_N}\delta L\end{array}$$

(10)

where superscript $n^{\prime }$ donates the calculation navigation reference frame, and $\varphi ={\left[\begin{array}{ccc}{\varphi }_x& {\varphi }_y& {\varphi }_z\end{array}\right]}^T$ is the Euler error angle vector. $c({\varphi }_i)$ and $s({\varphi }_i)$ denote $\cos {\varphi }_i$ and $\sin {\varphi }_i$, respectively.

$$C_n^{n^{\prime }}=\left[\begin{array}{ccc}c({\varphi }_y)c({\varphi }_z)-s({\varphi }_y)s({\varphi }_x)s({\varphi }_z)& c({\varphi }_y)s({\varphi }_z)+s({\varphi }_y)s({\varphi }_x)c({\varphi }_z)& -s({\varphi }_y)c({\varphi }_x)\\ -c({\varphi }_x)s({\varphi }_z)& c({\varphi }_x)c({\varphi }_z)& s({\varphi }_x)\\ s({\varphi }_y)c({\varphi }_z)+c({\varphi }_x)s({\varphi }_x)s({\varphi }_z)& s({\varphi }_y)s({\varphi }_z)-c({\varphi }_y)s({\varphi }_x)c({\varphi }_z)& c({\varphi }_x)c({\varphi }_y)\end{array}\right]$$

(11)

$$C_{\omega }=\left[\begin{array}{ccc}c({\varphi }_y)& 0& -s({\varphi }_y)c({\varphi }_x)\\ 0& 1& s({\varphi }_x)\\ s({\varphi }_y)& 0& c({\varphi }_y)c({\varphi }_x)\end{array}\right]$$

(12)

$${\widehat{\mathrm{\omega }}}_{in}^n={\widehat{\mathrm{\omega }}}_{ie}^n+{\widehat{\mathrm{\omega }}}_{en}^n$$

(13)

$${\mathrm{\delta }\mathrm{\omega }}_{in}^n={\mathrm{\delta }\mathrm{\omega }}_{ie}^n+{\mathrm{\delta }\mathrm{\omega }}_{en}^n$$

(14)

$${\mathrm{\delta }\mathrm{\omega }}_{ib}^b={\mathrm{\epsilon }}^b+w_g^b$$

(15)

where ${\widehat{\mathrm{\omega }}}_{ie}^n$ is the calculated Earth’s rotating angular rate, ${\widehat{\mathrm{\omega }}}_{en}^n$ is the calculated angular rate vector, and ${\mathrm{\delta }\mathrm{\omega }}_{in}^n$ is the calculated error vector of ${\mathrm{\omega }}_{in}^n$. ${\mathrm{\delta }\mathrm{\omega }}_{ie}^n$ and ${\mathrm{\delta }\mathrm{\omega }}_{en}^n$ are respectively the error vectors of ${\widehat{\mathrm{\omega }}}_{ie}^n$ and ${\widehat{\mathrm{\omega }}}_{en}^n$ ${\mathrm{\epsilon }}^b$ and $w_g^b$ are the gyro constant drift vector and the zero-mean Gaussian white noise vector, respectively. ${\widehat{f}}^b$ and $\mathrm{\delta }{f}^b$ denote the specific force vector and its corresponding error vector, respectively. ${\widehat{v}}^n$ and $\mathrm{\delta }{v}^n$ are calculated velocity vector and its corresponding error vector, respectively. $R_M$ and $R_N$ are the Earth’s radii of the meridian circle and the prime vertical circle, respectively.

4.2. The Dimension Reduction Gauss-Hermite Filter

The Gauss-Hermite filter (GHF) is one of the sigma point filters. It has proved to be efficient and successful in solving estimation problems when the state and noise distributions are Gaussian. It is usually used as a benchmark algorithm, since its accuracy and stability are the highest among numerous Gaussian approximation filters [23,25,26] (the algorithm framework see Appendix B). However, the “curse of dimensionality” would seriously affect the real-time performance for high dimensional systems [23,27]. From Section 4.1, it is known that only misalignment angle suffers from nonlinearity in the nonlinear error model. This means it is possible to employ a dimension reduction GHF to deal with the alignment task.

In order to establish an accurate horizontal reference frame and reduce the amount of calculation, we employ the dimension reduction GHF algorithm.

The large misalignment angle error model of SINS alignment is a typical nonlinear model that can be described as a general form:

$$x_{k+1}=F_k({\mathrm{\xi }}_k)x_k+g_k({\mathrm{\xi }}_k)+w_k$$

(16)

$$y_k=H_k({\mathrm{\varsigma }}_k)x_k+h_k({\mathrm{\varsigma }}_k)+v_k$$

(17)

where ${\mathrm{\xi }}_k$ is the first $l$ components of $x_k$, ${\mathrm{\varsigma }}_k$ is arbitrary $s$ components of $x_k$.

The dimension reduction GHF algorithm is shown in Figure 3.

Figure 3. The flow chart of the dimension reduction GHF.

Figure 3. The flow chart of the dimension reduction GHF.

where,

$${\mathrm{\Phi }}_{k-1}({\mathrm{\xi }}_{k-1})=F_{k-1}({\mathrm{\xi }}_{k-1})\left({\widehat{x}}_{k-1}+S_{k-1}\left[\begin{array}{c}{(S_{k-1}^{\mathrm{\xi }})}^{-1}({\mathrm{\xi }}_{k-1}-{\widehat{\xi }}_{k-1})\\ 0\end{array}\right]\right)+g_{k-1}({\mathrm{\xi }}_{k-1})$$

(18)

$${\mathrm{\Psi }}_{k-1}({\mathrm{\xi }}_{k-1})={\mathrm{\Phi }}_{k-1}({\mathrm{\xi }}_{k-1}){\mathrm{\Phi }}_{k-1}^T({\mathrm{\xi }}_{k-1})+F_{k-1}({\mathrm{\xi }}_{k-1})S_{k-1}\left[\begin{array}{cc}0& 0\\ 0& I_{n-l}\end{array}\right]S_{k-1}^T{F}_{k-1}^T({\mathrm{\xi }}_{k-1})$$

(19)

$${\mathrm{\Theta }}_k({\mathrm{\varsigma }}_k)={{\widehat{x}}^{\prime }}_{k/k-1}+S_{k/k-1}\left[\begin{array}{c}{(S_{k/k-1}^{\mathrm{\varsigma }})}^{-1}({\mathrm{\varsigma }}_k-{\widehat{\mathrm{\varsigma }}}_{k/k-1})\\ 0\end{array}\right]$$

(20)

$${\mathrm{\Omega }}_k({\mathrm{\varsigma }}_k)={\mathrm{\Theta }}_k({\mathrm{\varsigma }}_k){\mathrm{\Theta }}_k^T({\mathrm{\varsigma }}_k)+S_{k/k-1}\left[\begin{array}{cc}0& 0\\ 0& I_t\end{array}\right]S_{k/k-1}^T$$

(21)

$${\mathrm{{\rm M}}}_k({\varsigma }_k)={H^{\prime }}_k({\mathrm{\varsigma }}_k){\mathrm{\Theta }}_k({\mathrm{\varsigma }}_k)+h_k({\mathrm{\varsigma }}_k)$$

(22)

$${\mathrm{\Pi }}_k({\mathrm{\varsigma }}_k)={\mathrm{{\rm M}}}_k^T({\mathrm{\varsigma }}_k){\mathrm{{\rm M}}}_k^T({\mathrm{\varsigma }}_k)+{H^{\prime }}_k({\mathrm{\varsigma }}_k){S^{\prime }}_{k/k-1}\left[\begin{array}{cc}0& 0\\ 0& I_t\end{array}\right]{({S^{\prime }}_{k/k-1})}^T{F}^T({\mathrm{\varsigma }}_k)$$

(23)

where, $P^{\mathrm{\xi }}$ denotes the first l-th rows and the first l-th columns of the matrix $P$; $S^{\mathrm{\xi }}$ and $S$ are obtained from $P^{\mathrm{\xi }}$ and $P$, respectively, through a Cholesky decomposition; that is $P=S{S}^T$, $P^{\mathrm{\xi }}=S^{\mathrm{\xi }}{(S^{\mathrm{\xi }})}^T$. $\widehat{\xi }$ denotes the first l-th components of $\widehat{x}$, and $m=n-l$. The quadrature points $\left\{{\mathrm{\xi }}_i\right\}$ and the associated weights $\left\{{\mathrm{\omega }}_i\right\}$ are determined by the Gauss–Hermite quadrature rule (see Appendix C).

4.3. The Horizontal Alignment for the Large Misalignment Angle Model Based on the Dimension Reduction Gauss-Hermite Filter

As mentioned above, the nonlinear model for horizontal alignment is established under the large misalignment angle in this paper. Considering 13 state variables—the east velocity error $\mathrm{\delta }{v}_E$ and the north velocity erro $\mathrm{\delta }{v}_N$; the Euler misalignment angle errors ${\varphi }_x$ ${\varphi }_y$ and ${\varphi }_z$; the latitude error $\mathrm{\delta }L$ and the longitude erro $\mathrm{\delta }\mathrm{\lambda }$; the accelerometer zero-biases ${\nabla }_x^b$ ${\nabla }_y^b$ and ${\nabla }_z^b$; the constant gyro drifts ${\mathrm{\epsilon }}_x^b$ ${\mathrm{\epsilon }}_y^b$ and ${\mathrm{\epsilon }}_z^b$—the state vector is built up as

$$X={\left[\begin{array}{ccccccccccccc}\mathrm{\delta }{v}_E& \mathrm{\delta }{v}_N& {\varphi }_x& {\varphi }_y& {\varphi }_z& \mathrm{\delta }L& \mathrm{\delta }\lambda & {\nabla }_x^b& {\nabla }_y^b& {\nabla }_z^b& {\mathrm{\epsilon }}_x^b& {\mathrm{\epsilon }}_y^b& {\mathrm{\epsilon }}_z^b\end{array}\right]}^T$$

The corresponding state equation is written as:

$${\dot{\text{X}}}_{k+1}=f(X_k)+W_k$$

The state function $f(·)$ is obtained from Equations (8)–(10).

We choose the quadrature point $m=3$, then the total number of points $N_p=3^{13}$, and that is a great amount of computation. However, according to Section 4.1, we know that only the Euler misalignment angle errors ${\varphi }_x$, ${\varphi }_y$, and ${\varphi }_z$ are suffering from nonlinearity; therefore, the dimension reduction Gauss-Hermite filter proposed in Section 4.2. can be adopted, and the number of points will be reduced to $N_p=3^3$. That greatly reduces the computational burden.

5. Calculation of the Gravity Direction

In Section 5, we present a method to set-up an accuracy horizontal reference frame in detail. After the fine alignment, we obtain the transfer matrix $C_b^h$, that is to say, the accuracy horizontal reference frame is established. Then, the projection of the gravity in the $i_{b0}$ frame can be calculated as:

$$g^{i_{b0}}(t)=C_b^{i_{b0}}(t){[C_b^h]}^{-1}{g}^h$$

(24)

where, $g^h=g^n={\left[\begin{array}{ccc}0& 0& g\end{array}\right]}^T$, $C_b^{i_{b0}}(t)$ is updated by the gyros’ output in real time, and $C_b^h$ is obtained from the fine horizontal alignment.

In order to improve the accuracy further, a weighted smoothing algorithm is adopted to inhibit the interference noise caused by the winds and waves. The algorithm is described as follows:

Assume that $t_i(i\in (1,N))$ is the sampling period, $g_i$ is the corresponding sampling data, and the weight coefficient is 1. Then, the smoothed data is calculated as:

$$\overline{g}=\frac{1}{N+1}{\displaystyle \sum _{i=-N/2}^{N/2}{g}_i}$$

(25)

Since it is a coning motion of gravity in an inertial frame ($i_{b0}$ frame or $i$ frame), as Figure 4, the projection of $g^{i_{b0}}$ in the $i_{b0}$ frame is a sine curve, and the period is 24 h. Compared with the alignment time, the period is so long that we can consider $g^{i_{b0}}(t)$ as linear $g$.

Then,

$$g_N^{i_{b0}}=C_{t_M}^{t_N}\overline{g}$$

(26)

where the transfer matrix from $n_M$ frame (at $t_M$) to $n$ frame (at $t_N$) could be calculated as follows:

$$C_{t_M}^{t_N}=C_b^{ib0}(t_N)\cdot {\left[C_b^{ib0}(t_M)\right]}^{-1}$$

(27)

We can smooth $g^{i_{b0}}$ at 1 and 5 min respectively with Equation (25) after the fine horizontal alignment, and then $C_i^{i_{b0}}$ can be calculated with Equation (4).

Finally, by substituting $C_i^{i_{b0}}$ into Equation (1), the alignment can be completed.

Figure 4. The coning motion of gravity.

Figure 4. The coning motion of gravity.

The flow chart of the improved alignment algorithm is shown in Figure 5.

Figure 5. Flow chart of the improved alignment algorithm.

Figure 5. Flow chart of the improved alignment algorithm.

As shown in the dashed box, we use a clever method to construct matrix $V^{i_{b0}}$. This improvement makes the traditional inertial alignment algorithm able to resist interference.

6. Simulation

In order to test the technique proposed in this paper, a simulation is carried out to compare with the conventional inertial frame alignment algorithm. The main parameters are set as follows in Table 3:

Table 3. Specifications of IMU.

Parameters	Gyro	Accelerometer
Constant bias	0.01°/h	${10}^{-4}g$
Random noise	$0.05$°$\sqrt{\text{h}}$	$0.5\times {10}^{-4}g/\sqrt{\text{Hz}}$

Table 3. Specifications of IMU.

Parameters	Gyro	Accelerometer
Constant bias	0.01°/h	${10}^{-4}g$
Random noise	$0.05$°$\sqrt{\text{h}}$	$0.5\times {10}^{-4}g/\sqrt{\text{Hz}}$

(1) The latitude and longitude: $L=45.7796$°, $\lambda =126.6705$°;

(2) The misalignment angle: ${\varphi }_x=10$°, ${\varphi }_y=10$°, ${\varphi }_z=10$°;

(3) The sampling period is 0.1 s;

(4) The initial attitude is a random value.

(5) The first sampling time is $t_{k1}=70$ s, and the second is $t_{k2}=300$ s.

(6) We set up two situations to test the proposed algorithm, and the model is as follows:

Situation 1: Assume the ship is in the state of rest, without any interference.

Situation 2: Assume that the ship is on the berth and is rocked by the surf and wind. The pitch, roll, and yaw models of a marine vehicle are given by:

$$\mathrm{\psi }={\mathrm{\psi }}_0+{\mathrm{\psi }}_m\sin (2\mathrm{\pi }t/T_{\mathrm{\psi }})$$

(28)

$$\mathrm{\theta }={\mathrm{\theta }}_0+{\mathrm{\theta }}_m\sin (2\mathrm{\pi }t/T_{\mathrm{\theta }})$$

(29)

$$\mathrm{\gamma }={\mathrm{\gamma }}_0+{\mathrm{\gamma }}_m\sin (2\mathrm{\pi }t/T_{\mathrm{\gamma }})$$

(30)

where, $\mathrm{\psi }$ $\mathrm{\theta }$, and $\mathrm{\gamma }$ are yaw, pitch, and roll angles, respectively; the initial attitudes are ${\mathrm{\psi }}_0$ = 30°, ${\mathrm{\theta }}_0={\mathrm{\gamma }}_0=0$; the sway periods are $T_{\mathrm{\psi }}=6$ s, $T_{\mathrm{\theta }}=10$ s, and $T_{\mathrm{\gamma }}=8$ s; ${\mathrm{\psi }}_m$ = 1°, ${\mathrm{\theta }}_m$ = 5°, and ${\mathrm{\gamma }}_m$ = 5°.

In the mooring condition, the horizontal velocities are small values because the mooring line secures the ship to the wharf, but vertical velocity may be not as small as horizontal velocities since a ship will heave along with the sea level fluctuation. So, velocity interference models are given by:

$$V_{dz}=0.5\cdot \sin ({\mathrm{\omega }}_{dz}t+{\mathrm{\phi }}_{dz})$$

(31)

$$V_{dx}=0.02\cdot \sin ({\mathrm{\omega }}_{dx}t+{\mathrm{\phi }}_{dx})$$

(32)

$$V_{dy}=0.02\cdot \sin ({\mathrm{\omega }}_{dy}t+{\mathrm{\phi }}_{dy})$$

(33)

where ${\mathrm{\omega }}_d=\frac{2\mathrm{\pi }}{T_d}$; $T_{dz}=8$ s; $T_{dx}=T_{dy}=2$ s; ${\mathrm{\phi }}_{dx},{\mathrm{\phi }}_{dy},{\mathrm{\phi }}_{dz}$ are random values in $[0,2\mathrm{\pi }]$.

The parameters of the dimension reduction GHF are chosen as follows:

$$\begin{array}{c}{\widehat{x}}_0=[\begin{array}{ccccccc}{10}^{\text{o}}& {10}^{\text{o}}& {10}^{\text{o}}& 0.1\text{m/s}& 0.1\text{m/s}& 0.1\text{m}& 0.1\text{m}\end{array}\\ \begin{array}{cccccc}\begin{array}{cc}& 1\times {10}^{-4}{g}_0\end{array}& 1\times {10}^{-4}{g}_0& 1\times {10}^{-4}{g}_0& {0.01}^{\text{o}}\text{/h}& {0.01}^{\text{o}}\text{/h}& {0.01}^{\text{o}}\text{/h}\end{array}]\end{array}$$

$$\begin{array}{c}{P}_0=diag[\begin{array}{ccccccc}{\left(5^{\text{o}}\right)}^2& {\left(5^{\text{o}}\right)}^2& {\left(5^{\text{o}}\right)}^2& {\left(0.2\text{m/s}\right)}^2& {\left(0.2\text{m/s}\right)}^2& {\left(0.2\text{m}\right)}^2& {\left(0.2\text{m}\right)}^2\end{array}\\ \begin{array}{cccccc}{\left(1\times {10}^{-4}{g}_0\right)}^2& {\left(1\times {10}^{-4}{g}_0\right)}^2& {\left(1\times {10}^{-4}{g}_0\right)}^2& {\left({0.01}^{\text{o}}\text{/h}\right)}^2& {\left({0.01}^{\text{o}}\text{/h}\right)}^2& {\left({0.01}^{\text{o}}\text{/h}\right)}^2\end{array}]\end{array}$$

$$Q=diag\left[\begin{array}{ccccc}{\left(1\times {10}^{-4}{g}_0\right)}^2& {\left(1\times {10}^{-4}{g}_0\right)}^2& {\left({0.01}^{\text{o}}\text{/h}\right)}^2& {\left({0.01}^{\text{o}}\text{/h}\right)}^2& {\left({0.01}^{\text{o}}\text{/h}\right)}^2\end{array}\right]$$

$$R=diag\left[\begin{array}{cc}{\left(0.01\text{m/s}\right)}^2& {\left(0.01\text{m/s}\right)}^2\end{array}\right]$$

For a fair comparison, 100 independent Monte Carlo runs are carried out. The results are as follows; the RMS error results are used to test the horizontal alignment accuracy and time consumed by dimensionality reduction GHF. RMS error is defined in Appendix D.

From Figure 6 and Figure 7, it can be noted that the pitch error and roll error converge to the order of minutes within a few seconds. Figure 8, Figure 9 and Figure 10 show the pitch error, roll error, and yaw error calculated by the traditional algorithm and the proposed algorithm, respectively. Their statistical results are shown in Table 4. From Table 4, we can tell that the accuracy of heading alignment is roughly the same (mean value, standard deviation and maximum value). However, the horizontal alignment results from the proposed algorithm are obvious better, since the standard deviation and the maximum value are smaller; the standard deviation in particular is an order of magnitude smaller than the traditional algorithm.

Table 4. Statistics of situation 1.

	Traditional Algorithm (min)			Improved Algorithm (min)
	Pitch Error	Roll Error	Yaw Error	Pitch Error	Roll Error	Yaw Error
Mean	−0.1677	0.0156	0.3089	−0.0001	−0.2679	0.6363
Std	0.3475	0.3467	3.0679	0.0231	0.0163	4.2559
Max	0.6472	0.5212	7.1117	0.0723	0.3059	11.9556

Table 4. Statistics of situation 1.

	Traditional Algorithm (min)			Improved Algorithm (min)
	Pitch Error	Roll Error	Yaw Error	Pitch Error	Roll Error	Yaw Error
Mean	−0.1677	0.0156	0.3089	−0.0001	−0.2679	0.6363
Std	0.3475	0.3467	3.0679	0.0231	0.0163	4.2559
Max	0.6472	0.5212	7.1117	0.0723	0.3059	11.9556

Figure 6. The RMS error of pitch misalignment angle in situation 1.

Figure 6. The RMS error of pitch misalignment angle in situation 1.

Figure 7. The RMS error of roll misalignment angle in situation 1.

Figure 7. The RMS error of roll misalignment angle in situation 1.

Figure 8. Pitch error result in situation 1.

Figure 8. Pitch error result in situation 1.

Figure 9. Roll error result in situation 1.

Figure 9. Roll error result in situation 1.

Figure 10. Yaw error result in situation 1.

Figure 10. Yaw error result in situation 1.

Figure 11 and Figure 12 show the horizontal alignment results of the nonlinear filter in situation 2. The pitch error and roll error also converge rapidly (within tens of seconds) even though they converge slower than situation 1. Figure 13, Figure 14 and Figure 15 show the pitch error, roll error, and yaw error calculated by traditional algorithm and the proposed algorithm in situation 2, respectively. Their statistical results are shown in Table 5. From Table 5, we can draw the conclusion that the proposed algorithm performs much better than the traditional one, because not only is the horizontal alignment accuracy an order of magnitude higher than the traditional one’s (standard deviation and maximum value), but also the heading alignment accuracy of the proposed algorithm is better than the traditional one’s. Compared with Table 4 and Table 5, we can see that angular velocities and velocities have little effect on the proposed algorithm, but will greatly affect the precision of the traditional algorithm.

Figure 11. The RMS error of pitch misalignment angle in situation 2.

Figure 11. The RMS error of pitch misalignment angle in situation 2.

Figure 12. The RMS error of roll misalignment angle in situation 2

Figure 12. The RMS error of roll misalignment angle in situation 2

Figure 13. Pitch error result in situation 2.

Figure 13. Pitch error result in situation 2.

Figure 14. Roll error result in situation 2.

Figure 14. Roll error result in situation 2.

Figure 15. Yaw error result in situation 2.

Figure 15. Yaw error result in situation 2.

Table 5. Statistics of situation 2.

	Traditional Algorithm (angular minute)			Improved Algorithm (angular minute)
	Pitch Error	Roll Error	Yaw Error	Pitch Error	Roll Error	Yaw Error
Mean	−1.6843	−0.0872	2.5054	−0.0172	−0.2397	4.1585
Std	0.5111	0.5033	29.3313	0.02248	0.0174	4.2575
Max	2.4103	0.7588	44.1109	0.0680	0.2863	14.1987

Table 5. Statistics of situation 2.

	Traditional Algorithm (angular minute)			Improved Algorithm (angular minute)
	Pitch Error	Roll Error	Yaw Error	Pitch Error	Roll Error	Yaw Error
Mean	−1.6843	−0.0872	2.5054	−0.0172	−0.2397	4.1585
Std	0.5111	0.5033	29.3313	0.02248	0.0174	4.2575
Max	2.4103	0.7588	44.1109	0.0680	0.2863	14.1987

7. Experiments

In order to evaluate the performance of the proposed self-alignment method for SINS, in this section, the mooring experiment was conducted in the East Sea of China. In this experiment, the ship was moored to the pier. A self-made SINS was used for the experiment, and the attitude reference was given by a PHINS (made by iXBlue Company) as shown in Figure 16. The self-made SINS and the PHINS were fixed on a rigid aluminum alloy board, and then the installation error was measured and compensated for in the stationary state. The data acquisition computer collected the data of the self-made SINS and the PHINS synchronously. We carried out the alignment experiments three times.

The PHINS worked in GPS aided mode, its performance is as follows: pitch and roll errors are less than 0.01°, and heading error is less than 0.02°. In this experiment, we used the dimension reduction GHF mentioned in Section 4.2 to implement horizontal alignment. During the mooring experiments, the parameters of the dimension reduction GHF were optimally chosen as follows:

$$\begin{array}{c}{\widehat{x}}_0=[\begin{array}{ccccccc}{0}^{\text{o}}& 0^{\text{o}}& 0^{\text{o}}& 0\text{m/s}& 0\text{m/s}& 0\text{m}& 0\text{m}\end{array}\\ \begin{array}{cccccc}\begin{array}{cc}& 1\times {10}^{-4}{g}_0\end{array}& 1\times {10}^{-4}{g}_0& 1\times {10}^{-4}{g}_0& {0.01}^{\text{o}}\text{/h}& {0.01}^{\text{o}}\text{/h}& {0.01}^{\text{o}}\text{/h}\end{array}]\end{array}$$

$$\begin{array}{c}{P}_0=diag[\begin{array}{ccccccc}{\left(1^{\text{o}}\right)}^2& {\left(1^{\text{o}}\right)}^2& {\left(3^{\text{o}}\right)}^2& {\left(0.2\text{m/s}\right)}^2& {\left(0.2\text{m/s}\right)}^2& {\left(1\text{m}\right)}^2& {\left(1\text{m}\right)}^2\end{array}\\ \begin{array}{cccccc}{\left(1\times {10}^{-4}{g}_0\right)}^2& {\left(1\times {10}^{-4}{g}_0\right)}^2& {\left(1\times {10}^{-4}{g}_0\right)}^2& {\left({0.01}^{\text{o}}\text{/h}\right)}^2& {\left({0.01}^{\text{o}}\text{/h}\right)}^2& {\left({0.01}^{\text{o}}\text{/h}\right)}^2\end{array}]\end{array}$$

$$Q=diag\left[\begin{array}{ccccc}{\left(1\times {10}^{-4}{g}_0\right)}^2& {\left(1\times {10}^{-4}{g}_0\right)}^2& {\left({0.01}^{\text{o}}\text{/h}\right)}^2& {\left({0.01}^{\text{o}}\text{/h}\right)}^2& {\left({0.01}^{\text{o}}\text{/h}\right)}^2\end{array}\right]$$

$$R=diag\left[\begin{array}{cc}{\left(0.1\text{m/s}\right)}^2& {\left(0.1\text{m/s}\right)}^2\end{array}\right]$$

Figure 16. Self-made SINS, PHINS, and data acquisition computer.

Figure 16. Self-made SINS, PHINS, and data acquisition computer.

The results are shown as follows. Figure 17 shows the estimations of misalignment angle.

From Figure 17, the estimations of misalignment angle converge within a few seconds. In order to ensure the estimations of misalignment angle are available, the pitch error and roll error are compensated for after 60 s. Figure 18 shows the attitude of the PHINS and the self-made SINS. For convenience, the initial 60 s part was omitted. We note that the difference between the two curves is very small (less than 0.05′). This indicates that the accurate horizontal reference frame is already established successfully. It is also proved that the estimations of the pitch error and roll error are accurate. From the attitude curves in Figure 18 and the velocity curves in Figure 19, we can also find that there are periodic disturbances during the alignment process.

Figure 17. Estimations of misalignment angle.

Figure 17. Estimations of misalignment angle.

Figure 18. Attitude of PHINS and self-made SINS.

Figure 18. Attitude of PHINS and self-made SINS.

Figure 19. Velocity of PHINS.

Figure 19. Velocity of PHINS.

Figure 20 presents the alignment results for the 3-time experiment. It is clear from Figure 20 that the alignment results of the improved algorithm are better than the traditional algorithm. The pitch errors and the roll errors of the improved algorithm are less than 0.06°, and yaw errors are less than 0.2°. That meets the accuracy requirements of a medium-accuracy marine SINS.

Figure 20. Comparison of the misalignment angles. (a) Pitch error; (b) roll error; (c) yaw error.

Figure 20. Comparison of the misalignment angles. (a) Pitch error; (b) roll error; (c) yaw error.

8. Conclusions

An improved inertial frame alignment algorithm based on horizontal alignment for marine SINS is proposed in this paper. The major improvement of this work is establishing a horizontal reference frame using a dimension reduction Gauss-Hermite filter. Based on that, the projection of gravity in the body inertial reference frame can be calculated and take the place of the accelerometer output to calculate the attitude matrix.

The dimension reduction Gauss-Hermite filter algorithm is detailed in this paper. The simulation and experimental results indicate that it can quickly and accurately complete the horizontal alignment. However, the parameters P₀, Q, and R which affect the filtering performance are usually chosen according to prior knowledge, as we have not yet found a mathematical method to get the optimal parameters. This means the selection of P₀, Q, and R is rather a matter of tuning. We also derive the error propagation equation and point out that the inertial frame alignment algorithm has the same theoretical accuracy as other algorithms.

The results of the simulation and the experiment also show that, compared with the traditional inertial frame alignment algorithm, the proposed algorithm can resist velocity and angular velocity interference to obtain higher accuracy, and meets the requirement of a medium-accuracy inertial navigation system.