An Improved In-Motion Coarse Alignment Method for SINS/GPS Integration with Initial Velocity Error Suppression

The integrated system with the strapdown inertial navigation system (SINS) and the global positioning system (GPS) is the most popular navigation mode. It has been used in many navigation fields. Before the integrated system works properly, it must determine the initial attitude for SINS. In SINS/GPS-integrated systems, the navigational velocity can be used to carry out the initial alignment when the system is installed in the in-motion vehicle. However, the initial velocity errors are not considered in the current popular in-motion alignment methods for SINS/GPS integration. It is well-known that the initial velocity errors must exist when the initial velocity is obtained from the GPS outputs. In this paper, an improved method was proposed to solve this problem. By analyzing the original observation vectors in the in-motion coarse alignment method, an average operation was used to construct the intermediate vectors, and the new observation vector can be calculated by subtracting the intermediate vector from the original observation vector. Then, the initial velocity errors can be eliminated from the newly constructed observation vector. Thus, the interferences of the initial velocity errors for the initial alignment process can be suppressed. The simulation and field tests are designed to verify the performance of the proposed method. The tests results showed that the proposed method can obtain the higher accurate results than the current methods when the initial velocity is considered. Additionally, the results of the proposed method were similar to the current methods when the initial velocity errors were not considered. This shows that the initial velocity errors were eliminated effectively by the proposed method, and the alignment accuracy were not decreased.


Introduction
Navigation system is important for many applications, such as autonomous underwater vehicle (AUV), land vehicles, military systems and so on [1][2][3]. In many navigation systems, the strapdown inertial navigation system has the self-contained properties, and high sampling rates. Thus, SINS was found to be widely used for many applications [4,5]. However, the accumulated errors, which is caused by the measured errors of the inertial sensors, will be contained in the SINS outputs. Then, the positioning accuracy of SINS degrades. To address this problem, the SINS-based integrated navigation system was investigated [6,7]. Then, the accumulated errors can be corrected by the external measurements. In these integrated navigation systems, the SINS/GPS-integrated system is one of the most popular categories [8,9].

Related Works
The most popular methods for in-motion coarse alignment process are based on the inertial frame theory [13][14][15]. Thus, based on the inertial frame theory, the chain rule of the DCM can be given by: Additionally, the attitude rates of C b(0) b(t) and C n(t) n(0) are given by: where b denotes the body frame (b-frame), it is the right-forward-up (RFU) orthogonal frame in this paper; n denotes the navigation frame (n-frame), it is the east-north-up (ENU) orthogonal frame in this paper; b(0) and n(0) denote the b-frame and n-frame at the beginning of the alignment process, they are the non-rotating frame during the whole alignment procedure. In the alignment process, the angular velocity ω b ib can be obtained instead by the gyroscopes' outputs. Additionally, the ω n in can be calculated by the velocity and position measurements of the GPS.
According to (1) and (2), only the DCM C n b (0) is unknown. If C n b (0) can be determined, the initial alignment can be finished. Referring to [13], the initial alignment process was transformed as the attitude determination problem with the observation vectors: where where f b denotes the specific force, it can be measured by the accelerometers; ω n ie denotes the Earth rate in n-frame; g n denotes the gravity in n-frame. It is noted that the reference vector α v can be calculated by the outputs of accelerometers and gyroscopes in the inmotion coarse alignment process. Additionally, the observed vector β v can be calculated by the outputs of the GPS.
Using the observation vectors, the K matrix can be constructed by: where According to Equations (5) and (6), the attitude quaternion can be extracted from the matrix K [15]. Thus, the DCM C n b (0) can be obtained from the attitude quaternion. Then, the initial alignment process can be finished.

Problem Statement
Based on the aforementioned related works, which are shown in [13][14][15], the in-motion coarse alignment methods with SINS/GPS integration are implemented. Based on these methods, many researchers were devoted to extend the in-motion coarse alignment method to many other fields, such as underwater navigation vehicle [16,18], land vehicle and so on [19]. Additionally, the robust methods were proposed to address the outliers from the external auxiliary equipment, such as GPS, DVL and odometers [20][21][22]. However, from the observed vector β v , it can be found that there exists an initial velocity v n (0) in the observation vector. When SINS is under in-motion coarse alignment process, the initial velocity can be obtained from the GPS outputs. Due to the specifications' performance of the GPS, this initial velocity must be corrupted by the noises even outliers. If the initial velocity contains the errors, the alignment accuracy, which is based on the accuracy of the observation vector, will degrade. Moreover, in contrast to the real-time measurement errors of GPS, the initial velocity errors are the constant value during the whole alignment process. It is hard to use the outlier's detection method to suppress them. Therefore, how to suppress the initial velocity errors is worth focusing on. We will give the simple effective method to solve this problem. The detailed method can be found in the next section.

The New Observation Vectors
According to the GPS measurements, the initial velocity from the GPS outputs can be modelled as:ṽ whereṽ n (0) denotes the measured initial velocity from the GPS; v n (0) denotes the truth initial velocity; δv n 0 denotes the initial velocity error. Substituting (7) into (4), we can obtain the calculated observed vector as: whereṽ n denotes the real-time measured velocity from GPS; and β v = β v + C n(0) n(t) δv n . It is noted thatṽ n also contain the noises, but it is not a constant value during the whole alignment process, and it can be suppressed by the robust methods [22]. In this study, we only focused on the initial velocity errors.
From [13,22], the following equation was obtained: where are obtained from the gyroscopes and accelerometers. It is noted that the upper tilde means there are errors inC with the inertial sensors. From (8), using the discrete form of the observation vectors, the averaged observed vectors can be calculated by: where the subscript k denotes the vectors at time instant k; The M denotes the current time instant M.
Using (7), the new observed vector can be calculated by the vector subtraction: Based on (9), it was: whereα From the above deduction, it can be found that the initial velocity errors δv n 0 are eliminated from the new observed vectorsβ v,M . Thus, using the new observed vectorβ v,M , the alignment results will be more accurate than the previous method, which the initial velocity errors δv n 0 are not eliminated. Using the new observation vectorsβ v,M andα v,M , the initial attitude C n b (0) can be determined. Then, the initial alignment can be finished with (1). The algorithm flowchart of the proposed method is summarized in Algorithm 1. From Algorithm 1, it can be divided into three parts. Firstly, the initial parameters are needed to set. The initial parameters contain K matrix, DCM, and vector observations. Secondly, the outputs of the inertial sensors and the GPS are obtained. Using the outputs information, the alignment process can be carried out. Thirdly, the alignment process is carried out. Additionally, the alignment results are obtained at real-time. (4) Calculate the averaged observation vector according to (7)

Algorithm 1. Initial alignment method for SINS/GPS integration
Calculate the new observed vector by (8) Calculate the averaged reference vector according to (10) Extract q b0 n0 from K M , and transform q n0 b0 to C b0 n0 end if Calculate the current attitude according to

Simulation and Field Test Results
To verify the performance of the proposed method, the simulation and field tests were designed in this section. The alignment results are shown in the next two subsections. The current popular methods, which were proposed in [13,15], were designed as the comparative methods and four different initial velocity errors are considered.

Simulation Test
In the simulation tests, the Zigzag trajectory was designed for verification. The trajectory and the movement states of the vehicle are shown in Figures 1 and 2. The initial position of the vehicle was set as L = 32 • N, λ = 118 • E, where L and λ denote the latitude and longitude, respectively.

Simulation and Field Test Results
To verify the performance of the proposed method, the simulation and fie were designed in this section. The alignment results are shown in the next two tions. The current popular methods, which were proposed in [13,15], were designe comparative methods and four different initial velocity errors are considered.
Scheme 1: The current popular method [13] with the initial velocity errors [5 5 Scheme 2: The current popular method [13] with the initial velocity errors [1 1 Scheme 3: The current popular method [13] with the initial velocity errors [0.1 m/s. Scheme 4: The current popular method [15] with the initial velocity errors [5 5 Scheme 5: The current popular method [13] without the initial velocity errors Scheme 6: the proposed method with the initial velocity error [5 5 5] m/s.

Simulation Test
In the simulation tests, the Zigzag trajectory was designed for verification. T jectory and the movement states of the vehicle are shown in Figures 1 and 2. Th position of the vehicle was set as L = 32° N, λ = 118° E, where L and λ denote the and longitude, respectively.  The bias and the angular rate random walk of the gyroscopes were set as 0.01° 0.005°/√h. The bias and the velocity random walk of the accelerometers were set as The bias and the angular rate random walk of the gyroscopes were set as 0.01 • /h and 0.005 • / √ h. The bias and the velocity random walk of the accelerometers were set as 100 µg and 50 µg/ √ Hz. The sampling rate of inertial sensors was set as 200 Hz. The velocity noise of GPS was set as 0.1 m/s and the position noises of GPS was set as 10 m. The noises of GPS were correspondent to Gaussian distribution. The sampling rate of the GPS was set as 1 Hz.
The alignment results are shown in Figures 3-5. In Figure 3, the pitch errors are presented; it was found that the alignment errors of Schemes 1 and 4 were worse than other schemes. This is because the initial velocity errors in Schemes 1 and 4 were set as m/s. The alignment errors of Scheme 2 were less than the same one of Schemes 1 and 4. This is because the initial velocity errors of Scheme 2 were set as [1 1 1] m/s. However, with the same initial velocity errors [5 5 5] m/s, the proposed method, which is Scheme 6, can obtain the high accuracy alignment results of the pitch. From Figure 3, it was found that the alignment errors of the proposed method were around 0.005 • . The alignment results were close to Scheme 5, in which the initial velocity errors were not set. Additionally, the alignment results of Scheme 3 were also close to Scheme 5 because the initial velocity was set as [0.1 0.1 0.1] m/s, which is similar to the noises of the GPS outputs. These results showed that the different initial velocity errors will produce different alignment results. The larger initial velocity errors were contained in the GPS outputs and the large alignment errors will be contained in the alignment results of the traditional methods. The bias and the angular rate random walk of the gyroscopes were set as 0.01°/h and 0.005°/√h. The bias and the velocity random walk of the accelerometers were set as 100 µg and 50 µg/√Hz. The sampling rate of inertial sensors was set as 200 Hz. The velocity noise of GPS was set as 0.1 m/s and the position noises of GPS was set as 10 m. The noises of GPS were correspondent to Gaussian distribution. The sampling rate of the GPS was set as 1 Hz.
The alignment results are shown in Figures 3-5. In Figure 3, the pitch errors are presented; it was found that the alignment errors of Schemes 1 and 4 were worse than other schemes. This is because the initial velocity errors in Schemes 1 and 4 were set as [5 5 5] m/s. The alignment errors of Scheme 2 were less than the same one of Schemes 1 and 4. This is because the initial velocity errors of Scheme 2 were set as [1 1 1] m/s. However, with the same initial velocity errors [5 5 5] m/s, the proposed method, which is Scheme 6, can obtain the high accuracy alignment results of the pitch. From Figure 3, it was found that the alignment errors of the proposed method were around 0.005°. The alignment results were close to Scheme 5, in which the initial velocity errors were not set. Additionally, the alignment results of Scheme 3 were also close to Scheme 5 because the initial velocity was set as [0.1 0.1 0.1] m/s, which is similar to the noises of the GPS outputs. These results showed that the different initial velocity errors will produce different alignment results. The larger initial velocity errors were contained in the GPS outputs and the large alignment errors will be contained in the alignment results of the traditional methods.     In Figure 4, similar results were found, the roll errors of Scheme 5 were less than 0.005° after 200 s. The roll errors of Scheme 1 and Scheme 4 were larger than 0.1°. Although the alignment errors of Scheme 2 were less than the same one of Scheme 1 and Scheme 4, they were still larger than the proposed method. When alignment process lasts 200 s, compared with the alignment errors of Scheme 3, it was found that the roll errors of Scheme 6, which was the proposed method, were closer to the alignment results of Scheme 5. This conclusion shows that the proposed method could suppress the initial velocity errors effectively. It is noted that the roll errors of Scheme 6 had a similar convergence rate with the errors of Scheme 5. This conclusion showed that the vector subtraction operations did not weaken the characteristics of the vector observations.
In Figure 5, the yaw errors are shown. From Schemes 1, 2 and 4, it was found that the yaw accuracy was degraded by the initial velocity errors. In Scheme 1, the errors were larger than 5° when the alignment process lasted for 300 s. In Scheme 2, the errors were smaller than the errors of Scheme 1. This is because the initial velocity errors were small in Scheme 2. However, the errors were still larger than the errors of the proposed method. Although the alignment errors of Scheme 3 were less than 0.5°, they were still larger than the alignment errors of Scheme 5, which were not corrupted by the initial velocity errors. At the end of alignment process, the yaw errors were around 0.1° of Schemes 5 and 6. From the enlarged figure in Figure 5, it was found that the curve of Scheme 6, which was In Figure 4, similar results were found, the roll errors of Scheme 5 were less than 0.005 • after 200 s. The roll errors of Scheme 1 and Scheme 4 were larger than 0.1 • . Although the alignment errors of Scheme 2 were less than the same one of Scheme 1 and Scheme 4, they were still larger than the proposed method. When alignment process lasts 200 s, compared with the alignment errors of Scheme 3, it was found that the roll errors of Scheme 6, which was the proposed method, were closer to the alignment results of Scheme 5. This conclusion shows that the proposed method could suppress the initial velocity errors effectively. It is noted that the roll errors of Scheme 6 had a similar convergence rate with the errors of Scheme 5. This conclusion showed that the vector subtraction operations did not weaken the characteristics of the vector observations.
In Figure 5, the yaw errors are shown. From Schemes 1, 2 and 4, it was found that the yaw accuracy was degraded by the initial velocity errors. In Scheme 1, the errors were larger than 5 • when the alignment process lasted for 300 s. In Scheme 2, the errors were smaller than the errors of Scheme 1. This is because the initial velocity errors were small in Scheme 2. However, the errors were still larger than the errors of the proposed method. Although the alignment errors of Scheme 3 were less than 0.5 • , they were still larger than the alignment errors of Scheme 5, which were not corrupted by the initial velocity errors. At the end of alignment process, the yaw errors were around 0.1 • of Schemes 5 and 6. From the enlarged figure in Figure 5, it was found that the curve of Scheme 6, which was the proposed method, was consistent with the results of Scheme 5. This conclusion showed that the initial velocity errors were eliminated by the proposed method effectively. Moreover, the convergence rate of Scheme 6 was not degraded.
From Figures 3-5, it was found that the curves of the alignment errors of Schemes 5 and 6 were coincidental. These results show that the proposed method can eliminate the initial velocity error effectively.

Field Test
To show the performance of the proposed method in practical systems, the field test was designed. The experimental vehicle and equipment are shown in Figure 6. The navigational computer was produced by our team. It was combined with a PC104 board. The CPU (central processing unit) can operate up to 500 MHz. The GPS receiver was produced by the NovAtel, the BESTVEL and BESTPOS logs were used to output the velocity and position of the vehicle, the sampling rate of the GPS was set as 1 Hz. The reference system, which is named CPT7, was produced by NovAtel. The accuracies of the pitch, roll and yaw of CPT7 were 0.01 • , 0.01 • and 0.1 • , respectively. The specifications of inertial measurement unit (IMU), which were combined with triaxial accelerometers and gyroscopes, are shown in Table 1. position of the vehicle, the sampling rate of the GPS was set as 1 Hz. The re which is named CPT7, was produced by NovAtel. The accuracies of the yaw of CPT7 were 0.01°, 0.01° and 0.1°, respectively. The specifications o urement unit (IMU), which were combined with triaxial accelerometers a are shown in Table 1.   It was found that the specification of the inertial sensors was not a de this is because the errors of the inertial sensors were time-varying. The mo and the states of the vehicle are shown in Figures 7 and 8. The averaged m was around 20 m/s. Additionally, the alignment process was carried out w was moving at any time. The alignment results are shown in Figures 9-11          In Figure 9, the pitch errors are shown. Due to the big initial found that the alignment errors of Schemes 1 and 4 were larger than unsatisfactory. The pitch errors of Scheme 2 were around 0.05° wh  In Figure 9, the pitch errors are shown. Due to the big initial velocity found that the alignment errors of Schemes 1 and 4 were larger than 0.1°. T unsatisfactory. The pitch errors of Scheme 2 were around 0.05° when the a lasted 150 s. However, it was also larger than the proposed method, which Scheme 6. The errors of Schemes 3, 5 and 6 were closer to each other. In initial velocity errors were set as [0.1 0. 100 200 300 400 500 600 -1 0 1 Figure 11. The roll errors.
In Figure 9, the pitch errors are shown. Due to the big initial velocity errors, it was found that the alignment errors of Schemes 1 and 4 were larger than 0.1 • . The results were unsatisfactory. The pitch errors of Scheme 2 were around 0.05 • when the alignment time lasted 150 s. However, it was also larger than the proposed method, which was shown as Scheme 6. The errors of Schemes 3, 5 and 6 were closer to each other. In Scheme 3, the initial velocity errors were set as [0.1 0.1 0.1] m/s. Additionally, there were no initial velocity errors in Scheme 5. However, the proposed method, which was Scheme 6, contained the initial velocity errors, which were [5 5 5] m/s. These results showed that the initial velocity errors were suppressed by the proposed method. The alignment errors of pitch of Scheme 6 were less than 0.01 • .
In Figure 10, the roll errors are shown. The errors of Schemes 1 and 4 were also fluctuant. This was because the initial velocity errors were large, and there was no effective method to suppress them. The errors of Schemes 1 and 4 were around 0.1 • when the alignment process lasted 300 s. The errors of Scheme 2 were fewer than the errors of Schemes 1 and 4. This was because the initial velocity errors were small. However, they were still worse than the errors of Scheme 6. The errors of Schemes 3, 5 and 6 were similar, i.e., they were less than 0.01 • . In Scheme 3, the initial velocity errors were 0.1 m/s. Additionally, in Scheme 5, the initial velocity errors were 0 m/s. However, in Scheme 6, which was the proposed method, the initial velocity errors were 5 m/s. Compared with the results of Schemes 3 and 5, it was found that the large initial velocity errors in Scheme 6 were suppressed.
In Figure 11, the yaw errors are shown. The errors of Schemes 1 and 4 were large and were caused by the relatively large initial velocity errors. Moreover, the alignment errors of Scheme 2 were larger than 2 • when the alignment time lasted 300 s. The other three methods, which were Schemes 3, 5 and 6, had the similar alignment results. However, it is noted that the initial velocity errors in Schemes 3 and 5 were small. Moreover, the alignment errors of Scheme 6 were closer to Scheme 5 than Scheme 3. It was shown that although the yaw errors of Scheme 3 could converge towards less than 0.5 • , they were also affected by the initial velocity errors. Thus, the alignment errors were a little bigger than the proposed method. After 300 s, the yaw errors of Scheme 6 were less than 0.2 • , and 5 m/s initial velocity errors were contained in Scheme 6. The field test also verified the performance of the proposed method.

Conclusions
In this paper, an improved method for in-motion coarse alignment process of SINS/GPS integration was proposed. The initial velocity errors, which were contained in the observed vectors, were considered. If the initial velocity errors were contained in the observed vectors, the alignment error would have been large. To address this problem, the average operation was used to construct the intermediate observed vectors. Then, the new observed vectors were constructed by the intermediate observed vectors with the vector subtraction operations. In the new observed vectors, the initial velocity errors were eliminated effectively. Thus, the alignment accuracy was improved. Moreover, the characteristics of the vector observations were reserved. The simulation and field tests were designed to verify the performance of the proposed method. The results showed that the proposed method could suppress the influences of initial velocity errors on the initial alignment procedure. The proposed method can also be used in other in-motion coarse alignment processes.
Author Contributions: Conceptualization, Y.W. and X.X.; methodology, X.X.; software, X.N.; validation, Y.W. All authors have read and agreed to the published version of the manuscript.