Abstract
This paper presents a modified Sage-Husa adaptive Kalman filter-based SINS/DVL integrated navigation system for the autonomous underwater vehicle (AUV), where DVL is employed to correct the navigation errors of SINS that accumulate over time. When negative definite items are large enough, different from the positive definiteness of noise matrices which cannot be guaranteed for the conventional Sage-Husa adaptive Kalman filter, the proposed modified Sage-Husa adaptive Kalman filter deletes the negative definite items of adaptive update laws of the noise matrix to ensure the convergence of the Sage-Husa adaptive Kalman filter. In other words, this method sacrifices some filtering precision to ensure the stability of the filter. The simulation tests are implemented to verify that expected navigation accuracy for AUV can be obtained using the proposed modified Sage-Husa adaptive Kalman filter.
1. Introduction
Autonomous underwater vehicle (AUV) has been widely used in ocean exploration, where accurate navigation and positioning ability are essential to ensure the long voyage operation of AUV. Considering the underwater environment, various navigation methods cannot be applied to the AUV, such as optical navigation, radio navigation, and satellite navigation. In response to this problem, a lot of novel navigation methods are studied in [1–3].
Since the strapdown inertial navigation (SINS) is with the advantages of strong autonomy, high precision, and full navigation parameters, it is widely used in the navigation system of AUV. However, the navigation error of SINS is accumulated over time, which results in low navigation accuracy. In view of this problem, the Doppler velocimeter (DVL) is combined with SINS in [4–7] to construct the SINS/DVL integrated navigation system, where the navigation error of SINS is corrected by the DVL. To provide the accurate initial attitude angles to the SINS/DVL integrated navigation system, the initial attitude alignment algorithms for SINS/DVL are studied. In [8, 9], the robust alignment methods are proposed to improve the alignment accuracy in complex working environment. In [10], a new Kalman filter-based alignment method is presented to modify the initial attitude alignment algorithm. But the state equations of above studies ignore the error of DVL, such as the calibration coefficient error, the velocity deviation error, and the drift angle error. That results in limited navigation accuracy.
For the SINS/DVL integrated navigation system, how to integrate the output information of SINS and DVL to obtain high-precision navigation information is the core issue. Since the Kalman filter is able to estimate the system states in the presence of noises, it is widely applied in the SINS/DVL integrated navigation system. However, the inaccurate measurement model under the time-varying measurement noise results in substantial estimation errors or even filter divergence. To solve this problem, the interacting multiple model algorithm which uses more than one model is proposed in [11], where a variable model set based on the model probability weighted average of the model parameter is generated to pursue the real model. Considering the nonlinearity of the model of SINS/DVL, the square root unscented information filter is designed in [12], the randomly weighted cubature Kalman filter is discussed in [13], and the unscented Kalman filter (UKF) is employed in [14–16]. Furthermore, considering the influence of unknown environment and the inexact error model caused by model simplification, various adaptive Kalman filters are proposed in [17–20], where statistical characteristics of noises are online estimated in [17–19] and the recursive filtering gain is adaptively adjusted in [20]. However, the adaptive filter might be divergent when the positive definiteness of noise matrix cannot be guaranteed.
In this paper, a modified Sage-Husa adaptive Kalman filter-based SINS/DVL integrated navigation system is proposed to provide the AUV with accurate navigation parameters, where the adaptive update laws of the noise matrix are modified by deleting the negative definite items. Therefore, the filtering convergence can be guaranteed, while the expected filtering accuracy can be obtained.
The rest of this paper is organized as follows. The error equations of SINS and DVL are given in Section 2. The Kalman filter equation and a modified Sage-Husa adaptive Kalman filter for the SINS/DVL integrated navigation system are presented in Section 3. Numerical simulations are conducted to verify the superiority of the proposed approach in Section 4. Conclusions are drawn in Section 5.
2. Error Equations of SINS And DVL
2.1. Error Equations of Strapdown Inertial Navigation
When the AUV moves on the water surface, considering the drift of the gyroscopes and accelerometer, the error equations of SINS are presented.
Define the longitude and latitude as and , the longitude error and latitude error as and , the east and north velocity as and , the east and north velocity error as and , the earth radius as , the angular velocity of the earth as , the output of accelerometer in the navigation coordinate system as , , and , the horizontal drift of the accelerometer in the navigation coordinate system as and , and the drift of the gyroscope in the navigation coordinate system as , , and . Then, the attitude error equation is calculated as
The horizontal position error equation of SINS is calculated as
The horizontal velocity error equation of SINS is calculated as
The drift of the gyroscope in the navigation coordinate system can be described as where is the attitude matrix, and the drift of the gyroscope in the body coordinate system can be described as with the correlation time , the drift of the gyroscope in the body coordinate system , , and , and the white noise , , and .
2.2. Error Equations of the Doppler Velocimeter
For the four-beam Doppler velocimeter, the calibration coefficient error is defined as , the velocity deviation error is defined as , and the drift angle error is defined as . Then, the output of the Doppler velocimeter can be presented as where is the azimuth angle, is the track angle, and is the true velocity.
Since , , , and are small, ignoring the small quantities of the second order, equation (6) can be simplified as where with the correlation time and and the white noise and .
3. SINS/DVL Integrated Navigation System
3.1. State Equation and Measurement Equation
Since the navigation error of SINS is accumulated over time, the four-beam Doppler velocimeter is used to correct the navigation information of SINS. Define the state vector as. The state equation is presented as where is the noise matrix, is the state-transition matrix, is the noise distribution matrix, and with
Since the velocity is selected as the measurement, the measurement equation is presented as where
From equations (7) and (12), it is calculated that where and are the system measured noises.
Thus, equation (12) can be rewritten as where
By discretizing the state (equation (9)) and measurement (equation (15)), the Kalman filter equation of the SINS/DVL integrated navigation system is obtained as where is the state vector at time , is the measurement vector at time , is the measurement matrix at time , is the system measurement noise matrix at time , is the state vector at time , is the noise matrix at time , is the noise distribution matrix at time , is the state one-step transition matrix, and and are white Gaussian noises with , , and with the sampling period .
3.2. Modified Sage-Husa Adaptive Kalman Filter
Due to the fact that the measurement noises of the Kalman filter equation are usually time-varying and difficult to be accurately predicted in the practical environment, the filter accuracy of the conventional Kalman filter is easy to be decreased and even be divergent. In response to this problem, the Sage-Husa adaptive Kalman filter is proposed to improve the filter performance by estimating the unknown noises, whose calculation loop is presented in Figure 1. The Sage-Husa adaptive Kalman filter is designed as where
with the forgetting factor and .
To suppress filter divergence, must be nonnegative definite and must be positive definite. However, considering the existence of and in the above equation, the positive definiteness of and cannot be guaranteed. Since and have little influence on filter accuracy, they can be deleted to ensure the convergence of the Sage-Husa adaptive Kalman filter. In other words, this method sacrifices some filtering precision to ensure the stability of the filter.
Thus, and are modified as
From equation (21), it is obtained that both and are affected by the innovation vector . That means that the filter accuracy cannot be guaranteed when and a5re changing abnormally with . In response to this problem, is assumed to be known such that can be accurately estimated.
4. Simulation
In this section, numerical simulations are investigated to verify that expected navigation accuracy can be obtained under the proposed modified Sage-Husa adaptive Kalman filter-based SINS/DVL integrated navigation system for AUV.
The forgetting factor of the modified Sage-Husa adaptive Kalman filter is set as . For the gyroscopes of SINS, the constant drift is and the random drift is . For the accelerometers of SINS, the constant drift is and the random drift is . The velocity measurement error of DVL is . The initial covariance matrix is set as, the initial variance matrix of state noise is set as with , and the variance matrix of measurement noise is set as .
The simulations are presented in Figures 2–6. From the system states of the Kalman filter for the SINS/DVL integrated navigation system presented in Figure 2, it is observed that all the system states can converge to the constants. Then, velocities and positions of AUV under the SINS and SINS/DVL integrated navigation system are presented in Figures 3–6. From Figures 3 and 4, velocity measurements under two navigation systems are with local differences. Furthermore, the velocity errors shown in Figure 4 depicted that under the SINS/DVL integrated navigation system, the velocity errors converge to zero, where velocity errors of SINS are diverging. Positions of AUV under the SINS and SINS/DVL integrated navigation system are presented in Figure 5, and then, the position errors shown in Figure 6 depict that under the SINS/DVL integrated navigation system, the position errors of SINS/DVL are much smaller than those of SINS. In a word, the expected navigation accuracy is obtained using the proposed modified Sage-Husa adaptive Kalman filter-based SINS/DVL integrated navigation system.
5. Conclusion
For AUV, a modified Sage-Husa adaptive Kalman filter-based SINS/DVL integrated navigation system is designed in this paper to obtain expected navigation accuracy. The negative definite items of adaptive update laws of the noise matrix are deleted to guarantee the positive definiteness of noise matrices, such that the filter stability can be ensured. Simulations are presented to verify that expected navigation accuracy for AUV can be obtained using the proposed filter method.
Data Availability
The data supporting the findings of this study are available within the article.
Conflicts of Interest
The authors declare that they have no conflicts of interest.