Abstract
This paper proposes a twice rapid transfer alignment algorithm based on dual models in order to solve the problems such as long convergence time, poor accuracy, and heavy computation burden resulting from the traditional nonlinear error models. The quaternion matching method based on quaternion error model along with the extended Kalman filter (EKF) is applied to deal with the large misalignment in the first phase. Then in the second transfer alignment phase, velocity plus attitude matching method as well as classical Kalman filter is adopted. The simulation and the results of vehicle tests demonstrate that this method combines the advantages of both nonlinear and linear error models with the guarantee of accuracy and fastness.
1. Introduction
The technology of rapid transfer alignment for inertial navigation system with small attitude errors has been adequately studied in theoretical research and widely applied in engineering practice, satisfying the requirements of rapidity and high degree of accuracy [1]. However, the model is highly dependent on the small misalignment angles assumption and therefore is not applicable under the situation when the attitude errors become large. In this case, the traditional linear error model cannot accurately describe the error propagation characteristics of the inertial navigation system, which will inevitably degrade the performance of the filter, and sometimes even lead to instability and divergence. Consequently, some researchers studied the nonlinear error models [2–6] and proposed various kinds of nonlinear Kalman filters [7–11]. However, the state estimation of nonlinear error model has poor performance and a huge computing burden which requires a long time to converge. Hence, it cannot meet the requirements of stability and fastness.
Obviously both of the two models have their own shortcomings. In this paper, we aim to find a transfer alignment algorithm appropriate for arbitrary misalignment angles, which has the advantages of fast convergent rate and high accuracy. A twice transfer alignment algorithm based on dual models is proposed in this paper, which combines the advantages of both nonlinear and linear error models.
(A) Model Based on Additive Quaternion. The models are composed of the velocity error and the quaternion error propagation model, where the state variables can be presented aswhere is the quaternion error vector; indicates velocity vector with respect to Earth in the navigation frame; and are equivalent gyrodrift vector and equivalent accelerometer bias vector, respectively, in the body frame.
The quaternion is widely used as an effective method to compute the attitude of INS (inertial navigation system). According to the relationship between quaternion errors and tilt angle, quaternion errors can be used to describe the attitude error. The quaternion and velocity error differential equation can be given as follows:where is the quaternion error vector; is the rotation rate of the navigation frame with respect to the inertial frame defined in the navigation frame; represents the rotational angular velocity of SINS (Slave Inertial Navigation System) with respect to the inertial frame defined in the SINS body frame; the matrices , , and can be presented as follows: is the indicated transformation matrix of SINS from the body frame to the navigation frame; indicates specific force of SINS in its body frame; is the rotation rate of the Earth with respect to the inertial frame defined in the navigation frame; is the turn rotational angular velocity of the navigation frame with respect to the Earth; indicates velocity vector of SINS with respect to Earth in the navigation frame; is the nonlinear term in the equation of velocity error.
(B) Model of Rapid Transfer Alignment. This model was presented by Kain and Cloutier in [1]. The state variables are presented aswhere is defined as the rotation vector error between the true navigation frame and the indicated navigation frame . If is sufficiently small, the state equation can be expressed as follows:where and are the attitude error and velocity error vectors, respectively; indicates velocity vector in the navigation frame; indicates specific force in the navigation frame; is the Earth rate vector; is the turn rate of the navigation frame with respect to the Earth; is the direction cosine matrix of SINS; and () are equivalent gyrodrift vector and equivalent accelerometer bias vector, respectively, in the body frame; () is 3-axis component of INS installation error angle vector in the body frame.
(C) Transfer Alignment Based on Dual Model. By a large amount of simulation tests, we find out that attitude error can quickly converge to small angles when the error model based on quaternion is used under the condition of large misalignment angle. However, it still takes a long time to converge to a satisfactory accuracy. In this case, we intend to switch to the linear model to obtain faster convergence speed and higher accuracy. This approach is called a twice transfer alignment based on dual model. This method uses nonlinear model in first phase. Then after the estimation of attitude error converging to small angles, the linear error model is adopted to complete the twice transfer alignment.
2. Experimental Results
2.1. Simulation and Results
The simulation is performed in two different cases. In Case A, model based quaternion and EKF which is easily realized in engineering project are adopted in the first phase. Through analyzing the performance of the nonlinear system by simulation experiments, the time point to switch to linear error model and classical Kalman filter in the next stage is decided at the 10th second. In Case B, model based additive quaternion and EKF are utilized. The initial attitude errors in roll, pitch, and heading are all assumed to be 20°. The total time of the simulation is 60 s; the initial position is set at 32.5 degrees north latitude, 135 degrees east longitude, 500 meters height; the initial attitudes are all set at 0°; the initial north velocity is 50 m/s; east and vertical velocities are 0 m/s. The constant gyrodrifts and accelerometer biases are set at 10°/h, 5 mg, respectively. The simulation results are shown in Figures 1-2 and Table 1.
(a)
(b)
(c)
(a)
(b)
(c)
Figure 1 shows the alignment results in the first phase. The figures indicate that the attitude estimation error gets well into the range of the ideal accuracy during the first 10 s. After the calibration is done, the conventional transfer alignment process will work well in the following 50 s.
Figure 2 and Table 1 compare the attitude estimation error in Case A with that in Case B. It is clear that the red curves not only have higher rate of convergence, but also are smoother than blue curves. In particular, the red curves all converge within 0.005°. Instead, the blue curves fluctuate within a range of 0.22°. The results show that the dual model has obviously a faster convergent speed and higher accuracy than that in Case B.
2.2. Vehicle Test and Results
In order to verify the performance of the proposed algorithm, vehicle tests are conducted [12–14]. The test configuration is illustrated in Figure 3. A slab and four steel dowels are machined which could be used to artificially misalign the SINS from the MINS (Master Inertial Navigation System) in pitch, roll, and yaw. In practical engineering, the model based additive quaternion is difficult to establish. However, if the rough misalignment is known, the attitude of SINS can be calibrated directly and the first phase can be skipped, which means a twice transfer alignment can be directly conducted with the rough misalignment. The performance of transfer alignment is assessed with different assumed misalignments. Figure 4 illustrates the alignment results.
(a)
(b)
(c)
In Figure 3, the SINS and MINS are bolted to the slab in the test. The steel dowels are used to establish misalignments in three axes intentionally. In the test, the misalignments in pitch, roll, and yaw are set at about 10°, 0°, and 33°, respectively. The positive roll and return-to-level maneuver is adopted in tests, which can be called wing-rock maneuver. Once transfer alignment begins, one end of the slab is manually tilted up and set back down in the vehicle.
Figure 4 compares the estimated attitude misalignments in three axes, when the assumed misalignments used to calibrate attitude of SINS are different. In the figure, attitude misalignments are accurately estimated in spite of the fact that the assumed misalignments are different. The deviation of the estimated value is less than 0.1°.
3. Conclusion
In order to satisfy the requirements of rapidity and high accuracy in transfer alignment with large uncertainty, a transfer alignment algorithm based on dual model is presented. The simulation and vehicle test results demonstrate that this method could combine the advantages of nonlinear and linear error models which not only can be used in inertial navigation system with large uncertainty but also has the same high accuracy as the linear system.
Conflict of Interests
The authors declare that there is no conflict of interests regarding the publication of this paper.
Acknowledgments
This work was supported by NSAF (Grant no. U1330133) and the Natural Science Foundation of Jiangsu Province (Grant BK20130774).