A SINS/DVL/USBL integrated navigation and positioning IoT system with multiple sources fusion and federated Kalman filter

The navigation and positioning subsystem offers important position information for an autonomous underwater vehicle (AUV) system. It plays a crucial role during the underwater exploration and operations of AUV. Many scholars research underwater navigation and positioning. Various improved methods and systems were presented. However, as the diversity of the ocean environment, the random drift of the gyroscope, error accumulation, the variety of tasks, and other negative factors, the navigation and positioning results are uncertain and incredible. The accuracy, stability, and robustness are not guaranteed, which cannot meet the increasing application requirement. Therefore, we put forward a SINS/DVL/USBL integrated navigation and positioning IoT system with multiple resource fusion and a federated Kalman filter. In this method, we first present an improved SINS/DVL combined subsystem with a filtering gain compensation strategy. So we can enhance the accuracy and stability of the navigation and position system. Secondly, we proposed a USBL positioning subsystem with the Kalman filtering acoustic signals to improve USBL positioning performance. Lastly, we present a federated Kalman filter to fuse the positioning information from the SINS/DVL combined positioning subsystem and the USBL positioning subsystem. Through the three methods, we can enhance the positioning accuracy and robustness. Comprehensive simulation results indicated the feasibility and effectiveness of the proposed SINS/DVL/USBL integrated navigation and positioning system, which provides critical reference for other positioning method, and it also offers crucial position information for AUV to achieve high accuracy and efficiency tasks.


Introduction
In marine-related research fields, autonomous underwater vehicle (AUV) is applied widely to both civil and military-strategic areas [1,2]. And navigation or positioning information is the key for the AUV to figure out where it is and where to go [2][3][4]. The navigation and positioning system determines the efficiency and accuracy when AUV performs underwater missions [5]. However, as the diversity of the ocean environment and the AUV's limitation, it is hard to realize the high-precision navigation and positioning for AUV. And this can not satisfy the increasing needs of applications [5,6]. There are many different kinds of navigation and positioning subsystems, such as SINS, DVL, visual navigation, and USBL [7]. They have specific characteristics respectively [8]. We can integrate these subsystems to enhance the overall accuracy and the robustness of the positioning system. Through an integrated approach, we can get higher accuracy and stability of the navigation and positioning system in a long time by performing information fusion and filter correction.
The amount of computation in an integrated system increases with the number of navigation and positioning subsystems. The required computation resource maybe not be exceptionally high if there are only two navigation and positioning subsystems in an integrated system [9,10]. However, when three or more subsystems integrate into one system, the dimension will increase to a large scale which causes computation resources to rise at an excessively high level [11]. Under such circumstances, an ordinary filter can not meet the requirement. But the federated filter has the merit of compatibility and decentralized filter processing, which can simplify the computation and increase the stability of navigation and positioning systems.
Mumerous scholars have researched the combination of multiple navigation subsystems based on the federated filter [12,13]. When the information of three or more navigation subsystems is fused, the dimension of the multi-sensor information fusion method will be very high, resulting in the conventional filtering methods not meeting the needs [14]. In that case, the federated filter method simplifies the calculation and improves the system stability through its robust compatibility and dispersion of filters [15].
For these problems, with an IoT system of multiple positioning subsystems, we proposed a SINS/ DVL/USBL integrated navigation and positioning system based on a federated filter, which combines an improved SINS/DVL integrated positioning subsystem and an improved USBL positioning subsystem with Kalman filtering phase-difference. So we can improve the positioning accuracy and error tolerance ability of the navigation and positioning system. In this paper, we make the following contributions: (1) We present a SINS/DVL positioning subsystem based on the filtering gain compensation strategy to enhance the navigation accuracy and the dynamic tracking ability. (2) We propose a phase difference filtering USBL navigation subsystem to achieve a higher positioning accuracy. (3) To fuse the improved SINS/DVL subsystem and the USBL subsystem, we present a SINS/DVL/USBL integrated navigation and positioning system with multiple resource fusion and federated Kalman filter.

Related works
With high accuracy and robust requirement from underwater applications and the complex working environment, challenges are present for scholars and researchers on underwater navigation and positioning. They focus on researching the federated filter. That is a multiple-source information fusion filter developing continuously. We list these methods with their contributions in Table 1. Lei G et al. proposed a new federated Kalman adaptive filter [16]. During the information fusion stage, the weighted coefficient was assigned to different states adaptive to enhance the accuracy and robustness. Tang et al. presented a new federated adaptive filter [17]. When there was an outlier during navigation and positioning, factors could be adjusted automatically. The algorithm could restrain the outlier of navigation and positioning and improve the fault-tolerant ability of the positioning system. Gong et al. presented a transfer alignment method based on a federated filter [18], which divided the high-dimensional state vector into two parts. This method reduced the dimension of the system state vector Table 1 The MSE comparison of different methods

Method Contribution
AFKF [16] weighted coefficient Improved AFKF [17] factors adjusted automatically Transfer alignment [18] divided the high-dimensional state vector into two parts Improved covariance [19] derived a real-time estimates of improved covariance SINS/GPS/CNS/Radar integrated system [11] calculated the state parameters with dual-state detection Joint filter to fuse data [20] INS/CNS/DVL combined system Federated unscented Kalman filter [21] with different vehicle motion models to estimate Federated hybrid filter [22] utilizes a minimum variance criterion to fuse An adaptive filter [23] conquer the performance degradation Federated filter with a feedback scheme [24] GNSS/INS/visual odometry combined positioning system Federated Kalman filter for indoor positioning distance is estimated through RSS  [24]. In [25], a federated Kalman filter (FKF) was applied to indoor positioning. The FKF estimates the target's location. And the distance is estimated through the received signal strength (RSS), which may lead to a large positioning error. There are some navigation and position subsystems with the respective characteristics. We can connect them to form a positioning IoT system with data links. With the fusion of position information from these subsystems, we make full advances of them to enhance the position performance of the system.
For the problem of low accuracy and poor fault tolerance under complex tasks, we can utilize a federated filter based on multiple source information fusion to optimize the system and improve the system performance. Based on the above analysis, we study the information fusion technology of underwater vehicles in a complicated task environment. To achieve higher positioning accuracy, stability, and robustness, we put forward a SINS/DVL/ USBL integrated navigation and positioning system with a federated Kalman filter.

Basics
We introduce the concepts related to filter and Kalman filter as follows.
We always utilize a filter to filter the noise and interference in the data or signals. We classify it into the linear filter and the non-linear filter. The ordinary filter includes Kalman filter, medium filter, adaptive filter, Wiener filter, particle filter, etc.
Among these filters, the Kalman filter is a high-efficiency recursive filter. It can estimate the states of a dynamic system in a noisy environment or none complete measurement. Due to low computation complexity, data simplicity, and easy storage characteristics, the Kalman filter is applied widely in many fields, including combined navigation and positioning, information fusion, optimization control, object tracking, and fault diagnosis.
In the positioning subsystem, the Kalman filter estimates the position of the next moment based on the current observation position value. We define the state equation and observation equation of the classic linear Kalman filter: Here k denotes the moment value. X k is the system state value of moment k. F k, k − 1 is the transfer matrix of the system from moment k-1 to moment k. W k − 1 represents the system state value of moment k-1. Z k is the system state value. H k denotes the system observation matrix. V k is the systematic observation noise.
The time update equations are illustrated as (3) and (4).
Here Xk denotes the prior estimated value of system state at moment k-1. A is the transfer coefficient of system. X k−1 is the estimated value of system state at moment k-1. P − k denotes the covariance of the prior estimated error. P k − 1 is the covariance of the posterior estimated error. Q is the covariance coefficient of process excitation noise.
The state update equations are illustrated as (5) to (7), respectively.
Here K k is the Kalman gain at the moment k. P k denotes covariance of the posterior estimated error. H is the coefficient of the observation model. R is the covariance coefficient of process noise. X k is the estimated value of the  11:18 system state at the moment k. Z k is the observation value of the system. The operation of the Kalman filter consists of two parts: estimation and update. As shown in Fig. 1, at the estimation stage denoted as circle 2, the Kalman filter estimates the current state based on the state value of the last moment shown as (1); At the update stage denotes as circle 1, the Kalman filter optimizes the estimation value obtained at the estimation stage based on the current observation of system state. In this way, we can minimize the negative effect of noise and interference and gain an accurate result.

The federated Kalman filter
To gain relatively high positioning accuracy, we take full advantage of the SINS, DVL, and USBL for navigation and positioning information fusion. As shown in Fig. 2, the framework of the proposed integrated navigation and positioning system is composed of three subsystems: an integrated SINS/DVL subsystem, an improved USBL subsystem, and a global combined federated Kalman filter. The navigation and positioning output is also fed back to the improved SINS/DVL integrated subsystem for correction and compensation. So we can enhance the positioning accuracy and robustness.

Integrated SINS/DVL
We integrated a SINS and a DVL navigation and positioning model with a SINS/DVL local filter. In this local filter, we make full use of a filter gain compensationbased Kalman adaptive filter for rapid changing of AUV working status. In this case, we can obtain high positioning accuracy and stability. The SINS part serves as a critical reference for the SINS/DVL local filter and the SINS/ USBL local filter at the same time.

Improved USBL
Due to environmental noise and multi-path propagation of sonar signal, the phase difference information is affected by different degrees of noise, which leads to low positioning accuracy. In the subsystem, considering the  error sources and their affecting mechanism, we present a USBL positioning subsystem through the phase difference and Kalman filtering to enhance positioning performance.

Global combined federated Kalman filter
To fuse the SINS/DVL and USBL positioning subsystem, we utilize a federated Kalman filter, which performs as a multi-source fusion center of the positioning information. We can get the global best estimation of navigation and positioning. The global positioning results are also fed back to the SINS subsystem to correct positioning errors.

Integrated SINS/DVL
In this subsystem, we utilize an indirect filter to perform information fusion. In the indirect filter, we treat the deviation between the output of the SINS and the DVL as a reference value. We provide the error estimation result of combined navigation to SINS for correcting. We call this an output correction. Also, the error estimation is a feedback filter for system correction. We call it a feedback correction. We combine the output correction and the feedback correction. We show the framework of the proposed improved SINS/DVL navigation and positioning subsystem in Fig. 3. As shown in Fig. 3, the framework of the integrated SINS/DVL positioning subsystem is composed of a SINS, a DVL, and a SINS/DVL filter. To improve the accuracy and stability of the integrated navigation and positioning subsystem, we present a filter gain compensation adaptive filter to perform information fusion. In this section, we first model the SINS/DVL integrated navigation and positioning through error analysis. Then we present a filtering gain compensation-based adaptive filter to handle the navigation and positioning parameters. And we can achieve accurate positioning error information. Finally, with the error information, we obtain accurate navigation and positioning results through resolving.

SINS/DVL integrated positioning modeling
The error model of the combined navigation and positioning subsystem comprises the SINS error model and DVL error model. We select the positioning error, the velocity error, the drift angle error, the velocity offset error, and the scale factor error as the state variables to the filer. In addition, the DVL provides the velocity and drift angle of the AUV. We denote the system state variable as Here δv E and δv N are the speed error in the east and the north. α, β, γ represent the AUV misalignment angle, δL and δλ denote latitude and longitude error, ε E , ε N , ε U are gyroscope drift in the east, the north and the up direction. δv d is measuring velocity offset error for Doppler, δΔ is bias angle error， δC is the scale factor error.
We define the east speed error δv E and north speed error δv N : We calculate the platform misalignment angle in (3-5): Fig. 3 The framework of the integrated SINS/DVL subsystem We define the position error in (13): We calculate the gyro drift in the east, in the north and in the up direction as follows: U is error related time of the gyroscope in the east, the north, and the up direction.
We define the Speed error δv d , the drift angle error δΔ and the scale error δC of the Doppler log as follows: We describe the state equation in (21): We define the state equation of SINS in (22): In (21), we calculate the transfer matrix as follow: The difference between the velocity error of SINS and DVL is taken as the observation of the measurement equation which is defined as follow: We define the transfer matrix in (20): We define the variance matrix of noise: We calculate the variance matrix of measurement noise: Filtering gain compensation The gain K determines the correction value of state estimation on each moment. It correlates positively with the correction value. As the system state changes with time constantly. The estimated value of the state cannot track the state changing in time, which will lead to a significant positioning error, especially when the state changes rapidly. So we combine the gain compensation and adaptive Kalman filter. And we present a gain compensation-based adaptive Kalman filtering method, where the gain compensation and adaptive Kalman filter are combined.
We denote the filter gain as K n . At the time k, we compensate the original filtering gain with K g : We update the filter equation in (25): For the stability and robustness of the positioning system, the filtering gain K n should increase to track the rapid changing of motion and the system state. When the state is stable, the filtering gain can decrease to K g . We also define the K g in (32): Here λ is lower than 1. It determines the increasing scale of the filtering gain. k 0 is the moment of the state changing rapidly.

SINS/DVL local filter
The state equation of the SINS/ DVL local filter is defined as follows: Where the F k is state transfer matrix, which is determined by the error model of SINS/DVL. W k denotes the system noise.
For the SINS/ DVL integrated positioning subsystem, we take the difference between the calculated velocity of the SINS system and the velocity of DVL as the observation of the measurement equation. And the measurement equation of the SINS/DVL local filter is: Where H SINS/DVL and V SINS/DVL are defined as follows: The subsystem adopts the improved adaptive filtering algorithm based on filter gain compensation. Here H k is the observation coefficient matrix of the subsystem and V k is the observation noise matrix. Firstly, F k , G k , W k , H k , V k and the initial state variable X are substituted into the state equation and measurement equation of the system for one-step prediction, through which the estimation measurement value Ẑ k and X k,k−1 predicted at time k can be obtained. And the error of measurement value Z k at time k is calculated as follow: Then we judge the system is stable or not according to the stable criterion: If the criterion (36) holds, the system diverges. And we should adopt the gain compensation adaptive filter. Otherwise, the system converges. And we should take the gain compensation-based strong-tracking Kalman filter.

1) Gain compensation based adaptive Kalman filter
By substituting the corresponding parameters and quantities into formulas (39) to (48) for calculation, the steps of the gain compensation-based adaptive filter are as follow: Here X k is the system state variable. X k,k−1 denotes the estimation value of the system state variable at the moment k from the k-1 moment. F k, k − 1 represents the state transfer coefficient on X k-1 . K k is Kalman coefficient of the filtering gain compensation. H denotes the coefficient of the system observation equation. Z k is the observation value at the k moment. Ẑ k denotes the estimation value of observation at the k moment. P k, k − 1 is the covariance of the prior state estimation. P k denotes the covariance of the post-state estimation.
In formulas (39) to (44), r k , R k , q k and Q k are calculated from the time-varying noise estimation equation as follows: (39) 2) Gain compensation-based strong tracking Kalman filter By substituting the corresponding parameters and values into formulas (49) to (57) for calculation, the steps of the gain compensation-based strong tracking Kalman filter are as follows: In this way, the one-step filtering operation is realized through the above adaptive filtering and strong-tracking filtering to obtain the estimated value X k of the state variable at the next time.
Finally, the federated filter subsystem gains the state variable sequence X = { X 1 , X 2 ,…, X k ,…, X n }, whose final output and output the final result X n is the state result after filtering correction at the current time. And the X n = v En v Nn n n n L n n En Nn Un v dn Δ n C n T is the correct result of the error. Then we combine the current moment observation value Y n = [v En v Nn L n λ n v dn ] T of the integrated positioning subsystem. And we achieve the (48) corrected east, north velocity information, longitude and latitude information, and velocity information of DVL.

Improved USBL
We improve the USBL positioning method through the phase difference acquisition of the Kalman filter (KF) algorithm. As shown in Fig. 4, the system structure mainly consists of three parts: the array element setup part, the signal noise reduction part, and the positioning calculation part.
Firstly, in the array element setup part, we adopt a nonequidistant quaternary [26]. It reuses a specific element three times, improving the efficiency and accuracy of the acoustic signals captured from the underwater target. We utilize the array to obtain the time delay of the received signals from a target object to different elements.
Secondly, in the signal noise reduction part, we adopt a KF algorithm to filter out the white Gaussian noise in the acoustic signals. So we can obtain accurate phase difference information through a phase difference calculation sub-module to provide data for the next step.
Finally, in the positioning calculation part, we obtain the positioning result through the coordinate calculation formula according to the positioning model based on the obtained phase difference between signals with high accuracy.
Array element setup The non-equidistant quaternary array of the USBL system is composed as shown in Fig. 5. The distance between array element 1 and element 3 is d, which is less or equal to λ/2, where λ = 80mm and the x-axis angle θ = 45 ∘ . The distance between array element 2 and array element 3 is the same as that between array element 4 and array element 3, which is L = 8d. The advantage of this method is that the acoustic signal received by element 3 is reused three times, which reduces the number of redundant elements, enhances the length of the baseline, and further improves the positioning accuracy of the system.
We indicate the arrangement of the arrays in Fig. 5. Element 3 and 2 are two elements in the x-axis, which can receive acoustic signals from the positioning project. As different propagation distance between two received signals, there is a phase difference between the two received acoustic signals. Considering this case, the specific process of solving the USBL positioning coordinates is as follows: (58) x L = φ 23 R /2π L Where x L is the mapping coordinate of the object in the array. φ 23 denotes the phase difference between the arrays on the x-axis. R is the distance between the target and the array, which can be obtained directly by the measurement instrument.
In the same calculation manners, we obtain the mapping coordinate x d of element 1 on the x-axis through element 3 and element 1.
(59) Signal filtering Noise will cause significant interference to the positioning of underwater objects, so it is usually necessary to denoise the obtained underwater acoustic signals to achieve the positioning of underwater targets. To minimize noise interference and improve the positioning accuracy, the minimum mean square error is selected as the best estimation criterion to reduce the noise received by the sensor. Based on this idea, this  paper uses the KF algorithm to process the obtained signal based on minimum mean square error estimation.
When receiving the signals, we get the sampling sequence S = {s 1 , s 2 , s 3 , …, s k , …, s n } by sampling them. In the above equation, k denotes a serial number, 1 ≤ k ≤ n. And the sampling sequence can be normalized as follows: In this paper, the KF algorithm is used to filter the sampling sequence { u k }to reduce the noise.
Where k is the discrete-time, X = {X 1 , X 2 , X 3 , …, X k , …, X n } is the state value of the system and X k is the state value of the system at time k. u = {u 1 , u 2 , u 3 , …, u k , …, u n } is the observation signal of the system. W = {W 1 , W 2 , W 3 ,…, W k ,…, W n } is the input white noise; and W k is the noise value of the system at time k, H = 1 is the measurement matrix, and the specific value of the observed value u is the value of the input signal.
KF algorithm mainly consists of two parts: prediction and update. In the prediction part, the filter estimates the state of the current moment based on the state estimates of the previous moment. In the update section, the filter optimizes the predicted values from the prediction section based on the observations of the system state at the current time, and the final more accurate state estimate is obtained. The specific equations of the KF algorithm are as following: We define the time update equation:

State update equation follows:
Where A is the state transition factor of the state variable. The B denotes the input control factor matrix of the control vector. The H represents the system state observation The P − k is the system prior estimate covariance matrix. The P k denotes the system posterior estimate covariance matrix. The Q is the process excitation noise covariance coefficient. The R is the covariance coefficient of process noise. The I denotes the unit matrix. And the K k is the Kalman filter gain factor.
Then we can get the equation as following: Here the X k is the estimation value of the state variable. The X − k is the prior estimation of the state obtained from the state variable.
We utilize the KF algorithm described above to estimate the state variables of a random process represented by a linear random difference equation. The magnitude of the observed value of the system is the input signal value after adding noise. Combining with the established system state equation, the optimal input X k value is estimated by re-cursing the variance continuously, and the signal is smoothed.
Positioning calculation First, we introduce the traditional phase difference positioning principle. For singlefrequency CW signals, the most common information is based on the phase information obtained when the USBL target is positioned. Then, we calculate the target position through the phase difference of the acoustic signals between the elements. When multiple targets need to be located, different frequencies of acoustic signals can be used to distinguish them. The principle of USBL positioning based on acquiring phase difference of a singlefrequency signal is described below in Fig. 6.
The array elements of the USBL system are placed on the xoy plane, where element 1 is located at the origin of the xoy coordinate system. Element 2 is located on the positive semi-axis of the x-axis with a distance of d from the origin of the coordinates. And element 3 is located on the positive semi-axis of the y-axis, with a distance of d from the origin of the coordinates. The target is located at S with specific coordinates (x, y, z). After the target emits an acoustic signal, the array element receives the acoustic signal to locate the target. The system processes the received acoustic signal to get the specific location of the target.
From Fig. 5, the (x, y, z) direction cosine can be expressed as: Where α is the angle between the OS and the x-axis. β denotes the angle between the OS and the y-axis, and R is the target distance.
In Fig. 5, S ′ is the projection of the target S on the horizontal plane of xoy. And its angle to the x-axis is the horizontal azimuth θ of the target, which is determined by the formula (74): Where R is the target horizontal slant range, and z is the target depth.
Formulas (71) to (76) are the formulas for the positioning solution of USBL. We can calculate the corresponding parameters of the target object according to the above formulas.
Since the USBL array has a tiny size, we can approximate it by formulas (77) and (78): Where λ is the wavelength of the acoustic signal in the water, d denotes the distance between the elements of the array, φ represents the phase difference between signals received by two adjacent arrays on the x-axis, and ψ is the phase difference of the received signal for two adjacent arrays on the y-axis.
By substituting formulas (77) and (78) into formula (71) and (72), we can obtain the following results: Where R meets the equation R = c × Δt/2, in which c is the speed of sound propagation in water and Δt is the time difference between signal transmission and reception.

Global combined federated Kalman filter
Suppose the state variable of the federated filter from moment k-1 to moment k with state equation: The observation equation of the ith subsystem is as follows: Then the estimation of the local filter in the two subsystems is X 1 and X 2 . The corresponding estimation variance matrix is P 1 , and P 2 . And the covariance matrix of noise is Q 1 and Q 2 . The time update value of federated filter is X m . The estimated error variance matrix is P m . The covariance matrix is Q m . The optimal global estimation of the system X g and P g can be gained from (83)-(85).
The feedback to each subsystem can be allocated according to the following rules: Fig. 6 The principle of positioning The federated filter consists of two parts: filtering & estimation of local state, and information fusion of global state. As shown in Fig. 7, the SINS/DVL and USB navigation subsystem outputs local estimation X 1 and X 2 of the system, the estimated variance matrix P 1 and P 2 , and the noise variance matrix Q 1 and Q 2 , respectively. Then we input the local estimation value into the main federated filter and fuse with the reference subsystem from the SINS. Finally, we get X g and P g , which is the best estimation of the global state. Finally, the federated filter output the global navigation and positioning result and feedback to the SINS/DVL subsystem.
The federated Kalman filter includes two stages: time update and information fusion. During the time update stage, the global status estimation X m , estimated variance matrix of error P m , and covariance matrix Q m will be updated. And during the information fusion stage, we combine the status estimation of the global federated main filter and the local state estimation of each subsystem. Finally, the filter assigns information in a specific way, feedback the best estimation of global status and its variance matrix to each navigation and positioning system.

Performance evaluation
In this section, we evaluate the performance of the integrated navigation and positioning system based on a federated Kalman filter. We first set up the evaluation environment. Then we measure the performance, including the positioning accuracy, stability, and performance. We also make comparisons with other related methods.

Simulation setup Evaluation environment
We first set up a Matlab simulation environment. To validate and evaluate the accuracy and stability of the filtering procedure, we set up a motion model with timevarying motion states. We indicate a motion velocity and a motion trace in Figs. 8 and 9. More specifically, the total time of motion is 2000 s. An AUV first moves in the direction of the northeast. The speed in the north is 2.57 m/s, while the speed in the east is 0.27 m/s. After 1000 s, the AUV changes the direction to the east in the sale of 3°/s. After 600 s, the AUV changes direction to the north at the scale of 3°/s . It lasts 25 s. Finally, the AUV moves straightly. From 1000s to 2000 s, the velocity is 2.57 m/s. We conduct simulations based on the motion model and the data.

Evaluation parameters
We evaluate the positioning accuracy regarding the positioning error, the eastern velocity error, and the mean square error. The lower the positioning error is, the higher the positioning accuracy is.
For positioning efficiency, we evaluate the computation time of navigation and positioning. Short processing time indicates high positioning efficiency.

Reference methods
We select the improved SINS/DVL positioning method [4] and the federated Kalman filter-based positioning [25] (note as classic FKF) as the reference methods. We call our proposed federated-filter-based positioning method as improved FKF.

Positioning accuracy evaluation
We evaluate the position accuracy with the improved SINS/DVL positioning method, the classical federatedfilter-based positioning method.

Compare with improved SINS/DVL
With the simulation environment and the motion model of AUV, we simulate the movement of AUV and perform the federated-filter-based positioning. We evaluate the positioning accuracy of the positioning method and compare it with that of the improved SINS/DVL positioning method as shown in Fig. 10. From Fig. 10, we can conclude that the proposed federated filter-based positioning method gains higher accuracy. More specifically, the average distance error of the improved SINS/DVL is 0.20m in the direction of the east, while that of our proposed SINS/DVL/USBL federatedfilter is 0.02m. Positioning accuracy is improved almost ten times. We also compare the positioning error of the two positioning methods. The average positioning error of the proposed SINS/DVL/USBL federated-filter is less than 0.01m, while the average positioning error of the improved SINS/DVL is 0.27m. Although the improved SINS/DVL method can restrain the positioning error when the state changes rapidly at the time of 1000s. However, it can not restrain the positioning error caused by the state change rapidly at the time of the 1600s. But the proposed SINS/DVL/USBL federated filter-based positioning system can strain the positioning error caused by the state change rapidly at the time of 1000s and 1600s. So the SINS/DVL/USBL federated filter-based positioning system can gain higher stability and reliability, especially when a complex environment.

Compare with the federated filter-based positioning method
We also compare our proposed method with the classic federated filter. With the same simulation setting, we compare the positioning error of the proposed integrated system with that of the classic federated filter-based navigation and positioning system in Fig. 11. Figure 11 illustrates that the proposed improved federated filter-based navigation and positioning system gain  lower positioning error than the classic federated filter. It can enhance the positioning accuracy on a scale of 4-5 times on average. That's because based on the classic federated filter, we conduct optimization on the navigation and positioning system. More specifically, we utilize the filtering-gain-compensation-based adaptive filter, which can improve the positioning accuracy of the subsystem at a large scale. And the positioning accuracy of the whole navigation and positioning system is improved. We illustrate the east velocity error of the classical federated filter in Fig. 12. We can see that the improved federated filter-based navigation method gains higher accuracy. More specifically, the east velocity error of the improved federated filter-based method is 1/3 of that of the classic federated filter. And the velocity error fluctuates is also tiny. That's because the subsystem in the improved federated filter adopts an improved adaptive filter making the system more stable and more accurate.

MSE evaluation
We evaluate the mean square error (MSE) of the improved federated filter-based positioning system. We also compare it with the improved SINS/DVL system [4] and FKF [25] in Table 2.
From Table 1, we can note that the two federated filter-based positioning systems gain a lower MSE than the improved SINS/DVL system. And the improved federated filter has the lowest positioning error among these   And it also less on the west MSE by 91.65% and 97.32%, respectively. That is mainly due to the validation of the multiple-source information fusion. The proposed navigation and positioning method has better positioning performance. From the simulation above, the improved federated filter-based navigation and positioning method gains high positioning accuracy than the two other positioning methods, especially in a complex motion environment.

Positioning efficiency evaluation
We evaluate the efficiency of the improved federated filter-based navigation and positioning method in terms of the processing time. We also compare it with the other two methods as illustrated in Table 3.
From Table 2, we can see that the processing time of the improved federated method is the highest of the three methods. That means that the proposed method has the lowest efficiency. We also conclude that the federated filter methods including the proposed method and the classical method have lower efficiency than the improved SINS/DVL method. That is because the federated filter is the fusion of multiple positioning subsystems. And the processing time of the proposed method is double that of the classic federated filter method. That's because the proposed method adopts a more complex filter algorithm, which affects the processing efficiency. However, considering the accuracy and the stability improvement of the proposed federated filter-based system comprehensively, the loss of efficiency is acceptable.

Discussion
To gain higher positioning performance, we utilize the federated filter to fuse the positioning information in this paper. In each positioning subsystem, we also explore the Kalman filter to minimize the negative effect of noise and measurement error. We discuss the advantages and disadvantages.

Advantages
As shown in the Positioning accuracy evaluation subsection, we combine some positioning subsystems into a new positioning system to gain higher positioning accuracy and more robustness of the system. The improved federated filter-based positioning system can provide critical position information for AUVs and other related application systems.

Disadvantages
The disadvantage of the proposed method is the high computation amount, which leads to more processing time shown in Table 2. However, considering the improvement of positioning accuracy, the cost of processing time is accepted. For the overlaps and repetition, these subsystems provide different information, i.e., the DVL provides the velocity information. The INS provides estimated position information. The USBL provides relative position information. So there is no overlap and repetition in the federated Kalman filter-based positioning system.
The data in this paper are generated by our positioning model. So the data and results are available to us.

Conclusion
To improve the accuracy and stability of AUV, we presented an improved federated filter-based navigation and positioning system in this paper. In the system, we first utilized the filtering gain compensation-based SINS/DVL subsystem to improve the positioning accuracy. We also explore the phase difference filtering-based USBL subsystem. Then with the federated filter, we integrate the SINS/ DVL/USBL positioning subsystem and perform multiple sources information fusion to gain higher accuracy and more stable navigation and positioning. The simulation result illustrated that the presented federated filter-based navigation and positioning system could achieve higher performance. The contribution of this paper is to propose the federated Kalman filter-based combination positioning system. So that we can gain more accurate positioning results. The method also provides a critical reference value for other navigation and positioning method. However, it should be noted that the improvement is at the cost of high processing time. So the proposed method can be applied in non-real-time systems.