The main challenge of Strap-down Inertial Navigation System (SINS)/Doppler velocity log (DVL) navigation system is the external measurement noise. Although the Sage–Husa adaptive Kalman filter (SHAKF) has been introduced in the integrated navigation field, the precision and stability of the SHAKF are still the tricky problems to be overcome. The primary aim of this paper is to improve the precision and stability of underwater SINS/DVL system. To attain this, a SINS/DVL tightly integrated model is established, where beam measurements are used without transforming them to 3D velocity. The proposed improved SHAKF algorithm is based on variable sliding window estimation and fading filter. The simulations and vehicle test results demonstrate the effectiveness of the proposed underwater SINS/DVL tightly integrated navigation method based on the improved SHAKF. In addition, the position accuracy of the designed method outperforms that of the SHAKF method.
As humans explore the ocean, the demand for resource development, marine environmental surveys, and operations has also increased. Underwater vehicles can replace humans in deep ocean to complete the detection and development of marine resources, saving a lot of cost for production. Underwater vehicles have become one of the important tools for human ocean development [
At present, there are many kinds of navigation systems used on underwater vehicles such as Strap-down Inertial Navigation System (SINS), Long Baseline (LBL), Doppler velocity log (DVL), and Ultrashort Baseline (USBL) [
In integrated navigation systems, Kalman filter is a widely used information fusion algorithm. The wide application of Kalman filter began in the 1960s. The essence of this filter is a recursive process, which is the optimal minimum variance estimation of data. The filter can achieve an optimal estimate by fusing information from different sensors. The traditional Kalman filter can be applied to linear systems, but in fact many systems are nonlinear systems. So, extended Kalman filter (EKF) is proposed. This filter no longer requires the system to be linear, which compensates for the deficiency of traditional Kalman filter to some extent. Then, the unscented Kalman filter (UKF) based on the nonlinear system is proposed [
Reference [
To further improve the antijamming capability of the navigation system, some methods are proposed. On the one hand, a novel SINS/DVL tightly integrated model is built, which can work well when the number of DVL beam channels is less than 3. On the other hand, an improved Sage–Husa adaptive Kalman filter (ISHAKF) is introduced, which introduces the forgetting factor and variable sliding window.
The structure of this paper is as follows: a SINS/DVL tightly integrated model is designed in
The underwater SINS/DVL navigation system based on the loose combination takes the difference between the three-dimensional velocity of the DVL output and the three-dimensional velocity of the SINS. When the number of DVL beam channels is less than three, DVL will not be able to output 3D speed information, so the SINS/DVL integrated navigation system will not work well [
Structure of the proposed underwater SINS/DVL tightly integrated system.
The state equation of the SINS/DVL tightly system can be shown as follows:
The measurement equation of the SINS/DVL tightly integrated system model is given as
We give definitions as follows:
The relationship between frame
Figure
Substituting (
The measurement information is
So, the measurement transfer matrix
From (
In 1969, scholars A. P. Sage and G. W. Husa designed an adaptive filter algorithm, which can estimate the noise characteristics of the system in real time through the measurement output while performing state estimation [
The simplified SHAKF is based on (
The SHAKF algorithm can measure the noise
In view of the deficiencies of SHAKF algorithm, this paper makes improvements from two aspects. Firstly, the variable fading factor is designed to improve the adaptive ability of the filtering. Secondly, the variable sliding window method is used to measure the estimation of noise, and the estimation accuracy of the measurement noise is further improved.
(1) Based on the SHAKF algorithm, the fading factor
This paper proposes an adaptive adjustment function for fading factors. It can be known from formula (
From (
It can be known from (
Set the
(2) In order to enhance the estimation accuracy of measurement noise, a variable sliding window estimation method is designed in this paper. Through the measurement noise mean processing in the SHAKF, the estimation accuracy of the measurement noise is further improved. The following improvement is made to (
Window sliding adjustment process when
Window sliding adjustment process when
Figure
Figure
Process of the improved Sage–Husa adaptive filter for SINS/DVL.
Perform ISHAKF algorithm status update.
Calculate the forgetting factor according to the introduced forgetting factor adjustment function.
Calculate the filter gain according to the forgetting factor, and perform the ISHAKF filter measurement update at the same time.
Calculate the sliding window value, and perform the sliding window averaging processing on the measurement noise.
Determine if the
To illustrate the navigation performance of the proposed ISHAKF method, the simulation and vehicle experiment are designed. First, the trajectory is simulated, which simulates the underwater vehicle swinging motion. Second, the outperformance of the ISHAKF method is verified compared with Sage–Husa adaptive Kalman filter and Kalman filter in complex sea conditions. Finally, the vehicle test is designed, which simulates underwater SINS/DVL tightly integrated navigation system.
The accelerometer biases and the random walk noise are set as
Curve of moving trajectory.
Since the underwater vehicle movement is affected by the water flow, its motion should be a rocking motion. In order to simulate the swaying motion of the underwater vehicle, the attitude angle of the vehicle is given as
Curve of velocity and attitude.
In order to illustrate the anti-interference ability of the proposed ISHAKF method to external noise, different noise levels are set. The measurement noise covariance is set as
Curve of velocity error for three methods.
The navigation accuracy and anti-interference ability of three methods (KF, SHAKF, and ISHAKF) are compared by calculating the root-mean-square error (RMSE) of the velocity:
RMSE for three methods.
KF | SHAKF | ISHAKF | |
---|---|---|---|
ΔVN (m/s) | 0.246 | 0.139 | 0.138 |
ΔVE (m/s) | 0.271 | 0.145 | 0.135 |
ΔVU (m/s) | 0.061 | 0.058 | 0.056 |
From Table
Curve of position error for the three methods.
According to navigation system, the positioning performance of the three methods is compared, the above position error is analyzed, and the max error is calculated, whose results are listed in Table
Max error for three methods.
KF | SHAKF | ISHAKF | |
---|---|---|---|
ΔPN (m) | 113.137 | 25.816 | 14.303 |
ΔPE (m) | 57.966 | 26.946 | 14.559 |
ΔPU (m) | 0.604 | 1.014 | 1.014 |
In order to further compare the positioning accuracy of the three methods, the horizontal position error is calculated for the KF, SHAKF, and ISHAKF as shown in Figure
The horizontal position error for the three methods.
The vehicle experiment is designed to simulate underwater vehicle due to the limitations of the experimental conditions. The vehicle experiment device includes a PHINS of IXSEA Corporation, a navigation computer, a Flex Park 6 GPS receiver, and an inertial measurement unit. Meanwhile, PHINS is used as a reference navigation system to provide the accurate navigation information in real time. The vehicle equipment is shown in Figure
(a) Experimental vehicle and (b) equipment.
Instrument specifications.
Instruments | Parameters | Accuracy | Frequency (Hz) |
---|---|---|---|
IMU | Gyroscope bias | 200 | |
Gyroscope random walk | 200 | ||
Accelerometer bias variation | 200 | ||
Accelerometer output noise | 200 | ||
GPS | Velocity (RMS) | 0.03 m/s | 1 |
Position (L1) | 1.5 m | 1 | |
PHINS | Attitude (GPS aided mode) | 200 |
The entire test route is on the campus of Southeast University and lasts for 1000 s. Figure
Curve of moving trajectory.
Curve of attitude and velocity.
In order to verify the fault-tolerant performance of the designed SINS/DVL tightly integrated navigation method, the loosely integrated navigation method is introduced in this section. The DVL scale factor is 0.002. The initial attitude error is
Figure
Curve of velocity error (a) and mean and STD of the velocity errors (b) for two methods.
Curve of position error for two methods.
Curve of horizontal position error.
The curve of velocity error for two methods is shown in Figure
Curve of velocity error (a) and mean and STD of the velocity errors (b) for two methods.
The RMSE of velocity for SHAKF and ISHAKF method is listed in Table
RMSE for two methods.
SHAKF | ISHAKF | |
---|---|---|
ΔVN (m/s) | 0.196 | 0.171 |
ΔVE (m/s) | 0.220 | 0.204 |
ΔVU (m/s) | 0.142 | 0.130 |
Figure
Curve of position error for two methods.
Curve of horizontal position error for two methods.
Maximum absolute error (MAE) for two methods.
SHAKF | ISHAKF | |
---|---|---|
ΔPN (m) | 49.68 | 30.85 |
ΔPE (m) | 8.38 | 8.35 |
ΔPU (m) | 4.37 | 3.05 |
In order to improve the fault tolerance of SINS/DVL integrated navigation in complex environment, this paper proposes an improved Sage–Husa adaptive filtering SINS/DVL tight combination navigation method, which is improved from two aspects. Firstly, a new compact combined navigation model is derived based on the traditional SINS/DVL loose combination and the state equation and measurement equation are established. Secondly, based on the Sage–Husa adaptive Kalman filter algorithm, the forgetting factor and variable sliding window are introduced, which further improves the robustness and adaptive ability of the adaptive filter. Finally, simulation and in-vehicle tests verify that the proposed method can further enhance the robustness and navigation accuracy of the SINS/DVL system.
The raw/processed data required to reproduce these findings cannot be shared at this time as the data also form part of an ongoing study.
The authors declare that they have no conflicts of interest.
This work was supported in part by the National Natural Science Foundation of China under Grants 51775110, 51375088, 51979041, and 61473085; in part by the Natural Science Foundation of Jiangsu Province, China under Grant BK20190344; in part by the Postgraduate Research & Practice Innovation Program of Jiangsu Province, under Grant KYCX20_0109; in part by the Inertial Technology Key Lab Fund under Grant 614250607011709; in part by the Fundamental Research Funds for the Central Universities under Grants 2242019K40041 and 2242020k1G009; by Key Laboratory Fund for Underwater Information and Control under Grant 614221805051809; and in part by the Jiangsu Key Laboratory Fund for Green Ship Technology under Grant 2019Z01. Remaining funds were provided by cultivation project of National Natural Science Foundation of Southeast University under Grant 9S20172204.