A Novel Adaptive Two-Stage Information Filter Approach for Deep-Sea USBL/DVL Integrated Navigation

An accurate observation model and statistical model are critical in underwater integrated navigation. However, it is often the case that the statistical characteristics of noise are unknown through the ultra-short baseline (USBL) system/Doppler velocity log (DVL) integrated navigation in the deep-sea. Additionally, the velocity of underwater vehicles relative to the bottom of the sea or the currents is commonly provided by the DVL, and an adaptive filtering solution is needed to correctly estimate the velocity with unknown currents. This paper focuses on the estimation of unknown currents and measurement noise covariance for an underwater vehicle based on the USBL, DVL, and a pressure gauge (PG), and proposes a novel unbiased adaptive two-stage information filter (ATSIF) for the underwater vehicle (UV) with an unknown time-varying currents velocity. In the proposed algorithm, the adaptive filter is decomposed into a standard information filter and an unknown currents velocity information filter with interconnections, and the time-varying unknown ocean currents and measurement noise covariance are estimated. The simulation and experimental results illustrate that the proposed algorithm can make full use of high-precision observation information and has better robustness and navigation accuracy to deal with time-varying currents and measurement outliers than existing state-of-the-art algorithms.


Introduction
The underwater vehicle (UV) remains the best option for manipulation tasks, such as sampling, detailed inspection, and servicing subsea instruments. In deep-sea, the UV is dependent on the navigation ability to perform long-range and long-term missions near the sea bottom to support a wide range of ocean surveys. For the past decades, numerous underwater navigation schemes have been proposed [1][2][3]. Here, the ultrashort baseline (USBL) system/Doppler velocity log (DVL) integrated navigation is one of the most important ones, since it provides absolute position and is not prone to dead reckoning error in deep-sea. In particular, the USBL system and the DVL are complementary, and conveniently installed in the hull and extensively applied to underwater positioning [4,5]. Additionally, the UV is usually outfitted with a pressure sensor, and the vehicle depth is computed from the direct measurements of the ambient sea water pressure via standard equations for the properties of sea water [6,7]. Combined with the calibration and compensation for information provided by the innovation or the residual. Under the effect of unknown ocean currents, the estimation of measurement noise covariance can be damaged, and thereby lead to inaccurate and even divergent output of the filter.
In this study, a new filter is proposed to estimate the unknown time-varying currents and the real-time observation covariance matrix. The main contribution is the design of the unbiased adaptive two-stage information filters (ATSIFs) for deep-sea underwater vehicles to estimate the position, velocity, and time-varying unknown current velocity. The ATSIF involves two interconnected parts, one based on the classical information filter for state estimation, and the other based on the sequential least squares algorithm for unknown parameter estimation. The different forgetting factors are introduced to classical sequential least squares, and it controls how fast past observations are forgotten. An adaptive estimation method of the measurement noise is presented based on the epoch difference in the measurements. The acoustic system error caused by inaccurate sound velocity can be estimated by the ATSIF as well, which is convenient to be applied in our underwater navigation research. In addition, measurement equations for the USBL, DVL, and PG sensors are given, and simulation and experimental results are presented.
This paper is arranged as follows. Section 2 introduces the navigation system for the USBL/DVL integrated navigation system based on the slant ranges and the depth measurements. Then, the dynamic and measurement equations, and the data integration equations are given. The unbiased adaptive two-step information filter is described in Section 3. Section 4 describes the simulation and experiments. Finally, Section 5 presents the conclusions of this study.

System Model
As shown in Figure 1, the vehicle fixed coordinate system {b} and the outline of the vehicle are simplified as a dashed box. The positions of the sensor packages relative to the attitude and heading reference system (AHRS) are given by the lever arms vectors r b tp/tm ,r b dvl/tm ,r b pg/tm for the USBL transponder, DVL, and pressure gauge in the vehicle fixed frame, respectively.
Sensors 2020, 20, 3 of 20 ocean currents, the estimation of measurement noise covariance can be damaged, and thereby lead to inaccurate and even divergent output of the filter. In this study, a new filter is proposed to estimate the unknown time-varying currents and the real-time observation covariance matrix. The main contribution is the design of the unbiased adaptive two-stage information filters (ATSIFs) for deep-sea underwater vehicles to estimate the position, velocity, and time-varying unknown current velocity. The ATSIF involves two interconnected parts, one based on the classical information filter for state estimation, and the other based on the sequential least squares algorithm for unknown parameter estimation. The different forgetting factors are introduced to classical sequential least squares, and it controls how fast past observations are forgotten. An adaptive estimation method of the measurement noise is presented based on the epoch difference in the measurements. The acoustic system error caused by inaccurate sound velocity can be estimated by the ATSIF as well, which is convenient to be applied in our underwater navigation research. In addition, measurement equations for the USBL, DVL, and PG sensors are given, and simulation and experimental results are presented.
This paper is arranged as follows. Section 2 introduces the navigation system for the USBL/DVL integrated navigation system based on the slant ranges and the depth measurements. Then, the dynamic and measurement equations, and the data integration equations are given. The unbiased adaptive two-step information filter is described in Section 3. Section 4 describes the simulation and experiments. Finally, Section 5 presents the conclusions of this study.

System Model
As shown in Figure 1    The positions of the other sensors relative to the AHRS in the {m} coordinate are represented as r m tp , r m dvl , and r m pg , respectively. Then [34]: where C m b (ϕ bm ) ∈ R 3×3 denotes the direction cosine matrix (DCM) from {b} to {m}, ϕ bm is the attitude angle of {b} relative to {m}. The dynamics of the position and attitude are given by: where p n m (t) ∈ R 3 is the position of AHRS in the local navigation coordinate frame {n}; C n m (ϕ mn (t)) ∈ R 3×3 denotes the DCM from {m} to {n}, and ϕ mn (t) represents the attitude of the AHRS; v m d (t) ∈ R 3 and ω m m (t) represent the velocity of the AHRS relative to the fluid and angular velocity in {m}, respectively; v n c (t) denotes the ocean currents velocity in {n}; S(·) is the skew-symmetric matrix, which represents the cross product such that S(ω m m (t))a = ω m m (t) × a; w v (t) ∈ R 3 and w ϕ (t) ∈ R 3 represent the state stochastic perturbations of the velocity and turn rate, respectively.

Observation Model
As shown in Figure 2, the navigation system includes two parts: (1) An underwater vehicle equipped with a DVL sensor, PG, AHRS, and USBL transponder; and (2) a ship-decked unit with a GNSS antenna and a dunking transducer on a rigid pole. The relative coordinates in the {n} frame of the GNSS antenna and the USBL transducer can be calculated by attitude measurements. The relative position between the vehicle and ship can be measured by USBL, and the UV depth relative to the sea level can be communicated with the ship using an acoustic modem.
Sensors 2020, 20, 4 of 20 represent the state stochastic perturbations of the velocity and turn rate, respectively.

Observation Model
As shown in Figure 2, the navigation system includes two parts: (1) An underwater vehicle equipped with a DVL sensor, PG, AHRS, and USBL transponder; and (2) a ship-decked unit with a GNSS antenna and a dunking transducer on a rigid pole. The relative coordinates in the {n} frame of the GNSS antenna and the USBL transducer can be calculated by attitude measurements. The relative position between the vehicle and ship can be measured by USBL, and the UV depth relative to the sea level can be communicated with the ship using an acoustic modem. The measurement equations for the USBL transponder [8,[35][36][37] and the DVL [26] are given by Equations (4) and (5), respectively: where ( ) The measurement equations for the USBL transponder [8,[35][36][37] and the DVL [26] are given by Equations (4) and (5), respectively: where d i (t) denotes the distance between the transponder and the transducer measured by the USBL; p n r (i) ∈ R 3 is the position of the receiver in {n}, and n r is the number of receivers; ε USBL measurement noise; v d dvl (t) ∈ R 3 represents the velocity reading provided by the DVL; C d m (ϕ md ) is the rotation matrix from {m} to {d}; ε d dvl (t) represents the DVL measurement noise. In this research, the positioning accuracy of the vertical direction is better than 30 cm, which uses the differential global positioning system (DGPS) technology (e.g., StarFire, VeriPos, and Marinestar) based on the communication data link of marine satellites [38]. Then, the instantaneous elevation of the sea surface z(t) can be simplified, as represented by: where z tp (t) denotes the height of the real-time USBL transducer; z d represents the immersion of the transducer, which is usually assumed to be a constant and can be estimated; and w d tp represents the noise caused by the waves and the ship dynamic draft. Typically, the period of a wave is only a few seconds to a few minutes, while the cycle of tides is longer and can be considered almost constant within several minutes. The influence of waves can be removed by roughly utilizing the moving average method. Therefore, considering that the PG is calibrated, the measurement p pg (t) can be represented by: (7) where p atm (t) denotes the atmospheric pressure at the mean sea level; ρ(t) represents the mean seawater density; g is the acceleration of gravity; z n pg denotes the depth of the pressure gauge; ε pg represents the PG noise; z n m (t) represents the depth of the AHRS; and ε n pg represents the depth offset error.

Integrated Navigation Model
To reduce the complexity of the system dynamics and measurements, the calibration was accomplished and the exact orientation vector was obtained. A simple linear time-varying (LTV) model is used here: , and s = v n c ; d k and v k represent the vector of white Gaussian acceleration noises with zero mean, respectively; To achieve a discrete-time model of the dynamics (4), we assumed that the measurements are obtained with a constant sampling rate T. Then, the system can be described by: H k can be linearized and described by (4), (5), and (7) with the known parameters {ϕ mn (t), ϕ md , r m tp , p n r (i), r m dvl (t), z tp (t), r m pg }, . This is a simplified from for the design of an observer, as both the input and output of the system are known continuous bounded signals. Then, the classic augmented discrete-time model is usually used as follows: Sensors 2020, 20, 6029 6 of 19 where ξ k represents the random error. Thus, the KF method can be used for data integration in the system (10). However, the augmented form generally simplifies the physical model of the input error and neglects the strong time-varying characteristics of the unknown input u k . As shown in Table 1, we analyzed the influence of the unknown currents error on the system in this research and separated the unknown input from the augmented filter. The problem addressed in this study and the measurement characteristics of sensors can be summarized in the following statement. Table 1. Summary of measured quantities and variables to be estimated.

Estimate Measurement
The USBL observation contains obvious measurement noise, which is related to the distance and changes with space and time.

2.
The other systematic errors, such as calibration errors and constant deviations of depth gauges, can be corrected by augmenting parameters.

3.
There are time-varying and saltation ocean currents.

Adaptive Two-Stage Information Filter Design
To improve the estimation accuracy of the navigation state, unknown ocean current velocity, and measurement noise covariance, a modified two-stage adaptive information filter is proposed to trade the algorithm simplicity and efficiency.

The Two-Stage Information Filter
Considering a stochastic linear discrete-time system (10), w k and v k are independent random noise vectors with covariance matrices , respectively. If the state estimationx k−1 , P k−1 , and unknown parameter estimationû k−1 are given, according to system (10) and the variance-covariance propagation law, the predicted state and covariance are given as: The following information filter estimation can be obtained [39,40]: where (11) corresponds tox k−1 andû k−1 , respectively. Thex k can be determined by the two kinds of parameters based on Equation (8) as well. Therefore,x k can be written as intox k =x a k +x u k with: where the Equation (13) is the classic information filtering equation of the system (10) without considering the unknown inputû k−1 , which can be calculated recursively in the form of standard  (14) can be written as: whereû represents an estimation of u, and ∆ k−1 is the compensation term of the estimation of system noiseû k−1 . It is assumed that a time-varying matrix F k ∈ R n×p exists, such thatx u k = F kûk , and holds for all time. The detailed description of the similar methods can be found in [41,42]. Then: where the recursive linear equation of state estimationx u k is established based onû. The estimationû k of the system noise can be represented asû k =û k−1 + ∆û k . Then: Letting ∆ k−1 = F k ∆û k , then (13) becomes: and a recursive expression of F k can be obtained as follows: Now, Equation (15) can be rewritten as: Note that the right second term of (20) is used to compensate the error caused byû k−1 u, and its continuous time KF counterpart can be find in [40]. Now,x k can be computed with Equations (13) and (20), then: where Equation (17) is the expression of the adaptive filter designed in this study, and it is necessary to consider the recursive computing problem. Theû k is estimated with the observed values and the predicted values of the model. The innovation vector is defined as follows: It is straightforward to find that V k represents the standard Kalman filter innovation vector of the system (10) without considering u k . Then, the covariance of V k can be computed by the innovation vector also satisfies: Clearly, the (23) right term can be rewritten as (18) and (19) is subtracted with the left and right. Then: Sensors 2020, 20, 6029 So, the relationship between V k andû k−1 is given by (19) as: where Equation (21) can be regarded as an observation equation with unknown parameters u. Based on the steady characteristics of unknown input, the sequential least squares can be used to estimateû k sequentially. The detailed equations are given as: In the adaptive information filter, at the initial time instant k = 0, the initial statex 0 ∈ N(x 0 , P 0 ) is assumed to be a Gaussian random vector, and the basic algorithm of two-stage information filtering (TSIF) can be given as Algorithm 1. 1. Initialization: 3. Recursive computation: For k = 1, 2, 3, . . .
(45) 4. Output:x k andû k Proposition 1. The mathematical expectationsE[x k −x k ] and E[u k −û k ] tend to zero when k → ∞ . The details can be found in Appendix A.
The Algorithm 1 separates unknown ocean currents and navigation parameters into two parts. It is more convenient to reveal the effect of unknown ocean currents parameters on the function of Sensors 2020, 20, 6029 9 of 19 possible estimated parameters through the innovation vector. The ocean current parameters in different states can be estimated adaptively and more flexibly.

Diagnosis of Unknown Ocean Currents and the Saltation Ocean Currents
When the UV works near the sea bottom, the DVL can measure the velocity relative to the bottom of the sea without ocean currents. Otherwise, many diagnostic algorithms can be applied to filter anomalies through innovation vectors, residual vectors, or abnormal states. In this study, the prediction innovation vectorx k|k−1 will be reflected by the error of the prediction state vector, and the following error discriminant statistics can be constructed through a sliding window of n epochs: The mean is compared with a presupposed threshold to distinguish whether there is an abnormal state, i.e., whether there is an unknown ocean current. If there is an unknown ocean current, the unknown parameters can be estimated by the second-order filtering.

Diagnosis of Abnormal USBL Data
It is assumed that the location provided by USBL at k time is p usbl k , and the predicted value of the position at the last moment isp k−1 . It can be obtained that the velocity at this time is estimated to be (p usbl k −p k−1 )/(t k − t k−1 ). In deep sea, DVL's speed of measurement accuracy is much higher than the average speed obtained by the adjacent time of USBL. It can be used as an important criterion to judge whether the USBL data is abnormal.

Estimation of the Time-Varying Currents
When the time-varying ocean currents occurs, we can naturally introduce the forgetting factor λ ∈ [0, 1] into the recursive Equations (40)- (43). Rewriting Equations (40) and (42), then the adaptive SLS algorithm for estimating the ocean current velocity is as follows: The forgetting factor is used to increase the proportion of new observations in the estimation of ocean current velocity in filtering. If there is a slow change in ocean current velocity, the method can effectively estimate the ocean currents by using new observations. Consider the saltation ocean currents, which can cause serious effects on navigation. We consider introducing an auto-adjusting factor α k into the above recursive Equations (40)- (43).
By rewriting Equations (40) and (42) with the adaptive two-section weight function, the SLS algorithm for estimating the ocean current velocity can be rewritten as follows: is a discriminating statistical vector based on the innovations, and c ∈ [1, 2.5] is a presupposed threshold. The significance of Equation (48) is to reduce the influence of the previous system noise estimation on the current system noise estimation and improve the convergence of stepwise system noise. The gain matrix Ψ k is magnified by introducing the adaptive factor λ or α k , and depends more upon the new measurements. The basic algorithm of adaptive two-stage information filtering (TSIF) can be given as Algorithm 2. The ATSIF involves two interconnected parts, one based on the classical information filter for state estimation, and the ther based on the sequential least squares algorithm for unknown parameter estimation. The different forgetting factors are introduced to classical sequential least squares, and it controls how fast past observations are forgotten. 1. Initialization: (1). Information filtering (IF): (54) (2). Innovation and covariance: (3). Correction: (4). Modified state: x k =x a k +x u k .

The Adaptive Estimation of the Measurement Noise Covariance
Due to the characteristics of acoustic sensors and ocean environment noise, the R matrix will change obviously with time and space in practice. Consequently, the performance of the filter will degrade when using an inaccurate prior statistic covariance R. Therefore, it is crucial to estimate the statistical characteristics of the filter. There are numerous existing adaptive algorithms that can be used to estimate the measurement noise covariance. The IAE and RAE methods are adapted as follows: wherex k|k−1 andx k are the predicted and corrected estimation of the state, respectively. P k|k−1 denotes the covariance ofx k|k−1 , and P k denotes the covariance ofx k . e − k and e + k = y k − C kx k are the innovation and residue vectors at time k, respectively.
However, the innovation vectors and residual vectors are sensitive to (x k|k−1 ,û k|k−1 ) and (x k ,û k ), and the inaccurate (x k|k−1 ,û k|k−1 ) and (x k ,û k ) will affect the estimation of R. We consider the adaptive estimation of R from the y p k−1 = C k−1x k−1|k−2 and original observations to improve the estimation accuracy. By calculating the difference of the observed data in the adjacent time, the second-order difference (SOD) of observation can be used to calculate R as follows [43]: where y p k = H kx k|k−2 and the details can be found in Appendix B. The covariance matrix of observation noise can be obtained by selecting observation data for a short period time and the moving average method. If the sampling time T is small, the observed noise covariance can be estimated by (68). The SOD method can be viewed as a second-order difference of measurement than the IAE method. In USBL/DVL integrated navigation, when the accuracy of the dynamic model is higher than that of measurement, this method can generally achieve better estimation results. The potential advantage is that it can eliminate the system error items of the observation system with little change between epochs and obtain a stable covariance matrix of observation noise, which is beneficial to parameterization and separation of measurement system errors.
As shown in Figure 3, three discriminant conditions are referenced in the proposed filter. In order to improve the stability and accuracy of filtering, the AIF and ATSIF algorithms are used to estimate the state of the UV with different scenarios. The high-precision velocity measurement of DVL is used as the diagnostic condition of USBL anomaly observation, and the data quality of USBL can be controlled according to the equation in Section 3.2.2. When the USBL is normal and DVL is in the lock model, the AIF algorithm is used to get an accurate navigation solution. When the DVL is in the water tracking model, it is necessary to distinguish whether there are ocean currents according to the equation in Section 3.2.1. If the ocean currents occur, the ATSIF algorithm should be used to compensate the filtering error caused by the unknown currents.
Sensors 2020, 20, 12 of 20 As shown in Figure 3, three discriminant conditions are referenced in the proposed filter. In order to improve the stability and accuracy of filtering, the AIF and ATSIF algorithms are used to estimate the state of the UV with different scenarios. The high-precision velocity measurement of DVL is used as the diagnostic condition of USBL anomaly observation, and the data quality of USBL can be controlled according to the equation in Section 3.2.2. When the USBL is normal and DVL is in the lock model, the AIF algorithm is used to get an accurate navigation solution. When the DVL is in the water tracking model, it is necessary to distinguish whether there are ocean currents according to the equation in Section 3.2.1. If the ocean currents occur, the ATSIF algorithm should be used to compensate the filtering error caused by the unknown currents.

Simulation Results
In this section, a numerical example of an underwater vehicle tracking problem is given to illustrate the effectiveness of the proposed adaptive filtering approach. Consider an underwater vehicle with a constant velocity of 1 m/s at a depth of 1400 m. The positions of the USBL transducer and the underwater vehicle trajectory are depicted in Figure 4a, and the simulation parameters of the USBL, DVL, depth, and attitude measurements are given in Table 2.

Simulation Results
In this section, a numerical example of an underwater vehicle tracking problem is given to illustrate the effectiveness of the proposed adaptive filtering approach. Consider an underwater vehicle with a constant velocity of 1 m/s at a depth of 1400 m. The positions of the USBL transducer and the underwater vehicle trajectory are depicted in Figure 4a, and the simulation parameters of the USBL, DVL, depth, and attitude measurements are given in Table 2.

Simulation Results
In this section, a numerical example of an underwater vehicle tracking problem is given to illustrate the effectiveness of the proposed adaptive filtering approach. Consider an underwater vehicle with a constant velocity of 1 m/s at a depth of 1400 m. The positions of the USBL transducer and the underwater vehicle trajectory are depicted in Figure 4a, and the simulation parameters of the USBL, DVL, depth, and attitude measurements are given in Table 2.

USBL (m) DVL (m/s) Depth (m) Heading (°) Roll/Pitch (°) Gaussian Error
where L represents the slant range between the USBL transducer and the transponder; K is the scale factor; V denotes the velocity of the vehicle in the vehicle coordinates. The ocean currents direction is introduced to be north-south in this simulation, and the current velocity is simulated as Figure 4b.

USBL (m) DVL (m/s) Depth (m)
where L represents the slant range between the USBL transducer and the transponder; K is the scale factor; V denotes the velocity of the vehicle in the vehicle coordinates. The ocean currents direction is introduced to be north-south in this simulation, and the current velocity is simulated as Figure 4b.
The estimated current velocity error of the proposed algorithms with different adaptive factors is depicted in Figure 5a. It shows that when the adaptive factor of λ is close to 1, the historical ocean currents information occupies a large proportion in the filtering. The filtering is relatively stable at this time, but the convergence speed is slow while the saltation ocean currents happen. When the λ is small, the saltation ocean currents can be quickly estimated using the most recent observation information. However, the current estimates fluctuate greatly and the filtering shows instability. The proposed algorithm combined with the piecewise function can diagnose the time of saltation ocean currents and estimate the saltation ocean current velocity quickly and increase the stability of the ocean current estimation. The estimated horizontal position errors of the proposed algorithms are depicted in Figure 5b. It can be seen that the proposed two-section weight method gives a more accurate and robust estimation.
Considering the USBL accuracy changes with distance and a sudden change in the speed of sound, the scale factor K changes from 5 to 8 at 4100 epochs in this simulation. Figure 6 exhibits the estimation of USBL measurement noise covariance provided by SOD and RAE. RAE gives poor performance during the maneuvering motion because it works based on the residual sequence and is coupled with state estimation error. However, the SOD method can contribute accurate and stable estimations over almost the whole process. To validate the feasibility of the proposed algorithm, different K is implemented. Table 3 lists the root mean square errors (RMSEs) of all the approaches during different K. The proposed algorithm also outperforms other algorithms with different K.
proposed algorithm combined with the piecewise function can diagnose the time of saltation ocean currents and estimate the saltation ocean current velocity quickly and increase the stability of the ocean current estimation. The estimated horizontal position errors of the proposed algorithms are depicted in Figure 5b. It can be seen that the proposed two-section weight method gives a more accurate and robust estimation. Considering the USBL accuracy changes with distance and a sudden change in the speed of sound, the scale factor K changes from 5 to 8 at 4100 epochs in this simulation. Figure 6 exhibits the estimation of USBL measurement noise covariance provided by SOD and RAE. RAE gives poor performance during the maneuvering motion because it works based on the residual sequence and is coupled with state estimation error. However, the SOD method can contribute accurate and stable estimations over almost the whole process. To validate the feasibility of the proposed algorithm, different K is implemented. Table 3 lists the root mean square errors (RMSEs) of all the approaches during different K. The proposed algorithm also outperforms other algorithms with different K.   Considering the USBL accuracy changes with distance and a sudden change in the speed of sound, the scale factor K changes from 5 to 8 at 4100 epochs in this simulation. Figure 6 exhibits the estimation of USBL measurement noise covariance provided by SOD and RAE. RAE gives poor performance during the maneuvering motion because it works based on the residual sequence and is coupled with state estimation error. However, the SOD method can contribute accurate and stable estimations over almost the whole process. To validate the feasibility of the proposed algorithm, different K is implemented. Table 3 lists the root mean square errors (RMSEs) of all the approaches during different K. The proposed algorithm also outperforms other algorithms with different K.   From the simulation results, the performance of these filters is consistent with the theoretical analysis, which is stated in Section 3.

Deep-Sea Towed Vehicle Experiment
As shown in Figure 7, there is a deep-sea towed vehicle, GAPS ultra-short baseline acoustic positioning system, Phins Subsea strapdown inertial navigation system (SINS), and a survey vessel, respectively. The towed vehicle is equipped with an SINS, pressure gauge, DVL, and ultra-short baseline positioning system. The water depth of this experiment is about 1100 m, of which the depth meter, ultra-short baseline positioning system, and SINS inertial navigation system are the main navigation system and DVL is the auxiliary navigation system. Based on the depth data measured by the depth meter on the vertical channel, the navigation and positioning of the towing body in the horizontal direction of the deep-water area is mainly considered in this study.
The DVL used in this experiment is in bottom-lock velocity measurement mode (that is, when the distance between DVL and the sea floor is less than 200 m, the velocity of the towing body relative to the seafloor can be measured without considering the influence of ocean currents). The accuracy of USBL will decrease with the increase of depth. Figure 8 shows that USBL even has the situation of missing observation data in the deep-water area (6500 s after entering the water). At this time, DVL velocity measurement information can be used as an effective supplement to restrain the navigation divergence of the SINS system to a certain extent.
baseline positioning system. The water depth of this experiment is about 1100 m, of which the depth meter, ultra-short baseline positioning system, and SINS inertial navigation system are the main navigation system and DVL is the auxiliary navigation system. Based on the depth data measured by the depth meter on the vertical channel, the navigation and positioning of the towing body in the horizontal direction of the deep-water area is mainly considered in this study. The DVL used in this experiment is in bottom-lock velocity measurement mode (that is, when the distance between DVL and the sea floor is less than 200 m, the velocity of the towing body relative to the seafloor can be measured without considering the influence of ocean currents). The accuracy of USBL will decrease with the increase of depth. Figure 8 shows that USBL even has the situation of missing observation data in the deep-water area (6500 s after entering the water). At this time, DVL velocity measurement information can be used as an effective supplement to restrain the navigation divergence of the SINS system to a certain extent.
Due to it being difficult to obtain the absolute positioning result of the underwater dynamic vehicle, the adaptive real-time filtering result using RAE estimation is compared and analyzed with Since DVL does not measure the velocity relative to seawater, the measured data in this paper are only related to a part of the research content of this paper. As shown in Figure 9, there are kinds of information filtering methods for estimating the covariance of USBL using the USBL + RAE methods and the proposed SOD adaptive filtering method. The proposed method is smoother due to the coupling of slant data of USBL and depth observations. More specifically, in the case of USBL measurements, the maximum position error is approximately 1 m in shallow water, while the maximum position error is approximately 3 m. The positions error is changed significantly with ranging. In deep water, the positioning accuracy of the USBL is significantly reduced, and real-time estimation of the USBL random model is of great significance to integrated navigation. Compared with the traditional RAE method, the proposed method obtains more stable filtering results. Due to it being difficult to obtain the absolute positioning result of the underwater dynamic vehicle, the adaptive real-time filtering result using RAE estimation is compared and analyzed with the final post-processing navigation result obtained by the commercial post-processing software in this example. The post-processing navigation result combined with forward filtering and backward filtering with SINS data. It is considered to be a true and reliable UV position, velocity, and attitude.
Since DVL does not measure the velocity relative to seawater, the measured data in this paper are only related to a part of the research content of this paper.
As shown in Figure 9, there are kinds of information filtering methods for estimating the covariance of USBL using the USBL + RAE methods and the proposed SOD adaptive filtering method. The proposed method is smoother due to the coupling of slant data of USBL and depth observations. More specifically, in the case of USBL measurements, the maximum position error is approximately 1 m in shallow water, while the maximum position error is approximately 3 m. The positions error is changed significantly with ranging. In deep water, the positioning accuracy of the USBL is significantly reduced, and real-time estimation of the USBL random model is of great significance to integrated navigation. Compared with the traditional RAE method, the proposed method obtains more stable filtering results.
(c) (d) As shown in Figure 9, there are kinds of information filtering methods for estimating the covariance of USBL using the USBL + RAE methods and the proposed SOD adaptive filtering method. The proposed method is smoother due to the coupling of slant data of USBL and depth observations. More specifically, in the case of USBL measurements, the maximum position error is approximately 1 m in shallow water, while the maximum position error is approximately 3 m. The positions error is changed significantly with ranging. In deep water, the positioning accuracy of the USBL is significantly reduced, and real-time estimation of the USBL random model is of great significance to integrated navigation. Compared with the traditional RAE method, the proposed method obtains more stable filtering results.

Conclusions
An unbiased adaptive two-stage information filter was proposed to estimate the position of the UV and unknown current velocity. The time-varying unknown ocean current velocity and observed noise covariance were estimated, and it can be easily extended to underwater acoustic navigation filtering cases. According to simulation and experiment results, the following conclusions can be drawn: (1) The proposed approach can improve the performance of underwater acoustic navigation significantly; and (2) some stable coupling parameters, such as pressure sensors, can be effectively estimated by the new filtering algorithm. The design of the new algorithm was applied to the observation model of deep-sea acoustic navigation equipment, which provides a reference for the research of underwater integrated navigation.

Patents
In further research, the filter design with missing data of acoustic sensors needs to be considered as well. The algorithm can still be improved in the field of inertial navigation, and the adaptive estimation of system error and system noise variance in the high-dimensional navigation system are still worthy of further discussion. x k−1 = ξ k−1 − F k−1ũk−1 into (A3), then: Take the mathematical expectation at both sides of (A7), then: According to G k D k = Λ k−1 D T k Ψ k D k and Λ k = Λ k−1 − Λ k−1 D T k Ψ k D k Λ k−1 , the spectral norm of (I p − G k D k ) is less than 1 for all time. Then, E(ũ k ) tends to zero when k → ∞ , and: