Initial Alignment Algorithm Based on the DMCS Method in Single-Axis RSINS with Large Azimuth Misalignment Angles for Submarines

Since the inertial sensor error has been modulated effectively by the Rotation Modulation Technique (RMT), the Rotation Strapdown Inertial Navigation System (RSINS) has been widely used for submarines in order to satisfy the requirement of high-accuracy and long working duration. The performance of the initial alignment is main factor affecting the accuracy of the Strapdown Inertial Navigation System (SINS). The traditional initial alignment algorithm based on the compass method has bad performance when the misalignment angle is large, which will make the submarine SINS fail to launch properly in a complex operating environment. Since the RSINS uses the mathematical platform to calculate the navigation information, it allows multiple algorithms to run simultaneously, and different algorithms do not interact with each other. Thus, to improve the alignment accuracy, an initial alignment algorithm based on the Dual Mathematical Calculation System (DMCS) is proposed; moreover, to solve the problem of large azimuth misalignment angle, an improved DMCS-based alignment algorithm is also presented in this paper. Both simulations and experiments showed that the novel algorithm can effectively improve the initial alignment performance under the large misalignment angle environment, enhancing the environmental suitability of the RSINS.


Introduction
The Strapdown Inertial Navigation System (SINS) can provide the attitude, velocity and position of a vehicle by utilizing the gyroscope and the accelerometer to measure its linear velocity and angular rate, respectively. Due to its numerous merits, such as being autonomous, all-weather, strong real time, multi-parameter measurement, and so on, SINS has been widely used in submarines and other underwater vehicles [1][2][3]. However, the accumulated positioning error caused by the inertial sensor error will severely affect the navigation performance of underwater vehicles [4,5]. In addition, with the rapid development of underwater navigation technology, the requirement of SINS performance is much higher and higher. Therefore, to improve the SINS' performance in long-term navigation systems, reducing and compensating the inertial sensor error are becoming critical problems now.
Through the periodic rotation of the turntable, the inertial sensor error can be modulated by the Rotation Modulation Technique (RMT) into the sine or cosine signal, the integral of which is zero in an entire period, thereby restraining the influence of the inertia sensor error [6,7]. Without any external information, the RMT is an effective self-compensation technique, so it has been widely used in error suppression of the SINS. According to the number of rotational axes, the RMT is divided into three kinds, including the single-axis RMT, the dual-axis RMT and the tri-axis RMT [8,9]. Compared with The paper is organized as follows. Section 2 introduces the principles of the single-axis RSINS and the initial alignment of the RSINS. A novel alignment algorithm based on the DMCS and traditional compass self-aligning method and an improved DMCS-based initial alignment algorithm with a large azimuth misalignment angle of the RSINS for submarines are proposed in Section 3. Sections 4 and 5 give the simulations and real tests, respectively. Section 6 concludes the whole manuscript.

The Principle of Single-Axis RSINS
In the RSINS, the Inertial Measurement Unit (IMU) is installed on a turntable, which is mounted on the vehicle. The IMU is rotated according to a certain rule by controlling the turntable, and thus, the inertial sensor error can be modulated as a periodic signal, which can be eliminated during an integration period, reducing the influence of the inertial sensor on the navigation accuracy. The navigation solution of the RSINS is similar to the one of the traditional SINS, except that the output attitude should be decoupled according to the rotation information given by the turntable [24]. Therefore, in this manuscript, to improve the accuracy of the submarine's navigation system, the RSINS based on the RMT is used.
The installation diagram of single-axis RSINS is illustrated in Figure 1. The ox s y s z s coordinate frame, denoted by s, is the body coordinate frame that rotates with the single rotation axis; b is the body coordinate frame actually used in the RSINS. Assume that s and b are coincident when the single-axis turntable is at the zero position, and in this installation mode, the relationship between the measurement in b and s is shown as follows: where f b , f s are the specific force in the b frame and s frame; ω b ib , ω s is denote the angular rate in the b frame and s frame, respectively; ω b sb is the projection of the rotation angular rate of the b frame to s frame in the b frame; C b s indicates the coordinate transformation matrix between the s frame and b frame. Since the rotation angular rate of the turntable ω r is known, C b s can be described as: where t is the rotation time.
Define thatf s andω s is are inertial sensor measurements: In the case that C b s and ω b sb are already known, we can obtain the measurement in b, denoted bỹ Therefore, the corresponding sensor error is shown as follows: From Equations (6) and (7), we can see that sensor errors in the x-axis and y-axis are modulated as periodic signals, whose integrals are zeros throughout the period theoretically. After setting an appropriate rotating angular rate ω r utilizing the method presented in [27], this kind of periodic signal can be eliminated to some extent. Therefore, the single-axis RSINS can effectively reduce the influence of the inertial sensor error on the navigation accuracy, thereby improving the navigation accuracy.

Coarse Alignment
Since the RSINS's working principle is similar to the traditional SINS, except the attitude decoupling, the coarse alignment of RSINS is also divided into the horizontal coarse alignment and the azimuth coarse alignment, which are usually carried out at the same time.
The horizontal coarse alignment is performed by utilizing the gravity vector, which is used to calculate the horizontal attitude angle of vehicles in the current moment roughly. To simplify the analysis without loss of generality, we assume that the body frame b coincides with the navigation coordinate frame (n), that means C n b = I, and then, the accelerometer bias and the gyroscope drift in n frame after modulation are: cos ω r t − ε sy sin ω r t ε y = ε sx sin ω r t + ε sy cos ω r t ε z = ε sz cos ω r t − ε sy sin ω r t (9) wherein the variable V x,y,z means the component of the variable V in the n frame along with the x-axis, y-axis and z-axis.
Therefore, the horizontal components of the gravity acceleration measured by accelerometers and gyroscopes are shown as follows: where g is the gravity acceleration; α, β are the north and east horizontal misalignment angles, respectively. In horizontal coarse alignment, the initial horizontal attitude of vehicles can be roughly calculated by using Equation (8) in a short time.
The azimuth rough alignment uses the projection of the Earth's rotation angular rate in the local north direction to roughly calculate the azimuth attitude angle of vehicles at the current moment, shown as (9).
wherein L is the local latitude, ω ie denotes the Earth's rotation angular rate and γ presents the azimuth misalignment angle. Therefore, in the coarse alignment, initial attitude angles of the vehicle can be calculated roughly and quickly by utilizing the gravitational acceleration and the Earth's rotation angular rate [23].

Fine Alignment
Based on the coarse alignment, the fine alignment is proceeded. Compared with the coarse alignment, the fine alignment time is longer, and the accuracy is higher. Aimed at the traditional SINS or RSINS for ships and submarines, the fine alignment is usually done in about thirty minutes, while the coarse alignment is done in two minutes. The level of the fine alignment accuracy can be decreased from degrees to arcmin. Similarly, the fine alignment is also divided into the horizontal fine alignment and the azimuth fine alignment. The former will be proceeded firstly, and then, both of them proceed at the same time [3,23].
In the horizontal fine alignment, the difference between the gravity acceleration and the actual measured acceleration is used. Acceleration errors and misalignment angles in north and east horizontal loops are shown as: where δV x , δV y are the vehicle's velocity errors, respectively; and R is the radius of the Earth.
As we all known that in a pure SINS, the system is in a critical stable state and its errors are periodicity oscillating with the Schuler oscillation frequency [8,25], it is necessary to exert control over the system to ensure its stability. Introducing the damping in the SINS horizontal loop and changing the integral part into the inertial part of the acceleration, then the system will work in the damping state [1,17,28]. To further shorten the oscillation period, the proportional parameter 1/R is replaced by k N and k E in the north and east loops, receptively. Diagrams of the horizontal fine alignment are shown in Figure 2.
According to the Mason gain formula, the characteristic polynomial of the RSINS can be obtained as: According to the final value theorem, the steady-state error of the RSINS can be obtained as: In addition, to eliminate the influence of the gyro bias and azimuth misalignment angle on the steady accuracy of the horizontal alignment, an integration link k U /s is introduced to calculate the control angle rate. This is as shown in Figure 3. According to the Mason gain formula, the characteristic polynomial of the RSINS can be obtained as: Now, the system is still asymptotically stable, and the steady-state error can be expressed as: The azimuth alignment is performed on the basis of the horizontal alignment, aimed at the SINS mathematical platform being northward and determining the initial azimuth angle. In traditional initial alignment methods, the compass alignment method is the most commonly-used azimuth alignment, using the compass effect to adjust the azimuth misalignment angle. The coupling relationship between the azimuth error and the north horizontal loop is called the compass effect, so this kind of initial alignment, the horizontal alignment plus the azimuth compass alignment, is usually named as the compass alignment method directly. Assuming that the azimuth misalignment angle is small, which means, k(γ) = sin γ/γ, γ ≈ 0, sin γ ≈ γ, k(γ) ≈ 1, the diagram of the traditional compass alignment method is shown in Figure 4. According to the Mason gain formula, the characteristic polynomial of the system can be obtained as: The system is still asymptotically stable system, and the steady-state error can be expressed as: From Equations (15), (17) and (19), we can see that horizontal components of inertial sensor errors are modulated to periodic signals, while the vertical component is not. Equation (19) presents that the steady-state error of the azimuth misalignment is related to ε z , which cannot be modulated by the single-axis RMT. Thus, in single-axis RSINS, the accuracy of azimuth misalignment angle cannot be significantly improved, different from the ones of horizontal misalignment angles. Furthermore, if the azimuth misalignment is large, nonlinear terms will appear in the traditional compass alignment method, seriously affecting the accuracy of the initial alignment.

Initial Alignment Algorithm Based on the DMCS
Comparing Equation (15) with (17), their difference contains the azimuth information, expressed as: where α 1 s , α 2 s denote the north horizontal misalignment angle of the RSINS, which is illustrated in Figures 2a and 3a, respectively. If ignoring the east gyroscope drift, the azimuth misalignment angle can be obtained by Equation (20). Therefore, after the coarse alignment of the RSINS, control methods, illustrated in Figures 2 and 3, are used to proceed with the horizontal alignment, respectively. When these two sets of alignment systems are stabilized, the azimuthal alignment can be achieved by Equation (20), achieving the initial alignment of the RSINS.
On the basis of the above theory, a novel initial alignment algorithm based on the DMCS is proposed in this manuscript. In this alignment algorithm, after completing the coarse alignment, two control modes are executed at the same time in the horizontal fine alignment, and then, the azimuth alignment can be achieved by obtaining the azimuth information from these two horizontal misalignment angles. Besides, the better result obtained by using the second control model shown in Figure 3 is taken as the final one. In this algorithm, the horizontal fine alignment shown as Figure 2 is named "System 1", while the one shown as Figure 3 is named "System 2".
Since System 1 and System 2 share the same coarse alignment result, the initial misalignment angles of these two systems are the same. According to the above analysis, we can see that the focus of this proposed alignment algorithm is the calculation of control information and parameters. Taking System 1 as an example, the control angular rate added to the calculation of the RSINS attitude matrix is denoted by ω n1 c : where ω 1 cx , ω 1 cy are calculated as Figure 5. In Figure 5,C n1 b is the strapdown attitude matrix calculated by using System 1, andf b is the measured acceleration. In order to ensure the stability, the parameters of System 1 are: where ξ is the damping coefficient and ω n is the natural oscillation frequency. Similarly, the control angular rate of System 2 is denoted by ω n2 c , and it can be calculated as Figure 6. Here,C n2 b is the strapdown attitude matrix calculated in System 2. The parameters are set: In addition, due to the lack of a control angular rate in the azimuth during the horizontal alignment, the change of the azimuth misalignment angle is: Since ω ie cos L · α and ε z are both very small, γ can be approximated as a constant during the whole alignment process. That is, the azimuthal misalignment angles of System 1 and System 2 are the same. Assuming that φ n1 is the misalignment angle between the calculated strapdown attitude matrix C n1 b and the true strapdown attitude matrixC n b while φ n2 is the misalignment angle betweenC n2 b and C n b . The following equation holds: From Equation (25), we can obtain: where φ ni (i = 1, 2) is the shew-symmetric matrix: Superscripts 1 and 2 in the above equation represent the relevant parameters of System 1 and System 2, respectively. Assuming that ∆α = α 1 − α 2 , when System 1 and System 2 are both stable, the following equation is satisfied: Define thatγ is the estimation of γ, and: Thus, the azimuth misalignment angle can be estimated in real time by utilizing Equations (26) and (29), and the estimated error of the stable system is: In order to ensure the stability of the azimuth alignment loop shown in Figure 4, the parameters in Figure 4 should be set as: Therefore, we can realize the novel alignment algorithm based on the DMCS, shown as Figure 7. After completing the coarse alignment, the dual systems are executed simultaneously, calculating the strapdown attitude matrixC n1 b andC n2 b . Then, the azimuth misalignment angle can be estimated by Equations (26) and (29). Afterwards, the corrected attitude matrixC n b can be obtained by compensating C n2 b , achieving the fine alignment of the RSINS. Through the above theoretical analysis, we can see that this novel DMCS-based initial alignment algorithm of RSINS can not only eliminate the influence of the vertical gyroscope drift, but also improve the alignment accuracy availably. However, in the above analysis, the azimuth misalignment angle is ideally assumed to be a small one. However, it is extremely difficult to guarantee in actual systems, especially when the vehicle is violently maneuvering. Therefore, the presented DMCS-based alignment algorithm with a large azimuth misalignment angle should be discussed further.

Improved DMCS-Based Alignment Algorithm with a Large Misalignment Angle
When the azimuth misalignment angle is large, the approximation, γ ≈ 0, sin γ ≈ γ and cos γ ≈ 1, is no longer applicable. Correspondingly, the control block diagram will change to Figure 8.
In Figure 8a,b, ∆A y , ε x , ∆A x , ε y are equivalent senor errors shown as Equations (8) and (9) without the approximation. According to the final value theorem, under large azimuth misalignment, the steady state value of the horizontal misalignment angle in System 1 is: Besides, when the azimuth misalignment angle is large, the ω ie cos L evolves from linear to nonlinear, indicated by ω ie cos L · k(γ). In this case, k U should be set as: Similarly, the control block diagram of System 2 also changes into Figure 9: According to the final value theorem, with large misalignment angle, the steady-state value of the horizontal misalignment angle in System 2 is the same as Equation (17), only the sensor error is not approximated. However, the parameters are usually set as Equation (31) due to the unknown k(γ), resulting in non-optimal pole configuration. With the increase of γ, the gap between k U and k * U is gradually increased, this reducing the alignment speed of the azimuth alignment.
Moreover, the error source of the horizontal fine alignment is changed when the misalignment angle is large, and the proposed DMCS-based alignment algorithm described in Section 3.1 is no longer applicable. Now, we will improve this alignment algorithm in this part.
Assume that: When System 1 and System 2 are both stable, we can obtain: Define thatŝ andĉ are estimated values of sin γ and cos γ, respectively: Therefore, the sine and cosine of the azimuth misalignment angle can be estimated by Equations (26) Equations (37) and (38) show that the azimuthal alignment system of the improved DMCS-based alignment algorithm constructed by Equation (36) is a third-order system. Its characteristic polynomial is:∆ (s) = (s 2 + 2ξω n s + ω 2 n )(s + ξω n ) If the pole can be configured optimally in the DMCS-based alignment algorithm, its characteristic polynomial: ∆(s) = (s 2 + 2ξω n s + ω 2 n )(s + ξω n ) 2 (40) Therefore, the azimuth alignment system of the DMCS-based alignment algorithm is a fourth-order system. Due to the lack of a pole in the third-order system, the suppression ability of the external disturbance will be weakened.
In order to ensure the alignment accuracy, the inertia link H(s) is added afterŝ andĉ, and estimations of sin γ and cos γ are re-established, denoted bys andc: where: According to the final value theorem, when the system is steady, the estimation errors of s andc are: Therefore, the schematic diagram of the improved DMSC-based alignment algorithm with a large azimuth misalignment angle is shown in Figure 10. From the design principle of this improved alignment algorithm, we can see that the system is linear and that the pole is not affected by the azimuth misalignment angle. Compared with the traditional compass alignment algorithm, the improved DMCS-based alignment algorithm can enhance not only the alignment accuracy, but also the alignment speed when the azimuth alignment angle is large.

Simulation and Analysis
In order to verify the feasibility of the initial alignment algorithm based on the improved DMCS for the single-axis RSINS in the condition of a large azimuth misalignment angle, the simulations are carried out in this section. As the comparison groups, the initial alignment algorithm based on the traditional compass method and the one based on the traditional DMCS are both simulated here.

Simulation Condition and Settings
The simulation model of RSINS is programmed by MATLAB. Taking the literature [27,29,30] and the actual situation of the turntable as references, the simulation conditions and settings are listed as follows: • Considering the b-frame coincides with the n-frame, the initial latitude and longitude are set as 45.7796 • and 126.6761 • , respectively; • The accelerometers' biases are set as 10 −4 g 0 ; the gyroscopes' biases are set as 10 −2• /h; • The random noises of the accelerometer and the gyroscope are set as 5 × 10 −5 g 0 and 5 × 10 −3• /h, respectively; • Scale factor errors of the accelerometer and the gyroscope are set as 10 ppm; • The natural oscillation frequency ω n is set to 0.02 Hz; • The initial horizontal misalignment angles are set as 0.2 • ,and the azimuth initial misalignment angles are set as 125 • and −170 • , respectively.

•
The rotation scheme is shown as Figure 11, and the rotating rate is 6 • /s.

Initial Alignment Results
Since the azimuth misalignment angle has more influence on system performance and the errors of horizontal misalignment angles are not much different, we focus on errors of azimuth misalignment   From Figure 12, it is obvious that the convergence time is about 25 min and the steady error is about 0.05 • by utilizing the initial alignment algorithm based on the traditional compass method; the convergence time is still nearly 25 min while the steady error is decreased to 0.005 • with the initial alignment algorithm based on the DMCS method; the convergence time is about 10 min and the steady error is 0.005 • when utilizing the improved DMCS-based initial alignment algorithm. Figure 13 shows the error curves of azimuth misalignment angles when the initial azimuth misalignment angle is −170 • . The convergence time is about 30 min, and the steady error is −0.05 • with the initial alignment algorithm based on the traditional compass method; the convergence time is still 30 min and the steady error is about 0.005 • when utilizing the initial alignment algorithm based on the DMCS method; the convergence time is reduced to 10 min and the steady error is about 0.005 • by utilizing the improved DMCS-based initial alignment algorithm.
It is obvious that when the azimuth misalignment angle is large, the initial alignment algorithm with the DMCS method and the one with the improved DMCS method can obtain the same final alignment results, that means the accuracy of these two algorithms is nearly the same; however, the convergence speed of the improved DMCS-based alignment algorithm is much faster.

Pure Inertial Navigation Results
It is known from the above simulation results that the improved DMCS-based initial alignment algorithm has better performance both in initial alignment time and accuracy compared with the other two alignment algorithms. The performance of pure inertial navigation will be tested here. It is assumed that the RSINS turns into the the pure inertial navigation status after the initial alignment status, which lasts 15 min. The simulation lasts 24 h, and the positioning error curves are shown in Figure 14. In the figures, the red dashed line denotes the positioning error curve by utilizing the initial alignment algorithm based on the traditional compass method under the static base (denoted as the 'traditional compass algorithm'); the blue dotted line denotes the positioning error by utilizing the initial alignment algorithm based on the DMCS method under the single-axis rotating base (denoted as the 'DMCS-based alignment algorithm'); the light blue solid line denotes the positioning error by utilizing the improved DMCS-based initial alignment algorithm under the rotating base (denoted as the 'improved DMCS-based alignment algorithm'). From Figure 14, we can see that: (1) When the initial heading error is 125 (3) Comparing the red line and the blue line, we can know that the RMT could improve the performance of the inertial navigation system due to the inertial sensors' mitigation. (4) Comparing the blue line and the light blue line, the results show that the initial alignment accuracy has a large effect on the performance of pure inertial navigation, and the improved DMCS-based alignment algorithm can achieve better navigation performance with little impact on the initial azimuth misalignment angle.

Test Environment Establishment
The test environment was established at the Inertial Test Center, Harbin Institute of Technology. The test equipment included the three-axis turntable, the tested IMU, the power supply unit and the data collection unit. The whole test environment is shown as Figure 15. The performance of the tested IMU is illustrated in Table 1, and the raw data were collected by utilizing the data collection unit and processed off-line.

Results and Analysis
In order to verify the effectiveness of the novel initial algorithms, the initial attitude and positioning errors were calculated by utilizing different algorithms. The traditional compass alignment algorithm and the DMCS-based alignment algorithm were taken as the contrast algorithms. In this section, Test 1 and Test 2 were analyzed by using the DMCS-based alignment algorithm and the improved DMCS-based alignment algorithm, while Test 3 and Test 4 were analyzed by using the traditional compass alignment algorithm. Test 1 and Test 2 gave different initial misalignment angles, to verify the algorithm's repeatability and stability. From the comparison of these three algorithm, the effectiveness and superiority can be fully validated. Tables 2 and 3 show the results of 15-min initial alignment and 25-min initial alignment, respectively. Since the three-axis turntable is not calibrated with true north, there is a fixed bias in the alignment result. Therefore, in order to verify the accuracy of the initial alignment, pure inertial navigation is operated based on the initial alignment result of 15 min and 25 min, respectively. The positioning error curves are shown as Figures 16 and 17. The positioning error is summarized in Table 4.  We can see from Figures 16,17 and Table 4 that:

•
The RMT could suppress the impact of the inertial sensors' error on the inertial navigation system effectively to enhance the performance of the inertial navigation system; • The performance of the initial alignment determines the accuracy of the inertial navigation system, and the initial alignment of high accuracy is the precondition and guarantee for high accuracy inertial navigation; • The initial alignment algorithm based on DMCS method has better performance and a faster convergence rate compared with the initial alignment algorithm based on compass method; what is more, the improved DMCS-based initial alignment algorithm can obtain the best performance when the azimuth misalignment angle is large.

Conclusions
In order to improve the applicability of the RSINS for submarines, a novel initial alignment algorithm is proposed in this manuscript. Considering the alignment accuracy of the traditional compass alignment method, we give a novel alignment algorithm based on the DMCS method; on that basis, taking the large azimuth misalignment angle into account, an improved DMCS-based alignment algorithm is proposed, suppressing the influence of the large misalignment angle on the initial alignment. Results of the simulations and experiments showed that this improved DMCS-based alignment algorithm is superior not only with respect to the accuracy, but also the convergence speed compared to the traditional algorithm when the misalignment angle is large, significantly enhancing RSINS's performance.