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

Abstract

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 and basics

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 and improved the calculation speed. Ma et al. presented a federated adaptive filter based on improved covariance [19], which derived real-time estimates of improved covariance according to maximum likelihood estimation criteria. And it introduced a scaling factor in each local filter to enhance the adaptive capability of the whole system. Xu et al. took a SINS/GPS/CNS/Radar integrated system as an example [11]. It calculated the parameters corresponding to each state with a dual-state detection. Wang Q et al. introduced a joint filter to fuse data [20]. When the speed-accuracy of DVL decreased. And the CNS could not work under severe weather conditions. The INS/CNS/DVL combined system could operate stably. Xu Q et al. designed a federated unscented Kalman filter (FUKF) with different vehicle motion models to estimate the vehicle attitude [21]. The result is promising. Li et al. designed a federated hybrid filter. In the filter [22], a minimum variance criterion is utilized to fuse the estimate of each local filter. Wang et al. presented a federated filter for a multiple-sensor cross-correlations strategy [23]. An adaptive filter was utilized as a local filter, which can conquer the performance degradation. To enhance the accuracy and robustness, Yue Z et al. proposed a federated filter with a feedback scheme for a GNSS/INS/visual odometry combined positioning system [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.

Table 1 The MSE comparison of different methods

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:

$${\boldsymbol X}_k=\boldsymbol F{{}_{k,k-1}\boldsymbol X}_{k-1}+{\boldsymbol W}_{k-1}$$

(1)

$${\boldsymbol Z}_k={\boldsymbol H}_k{\boldsymbol X}_k+{\boldsymbol V}_k$$

(2)

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).

$${\hat{\boldsymbol X}}_{\boldsymbol k}^{\hbox{-} }={\boldsymbol A}{\hat{\boldsymbol X}}_{k-1}$$

(3)

$$\boldsymbol P_k^-={\boldsymbol A\boldsymbol P}_{k-1}\boldsymbol A^T+\boldsymbol Q$$

(4)

Here ${\hat{\boldsymbol{X}}}_{\mathrm{k}}^{\hbox{-} }$ denotes the prior estimated value of system state at moment k-1. A is the transfer coefficient of system. ${\hat{\boldsymbol{X}}}_{k-1}$ is the estimated value of system state at moment k-1. ${\boldsymbol{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.

$${\boldsymbol K}_k={\boldsymbol P}_k\boldsymbol H^T\left({\boldsymbol H\boldsymbol P}_k^-\boldsymbol H^T+\boldsymbol R\right)^{-1}$$

(5)

$${\hat{\boldsymbol{X}}}_k={\hat{\boldsymbol{X}}}_k^{-}+{\boldsymbol{K}}_k\left({\boldsymbol{Z}}_k-\boldsymbol{H}{\hat{\mathbf{X}}}_k^{-}\right)$$

(6)

$${\boldsymbol P}_{\boldsymbol k}=\left(\boldsymbol I-{\boldsymbol K}_{\boldsymbol k}\boldsymbol H\right)\boldsymbol P_{\boldsymbol k}^-$$

(7)

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. ${\hat{\boldsymbol{X}}}_k$ is the estimated value of the 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.

Fig. 1

The filtering process of the linear Kalman filter

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.

Fig. 2

The framework of the combined navigation and positioning system

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 compensation-based 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.

Fig. 3

The framework of the integrated SINS/DVL subsystem

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 X = [δv_E δv_N α β γ δL δλ ε_E ε_N ε_U δv_d δΔ δC]^T. 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:

$${\displaystyle \begin{array}{l}\delta {v}_E={v}_N\tan L\cdot \delta {v}_E/{R}_{\mathrm{N}}+\left(2\Omega \sin L+{v}_E\tan L/{R}_{\mathrm{N}}\right)\delta {v}_N\\ {}+\left(2\Omega \cos {Lv}_N+{v}_N{v}_E{\sec}^2L/{R}_{\mathrm{N}}\right)\delta L-\beta g+\Delta {a}_E\end{array}}$$

(8)

$${\displaystyle \begin{array}{l}\delta {v}_N=\left(2\Omega \sin L+{v}_E\tan L/{R}_{\mathrm{N}}\right)\delta {v}_E\\ {}+\left(2\Omega \cos {Lv}_E+{v}_E^2\cdot {\sec}^2L/{R}_{\mathrm{N}}\right)\delta L+\alpha g+\Delta {a}_N\end{array}}$$

(9)

We calculate the platform misalignment angle in (3–5):

$$\alpha =-\delta {v}_N/R-\gamma \left(\Omega \cos L+{v}_E/R\right)+\beta \left(\Omega \sin L+{v}_E\tan L/R\right)+{\varepsilon}_E$$

(10)

$${\displaystyle \begin{array}{l}\beta =-\alpha \left(\Omega \sin L+{v}_E\tan L/R\right)-\delta L\Omega \sin L+\delta {v}_E/R\\ {}-\gamma {v}_E/R+{\varepsilon}_N\end{array}}$$

(11)

$${\displaystyle \begin{array}{l}\gamma =\delta L\left(\Omega \cos L+{v}_E{\sec}^2L/R\right)+\delta {v}_E\tan L/R+\beta {v}_N/R\\ {}+\alpha \left(\Omega \cos L+{v}_E/R\right)+{\varepsilon}_U\end{array}}$$

(12)

We define the position error in (13):

$$\delta L=\delta {v}_N/R$$

(13)

$$\delta \lambda =\delta {v}_E\sec L/R+{v}_E\tan L\sec L\delta L/R$$

(14)

We calculate the gyro drift in the east, in the north and in the up direction as follows:

$${\varepsilon}_E=-{\beta}_E{\varepsilon}_E+{w}_E$$

(15)

$${\varepsilon}_N=-{\beta}_N{\varepsilon}_N+{w}_N$$

(16)

$${\varepsilon}_U=-{\beta}_U{\varepsilon}_U+{w}_U$$

(17)

Here ${\beta}_E^{-1}$, ${\beta}_N^{-1}$, ${\beta}_U^{-1}$ 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:

$$\delta {v}_d=-{\beta}_d\delta {v}_d+{w}_d$$

(18)

$$\delta \Delta =-{\beta}_{\Delta}\delta \Delta +{w}_{\Delta}$$

(19)

$$\delta C=0$$

(20)

We describe the state equation in (21):

$${\dot{\mathbf{X}}}_{SINS/ DVL}={\mathbf{F}}_{SINS/ DVL}{\mathbf{X}}_{SINS/ DVL}+{\mathbf{G}}_{SINS/ DVL}{\mathbf{W}}_{SINS/ DVL}$$

(21)

We define the state equation of SINS in (22):

$${\mathbf{W}}_{SINS}={\left[0\kern0.5em 0\kern0.5em {a}_E\kern0.5em {a}_N\kern0.5em 0\kern0.5em 0\kern0.5em 0\kern0.5em {w}_E\kern0.5em {w}_N\kern0.5em {w}_U\kern0.5em {w}_d\kern0.5em {w}_{\Delta}\kern0.5em 0\right]}^T$$

(22)

In (21), we calculate the transfer matrix as follow:

(23)

$${\mathbf{F}}_{7\times 7}=\left[\begin{array}{lllllll}0& 0& 0& {F}_{14}& 0& 0& 0\\ {}{F}_{21}& 0& {F}_{23}& 0& 0& 0& 0\\ {}{F}_{31}& 0& {F}_{33}& {F}_{34}& 0& -g& 0\\ {}{F}_{41}& 0& {F}_{43}& 0& g& 0& 0\\ {}0& 0& 0& {F}_{54}& 0& {F}_{56}& {F}_{57}\\ {}{F}_{61}& 0& {F}_{63}& 0& {F}_{65}& 0& {F}_{67}\\ {}{F}_{71}& 0& {F}_{73}& 0& {F}_{75}& {F}_{76}& 0\end{array}\right]$$

(24)

The difference between the velocity error of SINS and DVL is taken as the observation of the measurement equation which is defined as follow:

$${\displaystyle \begin{array}{l}{\mathbf{Z}}_{SINS/ DVL}=\left[\begin{array}{c}\delta {v}_E-\delta {v}_{dE}\\ {}\delta {v}_N-\delta {v}_{dN}\end{array}\right]\\ {}\kern5em ={\mathbf{H}}_{SINS/ DVL}{\mathbf{X}}_{SINS/ DVL}+{\mathbf{V}}_{SINS/ DVL}\end{array}}$$

(25)

We define the transfer matrix in (20):

$${\mathbf{H}}_{SINS/ DVL}=\left[\begin{array}{lllllllllllll}0& 0& 1& 0& 0& 0& -{v}_N& 0& 0& 0& -\sin {K}_d& -{v}_N& -{v}_E\\ {}0& 0& 0& 1& 0& 0& {v}_E& 0& 0& 0& -\cos {K}_d& {v}_E& -{v}_N\end{array}\right]$$

(26)

$${\mathbf{V}}_{SINS/ DVL}={\left[\begin{array}{cc}{v}_E& {v}_N\end{array}\right]}^T$$

(27)

We define the variance matrix of noise:

$${\displaystyle \begin{array}{l}\mathbf{Q}=\mathit{diag}\Big({R}_{\delta {V}_E}{R}_{\delta {V}_N}{R}_{\alpha }{R}_{\beta }{R}_{\gamma }{R}_{\delta L}{R}_{\delta \lambda}\\ {}{R}_{\varepsilon E}{R}_{\varepsilon N}{R}_{\varepsilon U}{R}_{\delta Vd}{R}_{\delta \Delta}{R}_{\delta C}\Big)\end{array}}$$

(28)

We calculate the variance matrix of measurement noise:

$$\mathbf{R}=\mathit{diag}\left(\begin{array}{cc}{R}_{V_N}& {R}_{V_E}\end{array}\right)$$

(29)

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:

$${\textbf{K}}_n(k)=\mathbf K(k)+{\mathbf K}_g(k)$$

(30)

We update the filter equation in (25):

$$\hat{\mathbf K}\left(k+1\right)=\hat{\mathbf K}\left(k+1/k\right)+\hat{\mathbf K}\left(k+1\right)\left[\hat{\mathbf Z}\left(k+1\right)-\hat{\mathbf Z}\left(k+1/k\right)\right]$$

(31)

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):

$${\boldsymbol{K}}_g={\boldsymbol{K}}_z{\lambda}^{\left(k-{k}_0\right)}$$

(32)

Here λ is lower than 1. It determines the increasing scale of the filtering gain. k₀ 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:

$${\dot{\boldsymbol X}}_k={\boldsymbol F}_k{\boldsymbol X}_k+{\boldsymbol W}_k$$

(33)

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:

$${\boldsymbol Z}_{SINS/DVL}=\begin{bmatrix}{dv}_E-{dv}_{dE}\\{dv}_N-{dv}_{dN}\end{bmatrix}={\boldsymbol H}_{SINS/DVL}{\boldsymbol X}_{SINS/DVL}+{\boldsymbol V}_{SINS/DVL}$$

(34)

Where H_SINS/DVL and V_SINS/DVL are defined as follows:

$${\boldsymbol{H}}_{SINS/ DVL}=\left[\begin{array}{lllllllllllll}0& 0& 1& 0& 0& 0& -{v}_N& 0& 0& 0& -\sin {K}_d& -{v}_N& -{v}_E\\ {}0& 0& 0& 1& 0& 0& {v}_E& 0& 0& 0& -\cos {K}_d& {v}_E& -{v}_N\end{array}\right]$$

(35)

$${\boldsymbol{V}}_{SINS/ DVL}={\left[\begin{array}{cc}{v}_E& {v}_N\end{array}\right]}^T$$

(36)

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 ${\hat{\boldsymbol {Z}}}_k$ and ${\hat{\boldsymbol {X}}}_{k,k-1}$ predicted at time k can be obtained. And the error of measurement value ${\tilde{\boldsymbol {Z}}}_k$ at time k is calculated as follow:

$${\tilde{\boldsymbol {Z}}}_k={\hat{\boldsymbol {Z}}}_k-{\boldsymbol{H}}_k{\hat{\boldsymbol{X}}}_{k,k- 1}$$

(37)

Then we judge the system is stable or not according to the stable criterion:

$${\tilde{\boldsymbol{Z}}}_k{\tilde{\boldsymbol{Z}}}_k^T\le \gamma Tr\left[{\boldsymbol{H}}_k{\boldsymbol{P}}_k{\boldsymbol{H}}_k^T+{\boldsymbol{R}}_k\right]$$

(38)

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:

$${\hat{\boldsymbol{X}}}_k={\hat{\boldsymbol{X}}}_{k,k-1}+{\boldsymbol{K}}_k{\tilde{\boldsymbol{Z}}}_k$$

(39)

$${\hat{\boldsymbol{X}}}_{k,k-1}={\boldsymbol{F}}_{k,k-1}{\hat{\boldsymbol{X}}}_{k-1}+{\hat{\boldsymbol{q}}}_k$$

(40)

$${\hat{\boldsymbol{Z}}}_k={\boldsymbol{Z}}_k-{\boldsymbol{H}}_k{\hat{\boldsymbol{X}}}_{k,k-1}-{\hat{\boldsymbol{r}}}_k$$

(41)

$${\boldsymbol{K}}_k={\boldsymbol{P}}_{k,k-1}{\boldsymbol{H}}_k^T{\left({\boldsymbol{H}}_k{\boldsymbol{P}}_{k,k-1}{\boldsymbol{H}}_k^T+{\hat{\boldsymbol{R}}}_k\right)}^{-1}$$

(42)

$${\boldsymbol{P}}_{k,k-1}={\boldsymbol{F}}_{k,k-1}{\boldsymbol{P}}_{k-1}{\boldsymbol{F}}_{k,k-1}^T+{\hat{\boldsymbol{Q}}}_{k-1}$$

(43)

$${\boldsymbol{P}}_k=\left(\mathrm{I}-{\boldsymbol{K}}_k{\boldsymbol{H}}_k\right){\boldsymbol{P}}_{k,k-1}{\left(\mathrm{I}-{\boldsymbol{K}}_k{\boldsymbol{H}}_k\right)}^T+{\boldsymbol{K}}_k{\hat{\boldsymbol{R}}}_k{\boldsymbol{K}}_k$$

(44)

Here ${\hat{\boldsymbol{X}}}_k$ is the system state variable. ${\hat{\boldsymbol{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 ${\hat{\boldsymbol{X}}}_{k\hbox{-} 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. ${\hat{\boldsymbol{Z}}}_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), ${\hat{\boldsymbol{r}}}_k$, ${\hat{\boldsymbol{R}}}_k$, ${\hat{\boldsymbol{q}}}_k$ and ${\hat{\boldsymbol{Q}}}_k$ are calculated from the time-varying noise estimation equation as follows:

$${\hat{\boldsymbol{r}}}_k=\left(\mathrm{I}-{\boldsymbol{d}}_{k-1}\right){\hat{\boldsymbol{r}}}_{k-1}+{\boldsymbol{d}}_{k-1}\left({\boldsymbol{Z}}_k-{\boldsymbol{H}}_{k,k-1}{\hat{\boldsymbol{X}}}_{k,k-1}\right)$$

(45)

$${\hat{\boldsymbol{R}}}_k=\left(\mathrm{I}-{\boldsymbol{d}}_{k-1}\right){\hat{\boldsymbol{R}}}_{k-1}+{\boldsymbol{d}}_{k-1}\left({\tilde{\boldsymbol{Z}}}_k{\tilde{\boldsymbol{Z}}}_k^T-{\boldsymbol{H}}_{k,k-1}{\boldsymbol{P}}_k{\boldsymbol{H}}_k^T\right)$$

(46)

$${\hat{\boldsymbol{q}}}_k=\left(\mathrm{I}-{\boldsymbol{d}}_{k-1}\right){\hat{\boldsymbol{R}}}_{k-1}+{\boldsymbol{d}}_{k-1}\left({\hat{\boldsymbol{X}}}_k-{\boldsymbol{F}}_k{\hat{\boldsymbol{X}}}_{k-1}\right)$$

(47)

$${\hat{\boldsymbol{Q}}}_k=\left(\mathrm{I}-{\boldsymbol{d}}_{k-1}\right){\hat{\boldsymbol{Q}}}_{k-1}+{\boldsymbol{d}}_{k-1}\left({\boldsymbol{K}}_k{\tilde{\boldsymbol{Z}}}_k{\tilde{\boldsymbol{Z}}}_k^T{\boldsymbol{K}}_k^T+{\boldsymbol{P}}_k-{\boldsymbol{F}}_k{\boldsymbol{P}}_{k-1}{\boldsymbol{F}}_k^T\right)$$

(48)

• 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:

$${\boldsymbol{v}}_{0\left(k+1\right)}=\left\{\begin{array}{c}{\tilde{\boldsymbol{Z}}}_1{\tilde{\boldsymbol{Z}}}_1^T\kern3.5em \left(k=0\right)\\ {}\frac{\rho {\boldsymbol{v}}_{0(k)}+{\tilde{\boldsymbol{Z}}}_{k+1}{\mathbf{Z}}_{k+1}^T}{1+\rho}\left(k\ge 1,0\le \rho <1\right)\end{array}\right.$$

(49)

$${C}_{k+1}=\frac{Tr\left[{\mathbf{v}}_{0\left(k+1\right)}-{\boldsymbol{R}}_{k+1}-{\boldsymbol{H}}_{k+1}{\boldsymbol{Q}}_k{\boldsymbol{H}}_{k+1}^T\right]}{\sum \limits_{i=1}^n{\alpha}_i\left[{\Phi}_{k+1}{\boldsymbol{P}}_{k+1}{\Phi}_{k+1}^T{\boldsymbol{H}}_{k+1}^T{\boldsymbol{H}}_{k+1}\right]}$$

(50)

$${\lambda}_{i\left(k+1\right)}=\left\{\begin{array}{c}{\alpha}_i{C}_{k+1}\left({\alpha}_i{C}_{k+1}>1\right)\\ {}1\kern2em \left({\alpha}_i{C}_{k+1}<1\right)\end{array}\right.$$

(51)

$${\boldsymbol{\uplambda}}_{k+1}=\mathit{diag}\left[{\lambda}_{1\left(k+1\right)},{\lambda}_{2\left(k+1\right)},\dots, {\lambda}_{m\left(k+1\right)}\right]$$

(52)

$${\boldsymbol{P}}_{k,k-1}={\boldsymbol{\uplambda}}_{k-1}{\Phi}_{k,k-1}{\boldsymbol{P}}_{k,k-1}{\Phi}_{k,k-1}^T+{\hat{\boldsymbol{Q}}}_{k-1}$$

(53)

$${\boldsymbol{K}}_k={\boldsymbol{P}}_{k,k-1}{\boldsymbol{H}}_k^T{\left({\boldsymbol{H}}_k{\boldsymbol{P}}_{k,k-1}{\boldsymbol{H}}_k^T+{\hat{\boldsymbol{R}}}_k\right)}^{-1}$$

(54)

$${\hat{\boldsymbol{X}}}_k={\hat{\boldsymbol{X}}}_{k,k-1}+{\boldsymbol{K}}_k{\tilde{\boldsymbol{Z}}}_k$$

(55)

$${\hat{\boldsymbol{X}}}_{k,k-1}={\boldsymbol{F}}_{k,k-1}{\hat{\boldsymbol{X}}}_{k-1}+{\boldsymbol{GW}}_{k,k-1}$$

(56)

$${\boldsymbol{P}}_k=\left(\mathrm{I}-{\boldsymbol{K}}_k{\boldsymbol{H}}_k\right){\boldsymbol{P}}_{k,k-1}{\left(\mathrm{I}-{\boldsymbol{K}}_k{\boldsymbol{H}}_k\right)}^T+{\boldsymbol{K}}_k{\hat{\boldsymbol{R}}}_k{\boldsymbol{K}}_k$$

(57)

In this way, the one-step filtering operation is realized through the above adaptive filtering and strong-tracking filtering to obtain the estimated value ${\hat{X}}_k$ of the state variable at the next time.

Finally, the federated filter subsystem gains the state variable sequence $\hat{\boldsymbol{X}}$₌{ ${\hat{\boldsymbol{X}}}_1$, ${\hat{\boldsymbol{X}}}_2$,…, ${\hat{\boldsymbol{X}}}_k$,…, ${\hat{\boldsymbol{X}}}_n$ }, whose final output and output the final result ${\hat{\mathbf{X}}}_n$ is the state result after filtering correction at the current time. And the ${\hat{\boldsymbol{X}}}_n={\left[\delta {v}_{En}\ \delta {v}_{Nn}\ {\alpha}_n\ {\beta}_n\ {\gamma}_n\ \delta {L}_n\ {\delta \lambda}_n\ {\varepsilon}_{En}\ {\varepsilon}_{Nn}\ {\varepsilon}_{Un}\ \delta {v}_{dn}\ \delta {\Delta}_n\ \delta {C}_n\right]}^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 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.

Fig. 4

The framework of the underwater positioning

Firstly, in the array element setup part, we adopt a non-equidistant 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.

Fig. 5

The USBL positioning system array diagram [26]

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:

$${x}_L=\left({\lambda \phi}_{23}R\right)/2\pi L$$

(58)

Where x_L is the mapping coordinate of the object in the array. ϕ₂₃ 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.

$${x}_d=\left({\lambda \phi}_{13}R\right)/2\pi d$$

(59)

Assuming the noise background of each array element in the USBL system is independent, the phase differences ϕ_ij and ψ_ij have the same measurement accuracy Δϕ, which meets the equation Δϕ₁₃ = Δϕ₂₃ = Δϕ_.

Equation (59) is the positioning solution formula of the traditional USBL array, and Eq. (58) is the positioning solution formula of the new USBL array. After differential treatment of Eqs. (1) and (2), we can get:

$$\varDelta {x}_d=\frac{\lambda R}{2\pi d}\varDelta {\phi}_{13}=\frac{\lambda R}{2\pi d}\varDelta \phi$$

(60)

$$\varDelta {x}_L=\frac{\lambda R}{2\pi L}\varDelta {\phi}_{23}=\frac{\lambda R}{2\pi L}\varDelta \phi$$

(61)

Formulas (60) and (61) show that choosing an array arrangement with large spacing L = 8d can improve positioning accuracy by eight times. For the new array used in this paper, the problem of multi-value ambiguity of the phase difference can be solved by d-spacing elements (elements 1 and 3), and positioning accuracy can be solved by L-spacing elements (elements 2 and 3). The combination of the two methods can ultimately improve the system’s accuracy.

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₁, s₂, s₃, …, 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:

$${u}_k={s}_k/\left(\max (S)-\min (S)\right)$$

(62)

In this paper, the KF algorithm is used to filter the sampling sequence { u_k }to reduce the noise.

$${X}_k={H}^{\ast }{X}_{k-1}+{G}^{\ast }{W}_{k-1}$$

(63)

$${u}_k={H}^{\ast }{X}_k$$

(64)

Where k is the discrete-time, X = {X₁, X₂, X₃, …, 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₁, u₂, u₃, …, u_k, …, u_n} is the observation signal of the system. W = {W₁, W₂, W₃,…, 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:

$${\hat{X}}_k=A{\hat{X}}_{k-1}$$

(65)

$${P}_k^{-}={AP}_{k-1}{A}^T+Q$$

(66)

State update equation follows:

$${K}_k={P}_k^{-}{H}^T{\left({HP}_k^{-}{H}^T+R\right)}^{-1}$$

(67)

$${\hat{X}}_k={\hat{X}}_k^{-}+{K}_k\left({u}_k-H{\hat{X}}_k^{-}\right)$$

(68)

$${P}_k=\left(I-{K}_kH\right){P}_k^{-}$$

(69)

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 matrix. 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:

$${\hat{u}}_k={\hat{X}}_k$$

(70)

Here the ${\hat{X}}_k$ is the estimation value of the state variable. The ${\hat{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 ${\hat{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 single-frequency 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 single-frequency signal is described below in Fig. 6.

Fig. 6

The principle of positioning

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:

$$R\cos \alpha =x$$

(71)

$$R\cos \beta =y$$

(72)

$$R=\sqrt{x^2+{y}^2+{z}^2}$$

(73)

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):

$$\theta ={tg}^{-1}\left(y/x\right)={tg}^{-1}\left(\cos \beta /\cos \alpha \right)$$

(74)

$$r=\sqrt{x^2+{y}^2}$$

(75)

$$z=\sqrt{R^2-{r}^2}$$

(76)

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):

$$\phi =\left(2\pi d\cos \alpha \right)/\lambda$$

(77)

$$\Psi =\left(2\pi d\cos \beta \right)/\lambda$$

(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:

$$x=\left(\lambda \phi R\right)/\left(2\pi d\right)$$

(79)

$$y=\left(\lambda \varphi R\right)/\left(2\pi d\right)$$

(80)

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:

$${\boldsymbol{X}}_k={\boldsymbol{F}}_{k,k-1}{\boldsymbol{X}}_{k-1}+{\boldsymbol{W}}_{k-1}$$

(81)

The observation equation of the ith subsystem is as follows:

$${\boldsymbol{Z}}_{ik}={\boldsymbol{H}}_{ik}{\boldsymbol{X}}_{ik}+{\boldsymbol{V}}_{ik}$$

(82)

Then the estimation of the local filter in the two subsystems is ${\hat{\boldsymbol{X}}}_1$ and ${\hat{\boldsymbol{X}}}_2$. The corresponding estimation variance matrix is P₁, and P₂. And the covariance matrix of noise is Q₁and Q₂. The time update value of federated filter is ${\hat{\boldsymbol{X}}}_m$. The estimated error variance matrix is P_m. The covariance matrix is Q_m. The optimal global estimation of the system ${\hat{\boldsymbol{X}}}_g$and P_gcan be gained from (83)-(85).

$${\boldsymbol{P}}_g^{-1}{\hat{\boldsymbol{X}}}_g={\boldsymbol{P}}_1^{-1}{\hat{\boldsymbol{X}}}_1+{\boldsymbol{P}}_2^{-1}{\hat{\boldsymbol{X}}}_2$$

(83)

$${\boldsymbol{Q}}_g^{-1}={\boldsymbol{Q}}_1^{-1}+{\boldsymbol{Q}}_2^{-1}$$

(84)

$${\boldsymbol{P}}_g^{-1}={\boldsymbol{P}}_1^{-1}+{\boldsymbol{P}}_2^{-1}$$

(85)

The feedback to each subsystem can be allocated according to the following rules:

$${\hat{\boldsymbol{X}}}_i={\hat{\boldsymbol{X}}}_g$$

(86)

$${\boldsymbol{Q}}_i^{-1}={\xi}_i{\boldsymbol{Q}}_g^{-1}$$

(87)

$${\boldsymbol{P}}_i^{-1}={\xi}_i{\boldsymbol{P}}_g^{-1}$$

(88)

$${\displaystyle \begin{array}{cc}{\xi}_1+{\xi}_2=1& 0\le {\xi}_i\le 1\end{array}}$$

(89)

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 ${\hat{\boldsymbol{X}}}_1$ and ${\hat{\boldsymbol{X}}}_2$ of the system, the estimated variance matrix P₁ and P₂, and the noise variance matrix Q₁ and Q₂, 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 ${\hat{\boldsymbol{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.

Fig. 7

The framework of the improved federated combined filtering based navigation and positioning system

The federated Kalman filter includes two stages: time update and information fusion. During the time update stage, the global status estimation ${\hat{\boldsymbol{X}}}_m$, estimated variance matrix of error P_m, and covariance matrix Q_mwill 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 time-varying 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.

Fig. 8

The motion velocity of evaluation

Fig. 9

The motion of the combined navigation and positioning system velocity of evaluation

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 federated-filter-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.

Fig. 10

The comparison between the improved SNIS/DVL and the improved FKF

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 federated-filter 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.

Fig. 11

The performance comparison of the two methods

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.

Fig. 12

The velocity error of the two methods

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.

Table 2 The MSE comparison of different methods

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 three methods. And the east MSE of the improved FKF is less than 2.016 × 10⁻⁴. Relative to FKF and improved SINS/DVL, the improved FKF can reduce the east MSE by 91.38% and 97.58, respectively. 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.

Table 3 The processing time comparison of different methods

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.