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

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.


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.

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 h z axis is aligned with n z axis, but the horizontal axes are arbitrary in the horizontal plane.

e-frame
Earth-centered Earth-fixed orthogonal reference frame. Its x e 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.
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.

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 ( ) n b C t (which represents the orientation of the b frame relative to the n frame) as per Equation (1): C t can be updated by the gyro output (its initial value is a unit matrix): where, i g and 0 b i g represent the projections of the gravity in the i frame and the 0 b i frame, respectively. Since n g is known, the projection in the inertial frame can be calculated as: ( ) i i e n e n g C t C g = (6) In order to restrain the interference of disturbing acceleration, we usually use the following equation instead of Equation (5) Taking into account that n e C , ( ) is the essential operation we have to carry out. Figure 2 illustrates the algorithmic scheme.

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 0 b i 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.

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: Velocity error equation: Position error equations: where superscript n′ donates the calculation navigation reference frame, and where ω n ie is the calculated Earth's rotating angular rate, ω n en is the calculated angular rate vector, and δω n in is the calculated error vector of ω n in . δω n ie and δω n en are respectively the error vectors of ω n ie and ω n en ε b and b g w are the gyro constant drift vector and the zero-mean Gaussian white noise vector, respectively. ˆb f and δ b f denote the specific force vector and its corresponding error vector, respectively. ˆn v and δ n v are calculated velocity vector and its corresponding error vector, respectively. M R and N R are the Earth's radii of the meridian circle and the prime vertical circle, respectively.

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: The dimension reduction GHF algorithm is shown in Figure 3.
where, ξ P denotes the first l-th rows and the first l-th columns of the matrix P ; ξ S and S are obtained from ξ P and P , respectively, through a Cholesky decomposition; that is

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 The corresponding state equation is written as: We choose the quadrature point 3 m = , then the total number of points 13 3 p N = , and that is a great amount of computation. However, according to Section 4.1, we know that only the Euler misalignment angle errors x φ , y φ , and 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 3 3 p N = . That greatly reduces the computational burden.

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 h b C , that is to say, the accuracy horizontal reference frame is established. Then, the projection of the gravity in the 0 b i frame can be calculated as: where, C t is updated by the gyros' output in real time, and h b C 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 (i (1, N)) i t ∈ is the sampling period, i g is the corresponding sampling data, and the weight coefficient is 1. Then, the smoothed data is calculated as: where the transfer matrix from M n frame (at M t ) to n frame (at N t ) could be calculated as follows: We can smooth 0  The flow chart of the improved alignment algorithm is shown in Figure 5. As shown in the dashed box, we use a clever method to construct matrix 0 b i V . This improvement makes the traditional inertial alignment algorithm able to resist interference.

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.
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: 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 Figures 6 and 7, it can be noted that the pitch error and roll error converge to the order of minutes within a few seconds. Figures 8-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.      The pitch error and roll error also converge rapidly (within tens of seconds) even though they converge slower than situation 1. Figures 13-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 Tables 4 and 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.

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 Traditional algorithm Improved algorithm 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:   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 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.

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 P0, 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 P0, 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. C t can be expressed as: Because Equations (2) and (3) are respectively the functions of latitude and time, and they are exactly known during the alignment process, so that δ n e C and δ ( ) C t is calculated through a differential equation:  which is measured by the gyros, represents the turn rate of the b frame with respect to the i frame; and ω =ω +ε the gyro bias. Obviously, ε b directly produces the error in the calculation of 0 ( ) b i b C t . We note that to solve Equation (7) we must carry out two integrations of ε b , which will certainly increase the calculation error.
In order to facilitate analysis, we define φ as the attitude error angle, Substitute Equations (A3)-(A5) into Equation (1), and ignore the second-order small items. We get: where the similarity transformation theorem for matrix is used, and Considering that

substitute Equation (A5) into Equation (4) and
ignore the second-order small items, and we can obtain: By solving Equation (17), we get: Then, by substituting Equation (18) into Equation (14), 2 n φ can be expressed as follows: cos ω sin ω 0 = ( )= sin sin ω sin cos ω cos cos sin ω -cos cos ω sin cos ω sin sin ω cos sin ω sin ω sin cos ω cos cos ω Next, let us analyze the error of Then, substitute Equation (A17) into Equation (5) and define ( ) Substitute Equation (A17) into Equation (A20) and ignore second-order small item, and we get According to Equation (6), we know that ( ) 1 i G − will not introduce any error during the calculation process. Therefore, According to reference [1], we have Compare Equations (A4) and (A24), we know that According to Equations (A16), (A32) and (A33), we can get the following result: the misalignment angle caused by gyro error is The misalignment angle caused by accelerometers' error is expressed as Equation (A32). For further analysis, assume that 360 t s = , 4 1 10 . Therefore, it can be calculated approximately and we find that the error terms consisting of t are very small and can be ignored. Therefore, 3 ε + tan cos ω n n n n e a g e e ie g g L L g Through analysis, we know that the pitch angle error and the roll angle error depend on the accelerometer bias. The yaw angle error is determined by equivalent east gyro drift, equivalent east accelerometer bias and latitude.

B. Gaussian Approximation Filters
Consider the following discrete-time nonlinear state-space model: where, {1, 2, } k ∈  is discrete time, k x and k y are the state vector and the measurement vector at time k , respectively; k w and k v are the noise vectors from two independent zero-mean Gaussian processes with their covariance matrices k Q and k R , respectively .
Under the assumption of Gaussian distributions, the general form of Gaussian approximation filtering algorithm can be summarized as follows: The integrals in can be approximated by the GHQ, UT, or cubature rule [20,22,23]. The generic quadrature-based Gaussian approximation filter is given as [22,23]. Prediction: ( )

C. Gauss-Hermite Quadrature Point Selection
For the univariate standard Gaussian distribution with m quadrature points ( , ) i i