Research on multi-source information fusion based on sliding window-factor graph for autonomous underwater vehicle

Abstract

Aiming at the high-precision real-time navigation requirements of autonomous underwater vehicle (AUV)-integrated navigation system, an AUV multi-source information fusion algorithm based on sliding window-factor graph (SW-FG) was proposed in this paper. First, the SW-FG model for the AUV-integrated navigation system is constructed using the factor graph (FG) theory. Second, historical factor nodes and estimated variable nodes are selected by a fixed-width window, and the information factors are updated by window sliding and updating. Finally, the forward and backward message passing of the FG are performed within the sliding window to achieve the smoothed optimal estimation of the current navigation state through the weighted combination of the two navigation solutions. Simulation results show that the proposed SW-FG algorithm can significantly enhance the real-time navigation solution accuracy of the AUV-integrated navigation system. Compared with the regular FG filtering algorithm, the horizontal positioning accuracy of the proposed SW-FG algorithm is improved by 17.17%. Semi-physical experimental results verify the reliability and effectiveness of the proposed scheme, and the horizontal positioning accuracy is improved by 7.28%.

Keywords

Autonomous underwater vehicle integrated navigation system sliding window-factor graph multi-source information fusion

Introduction

In recent years, with the increasing variety and number of underwater navigation sensors, how to effectively integrate the measurement information provided by various navigation sensors to further improve the underwater real-time navigation accuracy of autonomous underwater vehicle (AUV) has become a focus of attention both domestically and internationally (Liu et al., 2024; Sánchez et al., 2020). However, due to the complex underwater working environment and cost constraints of AUV, the small amount of available data, the poor quality of information, the limited solving time, and the short-term failure of available auxiliary sensing equipment/system will restrict the improvement of navigation accuracy in the process of real-time multi-source information fusion (Paull et al., 2014; Zhang et al., 2023). In response to the real-time navigation problem of multi-source information fusion, the moving time window is commonly used as a fixed interval to add the latest observation data to the real-time navigation solution in industry, to optimally smooth the navigation state within the window. Du et al. (2021) combined the sliding window technology with the state transformation extended Kalman filter to enhance the real-time navigation and positioning accuracy of the vehicle Strapdown Inertial Navigation System (SINS)/odometer-integrated navigation system. Xu et al. (2019) used a sliding window to update the mean vector and covariance matrix of sub-system observation in real-time to solve the problem of the time varying characteristics of the subsystems’ observed noise.

In recent years, the factor graph (FG) has emerged as an effective and versatile modeling method for multi-source information fusion in navigation systems. It has been verified in previous research work that the multi-source information fusion algorithm based on the FG model framework can accomplish the fusion of multiple asynchronous and heterogeneous information sources, as well as system reconstruction, through the simple addition or exclusion of associated factor nodes and edges, and it also boasts a plug and play capability (Kschischang et al., 2001; Loeliger et al., 2007). Moreover, the FG can avoid the accuracy loss of caused by asynchronous and heterogeneous navigation information sources in the multi-source information fusion process of AUV underwater navigation system, and it can provide higher real-time navigation accuracy than Kalman filter algorithm (Ma et al., 2019). The historical states in the FG model framework can also be estimated, which has the ability of post hoc data smoothing (Ma et al., 2019, 2020, 2021) Therefore, we can try to determine a reasonable time span using a sliding window, and select the starting and ending time of the estimated object and historical observation information source involved in navigation solution (Fan et al., 2020; Liu et al., 2021), to explore a better multi-source real-time fusion localization algorithm. The sliding window algorithm based on FG filtering can certainly provide more accurate real-time navigation results than the sliding window algorithm based on Kalman filtering. And the fixed width of the time window can improve the accuracy of navigation solution while occupying the least computer resources and space. At present, there are few research results of multi-source fusion localization combining FG and sliding window algorithm, and the existing methods have been unable to meet the needs of high-performance navigation and positioning.

Therefore, to overcome the limitations of existing methods, an AUV multi-source information fusion algorithm based on sliding window-factor graph (SW-FG) is proposed in this paper. By adding sliding window to the optimization range of the FG model, the algorithm further improves the real-time navigation and positioning accuracy. The “FG model of AUV-integrated navigation system” section constructs the FG model of AUV-integrated navigation system using FG theory. The “Multi-source information fusion algorithm based on SW-FG” section presents the multi-source information fusion algorithm based on SW-FG. The “Message passing and updating in SW-FG model” section introduces the message passing and updating in the SW-FG model. The “Experiments” section presents experiments. The “Conclusion” section provides conclusion.

FG model of AUV-integrated navigation system

In this paper, SINS, doppler velocity log (DVL), terrain-aided navigation (TAN), magnetic compass pilot (MCP), and depth meter (DM) are used as navigation equipment to achieve high-precision navigation and positioning for AUV-integrated navigation system. In addition, DM provides depth information directly. The FG model of the AUV-integrated inertial navigation system is shown in Figure 1. In this model, the hollow circles represent the variable nodes, while the black circles represent the factor nodes. The attitude error, velocity error, position error, gyroscope, and accelerometer constant drift and bias in each axis for the inertial sensor are selected as navigation state variables, which correspond to variable nodes $x$ in the FG model as follows:

x = {[\begin{matrix} {(ϕ^{n})}^{T} & {(δ v^{n})}^{T} & {(δ p^{n})}^{T} & {(ε_{b})}^{T} & {(\nabla_{b})}^{T} \end{matrix}]}^{T}

(1)

Figure 1.

Factor graph model of AUV-integrated inertial navigation system.

The measurement information obtained by inertial measurement unit (IMU), DVL, MCP, and TAN navigation sensors corresponds to the factor nodes $f^{IMU}$ , $f^{DVL}$ , $f^{MCP},$ and $f^{TAN}$ in the FG model, respectively. The representation of the corresponding factor node can be listed according to different process models and measurement models (Ma et al., 2019, 2020, 2021).

Prior factor node

A prior factor node represents a model containing prior information, which is generally known. Define and add a unary prior factor associated with a variable at the initial moment as follows:

f^{prior} (υ) \dot{=} d (υ)

(2)

For Gaussian distributions, the prior factor can be further given in terms of mean $μ_{υ}$ and variance $\sum_{υ}$ :

f^{prior} (υ) \dot{=} d (υ) = \exp (- \frac{1}{2} ∥ υ - μ_{υ} ∥_{\sum_{υ}}^{2})

(3)

IMU factor node

Define and add a new IMU factor node after receiving the IMU measurement information $z_{k}^{IMU} \dot{=} {f^{b}, ω^{b}}$ at $t_{k}$ time as follows:

f^{IMU} (x_{k + 1}, x_{k}) \dot{=} d (x_{k + 1} - h (x_{k}, z_{k}^{IMU}))

(4)

For Gaussian distributions, the IMU factor can be further given as follows:

f^{IMU} (x_{k + 1}, x_{k}) = \exp (- \frac{1}{2} ∥ {\hat{x}}_{k + 1} - h (x_{k}, z_{k}^{IMU}) ∥_{Λ_{i}}^{2})

(5)

An IMU factor connecting two variable nodes $x_{k}$ and $x_{k + 1}$ at two consecutive time instances $t_{k}$ and $t_{k + 1}$ . According to the IMU measurement information $z_{k}^{IMU} \dot{=} {f^{b}, ω^{b}}$ and the current navigation state $x_{k}$ , the navigation state $x_{k + 1}$ of the next moment is predicted, so as to update navigation states and the variable and expand factor nodes that is time propagation $x_{k + 1} = h (x_{k}, z_{k}^{IMU})$ .

Based on the objective fact that the measurement update frequency is always lower than the time update frequency in the inertial-based integrated navigation system, several consecutive conventional IMU factor nodes between two adjacent measurement information are equivalent to an IMU factor node according to the parameter change after filtering and the characteristics of AUV stable motion. This accelerates the SINS error estimation process between two adjacent measurement information and optimizes the FG model. The state transition matrix $Φ_{k / k - 1}$ and error covariance matrix $P_{k / k - 1}$ are not updated in a measurement update cycle. The system matrix is solved only according to the variation of SINS parameters, and the accumulation is carried out. At the arrival time of the measurement information, the state transition matrix is calculated as follows:

\tilde{F} = \frac{1}{200} \sum_{k = 1}^{200} F_{k}, Φ = I + \tilde{F} \cdot T

(6)

where $T$ is the measurement update period.

DVL factor node

The DVL measurement equation can be expressed as $z_{j}^{DVL} = h^{DVL} (x_{j}) + n_{j}^{DVL}$ ; where $h^{DVL} (\cdot)$ is the measurement function, $n^{DVL}$ is the DVL measurement noise. After receiving the DVL measurement information at $t_{j}$ time, define and add a new DVL factor node as follows:

f^{DVL} (x_{j}) \dot{=} d (z_{j}^{DVL} - h^{DVL} ({\hat{x}}_{j})) = \exp (- \frac{1}{2} ∥ z_{j}^{DVL} - h^{DVL} ({\hat{x}}_{j}) ∥_{Σ_{j}}^{2})

(7)

where $Σ_{j}$ is the DVL measurement noise covariance matrix. As can be seen from the cost function of the DVL factor node, it is only related to the state variable $x_{j}$ of the current moment, and has nothing to do with the state variable of the adjacent moment. Therefore, there is only one connection edge between the factor node $f^{DVL}$ and the variable node $x_{j}$ . Similarly, after receiving MCP, TAN, and other measurement information at other times, define and add new factor nodes, which can refer to the representation of DVL factor nodes.

Based on the above analysis, in the FG model, the SINS error states are abstracted as variable nodes, while the asynchronous heterogeneous navigation measurement information are abstracted as factor nodes. The IMU factor runs through the whole FG model framework. When new measurement information is detected, it is abstracted as factor nodes and added to the FG model. When the measurement information is not available, the corresponding factor nodes are denied to be added to the FG model without affecting the system structure. This means that the FG model framework can provide a plug and play capability by simply adding or refraining from adding the associated factor nodes from navigation sensors with strong flexibility and extensibility. With the passage of time, variable nodes and factor nodes will continue to increase, and the FG model corresponding to AUV-integrated navigation system will continue to expand.

Multi-source information fusion algorithm based on SW-FG

The FG model provides a structural foundation for sliding window, while the message passing and updating mechanism offers theory support for sliding window. The multi-source information fusion algorithm based on SW-FG is researched in this section.

The FG model is a typical time series model. In this section, the sliding time window algorithm is used to optimize it. The size defined by the sliding window boundary is called the window length, and the amplitude of the boundary change is called the sliding step size. The window length is generally set artificially, simply set a hypothesis or experience value, lack of a certain theoretical basis (Crouch et al., 2013). When the window length is fixed, the shorter the step size, the higher the algorithm operation efficiency, so that, the step size is also determined according to the specific situation. The sliding rules of sliding window include fixed step size, variable step size and fixed window length, variable window length (Roumeliotis and Bekey, 2002). Smoothing in real-time navigation essentially uses the historical information to constrain the current information. When the motion state changes, the correlation between the historical information and the current information is small. Generally, the sliding rule with fixed step size and variable window length is used to ensure that the algorithm can adjust the output rate of the estimated value adaptively according to the dynamic performance of the carrier. However, in the application scenario of AUV multi-source fusion positioning in this paper, the sliding rules of fixed step length and fixed window length are selected because the dynamic changes of signal and carrier motion state are relatively small, and the correlation between historical information and current information is large.

The principle diagram of AUV multi-source fusion localization algorithm based on SW-FG is shown in Figure 2. The length of the sliding window is fixed as $w$ and the step size is fixed as $d$ . The sliding window is updated in units of a fixed window constantly, so that, the variable nodes in the window are optimized for calculation. In this section, the range of the sliding window is based on the number of variable nodes, and the sliding window contains only a subset of all variable nodes and factor nodes.

Figure 2.

The principle diagram of AUV multi-source fusion localization algorithm based on sliding window-factor graph.

An example with a window length of three is analyzed as follows. The left boundary of sliding window 1 is $t_{k - 1}$ time, and the right boundary is $t_{k + 1}$ time, which involves three variable nodes: variable nodes $x_{k - 1}$ , $x_{k}$ , $x_{k + 1}$ at $t_{k - 1}$ , $t_{k}$ , $t_{k + 1}$ time, and five factor nodes: two factor nodes $f^{IMU}$ introduced by IMU measurement information, and factor nodes $f^{DVL}$ , $f^{MCP}$ , and $f^{TAN}$ introduced by the measurement information at $t_{k - 1}$ , $t_{k}$ , $t_{k + 1}$ time. At the next moment, the window slides 1 to the right, where the left boundary is $t_{k}$ time, and the right boundary is $t_{k + 2}$ time, which involves three variable nodes: variable nodes $x_{k}$ , $x_{k + 1}$ , $x_{k + 2}$ at $t_{k}$ , $t_{k + 1}$ , $t_{k + 2}$ time, and five factor nodes: two factor nodes $f^{IMU}$ introduced by IMU measurement information, $f^{TAN}$ factor node and two $f^{MCP}$ factor nodes introduced by the measurement information at $t_{k}$ , $t_{k + 1}$ , $t_{k + 2}$ time; the next moment, and so on.

The windowed range in the SW-FG algorithm is the range of the state variable nodes estimated by the message passing algorithm. The above optimal estimation problem of AUV multi-source information fusion in the sliding window can be expressed as follows:

Sliding window 1:

\begin{matrix} {\hat{x}}_{i} = \underset{x_{i}}{argmin} {\sum_{i = k - 1}^{k + 1} ∥ f (x_{i - 1}, z_{i}^{IMU}) - x_{i} ∥_{Λ_{i}}^{2} + ∥ h (x_{k - 1}) \\ - z_{k - 1}^{DVL} ∥_{Σ_{k - 1}^{DVL}}^{2} + ∥ h (x_{k}) - z_{k}^{MCP} ∥_{Σ_{k}^{MCP}}^{2} \\ + ∥ h (x_{k + 1}) - z_{k + 1}^{TAN} Σ_{k + 1}^{TAN} ∥^{2}} \end{matrix}

(8)

Sliding window 2:

\begin{matrix} {\hat{x}}_{i} = \underset{x_{i}}{argmin} {\sum_{i = k}^{k + 2} ∥ f (x_{i - 1}, z_{i}^{IMU}) - x_{i} ∥_{Λ_{i}}^{2} + ∥ h (x_{k}) \\ - z_{k}^{MCP} ∥_{Σ_{k}^{MCP}}^{2} + ∥ h (x_{k + 1}) - z_{k + 1}^{TAN} ∥_{Σ_{k + 1}^{TAN}}^{2} \\ + ∥ h (x_{k + 2}) - z_{k + 2}^{MCP} ∥_{Σ_{k + 2}^{MCP}}^{2}} \end{matrix}

(9)

Sliding window 3:

\begin{matrix} {\hat{x}}_{i} = \underset{x_{i}}{argmin} {\sum_{i = k + 1}^{k + 3} ∥ f (x_{i - 1}, z_{i}^{IMU}) - x_{i} ∥_{Λ_{i}}^{2} + ∥ h (x_{k + 1}) \\ - z_{k + 1}^{TAN} ∥_{Σ_{k + 1}^{TAN}}^{2} + ∥ h (x_{k + 2}) - z_{k + 2}^{MCP} ∥_{Σ_{k + 2}^{MCP}}^{2} \\ + ∥ h (x_{k + 3}) - z_{k + 3}^{DVL} ∥_{Σ_{k + 3}^{DVL}}^{2}} \end{matrix}

(10)

In the FG model, the range of applying the message passing algorithm for state estimation can be done over the entire FG network. However, the traversal estimation method makes the calculation amount of the algorithm increase continuously during the operation. And the accuracy of the latest state variable estimation is not significantly improved by the earlier observations. Therefore, by windowing the optimization range of the FG model, that is, limiting the range of variable and factor nodes participating in the optimization can reduce the amount of computation and improve the efficiency.

Message passing and updating in SW-FG model

Edge probability calculation is the key in FG reasoning, which can be divided into two categories: incremental smoothing and message passing solution. The core idea is to transform probability problem into nonlinear least squares optimization problem. In the SW-FG algorithm, considering the real-time performance and the requirements of high-precision navigation, the bidirectional FG sum-product algorithm is used to pass and update messages in the FG model to obtain higher accuracy navigation solution.

The linearized state space error model of AUV-integrated navigation system is discretized as (Hosseini et al., 2023):

\begin{matrix} x_{k} = Φ_{k / k - 1} x_{k - 1} + G_{k - 1} w_{k}, Φ_{k / k - 1} = I_{n \times n} + F_{k / k - 1} T \\ + O (T^{2}) \approx I_{n \times n} + F_{k / k - 1} T \end{matrix}

(11)

z_{k} = H_{k} x_{k} + v_{k}

(12)

where $x_{k}$ is the error vector of navigation state at $t_{k}$ time; $Φ_{k / k - 1}$ is the state transition matrix from $t_{k - 1}$ time to $t_{k}$ time; $G_{k - 1}$ and $w_{k}$ are the noise input matrix and system noise, respectively; $z_{k}$ and $v_{k}$ denote the measurement vector obtained by some auxiliary sensors and the measurement noise, respectively.

According to the linear discrete state space equation of AUV-integrated navigation system and the cut-set independence principle of FG theory, a more detailed FG model is shown in Figure 3. In Figure 3, the black solid arrow represents the forward propagation direction of the FG, and the red dashed arrow represents the backward propagation direction.

Figure 3.

The detailed decomposition and bidirectional message passing and updating of factor graph.

All messages in the FG model are assumed to follow a multidimensional Gaussian probability density distribution. The sliding window interval is defined as $[W_{k - w}, W_{k}]$ , where $W_{k - w}$ is the starting data of the sliding window, and $W_{k}$ is the ending data of the sliding window. In the initial time period, the solution is not performed when the window range is not satisfied. When the size of the sliding window is satisfied, the forward message passing and backward message passing of the FG are carried out in the sliding window, and the navigation state variable at the current time is realized by weighted combination of the navigation state estimation results of the forward and backward message passing, and the solution result is as follows:

m_{x_{k}} = V_{x_{k}} ({\vec{V}}_{x_{k}}^{- 1} {\vec{m}}_{x_{k}} + {\overset{\leftarrow}{V}}_{x_{k}}^{- 1} {\overset{\leftarrow}{m}}_{x_{k}})

(13)

V_{x_{k}} = {({\vec{V}}_{x_{k}}^{- 1} + {\overset{\leftarrow}{V}}_{x_{k}}^{- 1})}^{- 1}

(14)

where the mean ${\vec{m}}_{x_{k}}$ and variance matrix ${\vec{V}}_{x_{k}}$ represent the results of the forward message passing of the FG at the $t_{k}$ time; the mean ${\overset{\leftarrow}{m}}_{x_{k}}$ and variance matrix ${\overset{\leftarrow}{V}}_{x_{k}}$ represent the results of the backward message passing of the FG at the $t_{k}$ time; the mean $m_{x_{k}}$ and variance matrix $V_{x_{k}}$ represent the result of bidirectional filtering smoothing of the FG at the $t_{k}$ time.

Since the navigation solution result at the current time uses all the available measurement information in the sliding window, the navigation solution is more accurate. When the window is out of range, the window slides forward by one step, and the variable node and factor node at the previous time should be used as the prior information of the new window.

Experiments

To verify the effectiveness and reliability of the proposed AUV multi-source information fusion algorithm based on SW-FG, the numerical simulation and the semi-physical test are carried out in this section. In the numerical simulation, the regular FG algorithm, the sliding window federated Kalman filtering (SW-FKF) algorithm, the SW-FG algorithm based on incremental smoothing (SW-FG-incremental smoothing) and the SW-FG algorithm based on the sum-product algorithm (SW-FG-sum-product) proposed in this paper are, respectively, used to fuse the data of each navigation information source, and the performance of each algorithm is quantitatively compared and analyzed. In the semi-physical test, the validity of the SW-FG algorithm is verified by the vehicle test data.

Simulation experiment

Simulation setup

The simulation experiment is carried out in MATLAB environment. IMU measurements were generated by the gyroscope and the accelerometer at 100 Hz. The gyroscope constant biases in each axis were all set as 0.02 $° / hour$ , and the random biases were white Gaussian noise with zero mean and a standard-deviation of 0.01 $(^{°} / hour) H z^{0.5}$ ; the accelerometer constant biases in each axis were all set as 100 $μ g$ , and the random biases were white Gaussian noise with zero mean and a standard deviation of 10 $(μ g / Hz)^{0.5}$ . Other auxiliary navigation sensors are set in Table 1. SINS and DM have strong anti-interference ability, so that, they are kept active during the entire simulation period.

Table 1.

Navigation sensor setup.

Sensors	Updatefrequency (Hz)	Measurementnoise	Short-timefailureinterval (s)
DVL	1	0.1 m/seconds	150–350 seconds
TAN	0.1	10 m	450–550 seconds
MCP	2	0.5°	800–850 seconds
DM	0.5	0.1 m	–

Simulation results

Based on the above simulation conditions, 100 Monte Carlo simulation tests are carried out on the MATLAB software platform. The error curve of each navigation parameter is the first simulation result. The sliding window size of SW-FKF, SW-FG-incremental smoothing, and SW-FG-sum-product proposed in this paper is set to 30. The regular FG algorithm is that the sliding window size is 1.

The error curves of attitude, velocity, and position obtained using the above four algorithms are shown in Figure 4. Among them, the blue dotted line, the red dotted line, the purple dotted line, and the green solid line, respectively, represent the regular FG, SW-FKF, SW-FG-incremental smoothing, and SW-FG-sum-product proposed in this paper. To quantitatively analyze and compare the influence of the four algorithms on navigation accuracy, the statistical average magnitude of error (AME) and root mean square error (RMSE) of each navigation parameter are shown in Table 2.

Figure 4.

Comparison of attitude, velocity, and position errors.

Table 2.

AME/RMSE statistics of navigation error parameter.

Navigation parameters	Regular FG	SW-FKF	SW-FG-incremental smoothing	SW-FG-sum-product
$ϕ_{x} / (')$	0.6776/1.5881	0.5253/1.6096	0.5009/0.6069	0.4944/0.6070/61.78%
$ϕ_{y} / (')$	0.6913/1.5488	0.4598/1.3859	0.5040/0.6870	0.5160/0.6868/55.65%
$ϕ_{z} / (')$	2.5146/2.9923	2.2025/2.5928	2.3358/2.7732	2.3659/2.7646/7.6%
$δ V_{N} / (m / s) δ V_{N} / (m / s)$	0.0211/0.0268	0.0116/0.0176	0.0139/0.0168	0.0138/0.0169/36.94%
$δ V_{E} / (m / s) δ V_{E} / (m / s)$	0.0242/0.0269	0.0098/0.0218	0.0140/0.0181	0.0142/0.0173/35.69%
$δ L / (m)$	1.2333/1.6992	0.5886/1.7755	1.0216/1.2231	1.0222/1.2379/27.15%
$δ λ / (m)$	1.3978/1.7860	0.9805/1.7160	1.3017/1.6296	1.2868/1.6240/9.1%

As can be seen from Figure 4, during the whole simulation period, the performance difference of the four algorithms is small, and all of them can maintain relatively high navigation accuracy, which verifies the effectiveness and reliability of the proposed algorithm in this paper. As the state of navigation information source changes, the four algorithms are affected to some extent. Compared with the real situation, the estimation of navigation state based on SW-FKF algorithm has a large delay, which is because the algorithm requires all the observed information to arrive before information fusion, while the regular FG, the SW-FG-incremental smoothing and the SW-FG-sum-product algorithms can respond to changes in the observed information more quickly. This inevitably leads to that the navigation and positioning accuracy based on the SW-FKF algorithm is lower than that based on the SW-FG algorithm when the sliding window length is the same.

The positioning accuracy and stability of each algorithm can be quantitatively analyzed based on the statistical results of AME and RMSE in Table 2. Compared with the SW-FKF, SW-FG-incremental smoothing and the SW-FG-sum-product algorithms, the AME and RMSE of the regular FG algorithm are the highest among the four algorithms, and the estimation accuracy of its output position, velocity and attitude is significantly lower than that of the other two fusion algorithms with SW-FG. The SW-FG-incremental smoothing and the SW-FG-sum-product algorithms have the same level of navigation accuracy and stability. However, the SW-FG-incremental smoothing algorithm needs to store more state information in the sliding window, so that, the SW-FG-sum-product algorithm proposed in this paper is actually superior to the SW-FG-incremental smoothing algorithm in terms of computation and storage space. Compared with the conventional FG algorithm, the navigation parameters of the SW-FG and product algorithm proposed in this paper are improved to the extent shown in the last column of Table 2, and the horizontal positioning accuracy is increased by 17.17%.

In summary, the AUV multi-source information fusion algorithm based on SW-FG proposed in this paper can effectively improve the real-time navigation accuracy of the AUV-integrated navigation system.

Semi-physical experiment

Experiment description

A vehicle test was carried out to verify the proposed AUV multi-source information fusion algorithm based on SW-FG. The raw navigation data of the attitude, velocity, and position information were collected by the loosely coupled SINS/GNSS-integrated system consisting of PHINS from IXBLUE Company and FlexPark6 GNSS receiver from NovAtel Company. The frequency of the IMU test prototype was 100 Hz. On the basis of the heading angle reference information, a Gaussian white noise with an amplitude of 0.5° is added to simulate the MCP measurement data, and its update frequency is set to 2 Hz. The velocity of PHINS in the carrier coordinate system is obtained using the reference attitude and velocity information, and the Gaussian white noise with an amplitude of 0.1 m/seconds is added to simulate the DVL measurement data, and its update frequency is set to 1 Hz. On the basis of the reference horizontal position information, a Gaussian white noise with an amplitude of 10 m was added to simulate TAN measurement data, and its update frequency was set to 0.1 Hz. Similarly, the amplitude and frequency of depth information are 0.1 m and 0.5 Hz, respectively. Considering the real-time navigation and positioning accuracy, calculation and algorithm complexity of the AUV-integrated navigation system, the sliding window length was set as the empirical value w = 20.

Experiment results

In this section, the regular SW-FG (w = 1) is introduced as a comparison method. The comparison of horizontal position of AUV underwater-integrated navigation vehicle test based on the SW-FG algorithm (w = 1 and 20) is shown in Figure 5. The comparison results of attitude, velocity, and position errors based on the SW-FG algorithm (w = 1 and 20) are shown in Figure 6. Comparisons of the AME and the RMSE of navigation parameters are shown in Table 3.

Figure 5.

Comparison of horizontal position of AUV underwater-integrated navigation vehicle test.

Figure 6.

Comparison of attitude, velocity, and position errors of vehicle test.

Table 3.

Statistical AME/RMSE of navigation parameters.

Navigation parameters	w = 20 (AME/RMSE)	w = 1 (AME/RMSE)
$ϕ_{x} / (')$	7.4797/8.8331	7.5413/8.9025
$ϕ_{y} / (')$	6.3486/7.8157	6.3593/7.8610
$ϕ_{z} / (')$	2.0821/2.3554	2.1725/2.5517
$δ V_{N} / (m / s)$	0.0165/0.0218	0.0207/0.0275
$δ V_{E} / (m / s) δ V_{E} / (m / s)$	0.0146/0.0186	0.0184/0.0246
$Δ X / (m)$	0.9048/1.1577	1.0983/1.5496
$Δ Y / (m)$	2.3104/3.0014	2.4330/3.1037

As seen in Figure 6 and Table 3, the AUV multi-source fusion localization algorithm based on SW-FG proposed in this paper can obtain higher real-time navigation accuracy than the real-time FG algorithm, and the navigation parameters error is kept in a small range. Compared with the real-time FG algorithm, the horizontal velocity accuracy of the SW-FG algorithm is increased by 22.33%, and the horizontal positioning accuracy is increased by 7.26%.

Therefore, the semi-physical on vehicle test verifies the effectiveness and reliability of AUV multi-source fusion localization algorithm based on SW-FG proposed in this paper.

Conclusion

The AUV multi-source information fusion algorithm based on SW-FG was proposed in this paper. The algorithm windows the optimization range of the FG model of AUV-integrated navigation system, and updates by sliding in units of a fixed window continuously. In the sliding window, the forward and backward message passing of the FG are carried out, and the smooth optimal estimation of the current navigation state estimation is obtained by weighting the combination of the two navigation state estimation. The simulation results show that the proposed algorithm in this paper can improve the real-time navigation accuracy effectively. The effectiveness and reliability of the proposed algorithm are verified by the semi-physical vehicle test.

However, the general relationship between the short-time failure interval of navigation information source, the sliding window and the positioning error divergence speed is not further explored in this paper. Therefore, one of the research directions of future work is to explore the general selection criteria of the step size and window length of the sliding window. In addition, based on the algorithm proposed in this paper, constructing the cyclic FG model framework to suppress the random oscillation error caused by measurement information is another future research directions. The research results are expected to provide a feasible solution of multi-source fusion localization with robust performance for AUV-integrated navigation system.

Footnotes

Declaration of conflicting interests

The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding

The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This work was supported by the Foundation of Key Laboratory of Micro-Inertial Instrument and Advanced Navigation Technology, Ministry of Education, China (Grant No. SEU-MIAN-202402), the Fundamental Science (Natural Science) Foundation of the Jiangsu Higher Education Institutions of China (Grant No. 24KJB590003), the Wuxi University Research Start-up Fund for Introduced Talents (Grant No. 2023r016), the Subject of Educational Informatization in Colleges and Universities in Jiangsu Province, China (Grant No. 2023JSETKT040), and the Zhejiang Province Natural Science Foundation of China (Grant No. LQ24F030012).

ORCID iDs

Xiao-shuang Ma

Chen-long Li

Data availability statement

Data sharing is not applicable to this article as no data sets were generated or analyzed during the current study.

References

Crouch

McGregor

Stubbs

(2013) Dynamic graphs in the sliding-window model. In: Proceedings of the 21st Annual European Symposium on Algorithms (ESA), Sophia Antipolis, 2–4 September, Vol. 8125, pp. 337–348.

Wang

, et al. (2021) A SINS/odometer sliding window Kalman filtering technique based on ST-EKF. Navigation and Control 20(3): 26–33.

Fan

Zhang

Hao

, et al. (2020) Cooperative positioning and error estimation algorithm based on factor graph. Systems Engineering and Electronics 43(2): 499–507.

Hosseini

Noei

Rostami

(2023) Integrated navigation system (INS/auxiliary sensor) based on adaptive robust Kalman filter with partial measurements. Transactions of the Institute of Measurement and Control 45(2): 316–330.

Kschischang

Frey

Loeliger

(2001) Factor graphs and the sum-product algorithm. IEEE Transactions on Information Theory 47(2): 498–519.

Liu

Wang

Feng

, et al. (2021) A new visual/inertial integrated navigation algorithm based on sliding-window factor graph optimization. The Journal of Navigation 74(2): 343–359.

Liu

Sun

, et al. (2024) Experimental analysis of deep-Sea AUV based on multi-sensor integrated navigation and positioning. Remote Sensing 16(01): 199.

Loeliger

Dauwels

, et al. (2007) The factor graph approach to model-based signal processing. Proceedings of the IEEE 95(6): 1295–1322.

Liu

, et al. (2020) A novel Rauch–Tung–Streibel smoothing scheme based on the factor graph for autonomous underwater vehicles. Review of Scientific Instruments 91(7): 075109.

10.

Liu

, et al. (2021) A post-processing algorithm based on factor graph optimization for autonomous underwater vehicle integrated navigation. Transactions of the Institute of Measurement and Control 45(4): 595–607.

11.

Liu

Zhang

, et al. (2019) AUV multi-sensor integrated navigation algorithm based on factor graph. Journal of Chinese Inertial Technology 27(4): 454–459.

12.

Paull

Saeedi

Seto

, et al. (2014) AUV navigation and localization: A review. IEEE Journal of Ocean Engineering 39(1): 131–149.

13.

Roumeliotis

Bekey

(2002) Distributed multi-robot localization. IEEE Transactions on Robotics and Automation 18(5): 781–795.

14.

Sánchez

Papaelias

Márquez

(2020) Autonomous underwater vehicles: Instrumentation and measurements. IEEE Instrumentation and Measurement Magazine 23(2): 105–114.

15.

Lian

Liu

(2019) Multi-source integrated navigation algorithm for iterated maximum posteriori estimation based on sliding-window factor graph. Acta Armamentarii 40(4): 807–819.

16.

Zhang

Liu

, et al. (2023) Autonomous underwater vehicle navigation: A review. Ocean Engineering 273: 113861.