Abstract
When the strapdown inertial navigation system does not perform coarse alignment, the misalignment angle is generally a large angle, and a nonlinear error model and a nonlinear filtering method are required. For large azimuth misalignment, the initial alignment technology with a large azimuth misalignment angle is researched in this paper. The initial alignment technology with a large azimuth misalignment angle is researched in this paper. First, the SINS/GPS nonlinear error model is established. Secondly, in the view of observation gross errors and inaccurate noise statistical characteristics, an adaptive robust CKF algorithm is proposed. Finally, according to the simulation analysis and experiment, the adaptive robust CKF algorithm can augment the stability and improve the filter estimation precision and convergence rate.
1. Introduction
As an important means for exploring oceans, underwater vehicles are widely applied in civilian and military. With the continuous improvement of requirements for navigation accuracy, high-precision navigation becomes an important technical guarantee/support for underwater vehicles to reliably and safely complete underwater navigation tasks. Meanwhile, in the face of the complex underwater work environment, high-precision navigation is based on high-precision initial alignment. Also, initial alignment mainly contains the rough alignment process and fine alignment process; once the underwater vehicle encountered bad sea conditions, rough azimuth information is difficult to obtain in the rough alignment process so that the condition of initial alignment is usually with large azimuth misalignment angle. Therefore, in view of the work process and actual application requirements for the underwater vehicle, this paper aims to study the technology of initial alignment with large azimuth misalignment angle.
Initial alignment with large azimuth misalignment angle is the key technology of SINS [1, 2]. According to the characteristics of the nonlinear error model, nonlinear EKF and UKF are usually applied on tradition. However, the performance of the EKF depends on linearization to a large extent, in the face of a strongly nonlinear system, the higher-order terms of Taylor expansion of the nonlinear model cannot be ignored so that large truncation errors will be generated, and the filter accuracy will be reduced or even diverged. Also, when dealing with the high-dimensional system, the covariance in the UKF filter process will be nonpositive definite, resulting in the instability of the filter and the possibility of divergence. In view of the problems above, the cubature Kalman filtering (CKF) based on the spherical-radial cubature criterion is widely applied at the present time. This algorithm avoids the linearization of the nonlinear model and overcomes the shortcomings of EKF and other traditional nonlinear filtering algorithms [2].
If the satellite signal is unstable, there will be gross bias in the observations, which will affect the estimation effect of the filter. At the same time, we cannot get accurate noise characteristics in most cases [2, 3]; thus, the performance of filter estimation is greatly limited. In view of the above problems, an adaptive robust CKF algorithm based on the traditional CKF is proposed in this paper, which utilizes robust M estimation and an improved Sage–Husa suboptimal posterior estimator to improve the stability and adaptive ability of the filter.
2. SINS Large Azimuth Misalignment Angle Error Model
The conversion matrix between the navigation coordinate system used which is described by the Euler platform error angle and the mathematical platform coordinate system is as follows:
is , and is .
Assuming that the angular velocity of the system relative to the system is , the error angular differential equation of the Euler platform is
2.1. Error Equation
According to Chang [4], it can be concluded that the SINS attitude error equation is
The velocity error equation is
2.2. Filtering Model
It is assumed that the measurement error of gyroscope is mainly random constant drift error and zero-mean Gaussian white noise . The measurement error of the accelerometer is mainly random normal zero deviation and zero-mean Gaussian white noise . If the horizontal misalignment angle is small and the height error is not considered, the SINS large azimuth error model is
The system state quantity is . The white noise of the gyroscope and accelerometer is .
Take velocity and position information as the measurement.
The system state equation and measurement equation are
is the process noise input matrix; is the measuring matrix, and ; is the process noise; is the measurement noise and follows Gaussian distribution .
3. Robust Kalman Filtering Algorithm
In SINS/GPS initial alignment, the gross errors will affect the estimated results [5, 6], and individual large gross errors will make a greater deviation. However, traditional Kalman filtering cannot effectively deal with gross errors; in order to solve the above problems, a robust Kalman filtering algorithm is proposed in this paper. The algorithm combines the robust estimation theory and adopted a more practical robust M estimation. On the basis of M estimation, the principle of equivalent weight is applied to transform it into the form of least squares [6]. How to choose weighting factors is the key of robust Kalman filtering. In this paper, weight function is used to replace the observed noise covariance matrix to reduce or eliminate the influence of the gross errors.
The original observation weight is ; then, its equivalence weight , and the weighting factors are as follows:
is the residual error of observation quantity; is the mean square error of , , is the weight reciprocal, variance factor , and and are and .
4. Adaptive Kalman Filtering Algorithm
Research shows that inaccurate noise characteristics often lead to unstable and even filter divergence [7, 8]. Therefore, adaptive filtering technology has been widely studied. Sage–Husa suboptimal unbiased maximum posterior (MAP) noise estimator has been widely used due to its advantages of simple calculation and clear principle [9]. However, Sage–Husa noise estimator cannot simultaneously estimate and measure system noise; otherwise, it will lead to filter divergence [9, 10]. In the actual environment, the measurement noise can be obtained by the physical characteristics of the sensor, but due to the influence of measurement instrument precision, external interference, and other factors, it is difficult to obtain the system noise accurately [11–13]. In view of the above problems, combining with the covariance matching criterion and introducing the adaptive attenuation factor, the divergence phenomenon of the adaptive filtering was suppressed, and the stability and adaptability were improved.
4.1. System Noise Estimation Method
In this paper, improved Sage–Husa is used to estimate the inaccurate system noise and :
, is the forgetting factor, and the higher the value of is, the faster the noise statistics change.
4.2. Suppression of Filter Divergence
Sage–Husa estimator usually diverges, so the covariance matching criterion is used in this paper. is used to determine whether the estimator diverges [14, 15]; if it diverges, it will be corrected as follows: , in which is the adjustable coefficient, is the residual sequence, and .where is the attenuation coefficient, usually about ; the larger the value of is, the more prominent the influence of the current residual vector is.
5. Adaptive Robust CKF Algorithm
The adaptive robust CKF combines the advantages of the robust Kalman filtering algorithm and the adaptive Kalman filtering algorithm, which can not only overcome the influence of the observed gross errors but also restrain the problem of filter divergence caused by the unknown or time-varying noise of the system.
Assume that the state equation and measurement equation of the nonlinear system are
and are the Gaussian white noise with zero mean, and the variance matrix is and .
Adaptive robust CKF algorithm is as follows:(1)Calculate the volume point and weight:(2)Time update:(3)Measurement update:
is the equal weight, and it can be obtained by the M estimation equivalence weight principle.
Suppose the original observation weight is , . And the power factor is
Divergent judgment is as follows.
Divergence is judged according to equation (15), and if divergence is detected, it is corrected according to equations (16)–(19). Otherwise, go directly to equation (20).
In order to solve imprecision of the system noise and the observation gross error, the adaptive robust CKF algorithm has a targeted solution: Sage–Husa adaptive filter is used to estimate system noise online, and the filter divergence is suppressed. The equivalent weight coefficient is used to adjust the influence degree of the observation noise error to obtain and .
6. Simulation
Combined with the adaptive robust CKF and SINS error model proposed in this paper, numerical simulation is carried out.
The initial estimate of system state ; initial misalignment angle , ; the initial velocity error is ; the constant drift of the gyroscope is ; random noise is ; the accelerometer having zero deviation is ; random noise is ; the GPS speed measurement error is ; then, the initial variance matrix , system noise matrix , and measurement noise array are(1)In a certain period of time, the velocity observation increases the coarse difference of , and the filter sampling frequency is also increased. Two nonlinear filtering methods, CKF and robust CKF, were compared, respectively. The simulation time is s. The simulation results are shown in Figures 1–3. As can be seen from Figures 1 and 2 and Table 1, the precision of the robust CKF estimation is higher for horizontal misalignment angle, the error curve is relatively smooth, and the convergence speed is fast. It can be seen from Figure 3 and Table 1 that the precision and convergence speed of the robust CKF estimation are better than those of CKF, and there are no more burrs, so the filtering is more stable.(2)On the basis of simulation condition (1), the actual system noise is assumed to be . The two filtering methods of robust CKF and adaptive robust CKF were compared, respectively. The simulation time is s. The simulation results are shown in Figures 4–6. As can be seen from Figures 4–6 and Table 2, the precision of the two filtering algorithms is almost the same as that of the eastbound misalignment angle, and the adaptive robust CKF can maintain a high performance for the estimation of northbound misalignment angle and azimuth misalignment angle; besides, the convergence speed is more faster.
7. Experiment Test
In order to verify the adaptive robust CKF filtering algorithm feasibility of actual initial alignment, based on the original measured data and offline analysis, test of the azimuth misalignment angle estimation error curve is shown in Figure 7. The experiment results are shown in Table 3.
From Figure 7 and Table 1, the azimuth misalignment estimation error of CKF is 5.21′, and the azimuth misalignment estimation error of adaptive robust CKF is 3.76′.
The test result shows that the alignment precision of the adaptive robust CKF filtering algorithm is higher than that of the traditional CKF filtering algorithm, which solves the error caused by inaccurate measurement information or system noise and improves the initial alignment precision.
8. Conclusion
In this paper, an adaptive robust CKF filtering algorithm is studied, which can simultaneously observe the errors caused by gross error and inaccurate system noise. Besides, the adaptive robust CKF can effectively realize the initial alignment of the large azimuth misalignment angle, improve the estimation precision and convergence speed of the misalignment angle, and enhance the stability and self-adaptive ability of the filtering algorithm under the condition of the observed information anomaly or the unknown system noise characteristic.
Conflicts of Interest
The authors declare that they have no conflicts of interest.