Abstract
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.
1. Introduction
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 [1]. With the gradual deepening of human beings in the field of ocean exploration, the tasks to be accomplished by underwater vehicles are becoming more and more complex and the working time is getting longer and longer. Now, the development of underwater vehicles faces many problems, such as the realization of high-precision autonomous navigation and positioning functions. The autonomous navigation system can provide navigation information for the underwater vehicle to ensure the correct travel.
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) [2]. The premise of acoustic navigation system applications such as Long Baselines and Ultrashort Baselines is that the reference matrix is placed in the working sea area beforehand. However, the debugging and arranging process would waste manpower and material resources. So they are not suitable for a wide range of navigation and position. An autonomous navigation system based on SINS requires navigation by means of inertial devices (gyroscopes and accelerometers) in the navigation system [3]. However, due to inertial measurement unit (IMU) installation errors and self-propelled problems, the navigation errors of the system will accumulate over time and will not be corrected in time. Therefore, the navigation accuracy will decrease after a period of time. DVL can provide relatively accurate speed information, which is self-contained, and the error does not accumulate over time. Therefore, SINS/DVL-based integrated navigation is a commonly used integrated navigation method for underwater vehicles [4].
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 [5, 6]. The algorithm realizes the recursion of the state quantity of the system by setting the sampling point, and U transforms the sampling point to replace the original value. In order to obtain better filter estimation, the above methods all require the system model to be accurate and the external interference signal is uncorrelated white noise with statistical characteristics. Otherwise, the filtering accuracy cannot be guaranteed, and even the filter divergence may be caused. However, in the actual SINS/DVL system, due to the error drift of the navigation device, the dynamic error caused by the vehicle manipulation, and the influence of the complex environment in the water, the system model is difficult to be completely accurate and the noise statistical characteristics are also uncertain. In order to further study the above issues, some researchers have introduced the Sage–Husa adaptive Kalman filter algorithm [7] to estimate the statistical characteristics of noise in real time.
Reference [8] pointed out the insufficiency of Sage–Husa adaptive filtering; that is, the measurement noise variance matrix R and the system noise variance matrix Q cannot be estimated online at the same time. Therefore, scholars have proposed a simplified Sage–Husa filter [9], which can estimate R when the Q is known, and the method has good adaptability. According to [10], a modified SHAKF is introduced to enhance the performance and increase the stability of the algorithm for the low cost EMES-INS/GPS tightly integrated system. The proposed Sage–Husa adaptive Kalman algorithm in Reference [11] is based on the adaptive weight that makes a better performance than KF for fiber optic gyroscope (FOG) drift error signal denoising. In order to solve the problem of reduced accuracy or the divergence of the filter, an adaptive Kalman filter (AKF) algorithm based on one-step smoothing filter is designed in [12] for integrated SINS/DVL systems. The variational Bayesian approach is introduced in Kalman filter in [13–15]. Parameter estimation and inference problems in Bayesian models are often very difficult. The adaptive Kalman model based on Sage–Husa is relatively simple and meets the real-time requirements of integrated navigation.
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 Section 2. In Section 3, the ISHAKF method is presented, which is based on the fading factor and the variable sliding window method. The simulation and vehicle test are designed to illustrate the superiority of the method in Section 4. and 5 is devoted to the conclusions.
2. SINS/DVL Tightly Integrated System
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 [16]. The tightly integrated navigation method proposed in this paper makes full use of DVL beam data to extend the measurement information to four dimensions. When the DVL beam information is less than three, the integrated navigation system can still work normally, further improving the fault tolerance of the system. A novel structure of the SINS/DVL tightly integrated navigation system is shown in Figure 1, where it can be seen that the differences among the original information of the four channels output by DVL and the SINS speed are used as the measurement information. At the same time, the three-dimensional SINS velocity information is extended to four dimensions through the transfer matrix. The system can estimate the DVL scale factor error, the gyro zero bias, and the add-on zero bias by Kalman filtering and finally feed back to the sensor output information to further improve the navigation accuracy of the underwater system.
2.1. State Equation
The state equation of the SINS/DVL tightly system can be shown as follows:where denotes the state transition matrix, is the system noise matrix, is the process noise vector, and denotes state vector, which can be obtained as follows:where represents attitude error, represents velocity error, denote position error, represents gyro bias, represents accelerometer bias, represents DVL scale factor, and represents pressure sensor bias. The system state transition matrix , the matrix , and can be expressed as follows [17]:where can be known in [17].
2.2. Measurement Equation
The measurement equation of the SINS/DVL tightly integrated system model is given aswhere is the measurement transfer matrix, is the measurement information noise, and is the measured matrix, which can be expressed aswhere denotes the velocity of SINS under the d frame and denotes the velocity of DVL measurement. The noise is
We give definitions as follows: represents the navigation frame, represents body frame, and represents the DVL frame. The relationship between SINS velocity and DVL velocity can be expressed aswhere is the direction cosine matrix and represents a transition from frame to frame and is the direction cosine matrix and represents a transition from frame to frame , which can be given aswhere represents the horizontal angle between the beams and the underwater vehicle. Commonly, there is . Taking DVL beam 1 as an example, its conversion relationship is shown in Figure 2.
Figure 2 is reproduced from [17]. (under the creative commons attribution license/public domain). We define the PS error model aswhere represents the true value, represents the PS biases, and represents the white noise. The DVL measurement error model can be expressed as [18]where represents the true value, represents the scale factor, and represents the white noise. Considering the installation angles error, the conversion relationship between frame b and frame d iswhere denotes the installation angle error between IMU and DVL. According to the above analysis, the velocity of SINS under frame d can be expressed as
Substituting (7) into (12), we can get
The measurement information is
So, the measurement transfer matrix can be expressed as
From (5), it can be seen that the DVL beam measurements are directly used without being transformed to frame b.
3. Improved Adaptive Filter Method
3.1. Sage–Husa Adaptive Kalman Filter
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 [19]. Sage–Husa adaptive algorithm is based on the Kalman filter to add time-varying noise estimator to estimate and correct the statistical characteristics of system noise in real time, thereby reducing model error, suppressing filter divergence, and enhancing filter accuracy. However, it is impossible to estimate all noise parameters such as system noise mean and variance and measurement noise mean and variance. Considering that system noise generally has less impact, the effects of system noise can be ignored. The currently used Sage–Husa filter is an adaptive filter algorithm, which is based on the estimated measurement variance matrix R. The system model can be established aswhere denotes state vector, denotes one-step transfer matrix from k–1 to k, denotes the system noise matrix, denotes measurement vector, is measurement matrix, denotes system noise, and is measurement noise. The noise matrices and satisfy the following statistical characteristics:where denotes covariance of the system noise and denotes covariance of the measurement noise. The standard Kalman process is as follows [20,21]:
The simplified SHAKF is based on (18)–(22) to increase the measurement noise estimator.
The SHAKF algorithm can measure the noise R in real time and achieve the adaptive effect while estimating the state.
3.2. Improved SHAKF Method
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 is introduced. Therefore, considering the idea of combining a fading factor, (21) can be given as
This paper proposes an adaptive adjustment function for fading factors. It can be known from formula (25) that when the error is large, should be reduced, thereby reducing the weight of the current measurement information. When the error is small, should increase, thereby increasing the weight of the current measurement information. Because the size of the fading factor can be determined by the error [22], we use the error mean as the error point. When it is less than the error mean , is adjusted closer to 1. When it is larger than the error mean , is adjusted closer to . According to the above analysis, the proposed adaptive adjustment function is as follows:
From (26), we know that the fading factor is controlled by the measurement error, which can ensure that the error swings around the error mean , which can improve the stability of the SHAKF. Normally, the value of ranges from 0.7 to 0.8, and the value of ranges from to 1. It can be seen from (26) that the forgetting factor is controlled by the error, which can ensure that the error swings around , and the change is slow, which can buffer the poor tracking effect caused by the change too fast.
It can be known from (23) that if the measurement noise of the actual system is smaller than the theoretical model value, will be smaller. If the state noise setting is too large, will be larger. Both of the above cases may cause , which makes it easy for to lose positive definiteness and cause filtering abnormality. To solve this problem, this paper adopts the sequential filtering method and limits the size of each element of the diagonal [23–25]. Assume is a diagonal matrix. Sequential filtering is used to perform the ith scalar sequential measurement update. The scalar measurement equation is
Set the value range to meet the following conditions:where denotes the th scalar. represents the th scalar element of the diagonal matrix at time k. Through the above processing, the measurement noise can be limited between and , thereby having better adaptive ability and reliability.
(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 (24) in the filtering algorithm:where m is the variable sliding window value. The range of values of m satisfies the following conditions:where N is the preset window maximum. When a large change in the R value is detected, m = 0, and the sliding window is incremented from 0 to N. When the value of m is greater than N, the window m = N. This method is well adapted to the variation of the R value. The two processes of window adjustment are shown in Figures 3 and 4. Figure 3 is a window adjustment process when m < N. It can be seen from the figure that as the observation data increases, the window m also gradually increases.
Figure 4 is a window sliding adjustment process when m > N. It can be seen from the figure that as the observation data increases, the moving window size remains N.
3.3. ISHAKF for SINS/DVL Tight System Scheme
Figure 5 is the structure of the improved Sage–Husa adaptive Kalman filter algorithm. The algorithm is based on the SINS/DVL tight combination navigation system designed in this paper, which can realize the noise estimation of four-dimensional velocity information. The ISHAKF algorithm is based on the SHAKF algorithm. Based on the original algorithm, the forgetting factor and variable sliding window method are introduced. The steps of implementing the ISHAKF algorithm are as follows.
4. Simulation and Vehicle Test
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.
4.1. Simulation
The accelerometer biases and the random walk noise are set as and, respectively. The gyroscope biases and the random walk noise are set as 0.02°/h and , respectively. The DVL scale factor is set as 0.002. The initial angle errors are set as 0.02°, −0.02°, and 0.5°, respectively. The DVL provides velocity information for four channels. The out frequencies of the IMU and DVL device are set as 200 Hz and 1 Hz, respectively. The vehicle movement start position is set to latitude and longitude . The curve of moving trajectory is shown in Figure 6. The altitude value of moving trajectory for underwater vehicle is 50 m, and the altitude value does not change during the whole movement.
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 aswhere the amplitudes of the swaying motion are set as , and , respectively, while the cycles of the swaying are 5 s, 7 s, and 6 s, respectively. The initial phases are set as , and , respectively. The curve of vehicle velocity and attitude is shown in Figure 7.
(a)
(b)
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 during 400 s∼700 s. Three methods are listed for SINS/DVL tightly integrated system. Figure 8 shows the curve of velocity error for KF, SHAKF, and ISHAKF.
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:where denotes the real velocity obtained by the simulation, is the number of signals, and is the measurement information. The RMSE has a good reflection of the measurement precision. So, the error data in Figure 8 is processed by the RMSE, and the results are shown in Table 1.
From Table 1 we know that the RMSE of north, east, and up velocity for ISHAKF are 0.138 m/s, 0.135 m/s, and 0.056 m/s, respectively, which has better performance than KF and SHAKF method. Figure 9 shows the curve of position error for KF, SHAKF, and ISHAKF. Compared with KF and SHAKF, it can be known that ISHAKF proposed in this paper has the smallest position error.
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 2. We can see that the north and east position errors for ISHAKF are 14.303 m and 14.559 m, respectively, which reduced by 44.59% and 45.96% than SHAKF method.
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 10. The max horizontal position errors for three methods are 126.30 m, 32.27 m, and 20.39 m, respectively, which shows that the ISHAKF method has the better accuracy than the KF and SHAKF method.
4.2. Vehicle Test
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 11. In vehicle experiment, the PHINS and GPS receiver are integrated, which works on the PHINS/GPS mode. So, PHINS provides relatively accurate navigation information including velocity position and attitude. The speed information output by the PHINS can be used to simulate the speed information provided by the DVL device. The instrument specifications are listed in Table 3.
(a)
(b)
The entire test route is on the campus of Southeast University and lasts for 1000 s. Figure 12 shows the motion trajectory. The curve of attitude and velocity is shown in Figure 13.
(a)
(b)
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. Two DVL beam missing regions are set: DVL beam channel 2 signal missing between 150 s and 200 s and DVL beam channel 4 signal missing between 600 s and 650 s.
Figure 14(a) shows the curve of velocity error, whose mean and standard deviation (STD) for two methods are shown in Figure 14(b). The position error for two methods is shown in Figure 15, where it can be known that the tightly integrated method proposed in this paper has better accuracy than the loosely integrated method. When the DVL beam channel information is less than 4, the loose combination cannot complete the combined navigation solution, so its speed error will continue to diverge. However, the tight combination method can still complete the combined navigation solution in the case of DVL beam loss and suppress the navigation error divergence. It can be know from the results that the tight combination method has better fault tolerance. The curve of the horizontal position error is shown in Figure 16, where it can be known that the max horizontal position error for tightly integrated method is 54.51 m, which is reduced by 63.9% compared to loosely integrated method.
(a)
(b)
The curve of velocity error for two methods is shown in Figure 17(a), in which the SHAKF is marked by blue line and the ISHAKF is marked by red line. Mean and standard deviation (STD) of velocity errors for two methods are shown in Figure 17(b), which displays that the mean and STD of ISHAKF are better than SHAKF method.
(a)
(b)
The RMSE of velocity for SHAKF and ISHAKF method is listed in Table 4. It can be seen that the north and east velocity errors for ISHAKF are 0.171 m/s and 0.204 m/s, respectively, which reduced by 12.7% and 7.2% compared to SHAKF method.
Figure 18 displays the curves of the north, east, and up position errors for SHAKF and ISHAKF. Compared with SHAKF, we know that the ISHAKF method has smaller position error. To further compare the positional accuracy of the two methods, the horizontal position error for the SHAKF and ISHAKF is displayed in Figure 19, which shows that the position accuracy based on ISHAKF is better than the SHAKF. The max horizontal position errors for SHAKF and ISHAKF are 49.76 m and 31.49 m. Compared with the SHAKF method, the accuracy of the ISHAKF method is improved by 36.71% which is listed in Table 5.
5. Conclusions
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.
Data Availability
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.
Conflicts of Interest
The authors declare that they have no conflicts of interest.
Acknowledgments
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.