Abstract
In the initial alignment process of strapdown inertial navigation system (SINS), large initial misalignment angles always bring nonlinear problem, which causes alignment failure when the classical linear error model and standard Kalman filter are used. In this paper, the problem of large misalignment angles in SINS initial alignment is investigated, and the key reason for alignment failure is given as the state covariance from Kalman filter cannot represent the true one during the steady filtering process. According to the analysis, an alignment method for SINS based on multiresetting the state covariance matrix of Kalman filter is designed to deal with large initial misalignment angles, in which classical linear error model and standard Kalman filter are used, but the state covariance matrix should be multireset before the steady process until large misalignment angles are decreased to small ones. The performance of the proposed method is evaluated by simulation and car test, and the results indicate that the proposed method can fulfill initial alignment with large misalignment angles effectively and the alignment accuracy of the proposed method is as precise as that of alignment with small misalignment angles.
1. Introduction
For its merits of complete independence, strong anti-interference ability, comprehensive, and high update frequency of navigation message, strapdown inertial navigation system (SINS) has been widely used for military application [1, 2]. As a navigation method based on integration operation, initial alignment should be done in SINS before navigation is started. The purpose of initial alignment of SINS is to determine the attitude matrix from body frame to navigation frame, that is, initial attitude, because initial velocity and position are easy to determine by external navigation systems, such as GPS [1–4]. In SINS alignment, misalignment angles are defined as the Euler angles which describe the rotation between calculated navigation frame and ideal navigation frame [1].
The compassing alignment method based on compass effect and the optimal estimation methods based on Kalman filter (e.g., transfer alignment, zero-velocity alignment) are the mostly used techniques to deal with alignment problems in SINS [2–8]. Among these, zero-velocity is widely used as external reference navigation information by virtue of the characteristic of no linear velocity for static vehicles in zero-velocity alignment due to its merits of allowing swinging motion and no requirement for external navigation system. The alignment process of zero-velocity can be divided into two stages in chronological order: the first one is coarse alignment based on analytical method and the second is fine alignment based on Kalman filter [2, 7, 8]. Among these stages, alignment result of coarse alignment is the precondition of fine one, and the accuracy of coarse alignment affects the accuracy of fine alignment.
Error propagation model and filter method are two key issues to address when Kalman filter or methods derived from Kalman filter are used to fulfill SINS alignment [2, 9]. The equations describing SINS algorithm are a set of nonlinear differential equations, so error propagation model of SINS is also nonlinear essentially [10]. The classical linear error propagation model either with or representations is obtained by the assumption of small misalignment angles, and the standard Kalman filter can fulfill initial alignment only with small misalignment angles when linear model is used [1–9]. However, because of environmental disturbance and sensor errors, large errors between initial attitude and the true one, that is, large misalignment problems or nonlinear problems, are often generated in SINS when analytical method is used for coarse alignment [11–15]. The problems addressed in this paper are concentrated on the investigation of initial alignment for SINS with large initial misalignment angles caused by low accuracy of coarse alignment.
Recently, SINS error propagation models and nonlinear estimation methods for large misalignment angles are widely reported in the literatures. Error propagation models can be divided into two types: one is nonlinear model for large misalignment angles, among which the large azimuth misalignment error model is the most representative [11]; the other is linearized method for nonlinear problem, which gains linear model for large misalignment angles by approximation or state transformation [12, 13]. Among nonlinear filters, extended Kalman filter (EKF) is widely used, and estimation methods based on approximate probability density distribution such as particle filter (PF) and unscented Kalman filter (UKF) are also popular currently [14, 15]. These filters can solve nonlinear function directly by using the evolution of probability density distribution without the need of detailed expansion equations of nonlinear function. Large misalignment problems can be solved with the above models and methods to some extent. However, there are still many shortages needed to be concerned, such as the complexity problem in nonlinear error model for large misalignment angles, state dimension extending problem in the linearization process, and heavy computational problem in PF and UKF. In addition, investigation on stability of the methods is still blank field when PF and UKF are used in engineering.
For the purpose of simplifying error model without adding computational load, an initial alignment method for large initial misalignment angles is proposed based on classical linear error propagation model and standard Kalman filter, in which the state covariance matrix of Kalman filter is reset several times before the steady filtering process is achieved. With the parameters resetting, the state covariance from Kalman filter can truly represent the true one and the high utilization of measurement vector can be ensured. Coupled with the help of closed loop correction mode, large misalignment angles can be converted into small ones, so that alignment can be finished effectively.
The rest of this paper is organized as follows. The classical linear error propagation model and standard Kalman filter used in zero-velocity alignment are described in Section 2. Then, the reason for alignment failure with large misalignment angles is analyzed in Section 3, and a potential solution method is proposed, which is verified by simulation. In Section 4, the validity of the proposed method is further verified by car test. Finally, some conclusions are given in Section 5.
2. Error Propagation Model and Kalman Filter for Zero-Velocity Alignment
2.1. System Equation
Choose velocity errors and misalignment angles of SINS as the state variables, and then the state vector of the system can be constructed as where and are east and north velocity errors, respectively, and , , and are misalignment angles of pitch, roll, and yaw, respectively. To vehicles running on the earth surface, the height and upward velocity can be set as zero. In this paper, east-north-up (ENU) is defined as navigation frame .
The system state equation can be constructed as [1] where is the state matrix and is the white system process noise with the power spectral density . Based on the classical linear error propagation model, the state matrix can be expressed as [1]where and are the east and north velocity from SINS, respectively, and are the rotation angular rate and radius of the Earth, respectively, is the latitude of vehicle position, and , , and denote the projection in frame of the measured value by accelerometer in frame.
2.2. Measurement Equation
When the vehicle is static, the zero-velocity information in navigation frame is used as external reference and the measurement vector can be constructed as where and are the measurement variables, which equal and from SINS, respectively.
The system measurement equation can be constructed as [1] where is measurement matrix and is the white measurement noise with the power spectral density . According to the relationship between measured vector and state vector, can be expressed as
2.3. Closed Loop Correction Mode
The system and measurement equations given in Sections 2.1 and 2.2 are derived by the assumption that initial misalignment angles are small. With (2) and (5), SINS alignment with small initial misalignment angles can be finished by standard Kalman filter, and the five equations of standard Kalman filter will be demonstrated in Section 3.1.
The “system” in (2) and (5) refers to the “error combination” of SINS navigation parameters. The state variables do not participate in SINS navigation solution, which means that the SINS solution and error estimation based on (2) and (5) are two independent processes. Based on this, closed loop correction mode shown in Figure 1 is used to feed the estimation of state variables back to SINS to correct navigation parameters, and the sequent SINS navigation solution will be carried out according to the corrected navigation parameters once the correction operation is run. When misalignment angles can be estimated by Kalman filter, the attitude curves from SINS solution will gradually approach the true values with the help of closed loop correction mode; in other words, the error attitude curves (misalignment angles) from SINS solution will reduce to zero (or limit alignment values).
2.4. Simulation
2.4.1. Simulation Setting
Inertial measurement unit (IMU) is composed of medium or low precision sensors. The gyro constant bias is set as 0.1°/h and random bias is white Gaussian noise with zero mean and standard variance 0.1°/h. The accelerometer constant error is 500 ug and random error is also white Gaussian noise with zero mean and standard variance 500 ug. With the above assumption about sensor precision, the limit alignment accuracy can be obtained by zero-velocity alignment method as follows: pitch—−1.7189′ (minute of degree); roll—1.7189′; and yaw—−27.0248′.
The vehicle is without linear movement, but with swinging motion. The swinging amplitudes of pitch, roll, and yaw are set as 1.8°, 2.4°, and 2.8°, respectively, and the corresponding oscillation cycle is 6, 8, and 10 s. Based on the above ideal motion values, the ideal sensor output can be gotten by back-stepping of navigation solution. When sensor errors are added into ideal sensor output, the true sensor data can be obtained, with which navigation solution and alignment operation can be run. Meanwhile, the ideal motion values can be used as references to judge the alignment results with different initial misalignment angles.
The initial parameters for Kalman filter are set as
There is no unified definition about the magnitude of large misalignment angles in SINS until now. To show how the initial misalignment angles with different magnitudes will affect alignment results, the following five sets of different misalignment angles are used in simulation: Condition 1 ; Condition 2 ; Condition 3 ; Condition 4 ; Condition 5 . Generally speaking, the level alignment accuracy is much higher than that of azimuth one obtained by coarse alignment with analytical method, so the level misalignment angles from Condition 2 to Condition 5 are all set as 5°.
In simulation, the update cycle of sensor data, navigation parameter, and alignment filtering are all set as 10 ms, and the statistical period is set as 1 s.
2.4.2. Simulation Results
The simulation lasts for 900 s, and the misalignment angle curves of different conditions are shown in Figure 2. It can be seen that all misalignment curves can converge to the curves of limit alignment accuracy in a short time under Conditions 1 and 2; under Condition 3, misalignment curves can converge but with a longer time than those of Conditions 1 and 2; under Condition 4, misalignment curves can converge but with a longer time and larger errors, while under Condition 5, misalignment curves cannot converge.
(a) Pitch
(b) Roll
(c) Yaw
3. Analysis for Alignment Failure and an Improved Method
3.1. Analysis for Alignment Failure
The simulation results in Section 2.4.2 indicate that there is a close relationship between initial misalignment angles and alignment results when the classical linear error propagation model and standard Kalman filter are used for zero-velocity alignment. Larger initial misalignment angles will produce lower accuracy, even alignment failure, so this alignment method cannot deal with nonlinear problems caused by large misalignment angles.
Nevertheless, the simulation results in Figure 2 also show that there is a convergence trend for all five different conditions at the beginning process, that is, 0–100 s in Figure 2(c). Maybe this short convergence process implies that Kalman filter can estimate misalignment angles to some extent, though the estimation is not very accurate. In view of this, the change of the state covariance from Kalman filter is further studied and compared with the true one. For the sake of brevity, Figure 3 only shows the covariance of yaw under Conditions 1 and 5, where the true covariance of yaw can be calculated according to the alignment error in Figure 2(c) with square operation. The curves in Figure 3 indicate that, under the condition of small misalignment angles, the covariance of yaw from Kalman filter is larger than the true one throughout the alignment process, while under the condition of large misalignment angles, the covariance is larger than the true one only at the beginning and one short period during the alignment and smaller than the true one during the whole steady filtering process. Here, the period of 300–900 s, during which the change of is smooth, is defined as the steady filtering process. The differences between Figures 3(a) and 3(b) indicate that the state covariance from Kalman filter cannot truly represent the true covariance of state vector under the condition of large misalignment angles.
(a) Yaw in Condition 1
(b) Yaw in Condition 5
In order to evaluate the effect of on alignment results, the equations composing standard Kalman filter are further studied. The five equations of standard Kalman filter are listed as follows [2, 9, 11–15].
Prediction for state vector
Update for state vector
Calculation for gain
Prediction for state covariance
Update for state covariance
In the above equations, the subscript denotes the th time-step, denotes the estimation of state vector, is the translation matrix which is a discretization of in (2), and is the gain matrix which represents the utilization degree between the measurement vector and prediction for state vector. In Kalman filter, a larger means a higher utilization degree of measurement vector.
Furthermore, by analyzing the relationship between (10) and (11), it can be drawn that, with a fixed , all parameters in (8)–(12) will be fixed; with a larger , a larger in (11) will be produced and a larger in (10) will be generated, which leads to a higher utilization degree of measurement vector.
The above analysis indicates that Kalman filter can use measurement vector with a higher utilization degree during the whole alignment process under the condition of small misalignment angles, while under the condition of large misalignment angles, Kalman filter cannot because the is smaller than true ones during the steady filtering process. At the same time, the convergence trends in Figure 2 at the beginning of alignment process can be attributed to large initial parameters of Kalman filter; that is, the initial is much larger than the true ones.
Based on the above analysis, it can be concluded that, under large misalignment condition, the change of state covariance from Kalman filter cannot truly represent that of state vector, and Kalman filter cannot utilize external reference navigation information effectively. It should be one of the key reasons which lead to the alignment failure when the classical linear error propagation model and standard Kalman filter are used for zero-velocity alignment with large misalignment angles.
3.2. A Possible Way to Solve This Problem
3.2.1. Kalman Filter with the Parameters Resetting
In alignment process, in order to get a higher convergence speed and make full use of external reference information, initial is usually set as a large value when Kalman filter is used.
Based on the understanding about the effect of and initial setting of , an initial alignment method for SINS based on multiresetting Kalman filter parameters as shown in Figure 4 is designed in this paper. In this method, the classical linear error propagation model and standard Kalman filter are used, but the parameters of Kalman filter will be reset before the steady filtering process to ensure the state covariance larger than the true one and improve utilization degree of measurement vector. In Figure 4, denotes the update cycle of sensor data, navigation solution, and alignment filtering; denotes the parameter resetting cycle; denotes the resetting number.
During the alignment process based on parameter resetting, at the moment , the coarse alignment results will be set as initial parameter for SINS solution and initial parameters such as will be set to Kalman filter; at the moment , the state covariance will be set to initial value; that is, ; when the preset resetting times are finished, no resetting but normal alignment will be done. According to the parameters defined in Kalman filter, the matrixes and that denote the process noise and measurement noise, respectively, will remain unchanged when the instrument precision and operating condition remain unchanged; thus the resetting operation to Kalman filter parameters refers in particular to in the proposed method.
As mentioned in Section 2.3, Kalman filter and SINS solution are two independent processes, so resetting Kalman filter parameters will only affect the parameters of the filter and the state estimation but have no effect on navigation parameters. That is to say, the state estimation before resetting (the convergence result at the beginning of filtering) will be preserved in SINS navigation solution. With the help of resetting and closed loop correction mode, large misalignment angles can be gradually converged to small ones until the classical linear error propagation model can be applied.
3.2.2. Analysis of the Proposed Method
(1) Analysis of Advantages and Disadvantages. Obviously, the proposed method in Section 3.2.1 adopts the standard Kalman filter rather than nonlinear filter, such as UKF and PF; thus it owns the advantage of less calculation. At the same time, it adopts the classical linear error propagation model rather nonlinear model; thus it owns less calculation and simplified model.
But during alignment process, the resetting operation should be executed several times, and accordingly the stable process of filtering should be done at the same times passively. Thus, a disadvantage about this method is the prolonged alignment time.
(2) Two Problems about the Proposed Method. Two problems caused by the resetting operation should be solved. The first one is how to select the resetting cycle and the second is how to select the resetting times. Generally speaking, there are two solutions to these problems. The first one is a fixed resetting cycle and resetting times which can be preset according to the instrument precision and operation condition, and the second one is adaptive ones which can be set adaptively according to some criteria. Obviously, the second is an ideal method, but it is difficult to be fulfilled.
The aim of this paper is to find an alignment method for SINS with large initial misalignment angles without adding model complexity and adding computational load; thus no general or adaptive rules about the selection of resetting cycle and times are studied. In the rest of this paper, fixed resetting cycle and times are selected.
In engineering, a certain safety factor should be added to the above parameters to ensure a successful alignment; thus the alignment time will further be prolonged.
3.3. Simulation
3.3.1. Simulation Setting
The simulation is conducted under the same conditions as those in Section 2.4.1, and Condition 5 is used to verify the validity of the proposed method.
3.3.2. Simulation Results
The simulation lasts for 900 s, and the of Kalman filter is reset at 50 s, 100 s, and 150 s, respectively, during alignment. The curves of misalignment angles are shown in Figure 5, where the curves from Section 2.4.2 under Condition 1 are also given for comparison. In Figure 5(c), the dotted lines perpendicular to the horizontal coordinate denote the resetting moment. The comparison of state covariance from Kalman filter under Conditions 1 and 5, with the true one under Condition 5 as the reference, is shown in Figure 6.
(a) Pitch
(b) Roll
(c) Yaw
The curves in Figure 5 indicate that initial alignment can be fulfilled by the classical linear error propagation and standard Kalman filter after resetting the covariance matrix three times. And the partial enlargement in Figure 5(c) indicates that the alignment accuracy processed for 600 s is as precise as that with small initial misalignment angles. The curves also show that there are two added passive stable processes compared with those in Figure 3, which is caused by the operation of resetting.
The curves in Figure 6 indicate that the covariance of yaw from Kalman filter under Condition 5 is larger than that of true one during the whole alignment process. Compared with that in Figure 3, the true covariance gradually converges to a small value, and when the alignment is processed for 600 s, the covariance from Kalman filter under Conditions 1 and 5 are approximately equal, which means that the alignment accuracy are approximately equal at that moment as shown in Figure 5(c).
From the above analysis, it can be concluded that, with the resetting to the state covariance of Kalman filter, initial alignment for SINS with large initial misalignment angles can be fulfilled with classical linear error propagation model and standard Kalman filter.
4. Car Test
4.1. Test Setup
Considering the small angular motion without linear movement caused by engine vibration under shutdown condition, the car test is executed to verify the validity of the proposed alignment method. The fiber-optic gyro SINS as shown in Figure 7 is provided by Southeast University and the precision parameters of sensor are listed in Table 1. In this type of SINS, the update frequencies of sensor data and navigation data are both 100 Hz, and PC104 with a CPU frequency of 1 GHz is used as the navigation computer. The navigation parameters from PHINS—a high precision SINS from IxSea, France, as shown in Figure 7—are used as reference. The precision of PHINS is listed in Table 2.
Three different alignment schemes as follows are compared: Scheme 1: standard Kalman filter with small initial misalignment angles; Scheme 2: standard Kalman filter with large initial misalignment angles; Scheme 3: standard Kalman filter with large initial misalignment angles but with resetting operation. For the three schemes, the filtering frequencies are all set as 100 Hz and the classical linear error propagation model is used for the calculation.
The test is run as offline semiphysical simulation based on the saved data. Before fine alignment, analytical coarse alignment is executed. The results of coarse alignment can be considered as small misalignment angles because the sensors used in this test are high and the vibration caused by engine is small. To simulate large misalignment angles, the angles are added to the coarse alignment results.
During the alignment process in Scheme 3, the of Kalman filter is reset at 50 s, 100 s, 150 s, and 200 s, respectively.
4.2. Test Results
The curves of alignment results from different schemes are shown in Figure 8. The trends and oscillating processes in Figure 8 are similar to those in Figure 5. The alignment results in Figure 8 indicate that standard Kalman filter can fulfill initial alignment with small initial misalignment angles but cannot with large initial misalignment angles, while standard Kalman filter with multiresetting can fulfill alignment with large initial misalignment angles.
(a) Pitch
(b) Roll
(c) Yaw
Table 3 lists the statistic results in 300~500 s of Scheme 1, Scheme 3, and PHINS. According to the attitude of PHINS, alignment accuracy of Scheme 3 is as precise as that of Scheme 1 after alignment for 300 s.
From Figure 8 and Table 3, it can be concluded that, with the help of multiresetting, zero-velocity alignment with large initial misalignment angles can be fulfilled when classical linear error propagation model and standard Kalman filter are used.
4.3. Further Comparison between Simulation and Car Test
In Section 3.3, three times of resetting are required to fulfill alignment, while in Section 4.2, four times are required. There are two possible reasons for this difference: the first is that it is easy to find a set of Kalman parameters fit for sensors in simulation, while it is difficult in engineering, and the second is that the initial misalignment angles are larger in car test than those in simulation because of the added coarse alignment errors. Although no general rules about the selection of resetting times are proposed in this paper, the difference in Sections 3.3 and 4.2 implies that more resetting times are needed when larger initial misalignment angles appear.
5. Conclusions
The problem of large misalignment angles in SINS initial alignment is investigated in this paper, and simulation results verified that nonlinear problem brought by large initial misalignment angles will cause alignment failure when the classical linear error model and standard Kalman filter are used. The reasons for alignment failure are discussed in detail, and the analysis indicates that the state covariance of Kalman filter is smaller than the true one and the measurement vector cannot be effectively utilized during the steady filtering process. At the same time, the simulation also shows that there is a convergence trend for misalignment angles at the beginning of alignment process when the state covariance is much larger than the true one.
Based on the analysis of state covariance, an initial alignment method for SINS is designed, in which the classical linear error propagation model and standard Kalman filter with multiresetting are used. The state covariance of Kalman filter is reset several times before the steady filtering process and ensures the state covariance larger than the true one, which improves the utilization degree of measurement vector. This proposed method does not change the classical error propagation model and filter structure and owns the merits of simple model and less calculation.
Simulation and car test results both indicate that the proposed method can fulfill initial alignment with large initial misalignment angles effectively and the alignment accuracy can be as precise as that with small ones.
Conflict of Interests
The authors declare that there is no conflict of interests regarding the publication of this paper.
Acknowledgments
This work was supported in part by the National Natural Science Foundation (61273056) and the Chinese university research and operation expenses (104.205.2.5).