Coarse Alignment Technology on Moving Base for SINS Based on the Improved Quaternion Filter Algorithm

Initial alignment of the strapdown inertial navigation system (SINS) is intended to determine the initial attitude matrix in a short time with certain accuracy. The alignment accuracy of the quaternion filter algorithm is remarkable, but the convergence rate is slow. To solve this problem, this paper proposes an improved quaternion filter algorithm for faster initial alignment based on the error model of the quaternion filter algorithm. The improved quaternion filter algorithm constructs the K matrix based on the principle of optimal quaternion algorithm, and rebuilds the measurement model by containing acceleration and velocity errors to make the convergence rate faster. A doppler velocity log (DVL) provides the reference velocity for the improved quaternion filter alignment algorithm. In order to demonstrate the performance of the improved quaternion filter algorithm in the field, a turntable experiment and a vehicle test are carried out. The results of the experiments show that the convergence rate of the proposed improved quaternion filter is faster than that of the tradition quaternion filter algorithm. In addition, the improved quaternion filter algorithm also demonstrates advantages in terms of correctness, effectiveness, and practicability.


Introduction
The strapdown inertial navigation system (SINS) plays an important role in both military and civil navigation fields, and has become a core navigation system because of advantages in autonomy, continuity, and comprehensiveness [1]. Inertial sensors and the Doppler velocity log are always treated as primary navigation devices of vehicles [2]. SINS can track the position and orientation of the carrier relative to a known starting point based on the measurements provided by the self-contained accelerometers and gyroscopes [3,4]. The process of roughly estimating the initial attitude transformation matrix of the carrier is called coarse alignment [5].
In-motion coarse alignment is performed using a swing base or a moving base. Since the carrier has no linear velocity using a swing base, other external auxiliary information is not required to complete the process of coarse alignment. Currently, typical coarse alignment algorithms in the inertial frame on a swing base include three categories: a solidified analytical algorithm based on double-vector attitude determination, an optimal quaternion algorithm solving the Wahba [6] problem, and a filter algorithm for parameter estimation. Gaiffe proposed a coarse alignment algorithm on the swing . In fact, specific force equation can be simplified as follows: where, g n is the acceleration of gravity in n coordinate, v b is the theoretical value of velocity in b coordinate, ω b ib is the theoretical value of angular velocity in b coordinate, and f b s f is the theoretical value of acceleration in b coordinate. The superscript * indicates the conjugate quaternion of the corresponding quaternion.
Equation (5) can be further transformed: Use integral algorithm on both sides of the equation to eliminate the interferer acceleration error.

Quaternion Filter Algorithm
The optimal quaternion algorithm uses the optimal attitude estimation method to obtain the initial attitude quaternion. The quaternion filter algorithm builds the linear measurement model to estimate quaternion directly [21]. In fact, the principles of quaternion filter algorithm and optimal quaternion algorithm are the same. The difference between them lies only in how to get the constant quaternion q n 0 b 0 . The optimal quaternion algorithm uses the optimal estimation method to obtain the eigenvectors corresponding to the minimum eigenvalue of K matrix, while quaternion filter algorithm calculates quaternion q n 0 b 0 directly, using filter algorithm.

Measurement Model
According to Equation (11), measurement model of the quaternion is: where, H k = M(β(k)) − M(α(k)), Q k is the q n 0 b 0 at moment of t k . Because the measurement is always zero, the measurement equation is also known as the pseudo-measurement model. For the measurement noise model, the accelerometer measurement α(k) contains measurement error, noise and random linear motion disturbance. When the carrier is moving, a measurement error of external velocity is also introduced. So the observation matrix H k contains various errors.

State Space Model
The quaternion q n 0 b 0 should be considered as a constant quaternion during the process of alignment. When q n 0 b 0 is regarded as the estimated parameter, the state space model is very simple, as follows: where, Q k+1 and Q k are the initial quaternion at moment of t k+1 and t k . Combine the Equations (14) and (15) to build Kalman filtering equation for estimating the constant attitude quaternion q is estimated by improved Kalman filter algorithm; the specific estimation process is as follows,Q where,Q 0 = [ 1 0 0 0 ] T , P 0 = aI (a is a large positive number). R 0 = cI (the recommended value of c is 0.1). The above algorithm uses the innovation e k to calculate the filter gain, so the statistical characteristics of velocity disturbances need not to be known. The filter gain is determined adaptively according to the innovation at each filtering moment, which can accelerate the convergence rate.

Alignment Error Analysis of Quaternion Filter Algorithm
According to Equation (9), β(t) is the integration of the local gravitational acceleration g transformed from n coordinate to i coordinate. Because local gravitational acceleration can be exactly known, the observation vectors β(t) and M(β(t)) can be considered error free. On an in-motion base, the observation vector α(t) contains the integration of accelerometers, which include constant bias and random errors. Therefore, in Equation (14), the observation matrix H k also contains errors, which results in a slow convergence rate. The coarse alignment error of the quaternion filter algorithm is analyzed as follows.
The output of accelerometers with errors isf b is the constant bias of accelerometers and r b is the random error and disturbance. The output of gyroscopes with errors where ε b is the constant drift of gyroscopes and χ b is the random drift. The model of velocity with errors isv b = v b + δv b , where δv b is the error of velocity. According to Equation (8), the observation vector v b 0 with errors at kth sampling moment is as follows.
Set the vectors as: The observation matrix H k with errors isĤ k = M(β(k)) − M(α(k)), so: By moving the right part to the left, the following is obtained: From Equation (25), we see that the actual observation model includes constant bias and random error of accelerometer, constant and random drift of gyroscope, and a velocity error. However, these errors are all ignored in Equation (14), which may result in the slow convergence rate of quaternion filter algorithm.
In order to verify the above theory, taking the constant bias of accelerometers as an example, different values of constant bias are compared in Table 1. The other simulation conditions are the same. Attitude errors of alignment are shown in Figure 1.
The observation matrix k H with errors is By moving the right part to the left, the following is obtained: From Equation (25), we see that the actual observation model includes constant bias and random error of accelerometer, constant and random drift of gyroscope, and a velocity error. However, these errors are all ignored in Equation (14), which may result in the slow convergence rate of quaternion filter algorithm.
In order to verify the above theory, taking the constant bias of accelerometers as an example, different values of constant bias are compared in Table 1. The other simulation conditions are the same. Attitude errors of alignment are shown in Figure 1.  In Figure 1, it can be seen that the larger the constant bias, the slower the process of the coarse alignment, and lower the accuracy of alignment. In fact, accelerometer error is generally large. In addition, the constant and random drift of the gyro and velocity errors are similar to the constant bias of the accelerometer, so quaternion filter algorithm needs to be improved.

Improved Quaternion Filter Algorithm
As discussed above, if the constant bias and random error of accelerometer, the constant drift and random drift of gyroscope, and the velocity error can all be estimated and compensated for, the convergence rate of quaternion filter algorithm will be improved significantly. A digital filter can be designed to eliminate the disturbances of the accelerometer, gyroscope and DVL, but the design of digital filter is complicated because the characteristics of these disturbances must be analyzed, the parameters adjusted, an appropriate transitional zone designed. Yet, once the filter is designed to estimate unreasonable inputs, the useful information will also be filtered out.
According to the principles of the optimal quaternion algorithm, the quaternion filter algorithm can be improved by constructing K matrix to accelerate the convergence rate. K matrix is constructed as follows: Ignoring the terms of the second order errors and random disturbances, Equation (26) is simplified as: According toĤ k = M(β(k)) − M(α(k)), it's easy to determine that the elements ofĤ k are real numbers smaller than 1. Hence, the influences of accelerometer errors, gyroscope errors and velocity error in Equation (29) are greatly reduced.
If the accelerometer errors, gyroscope errors and velocity error are unknown, the observation model Equation (14) can be described by: where, v k indicates the related noises of accelerometer, gyroscope and DVL, which can be considered as the white noise with a mean of 0.

Experimental Environments
In order to demonstrate the performance of the improved quaternion filter algorithm in the actual environment, two coarse alignment experiments are carried out. One is an alignment experiment on a swing base based on a three-axis turntable, while the other is an alignment experiment on a moving base based on the vehicle. The characteristics of the inertial measurement unit (IMU) used in the experiments are as follows, the constant drift stability of each gyro is less than 0.01 (•) /h(1σ), random walk coefficient is less than 0.01 (•) /h(1σ), the bias of each quartz flexible accelerometer is less than 5 × 10 −5 g. The initial position of the experiment is 32.05 • (N) and 118.0 • (E).
The experiment environment on a swing base is shown in Figure 2, and the corresponding structural diagram is shown in Figure 3. It mainly consists of a fiber optic gyro inertial system (FOSN), a navigation computer (alignment algorithm is running in it), a computer used for storing, a GPS receiver, and the three-axis turntable. The navigation computer infuses outputs of the IMU and GPS receiver, and sends the result to the storing computer through the network. The storing computer compares the result of the coarse alignment from navigation computer with the attitude values of the turntable, and calculates the alignment accuracy. . The initial position of the experiment is 32.05°(N) and 118.0°(E). The experiment environment on a swing base is shown in Figure 2, and the corresponding structural diagram is shown in Figure 3. It mainly consists of a fiber optic gyro inertial system (FOSN), a navigation computer (alignment algorithm is running in it), a computer used for storing, a GPS receiver, and the three-axis turntable. The navigation computer infuses outputs of the IMU and GPS receiver, and sends the result to the storing computer through the network. The storing computer compares the result of the coarse alignment from navigation computer with the attitude values of the turntable, and calculates the alignment accuracy.

IMU Inner frame
Intermediate frame Outer frame Turntable experiment on a moving base based on the vehicle. The characteristics of the inertial measurement unit (IMU) used in the experiments are as follows, the constant drift stability of each gyro is less than h  , the bias of each quartz flexible accelerometer is less than . The initial position of the experiment is 32.05°(N) and 118.0°(E). The experiment environment on a swing base is shown in Figure 2, and the corresponding structural diagram is shown in Figure 3. It mainly consists of a fiber optic gyro inertial system (FOSN), a navigation computer (alignment algorithm is running in it), a computer used for storing, a GPS receiver, and the three-axis turntable. The navigation computer infuses outputs of the IMU and GPS receiver, and sends the result to the storing computer through the network. The storing computer compares the result of the coarse alignment from navigation computer with the attitude values of the turntable, and calculates the alignment accuracy.

Inner frame
Intermediate frame Outer frame Turntable  The structure diagram of the vehicle-based alignment experiment is shown in Figure 4. In order to evaluate the accuracy, the high-precision fiber optic gyro SINS (PHINS), produced by French company iXBlue, was chosen as a reference. The PHINS and FOSN are mounted on a rigid board with parallel heading angles, as shown in Figure 5. The experiment environment is shown in Figure 6. PHINS operates in the mode of integrated navigation aided by GPS, and sends the result to the storing computer, which is regarded as the real alignment reference. DVL provides the velocity to the navigation computer for assisting FOSN to complete the initial alignment. The storing computer stores the alignment results of FOSN and PHINS, and assesses the alignment accuracy of proposed algorithm. The project is used in the underwater environment. Since the experiment condition is limited, we use the vehicle experiment instead. The reference velocity is provided by PHINS with a constant error of 0.2 m/s and a random error of 0.005 m/s. Figure 6. PHINS operates in the mode of integrated navigation aided by GPS, and sends the result to the storing computer, which is regarded as the real alignment reference. DVL provides the velocity to the navigation computer for assisting FOSN to complete the initial alignment. The storing computer stores the alignment results of FOSN and PHINS, and assesses the alignment accuracy of proposed algorithm. The project is used in the underwater environment. Since the experiment condition is limited, we use the vehicle experiment instead. The reference velocity is provided by PHINS with a constant error of 0.2 m/s and a random error of 0.005 m/s.   with parallel heading angles, as shown in Figure 5. The experiment environment is shown in Figure 6. PHINS operates in the mode of integrated navigation aided by GPS, and sends the result to the storing computer, which is regarded as the real alignment reference. DVL provides the velocity to the navigation computer for assisting FOSN to complete the initial alignment. The storing computer stores the alignment results of FOSN and PHINS, and assesses the alignment accuracy of proposed algorithm. The project is used in the underwater environment. Since the experiment condition is limited, we use the vehicle experiment instead. The reference velocity is provided by PHINS with a constant error of 0.2 m/s and a random error of 0.005 m/s.

Alignment Experiment on Swing Base Based on Turntable
A FOSN is mounted on the three-axis turntable, whose swing mode is set as follows: (1) the swing center of pitch is 2°, swing amplitude is 8°, and swing frequency is 0.15 Hz; (2) the swing center of roll is −2°, swing amplitude is 10°, and swing frequency is 0.2 Hz; (3) the swing center of heading is set at multiple angles, namely 0°, 45°, 90°, 135°, 180°, 225°, 270°, and 315°, swing amplitude is 6°, and swing frequency is 0.125 Hz. To show characteristics of different algorithms, error curves of the optimal quaternion algorithm, the quaternion filter algorithm, and the improved quaternion filter algorithm are compared in Figure 7.

Alignment Experiment on Swing Base Based on Turntable
A FOSN is mounted on the three-axis turntable, whose swing mode is set as follows: (1) the swing center of pitch is 2 • , swing amplitude is 8 • , and swing frequency is 0.15 Hz; (2) the swing center of roll is −2 • , swing amplitude is 10 • , and swing frequency is 0.2 Hz; (3) the swing center of heading is set at multiple angles, namely 0 • , 45 • , 90 • , 135 • , 180 • , 225 • , 270 • , and 315 • , swing amplitude is 6 • , and swing frequency is 0.125 Hz. To show characteristics of different algorithms, error curves of the optimal quaternion algorithm, the quaternion filter algorithm, and the improved quaternion filter algorithm are compared in Figure 7.

Alignment Experiment on Swing Base Based on Turntable
A FOSN is mounted on the three-axis turntable, whose swing mode is set as follows: (1) the swing center of pitch is 2°, swing amplitude is 8°, and swing frequency is 0.15 Hz; (2) the swing center of roll is −2°, swing amplitude is 10°, and swing frequency is 0.2 Hz; (3) the swing center of heading is set at multiple angles, namely 0°, 45°, 90°, 135°, 180°, 225°, 270°, and 315°, swing amplitude is 6°, and swing frequency is 0.125 Hz. To show characteristics of different algorithms, error curves of the optimal quaternion algorithm, the quaternion filter algorithm, and the improved quaternion filter algorithm are compared in Figure 7.  Figure 7 shows that the convergence rate of that quaternion filter algorithm is so slow that it is not suitable for practical use. The horizontal attitude accuracies of the optimal quaternion algorithm and the improved quaternion filter algorithm are similar to one another. The means and standard deviations of their alignment errors for heading are respectively 0.1431° and 0.1232°, and 0.1302° and 0.0315°. Although the mean values of two algorithms are close, the standard deviation of the latter is much smaller than the former because the latter is more stable.
In order to thoroughly compare the alignment effects of the optimal quaternion algorithm and the improved quaternion filter algorithm, the errors are shown at three alignment moment, such as 50 s, 80 s, and 100 s, in Table 2.  Figure 7 shows that the convergence rate of that quaternion filter algorithm is so slow that it is not suitable for practical use. The horizontal attitude accuracies of the optimal quaternion algorithm and the improved quaternion filter algorithm are similar to one another. The means and standard deviations of their alignment errors for heading are respectively 0.1431 • and 0.1232 • , and 0.1302 • and 0.0315 • . Although the mean values of two algorithms are close, the standard deviation of the latter is much smaller than the former because the latter is more stable.
In order to thoroughly compare the alignment effects of the optimal quaternion algorithm and the improved quaternion filter algorithm, the errors are shown at three alignment moment, such as 50 s, 80 s, and 100 s, in Table 2. As Figure 7 shows, both optimal quaternion algorithm and improved quaternion filter algorithm can converge rapidly. However, the error curve of the heading angle of the former algorithm fluctuates at the beginning of the alignment. This is because the information on K matrix at the initial moment is so insufficient that the eigenvectors of K matrix are unstable. Although the improved quaternion filter algorithm also uses the attitude quaternion from K matrix, it uses an extended recursive least square algorithm, whose gain is determined adaptively, by innovation at each filtering moment. Therefore, the improved quaternion filter algorithm is more stable than the optimal quaternion algorithm. In Table 2, it can be seen that at 100 s, the alignment accuracy of the improved quaternion filter algorithm is obviously better than that of the optimal quaternion algorithm. Hence we conclude that the improved quaternion filter algorithm has distinct advantages in convergence rate and stability.

Alignment Experiment on Moving base Based on Vehicle
In order to further verify the advantages of the improved quaternion filter algorithm on the moving base assisted by external velocity, an alignment experiment based on vehicle is carried out. The route of the vehicle is shown in Figure 8, looking like a circle inside Southeast University. This experiment mainly focuses on the influence of external velocity on the alignment results.  Figure 7 shows, both optimal quaternion algorithm and improved quaternion filter algorithm can converge rapidly. However, the error curve of the heading angle of the former algorithm fluctuates at the beginning of the alignment. This is because the information on K matrix at the initial moment is so insufficient that the eigenvectors of K matrix are unstable. Although the improved quaternion filter algorithm also uses the attitude quaternion from K matrix, it uses an extended recursive least square algorithm, whose gain is determined adaptively, by innovation at each filtering moment. Therefore, the improved quaternion filter algorithm is more stable than the optimal quaternion algorithm. In Table 2, it can be seen that at 100 s, the alignment accuracy of the improved quaternion filter algorithm is obviously better than that of the optimal quaternion algorithm. Hence we conclude that the improved quaternion filter algorithm has distinct advantages in convergence rate and stability.

Alignment Experiment on Moving base Based on Vehicle
In order to further verify the advantages of the improved quaternion filter algorithm on the moving base assisted by external velocity, an alignment experiment based on vehicle is carried out. The route of the vehicle is shown in Figure 8, looking like a circle inside Southeast University. This experiment mainly focuses on the influence of external velocity on the alignment results.

Influence of Filtering Frequency of External Velocity on Alignment
In this experiment, DVL provides the external velocity, and its frequency is set at different value, such as 200 Hz, 50 Hz and 1 Hz. The attitude error of the improved quaternion filter algorithm is shown in Figure 9. In this experiment, DVL provides the external velocity, and its frequency is set at different value, such as 200 Hz, 50 Hz and 1 Hz. The attitude error of the improved quaternion filter algorithm is shown in Figure 9. In Figure 9, it is shown that, at 1 Hz, the heading angle converges slowly and obviously fluctuates. With increasing velocity output frequency, the convergence rate is faster and the alignment result is better. At 300 s, the heading angle errors at 1 Hz, 50 Hz, and 200 Hz are respectively −1.788°, −1.389°, and −1.384°. In a low frequency situation such as 1Hz, the heading angle error accumulates mainly in the acceleration and deceleration process of the vehicle, because the In Figure 9, it is shown that, at 1 Hz, the heading angle converges slowly and obviously fluctuates. With increasing velocity output frequency, the convergence rate is faster and the alignment result is better. At 300 s, the heading angle errors at 1 Hz, 50 Hz, and 200 Hz are respectively −1.788 • , −1.389 • , and −1.384 • . In a low frequency situation such as 1Hz, the heading angle error accumulates mainly in the acceleration and deceleration process of the vehicle, because the estimation of velocity difference in every SINS calculation period is inaccurate. The compensation method is shown in Figure 10, where the velocity difference in velocity output period is evenly distributed across every strapdown calculation cycle. Ts is the SINS calculation frequency, T is the velocity output period, a is the velocity difference in SINS calculation period, and V(Ti) is the velocity value at the moment of Ti. In Figure 9, it is shown that, at 1 Hz, the heading angle converges slowly and obviously fluctuates. With increasing velocity output frequency, the convergence rate is faster and the alignment result is better. At 300 s, the heading angle errors at 1 Hz, 50 Hz, and 200 Hz are respectively −1.788°, −1.389°, and −1.384°. In a low frequency situation such as 1Hz, the heading angle error accumulates mainly in the acceleration and deceleration process of the vehicle, because the estimation of velocity difference in every SINS calculation period is inaccurate. The compensation method is shown in Figure 10, where the velocity difference in velocity output period is evenly distributed across every strapdown calculation cycle. Ts is the SINS calculation frequency, T is the velocity output period, a is the velocity difference in SINS calculation period, and V(Ti) is the velocity value at the moment of Ti.  In Figure 11, it can be seen that, under conditions of different constant error of velocity, horizontal attitude error can converge fast, while the error of the heading angle increases with the increasing of constant error. The heading angle errors are similar when constant errors are 0.1m/s and 1 m/s, at which point the heading angle errors are −1.787 • and −1.807 • . In comparison, the heading angle error is −2.099 • when constant error is 2m/s. The constant error of external reference velocity influences the alignment accuracy in accordance with the term of acceleration ω b ib × v b , but its influence is limited.

Influence of Random Error of External Velocity
The constant error is assumed to be zero to analyze the influence of random error of velocity on the alignment accuracy. Three different values of white noise random errors are selected: 0.05 time, 0.1 time and 0.2 time. The alignment error is shown in Figure 12.
horizontal attitude error can converge fast, while the error of the heading angle increases with the increasing of constant error. The heading angle errors are similar when constant errors are 0.1m/s and 1 m/s, at which point the heading angle errors are −1.787° and −1.807°. In comparison, the heading angle error is −2.099° when constant error is 2m/s. The constant error of external reference velocity influences the alignment accuracy in accordance with the term of acceleration b b ib × ω v , but its influence is limited.

Influence of Random Error of External Velocity
The constant error is assumed to be zero to analyze the influence of random error of velocity on the alignment accuracy. Three different values of white noise random errors are selected: 0.05 time, 0.1 time and 0.2 time. The alignment error is shown in Figure 12. In Figure 12 it is shown that with increasing random velocity error, the alignment result converges more slowly. At the alignment moment of 300 s, heading angle errors corresponding to the random error are, from smallest to largest, −2.292 • , −3.284 • , and −5.722 • . Compared with the influence of constant error of velocity, the influence of random error is larger. This is because random error of velocity influences the alignment accuracy by both of differential velocity Therefore, as shown in Figure 10 and Equation (5), during the whole process of alignment, random error has been accumulating within the external reference velocity update period.
The three experiments described above show that when the output frequency of external velocity is low, such as 1 Hz, the alignment error will increase, with external velocity becoming larger. Further, the influence of random error of external velocity on alignment result is more obvious than that of constant error. In order to compare the optimal quaternion algorithm to the improved quaternion filter algorithm described in this paper, it is more practical to assume that the velocity error model consists of constant error than random error, as in the four cases shown in the Table 3. The alignment results of the optimal quaternion algorithm and the improved quaternion filter algorithm for case 4 are as shown in Figure 13, and the heading angle errors for all case at 250 s and 500 s are shown in Table 4. The constant error is assumed to be zero to analyze the influence of random error of velocity on the alignment accuracy. Three different values of white noise random errors are selected: 0.05 time, 0.1 time and 0.2 time. The alignment error is shown in Figure 12.   In Figure 13, it can be seen that, in the process of coarse alignment on the moving base, the optimal quaternion algorithm obviously fluctuates, while the curve of the improved quaternion filter algorithm is smooth. Therefore, the improved quaternion filter algorithm shows better stability performance. Table 4 shows that the alignment accuracies of the heading angles of the two algorithms are similar at 500 s, but the accuracy of the improved quaternion filter algorithm is better than that of the optimal quaternion algorithm at 250 s. This shows that the convergence rate of the improved quaternion filter algorithm is faster. Comparing the alignment results of case 2 and case 3 also verified that the influence of random error is larger than that of constant error.

Conclusions
Tackling the slow convergence problem of the quaternion filter algorithm, an improved quaternion filter algorithm is proposed which constructs the K matrix based on the principle of the optimal quaternion algorithm. Turntable-and vehicle-based experiments were carried out to verify the characteristics of the proposed algorithm. The turntable-based experimental results show that the convergence rate of the improved quaternion filter algorithm is faster than that of the quaternion filter algorithm. The vehicle-based experimental results show that the alignment accuracy of the improved quaternion filter algorithm is more stable, while the heading angle of the optimal quaternion algorithm fluctuates mightily at the beginning of alignment. In addition, the effect of the random error of external velocity on alignment accuracy is greater than that of the constant error. In summary, the improved quaternion filter algorithm can effectively improve the alignment accuracy and accelerate the convergence rate on the moving base.