An Improved Adaptive Kalman Filter for Underwater SINS/ DVL System

'e 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. 'e 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. 'e proposed improved SHAKF algorithm is based on variable sliding window estimation and fading filter. 'e simulations and vehicle test results demonstrate the effectiveness of the proposed underwater SINS/DVL tightly integrated navigationmethod based on the improved SHAKF. In addition, the position accuracy of the designed method outperforms that of the SHAKF method.


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. e 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]. e 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.
erefore, 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.
erefore, 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. e wide application of Kalman filter began in the 1960s. e essence of this filter is a recursive process, which is the optimal minimum variance estimation of data. e filter can achieve an optimal estimate by fusing information from different sensors. e traditional Kalman filter can be applied to linear systems, but in fact many systems are nonlinear systems. So, extended Kalman filter (EKF) is proposed.
is filter no longer requires the system to be linear, which compensates for the deficiency of traditional Kalman filter to some extent. en, the unscented Kalman filter (UKF) based on the nonlinear system is proposed [5,6]. e 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. erefore, 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. e 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. e variational Bayesian approach is introduced in Kalman filter in [13][14][15]. Parameter estimation and inference problems in Bayesian models are often very difficult. e 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. e 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. e simulation and vehicle test are designed to illustrate the superiority of the method in Section 4. and 5 is devoted to the conclusions.

SINS/DVL Tightly Integrated System
e underwater SINS/DVL navigation system based on the loose combination takes the difference between the threedimensional 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]. e 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.
e 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.

State Equation.
e state equation of the SINS/DVL tightly system can be shown as follows: where F denotes the state transition matrix, G is the system noise matrix, W is the process noise vector, and X denotes state vector, which can be obtained as follows: where ϕ represents attitude error, δV represents velocity error, δλ, δL, δh denote position error, ε represents gyro bias, ∇ represents accelerometer bias, K D represents DVL scale factor, and b PS represents pressure sensor bias. e system state transition matrix F, the matrix G, and W can be expressed as follows [17]: where M aa , M av , M ap , M va , M vv , M pv , M pp , M vp can be known in [17].

Measurement Equation.
e measurement equation of the SINS/DVL tightly integrated system model is given as where H is the measurement transfer matrix, V is the measurement information noise, and Z is the measured matrix, which can be expressed as where We give definitions as follows: n represents the navigation frame, b represents body frame, and d represents the DVL frame. e relationship between SINS velocity and DVL velocity can be expressed as where C b n is the direction cosine matrix and represents a transition from frame n to frame b and C d b is the direction cosine matrix and represents a transition from frame b to frame d, which can be given as where α represents the horizontal angle between the beams and the underwater vehicle. Commonly, there is α � 70°. 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 as where H PS represents the true value,δb PS represents the PS biases, and w PS represents the white noise. e DVL measurement error model can be expressed as [18] where V d DVL represents the true value, K D represents the scale factor, and w D represents the white noise. Considering  Figure 1: Structure of the proposed underwater SINS/DVL tightly integrated system. the installation angles error, the conversion relationship between frame b and frame d is where ϕ � φ x φ y φ z T 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 e measurement information is So, the measurement transfer matrix H can be expressed as From (5), it can be seen that the DVL beam measurements are directly used without being transformed to frame b.

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. e currently used Sage-Husa filter is an adaptive filter algorithm, which is based on the estimated measurement variance matrix R. e system model can be established as where X k denotes state vector, Φ k,k−1 denotes one-step transfer matrix from k-1 to k, Γ k−1 denotes the system noise matrix, Z k denotes measurement vector, H k is measurement matrix, W k−1 denotes system noise, and V k is measurement noise. e noise matrices W k−1 and V k satisfy the following statistical characteristics: where Q k denotes covariance of the system noise and R k denotes covariance of the measurement noise. e standard Kalman process is as follows [20,21]: Figure 2: e relationship between frame b and frame d. 4 Mathematical Problems in Engineering e simplified SHAKF is based on (18)- (22) to increase the measurement noise estimator.
e SHAKF algorithm can measure the noise R in real time and achieve the adaptive effect while estimating the state.

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. erefore, considering the idea of combining a fading factor, (21) can be given as is 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 E[ε k ] as the error point. When it is less than the error mean E[ε k ], λ is adjusted closer to 1. When it is larger than the error mean E[ε k ], λ is adjusted closer to λ min . 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 E[ε k ], which can improve the stability of the SHAKF. Normally, the value of λ min ranges from 0.7 to 0.8, and the value of λ ranges from λ min 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 E[e(n)], 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, ε k ε T k will be smaller. If the state noise setting is too large, H k P k,k−1 H T k will be larger. Both of the above cases may cause ε k ε T k − H k P k,k−1 H T k < 0, which makes it easy for R k 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 R k diagonal [23][24][25]. Assume R k is a diagonal matrix. Sequential filtering is used to perform the ith scalar sequential measurement update. e scalar measurement equation is Set the R k value range to meet the following conditions: where i denotes the ith scalar. R (i) k represents the ith scalar element of the diagonal matrix R k at time k. rough the above processing, the measurement noise R k can be limited between R (i) min and R (i) max , 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. rough the measurement noise mean processing in the SHAKF, the estimation accuracy of the measurement noise is further improved. e following improvement is made to (24) in the filtering algorithm: where m is the variable sliding window value. e 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. is method is well adapted to the variation of the R value. e 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. Figure 5 is the structure of the improved Sage-Husa adaptive Kalman filter algorithm. e 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. e ISHAKF algorithm is based on the SHAKF algorithm. Based on the original algorithm, the forgetting factor and variable sliding window method are introduced. e steps of implementing the ISHAKF algorithm are as follows.
(2) Calculate the forgetting factor according to the introduced forgetting factor adjustment function.

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.

Simulation.
e accelerometer biases and the random walk noise are set as 100μ g and100μ g/ ��� Hz √ , respectively. e gyroscope biases and the random walk noise are set as 0.02°/h and 0.02°/ , respectively. e DVL scale factor is set as 0.002. e initial angle errors are set as 0.02°, −0.02°, and 0.5°, respectively. e DVL provides velocity information for four channels. e out frequencies of the IMU and DVL device are set as 200 Hz and 1 Hz, respectively. e vehicle movement start position is set to latitude 32.056°N and longitude 118.794°E. e curve of moving trajectory is shown in Figure 6. e 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 as where the amplitudes of the swaying motion are set as θ m � 3°, c m � 4°, and ψ m � 3°, respectively, while the cycles of the swaying are 5 s, 7 s, and 6 s, respectively. e initial phases are set as θ 0 � 0°, c 0 � 0°, and ψ 0 � 0°, respectively. e curve of vehicle velocity and attitude is shown in Figure 7.
In order to illustrate the anti-interference ability of the proposed ISHAKF method to external noise, different noise levels are set. e measurement noise covariance is set as R �   Figure 5: Process of the improved Sage-Husa adaptive filter for SINS/DVL. during 400 s∼700 s. ree methods are listed for SINS/DVL tightly integrated system. Figure 8 shows the curve of velocity error for KF, SHAKF, and ISHAKF. e 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 X i denotes the real velocity obtained by the simulation, n is the number of signals, and X i is the measurement information. e 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. e 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.

Vehicle Test.
e vehicle experiment is designed to simulate underwater vehicle due to the limitations of the experimental conditions. e 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. e 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. e speed information output by the PHINS can be used to simulate the speed information provided by the DVL device. e instrument specifications are listed in Table 3.   Mathematical Problems in Engineering e entire test route is on the campus of Southeast University and lasts for 1000 s. Figure 12 shows the motion trajectory. e curve of attitude and velocity is shown in Figure 13.
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. e DVL scale factor is 0.002. e initial attitude error is[0.1°, 0.1°, 0.5°]. 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). e 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. e 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. e 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. e 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. e 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.

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
e 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
e authors declare that they have no conflicts of interest.