FGO-ILNS: Tightly Coupled Multi-Sensor Integrated Navigation System Based on Factor Graph Optimization for Autonomous Underwater Vehicle

Jiangbo Song, Wanqing Li, Ruofan Liu, Xiangwei Zhu Jiangbo Song is with the School of System and Engineering, Sun Yat-sen University, Guangzhou 510006, China (e-mail: [email protected]). Wanqing Li is with the School of Aeronautics and Astronautics, Shenzhen Campus of Sun Yat-sen University, Shenzhen 518107, China(e-mail: [email protected]).Ruofan Liu is with the School of Information and Communication Engineering, Shenzhen Campus of Sun Yat-sen University, Shenzhen 518107, China (e-mail: [email protected]).Xiangwei Zhu is with the School of Information and Communication Engineering, Shenzhen Campus of Sun Yat-sen University, Shenzhen 518107, China, with Shenzhen Key Laboratory of Navigation and Communication Integration and with Southern Marine Science and Engineering Guangdong Laboratory (Zhuhai) (e-mail: [email protected]).

Abstract

Multi-sensor fusion is an effective way to enhance the positioning performance of autonomous underwater vehicles (AUVs). However, underwater multi-sensor fusion faces challenges such as heterogeneous frequency and dynamic availability of sensors. Traditional filter-based algorithms suffer from low accuracy and robustness when sensors become unavailable. The factor graph optimization (FGO) can enable multi-sensor plug-and-play despite data frequency. Therefore, we present an FGO-based strapdown inertial navigation system (SINS) and long baseline location (LBL) system tightly coupled navigation system (FGO-ILNS). Sensors such as Doppler velocity log (DVL), magnetic compass pilot (MCP), pressure sensor (PS), and global navigation satellite system (GNSS) can be tightly coupled with FGO-ILNS to satisfy different navigation scenarios. In this system, we propose a floating LBL slant range difference factor model tightly coupled with IMU preintegration factor to achieve unification of global position above and below water. Furthermore, to address the issue of sensor measurements not being synchronized with the LBL during fusion, we employ forward-backward IMU preintegration to construct sensor factors such as GNSS and DVL. Moreover, we utilize the marginalization method to reduce the computational load of factor graph optimization. Simulation and public KAIST dataset experiments have verified that, compared to filter-based algorithms like the extended Kalman filter and federal Kalman filter, as well as the state-of-the-art optimization-based algorithm ORB-SLAM3, our proposed FGO-ILNS leads in accuracy and robustness.

Index Terms:

SINS/LBL coupled, Underwater navigation, Factor graph optimization, Sensor fusion

I Introduction and Related Work

With the increase of human exploration activities in the ocean, autonomous underwater vehicles (AUVs) require better navigation and positioning capabilities to complete tasks [1]. Therefore, underwater multi-sensor fusion navigation and positioning technology has become a research hotspot. To cope with the challenging underwater navigation and positioning, the number and types of sensors installed on AUVs have increased, and more research has focused on how to effectively fuse data from various sensors.

Refer to caption — Figure 1: The positioning principle of floating LBL system.

Due to the problem of error accumulation in strapdown inertial navigation system (SINS), to reduce the cost of sensors, the combination of SINS and Doppler velocity log (DVL)[2] is generally used in AUV navigation. In previous studies, [3, 4] improved the Kalman filter (KF) for SINS/DVL integrated navigation. In [5], the authors added a pressure sensor (PS) to the combination of SINS/DVL to improve the accuracy of AUV elevation positioning. In [6], the authors fused SINS/DVL/magnetic compass pilot (MCP)/PS via federated Kalman filter (FKF), to constrain the speed, yaw, and depth of the strapdown inertial navigation. However, the above sensor combination algorithm with SINS/DVL as the core still has the problem of error accumulation during large-scale navigation, and the AUVs need to surface for global navigation satellite system (GNSS) calibration, seriously affecting AUV operational efficiency. The acoustic positioning system does not have the problem of error accumulation and is divided into the long baseline (LBL), short baseline (SBL), and ultra-short baseline (USBL) according to the different baselines. In [7, 8, 9], the researchers used the adaptive Kalman filter (AKF) or unscented Kalman filter (UKF) to combine SINS and USBL. However, compared with LBL, the positioning accuracy of USBL is relatively low. Moreover, the scope of LBL is very wide, and the baseline length can reach several kilometers to tens of kilometers. Therefore, it is very important to study the combination of SINS and LBL for the high-precision navigation of AUVs in large-scale environments [10, 11, 12, 13].

A lot of meaningful work in filter-based SINS/LBL coupled navigation algorithms has been done before. In [14, 15], the authors tightly integrated SINS and LBL through EKF to achieve high-precision positioning. In [16], the authors coupled SINS/LBL through an improved particle filter (PF). In our previous work [17], to solve the problem of height instability in SINS/LBL tightly coupled integrated navigation, we proposed an adaptive algorithm named AHKF. The above works all are based on traditional filter-based fusion methods including KF and its extended filter algorithms.

However, the information update frequency of different sensors is usually not synchronized, that is, the problem of inter-frequency heterogeneity. The information fusion mechanism of filter-based methods will only be executed when the measurement information of all sensors is received. Therefore, it will have a certain impact on the accuracy and real-time performance of the filter-based integrated navigation system. In addition, the working states of different sensors are uncertain, and the sensors may be unstable or fail in some scenarios. The filter-based methods will face problems such as information failure and system reconstruction after the information source changes. For the problem of multi-sensor data with different frequencies and heterogeneity, the data fusion algorithm based on factor graph optimization (FGO) has great advantages [18].

The factor graph method was first used in the coding field. As a probabilistic graph model, it has gradually gained attention in other fields such as artificial intelligence and signal processing. Optimization methods based on factor graphs are also widely used in simultaneous localization and mapping (SLAM) problems [19, 20, 21]. The FGO also provides a new idea for the information fusion of the navigation system [22]. In [23], FGO was used for the combination of GNSS/INS; in [24], a camera, inertial measurement unit (IMU), GPS, magnetometer, and barometer were fused within the framework of FGO to obtain a large-scale positioning solution. In terms of underwater SLAM based on FGO [25]. The current research mainly focuses on the local small-scale environment, lacking global positioning information [26, 27]. The underwater large-scale SLAM problem remains an open research [28]. In terms of FGO-based underwater positioning research, in [29], the authors integrated SINS, USBL, and DVL in the FGO framework; In [30], the authors utilized the factor graph to optimize the fusion of IMU, DVL, terrain-assisted navigation (TAN), MCP and PS. However, further investigation is required to develop robust and efficient solutions for this complex problem. [29, 30] are similar to our work, but compared to them, our work has the following advantages:

1.

The inertial information is tightly coupled with the original acoustic observation information. Under the framework of factor graph optimization, we integrate LBL slant distance information and IMU preintegration information to obtain more robust and accurate positioning. In contrast, the literature [29, 30] uses loose coupling for data fusion.
2.

Higher precision acoustic positioning system. Compared to the USBL[29] and TAN [30] technologies, the LBL system used in our paper has a wider measurement range and higher positioning accuracy. It is more suitable for tasks that require high-precision underwater positioning.
3.

Real-time unification of time and space above and below water.The USBL requires the installation of an acoustic transducer on the seabed and prior measurement of its absolute position. The TAN requires prior measurement of the bottom map and the absolute position of the corresponding fingerprint library. The positioning accuracy of TAN is generally in the hundreds of meters. In contrast, the floating LBL and GNSS used in our paper ensure real-time synchronization of accurate position information for the acoustic buoy above and below water.
4.

Smaller amount of computation. We use marginalization technology to optimize the state variables within the sliding window, making full use of historical observation information, controlling the size of the factor graph, and reducing computational load. In contrast, [29, 30] does not use a sliding window mechanism. Otherwise, when AUVs navigate large-scale environments, their computational load increases sharply, resulting in a burden on computing resources and difficulty in guaranteeing real-time performance.

Therefore, the SINS/LBL tightly coupled navigation algorithm based on FGO has important research significance for AUV large-scale and high-precision navigation.

To enable underwater large-scale navigation and positioning more accurate and robust, we propose a factor graph optimization-based SINS/LBL tightly coupled navigation system called FGO-ILNS, which can be compatible with more sensors for fusion. As shown in Fig. 1, to save the cost of LBL deployment, this paper adopts the floating LBL system [31]. In addition, to address the issue of time offsets between the measurements of other sensors and LBL measurements, we use forward-backward IMU preintegration to assist in constructing sensor factors. This not only achieves tight coupling between IMU and other sensors but also makes full use of sensor measurements for optimal estimation. Moreover, FGO-ILNS adopts the sliding window method to balance the computational load and accuracy of the system to a certain extent. Therefore, in the underwater large-scale navigation scenario, the proposed FGO-ILNS provides a reference for solving the problems of different frequency heterogeneity and system robustness of AUV sensors. Our main contributions are as follows:

•

A SINS/LBL tightly coupled navigation system based on factor graph optimization is proposed for underwater positioning. The original acoustic LBL slant range is tightly coupled IMU reintegration. Moreover, the system can be flexibly expanded to incorporate different sensors for navigation and positioning, adapting to various challenging underwater environments.
•

Considering the time offset between sensor measurements, we propose a sensor factor modeling method using forward and backward IMU preintegration, and derive the residual models of DVL, MCP, PS, and GNSS.
•

Our approach utilizes a sliding window method to reduce the computational load of FGO. Experiments prove that our approach can effectively balance computational efficiency and accuracy, making it well-suited for real-time applications.
•

Different simulation experiments and public KAIST datasets experiments demonstrate the superiority of the proposed FGO-ILNS in accuracy and robustness when compared with filtering-based methods and the state-of-the-art (SOTA) optimization-based algorithm ORB-SLAM3.

The remainder of this paper is organized as follows. Section II introduces the system overview of FGO-ILNS. Details of the proposed FGO-ILNS are presented in Section III. Experiments in Section IV perform the comparisons and analyses between different integrated algorithms. Finally, Section V summarizes the conclusions of this work and presents an outlook for future work.

II System Overview

The proposed FGO-ILNS is mainly divided into three modules: the sensor module, the preprocessing module, and the graph optimization module. Fig. 2 shows the schematic of the FGO-ILNS system.

II-1 Sensors module

The data input interface of the sensors, mainly including IMU, LBL, GNSS, DVL, MCP, and PS.

II-2 Preprocessing module

Preprocess the navigation data of the sensors, and construct the relevant factors according to the sensor measurements time offset. Based on the time interval of LBL measurement values, we carry out IMU preintegration and employ the Position Focused (PF) algorithm [32] to construct LBL factor f_LBL and IMU preintegration factor f_IMU. Given that the available times for GNSS, DVL, MCP, and PS measurement values have a time offset with LBL measurement values, we conduct a time offset detection. Subsequently, we utilize the Forward-Backward Preintegration Focused (FBPF) algorithm to construct corresponding sensor factors f_GNSS, f_DVL, f_MCP, f_PS.

II-3 Optimization module

At different moments, when the sensor measurement values are available, add the factors corresponding to the sensors to the factor graph. Then, within the sliding window, construct an optimization problem, perform optimization processing, and finally obtain the optimal state estimation.

III Methodology

In this section, we mainly introduce the methodology of the proposed FGO-ILNS. We first give the notation and definitions. Then, the principles of FGO are conducted including a detailed construction process of related sensor factors. In particular, we derive the LBL slant range difference factor, and using forward-backward IMU preintegration derive factors for GNSS, DVL, MCP, and PS. Finally, we describe the marginalization method and the sliding window mechanism of FGO-ILNS.

III-A Notation and Definitions

We equip the AUV with SINS, LBL, DVL, MCP, PS, and GNSS. Among them, GNSS provides global positioning information for AUVs before entering the water and when they surface, and LBL provides global positioning information for AUVs within the range of acoustic buoy.

Our system adopts a sliding window factor graph optimization approach and the estimated states $X$ inside the window can be summarized as:

X=[{x_{0}},{x_{1}},\cdots{x_{n}}]

(1)

{x_{i}}=[{p_{i}},{v_{i}},{q_{i}},{\nabla_{a}},{\nabla_{g}}],i\in[0,n]

(2)

where $x_{i}$ is the SINS states at time epoch, including position p, velocity v, orientation q, accelerometer bias $\nabla_{a}$ , gyroscope bias $\nabla_{g}$ , and the sliding window size $n$ .

Defining the state variable $x_{i}$ at the time as a variable node, including the position, velocity, attitude angle, and inertial instrument error state quantity; The measurement information of IMU, SINS, DVL, MCP, PS, GNSS navigation sensors as corresponding factor nodes f_IMU, f_LBL, f_DVL, f_MCP, f_PS, f_GNSS. Note that the IMU in Fig. 3 is the measurement unit of the SINS.

Fig. 3(c) shows the factor graph of our system. The initial position of the system is obtained according to GNSS, and a priori factor is constructed by the IMU factor node at the initial moment, as Fig. 3(a) shows. The states are updated through IMU preintegration as Fig. 3(b) shows, and detecting whether there are measurement data from other sensors at the same time. Also, note that the measurement data of the sensor can be updated synchronously or asynchronously. As we marginalize the factors that are moved out of the sliding window, the old factors will participate in the subsequent optimization process as new prior factors, as Fig. 3(c) shows. In the following, we will discuss the FGO algorithm and each factor in detail.

III-B Factor Graph Optimization Algorithm

Factor graph is one of the probabilistic graphical models. Others, such as Bayesian networks and Markov random fields, are well-known in the statistical modeling and machine learning literature[33]. They provide a powerful abstraction to gain insight into a specific reasoning problem, making it easier to think about and design solutions and to perform the actual reasoning by writing modular, flexible software [34, 35]. Next, we will introduce the application of factor graphs in navigation and positioning.

As a bipartite graph model, the factor graph can be expressed as $G=(F,X,E)$ , which describes the joint probability distribution of random variables. The nodes are divided into factor nodes ${\ f}_{i}\in F$ and variable nodes $x_{j}\in X$ . When the state variable node $x_{i}$ is related to the factor node $\ f_{i}$ , there is a connecting edge between them, denoted as $e_{\text{ij}}\in E$ . Therefore, the definition of a factor graph G can be described as a factorization of $f(X)$ :

f(X)=\mathop{\prod}\limits_{i}{f_{i}}({x_{i}})

(3)

A factor $f_{i}(x_{i})$ can be described by a residual function:

{f_{i}}({x_{i}})={\bf{r}}({z_{i}},{x_{i}})

(4)

where $\bf{r}(\bullet)$ is the corresponding residual function (also called the cost function), and $z_{i}$ is the measurement value.

We introduce factor graphs in the AUV multi-sensor integrated navigation problem. The navigation state of the AUV at time $t_{i}$ is described as a variable node $x_{i}\$ , and a set $X_{k}=\left\{x_{i}\right\}_{i=1}^{k}$ is defined at time $t_{k}$ that contains state variable information at all times in the past. Define the set $Z_{k}=\left\{z_{i}\right\}_{i=1}^{k}$ , which means all measurement information at time $t_{k}$ . $z_{i}\$ represents the actual measurement information obtained by different navigation sensors at time $t_{i}\$ . Therefore, the joint Probability Distribution Function (PDF) of all state variables and measurements can be expressed as:

\displaystyle P\left({X_{k}\mid Z_{k}}\right)\propto\ P\left({x_{0}}\right)\prod_{i=1}^{k}P\left({x_{i}\mid x_{i-1},z_{i}^{imu}}\right)\prod_{j=1}^{k}P\left({z_{j}\mid X_{i}^{j}}\right)

(5)

where $P(x_{0})$ is the prior information of all variables at the initial moment; $P\left(z_{j}\mid X_{i}^{j}\right)\text{\ \ }$ represents the measurement model, $X_{i}^{j}\subseteq X_{i}$ is a set of variable nodes; $P\left(x_{i}\mid x_{i-1},z_{i}^{\text{IMU}}\right)$ represents the process model, where is measurement information of IMU. In the joint probability density function $P\left(X_{k}\ \right|Z_{k})$ , each factor represents an independent term, namely:

P({X_{k}}|{Z_{k}})\propto{\prod_{i}}{f_{i}}(X_{k}^{i}),X_{k}^{i}\subseteq{X_{k}}

(6)

Using the available measurement information at all historical moments to calculate the optimal estimates of all state variables, we can use the maximum a posteriori probability (MAP) density to transform the AUV multi-sensor information problem into an equivalent nonlinear optimization problem. Taking the minimum value of the negative logarithm of (5) can obtain the MAP of all state variables. In our system, it can be described as:

$\displaystyle X_{k}^{*}$	$\displaystyle=\arg{\max_{{X_{k}}}}P({X_{k}}\|{Z_{k}})=\arg{\min_{{X_{k}}}}\{-\log P({X_{k}}\|{Z_{k}})\}$	(7)
	$\displaystyle=\arg{\min_{X}}$
	$\displaystyle\left\{{\begin{array}[]{*{20}{c}}{\left\\|{{{\bf{r}}_{prior}}-{{\bf{{\rm H}}}_{prior}}X}\right\\|^{2}}\\ {+\sum\limits_{i\in I}{\left\\|{{{\bf{r}}_{imu}}(\hat{z}_{i}^{imu},X)}\right\\|_{{\Lambda_{i}}}^{2}}+\sum\limits_{i\in L}{\left\\|{{{\bf{r}}_{lbl}}(\hat{z}_{l}^{lbl},X)}\right\\|_{{\Lambda_{l}}}^{2}}}\\ {+\sum\limits_{i\in G}{\left\\|{{{\bf{r}}_{lbl}}(\hat{z}_{g}^{gnss},X)}\right\\|_{{\Lambda_{g}}}^{2}+\sum\limits_{i\in D}{\left\\|{{{\bf{r}}_{dvl}}(\hat{z}_{d}^{dvl},X)}\right\\|_{{\Lambda_{d}}}^{2}}}}\\ {+\sum\limits_{i\in M}{\left\\|{{{\bf{r}}_{mcp}}(\hat{z}_{m}^{mcp},X)}\right\\|_{{\Lambda_{m}}}^{2}}+\sum\limits_{i\in P}{\left\\|{{{\bf{r}}_{ps}}(\hat{z}_{p}^{ps},X)}\right\\|_{{\Lambda_{p}}}^{2}}}\end{array}}\right\}$

where the operator $||\cdot||$ represents the squared Mahalanobis distance; $\Lambda$ is the noise covariance matrix. Equation (7) represents a least squares problem, which can be solved via nonlinear optimization theory. By adjusting the variable $X_{k}$ to minimize the objective function, the optimal estimated $X_{k}^{*}$ is obtained. Next, the specific sensor factor will be introduced.

III-C Sensors Factors

The optimal state estimation of FGO-ILNS can be represented by (7), which minimizes the sum of residuals constructed from the sensors. The optimal state estimation can be expressed visually as a factor graph, as shown in Fig. 3. In this subsection, related sensor factors are described in detail, especially the localization principle of the LBL system and the construction process of the slant range factor. We considered the time offset between the measurements of other sensors and LBL and used the FBPF method to derive and construct factors for GNSS, DVL, etc.

III-C1 LBL positioning principle and slant-range factor

When the slant-range measurement information of LBL is received at time $t_{n}$ , the LBL slant-range factor is constructed and added to the factor graph. The LBL measurement equation can be expressed as:

z_{l}^{lbl}={h^{lbl}}({x_{l}})+{n^{lbl}}

(8)

where $z_{l}^{lbl}$ is slant-range measurement of LBL; $n^{\text{lbl}}$ is the measurement noise of the LBL, and $h^{\text{lbl}}(x_{n})$ is the measurement function. The LBL slant-range residual can be expressed as:

{{\bf{r}}_{lbl}}\buildrel\Delta\over{=}\left\|{z_{l}^{lbl}-{h^{lbl}}({{\hat{x}}_{l}})}\right\|_{{\Lambda_{l}}}^{2}

(9)

Next, we discuss the observations and nonlinear measurement equations in detail. As shown in Fig. 4, the AUV position SINS-based is $({x_{s}},{y_{s}},{z_{s}})$ ; the real AUV position is $({x},{y},{z})$ ; the positions of the reference acoustic buoy are $({x_{0}},{y_{0}},{z_{0}})$ ; the other acoustic buoys are $({x_{j}},{y_{j}},{z_{j}})$ . The slant-range difference based on SINS is expressed as:

	$\displaystyle\rho_{i}^{sins}=$	$\displaystyle\sqrt{{{({x_{s}}-{x_{i}})}^{2}}+{{({y_{s}}-{y_{i}})}^{2}}+{{({z_{s}}-{z_{i}})}^{2}}}-$		(10)
		$\displaystyle\sqrt{{{({x_{s}}-{x_{0}})}^{2}}+{{({y_{s}}-{y_{0}})}^{2}}+{{({z_{s}}-{z_{0}})}^{2}}}$		(10)

Then linearized by Taylor relative to

\begin{array}[]{c}\rho_{i}^{sins}={R_{i}}-{R_{0}}+{e_{ix}}\delta x+{e_{iy}}\delta y+{e_{iz}}\delta z\end{array}

(11)

$\displaystyle{e_{ix}}$	$\displaystyle=\frac{{\partial\rho_{i}^{sins}}}{{\partial x}}=\frac{{{x_{s}}-{x_{i}}}}{{{R_{i}}}}-\frac{{{x_{s}}-{x_{0}}}}{{{R_{0}}}}$	(12)
$\displaystyle{e_{iy}}$	$\displaystyle=\frac{{\partial\rho_{i}^{sins}}}{{\partial y}}=\frac{{{y_{s}}-{y_{i}}}}{{{R_{i}}}}-\frac{{{y_{s}}-{y_{0}}}}{{{R_{0}}}}$
$\displaystyle{e_{iz}}$	$\displaystyle=\frac{{\partial\rho_{i}^{sins}}}{{\partial z}}=\frac{{{z_{s}}-{z_{i}}}}{{{R_{i}}}}-\frac{{{z_{s}}-{z_{0}}}}{{{R_{0}}}}$

where $R_{i}$ represents the slant-range between the AUV and the acoustic buoy; $R_{0}$ represents the slant-range between the AUV and the reference acoustic buoy, which are:

\begin{array}[]{l}{R_{i}}=\sqrt{{{({x_{s}}-{x_{i}})}^{2}}+{{({y_{s}}-{y_{i}})}^{2}}+{{({z_{s}}-{z_{i}})}^{2}}}\\ {R_{0}}=\sqrt{{{({x_{s}}-{x_{0}})}^{2}}+{{({y_{s}}-{y_{0}})}^{2}}+{{({z_{s}}-{z_{0}})}^{2}}}\end{array}

(13)

The slant-range difference based on LBL is:

\rho_{i}^{lbl}={R_{i}}-{R_{0}}+\delta{R_{i}}+{\nu_{\delta{R_{i}}}}

(14)

where $\delta{R_{i}}$ indicates the error of the $\rho_{i}^{lbl}$ ; ${\nu_{\delta{R_{i}}}}$ is drive white noise. Knowing the measurement information of the acoustic slant distance, the position of the AUV can be obtained by the principle of three-ball intersection positioning. Therefore, the slant-range difference observations can be expressed as:

	$\displaystyle z_{n}^{lbl}$	$\displaystyle=\delta{\rho_{i}}=\rho_{i}^{sins}-\rho_{i}^{lbl}$		(15)
		$\displaystyle={e_{ix}}\delta x+{e_{iy}}\delta y+{e_{iz}}\delta z-\delta{R_{i}}-{\nu_{\delta{R_{i}}}}$		(15)

Note that inertial navigation measurements are recorded in geodetic coordinates. We need to transfer geodetic coordinates to rectangular coordinates. The replaced equation is:

$\displaystyle\delta x=$	$\displaystyle\delta h\cos L\cos\lambda-({R_{E}}+h)\sin L\cos\lambda\delta L$	(16)
	$\displaystyle-({R_{E}}+h)\cos L\sin\lambda\delta\lambda$
$\displaystyle\delta y=$	$\displaystyle\delta h\cos L\sin\lambda-({R_{E}}+h)\sin L\sin\lambda\delta L$
	$\displaystyle-({R_{E}}+h)\cos L\cos\lambda\delta\lambda$
$\displaystyle\delta z=$	$\displaystyle\delta h\sin L+[{R_{E}}(1-{e^{2}})+h]\cos L\delta L$

The measurement function can be expressed as:

{h^{lbl}}({x_{l}})=H\cdot{x_{l}}=\left[{\begin{array}[]{*{20}{c}}{{0_{3\times 6}}}&{{H_{1(3\times 3)}}}&{{0_{3\times 6}}}&{-{I_{3\times 3}}}\end{array}}\right]{x_{l}}

(17)

{H_{1}}=\left[{\begin{array}[]{*{20}{c}}{{a_{11}}}&{{a_{12}}}&{{a_{13}}}\\ {{a_{21}}}&{{a_{22}}}&{{a_{23}}}\\ {{a_{31}}}&{{a_{32}}}&{{a_{33}}}\end{array}}\right]

(18)

where $I$ is the unit vector. The expression of $H_{1}$ is as follows:

$\displaystyle{a_{i1}}=$	$\displaystyle-({R_{E}}+h)\sin L\cos\lambda{e_{ix}}-({R_{E}}+h)\sin L\sin\lambda{e_{iy}}$	(19)
	$\displaystyle+[{R_{E}}(1-{e^{2}})+h]\cos L{e_{iz}}$
$\displaystyle{a_{i2}}=$	$\displaystyle-({R_{E}}+h)\cos L\sin\lambda{e_{ix}}-({R_{E}}+h)\cos L\cos\lambda{e_{iy}}$
$\displaystyle{a_{i3}}=$	$\displaystyle\cos L\cos\lambda{e_{ix}}+\cos L\sin\lambda{e_{iy}}+\sin L{e_{iz}}$

where $i=(1,2,3)$ ; $e$ is the oblate heart rate of the ellipsoid, and $R_{E}$ is the radius of curvature in the vertical meridian plane; $L,\lambda,h$ stand for longitude, latitude, and altitude.

III-C2 IMU preintegration factor

The IMU measurements can be modeled as a combination of the true motion of the platform and various sources of noise and bias. The true motion includes the linear acceleration and angular velocity of the platform, while the noise and bias can include measurement errors, sensor drift, and other sources of uncertainty. The detailed theory of IMU preintegration can be referred to [36]. The IMU’s formulation which the Coriolis and centrifugal forces are neglected and are modeled as:

\begin{array}[]{l}{{{\bf{\hat{a}}}}_{t}}={{\bf{a}}_{t}}+{{\bf{b}}_{{a_{t}}}}+{\bf{R}}_{w}^{t}{{\bf{g}}^{w}}+{{\bf{n}}_{a}}\\ {{\hat{\omega}}_{t}}={\omega_{t}}+{{\bf{b}}_{{w_{t}}}}+{{\bf{n}}_{w}}\end{array}

(20)

where including acceleration bias ${\bf{b}}_{{a}}$ , gyroscope bias ${\bf{b}}_{{w}}$ , and additive noise ${\bf{n}}$ . The raw gyroscope ${{\hat{\omega}}_{t}}$ , accelerometer measurements ${{{\bf{\hat{a}}}}_{t}}$ . We assume that the additive noise in acceleration and gyroscope measurements is Gaussian white noise. Slowly varying biases associated with accelerometers and gyroscopes are modeled as random walks. The preintegration terms $\hat{\alpha}_{i+1}^{{b_{k}}},\hat{\beta}_{{b_{i+1}}}^{{b_{k}}},\hat{\gamma}_{{b_{i+1}}}^{{b_{k}}}$ at time interval $[t_{k},t_{k+1}]$ can be derived as:

$\displaystyle\hat{\alpha}_{i+1}^{{b_{k}}}$	$\displaystyle=\hat{\alpha}_{{b_{i}}}^{{b_{k}}}+\hat{\beta}_{{b_{i}}}^{{b_{k}}}\delta t_{i+1}^{i}+\frac{1}{2}R(\hat{\gamma}_{{b_{i}}}^{{b_{k}}})({{\hat{a}}_{i}}-{b_{{a_{i}}}}){(\delta t_{i+1}^{i})^{2}}$	(21)
$\displaystyle\hat{\beta}_{{b_{i+1}}}^{{b_{k}}}$	$\displaystyle=\hat{\beta}_{{b_{i}}}^{{b_{k}}}+R(\hat{\gamma}_{{b_{i}}}^{{b_{k}}})({{\hat{a}}_{i}}-{b_{{a_{i}}}})\delta t_{i+1}^{i}$
$\displaystyle\hat{\gamma}_{{b_{i+1}}}^{{b_{k}}}$	$\displaystyle=\hat{\gamma}_{{b_{i}}}^{{b_{k}}}\otimes\left[{\begin{array}[]{*{20}{c}}1\\ {\frac{1}{2}({{\hat{w}}_{i}}-{b_{{w_{i}}}})\delta t_{i+1}^{i}}\end{array}}\right]$

Therefore, we can define the inertial residuals as:

{\bf{r}}_{imu}^{k}=\left[{\begin{array}[]{*{20}{c}}{R_{w}^{{b_{k}}}(p_{{b_{k+1}}}^{w}-p_{{b_{k}}}^{w}-v_{{b_{k}}}^{w}{\rm{\Delta}}{t_{k}}+\frac{1}{2}{g^{w}}{\rm{\Delta}}t_{k}^{2})-\hat{\alpha}_{{b_{k+1}}}^{{b_{k}}}}\\ {R_{w}^{{b_{w}}}(v_{{b_{k+1}}}^{w}-v_{{b_{k}}}^{w}+{g^{w}}{\rm{\Delta}}{t_{k}})-\hat{\beta}_{{b_{k+1}}}^{{b_{k}}}}\\ {2{{\left[{{{(q_{{b_{k}}}^{w})}^{-1}}\otimes q_{{b_{k+1}}}^{w}\otimes{{(\hat{\gamma}_{{b_{k+1}}}^{{b_{k}}})}^{-1}}}\right]}_{xyz}}}\\ {{b_{{a_{k+1}}}}-{b_{{a_{k}}}}}\\ {{b_{{w_{k+1}}}}-{b_{{w_{k}}}}}\end{array}}\right]

(22)

III-C3 GNSS, DVL, MCP, and PS factor based on IMU FBPF

When other sensor observations are available, we add the corresponding factors to the factor graph to constrain the state variables. Considering that when LBL is available, the preintegration time interval is the time of adjacent LBL observations, as shown in Fig. 5(a). The factor corresponding time of other sensors does not strictly correspond to the time of IMU preintegration, and there will be a time delay, as shown in Fig. 5(b), (c), and (d). To connect the sensor factors with the nearby state variables, we propose a forward-backward IMU preintegration method to construct GNSS, DVL, MCP, and PS factors. Assuming that the available time of the sensor is ${t_{j}}\in[{t_{k}},{t_{k+1}}]$ , judge the time interval between ${t_{j}}$ and ${t_{k}},{t_{k+1}}$ , respectively.

If ${t_{j}}$ is close to $t_{k}$ , use forward IMU preintegration, and associate the sensor factor with $x_{k}$ . The corresponding sensor factors established according to the forward IMU preintegration algorithm are:

$\displaystyle{\bf{r}}_{{gnss_{j}}}^{k}$	$\displaystyle={{\hat{p}}_{gnss}}-(p_{{b_{k}}}^{\omega}+v_{{b_{k}}}^{w}\delta t_{j}^{k}-\frac{1}{2}{g^{w}}{(\delta t_{j}^{k})^{2}}+R_{{b_{k}}}^{w}\hat{\alpha}_{{b_{j}}}^{{b_{k}}})$	(23)
$\displaystyle{\bf{r}}_{dv{l_{j}}}^{k}$	$\displaystyle={{\hat{v}}_{dvl}}-(v_{{b_{k}}}^{w}-{g^{w}}\delta t_{j}^{k}+R_{{b_{k}}}^{w}\hat{\beta}_{{b_{j}}}^{{b_{k}}})$
$\displaystyle{\bf{r}}_{mc{p_{j}}}^{k}$	$\displaystyle=2{\left[{{{(q_{{b_{k}}}^{w})}^{-1}}\otimes{{\hat{q}}^{w}}_{mcp}\otimes{{(\hat{\gamma}_{{b_{j}}}^{{b_{k}}})}^{-1}}}\right]_{xyz}}$
$\displaystyle{\bf{r}}_{p{s_{j}}}^{k}$	$\displaystyle={{\hat{z}}_{ps}}-{e_{3}}(p_{{b_{k}}}^{\omega}+v_{{b_{k}}}^{w}\delta t_{j}^{k}-\frac{1}{2}{g^{w}}{(\delta t_{j}^{k})^{2}}+R_{{b_{k}}}^{w}\hat{\alpha}_{{b_{j}}}^{{b_{k}}})$

where $e_{3}=[0,0,1]$ . If ${t_{j}}$ is close to ${t_{k+1}}$ , the backward IMU preintegration method is used to construct the corresponding factor. To associate the sensor factor with $x_{k+1}$ , the preintegration recursive formula of IMU is:

$\displaystyle\hat{\alpha}_{i}^{{b_{k+1}}}=$	$\displaystyle\hat{\alpha}_{i+1}^{{b_{k+1}}}+\hat{\beta}_{i+1}^{{b_{k+1}}}\delta t_{i+1}^{i}+\frac{1}{2}R(\hat{\gamma}_{i+1}^{{b_{k+1}}})(-({{\hat{a}}_{i+1}}$	(24)
	$\displaystyle-{b_{{a_{i+1}}}})){(\delta t_{i+1}^{i})^{2}}$
$\displaystyle\hat{\beta}_{i}^{{b_{k+1}}}=$	$\displaystyle\hat{\beta}_{i+1}^{{b_{k+1}}}+R(\hat{\gamma}_{i+1}^{{b_{k+1}}})(-({{\hat{a}}_{i+1}}-{b_{{a_{i+1}}}}))\delta t_{i+1}^{i}$
$\displaystyle\hat{\gamma}_{i+1}^{{b_{k+1}}}=$	$\displaystyle\hat{\gamma}_{i+1}^{{b_{k+1}}}\otimes\left[{\begin{array}[]{*{20}{c}}1\\ {\frac{1}{2}(-({{\hat{w}}_{i+1}}-{b_{{w_{i+1}}}}))\delta t_{i+1}^{i}}\end{array}}\right]$

Therefore, the corresponding sensor factors according to the backward IMU preintegration are:

$\displaystyle{\bf{r}}_{gns{s_{j}}}^{k+1}=$	$\displaystyle{{\hat{p}}_{gnss}}-(p_{{b_{k+1}}}^{w}-v_{{b_{k+1}}}^{w}\delta t_{k+1}^{j}-\frac{1}{2}{g^{w}}{(\delta t_{k+1}^{j})^{2}}$	(25)
	$\displaystyle-R_{{b_{k+1}}}^{w}{{\hat{\alpha}}^{{b_{k+1}}}}_{{b_{j}}}{\rm{}})$
$\displaystyle{\bf{r}}_{dv{l_{j}}}^{k+1}=$	$\displaystyle{{\hat{v}}_{dvl}}-(v_{{b_{k+1}}}^{w}-{g^{w}}\delta t_{k+1}^{j}-R_{{b_{k+1}}}^{w}\hat{\beta}_{{b_{j}}}^{{b_{k+1}}})$
$\displaystyle{\bf{r}}_{mc{p_{j}}}^{k+1}=$	$\displaystyle 2{\left[{{{(q_{{b_{k+1}}}^{w})}^{-1}}\otimes{{\hat{q}}^{w}}_{mcp}\otimes{{(-\hat{\gamma}_{{b_{j}}}^{{b_{k+1}}})}^{-1}}}\right]_{xyz}}$
$\displaystyle{\bf{r}}_{p{s_{j}}}^{k+1}=$	$\displaystyle{{\hat{z}}_{ps}}-{e_{3}}(p_{{b_{k+1}}}^{w}-v_{{b_{k+1}}}^{w}\delta t_{k+1}^{j}-\frac{1}{2}{g^{w}}{(\delta t_{k+1}^{j})^{2}}$
	$\displaystyle-R_{{b_{k+1}}}^{w}{{\hat{\alpha}}^{{b_{k+1}}}}_{{b_{j}}}{\rm{}})$

III-D Sliding Window and Marginalization

To balance the calculation amount of the system and ensure the real-time performance of the system, the idea of the sliding window is adopted [37]. The number of optimization variables is limited by a fixed size. When the window slides, new factors will be added and old factors will be eliminated. To utilize the past state and measurement information as much as possible, the factor nodes and state nodes excluded from the window are marginalized, and the past information is retained in the current optimization through the Schur-Complement. The readers may refer to [38] for details of applying marginalization.

As the actual system running time increases and the amount to be optimized also increases, it is impossible to update every moment, and it is difficult to update and optimize all variables at one time. Therefore, to control the calculation scale, the sliding window method must be used.

In the proposed FGO-ILNS, the navigation data frequency of the LBL is 1 Hz, and the frequency of IMU is 200 Hz, hence we set the IMU preintegration interval as 1s, as shown in Fig. 6. The size of the sliding window is in seconds, t_{i, j}=1s. The window size (threshold for the number of preintegration factors) is set manually in the configuration file. As shown in Fig. 3(c), when the number of the IMU preintegration factors reaches the threshold, the old factors are marginalized via Schul-Complement, and new factors are added into the window at the same time. The marginalized information will be used as a priori factor in the optimization process. Only the factors in the optimization window are iteratively optimized each time, which effectively reduces the scale and calculation amount of the factor graph. In this paper, Ceres solver [39] is used to obtain a nonlinear optimization solution. The estimated value obtained by optimization is fed back to SINS to obtain accurate navigation information.

IV Experiment and Analysis

To verify that the proposed FGO-ILNS has higher positioning accuracy and robustness, three experiments are designed. Firstly, experiment I is designed to verify the positioning accuracy of FGO-ILNS: experiment I-a discusses the impact of different sliding windows on the positioning accuracy, and experiment I-b compares FGO-ILNS with EKF-ILNS. Then, we designed experiment II to verify the high robustness of FGO-ILNS plus: experiment II-a discusses the impact of adding different sensors on positioning performance, and experiment II-b evaluates the positioning performance when some sensors are unavailable, and compares it with the FKF-ILNS plus algorithm. Finally, experiment III utilizes the public KAIST urban 38 datasets to conduct semi-physical experimental verification, comparing FGO-ILNS plus with the state-of-the-art optimization-based ORB-SLAM3. Simulation experiment parameter settings are described in section IV-A. Unless otherwise stated, all experiments were performed on the same desktop with an Intel Core i7-11700K at 3.6GHz and 32GB RAM.

IV-A Experiment settings

IV-A1 Error setting for sensors

The sensor error settings [5, 17, 40, 41] involved in the FGO-ILNS simulation experiment are shown in Table I, where the buoy position of the LBL is represented by the local ENU coordinate system and the origin is the starting point of the AUV navigation.

TABLE I: Error Setting of the Sensors

Sensors	Source of Error	Error Setting
IMU	Gyroscope constant bias	25 ° $/h$
	Angular random walk	100 $ug$
	Accelerometer constant bias	0.1 ° $/sqrt(h)$
	Velocity random walk	0.1 $ug/sqrt(Hz)$
LBL	Acoustic buoy position	Buoy1:( 0, -500 $m$ , 0)
		Buoy2:(-400 $m$ , 0, 0)
		Buoy3:(-200 $m$ , 1000 $m$ , 0)
		Buoy4:(200 $m$ , 500 $m$ , 0)
	Ranging error	0.3 $m$
GNSS	Horizontal position accuracy	0.02 $m$
GNSS	Vertical position accuracy	0.01 $m$
DVL [39,40]	Output velocity STD	0.01 $m/s$
MCP [17]	Output yaw STD	0.01°
PS [5,17]	Output depth STD	0.01 $m$

IV-A2 Simulation of AUV motion state

The AUV mainly performs sinking, turning left, turning right, floating, accelerating, and constant speed. Trajectory, velocity, and attitude angle are shown in Fig. 7. We set the starting point of the AUV as the origin of the ENU coordinate system. The total simulation time is 2217 $s$ , including the initialization time of about 17 $s$ .

IV-B Experiment I: Verifying positioning accuracy of FGO-ILNS

To verify the positioning accuracy of the proposed FGO-ILNS, the influence of different sliding window settings on positioning accuracy was first discussed in Experiment I-a. Then, in Experiment I-b, we compared the FGO-ILNS with the EKF-ILNS algorithm under the same experimental settings.

IV-B1 Experiment I-a

In experiment I-a, sliding windows with different lengths were set, namely 10, 20, 30, 50, 200, and global optimization. The AUV simulation is carried out under the experimental settings in section IV-A.

From Fig. 8, it can be seen that: 1) First, in Fig. 8(b) and (c), the positioning error between 400 $s$ and 500 $s$ has a relatively obvious jump, especially when SW=20 or SW=30. The reason is that the AUV is sinking during that period and is in a highly dynamic motion state. As the sliding window size is increased to a certain size, the appearance of outliers is suppressed. FGO-ILNS is a method for solving underwater large-scale nonlinear least squares problems by transforming the problem into a graphical model. In SINS/LBL localization problems, factor graph optimization can effectively handle various sensor information, including relative position, absolute position, and orientation. In addition, factor graph optimization can effectively handle noise and uncertainty, thereby improving localization accuracy. Therefore, Fig. 8(g) shows that the horizontal positioning accuracy of FGO-ILNS under different sliding windows can reach the decimeter level, which meets the accuracy requirements of underwater navigation. Since the height direction error under different sliding windows is stable at 0.015 $m$ , it will not be discussed here. 2) Second, in Fig.8(g) and (h), as the size of the sliding window increases, the amount of calculation will increase accordingly especially when global optimization. However, the positioning accuracy does not appear to be significantly enhanced in the simulation results. In sliding window factor graph optimization, increasing the window size may affect the accuracy. This is because as the window size increases, the number of variables that need to be optimized also increases, which may increase the computational complexity and thus affect the accuracy. However, this also depends on specific factors such as the choice of optimization algorithm, the scale and complexity of the problem, etc. Therefore, in specific localization scenarios, an appropriate window size needs to be chosen to balance computational cost and accuracy. Consequently, we think that when SW=20, the positioning accuracy and performance of the FGO-ILNS are the relatively best in the dataset.

IV-B2 Experiment I-b

To verify the advancement of FGO-ILNS, the case of SW=20 is selected to compare with the classic EKF-based SINS/LBL coupled algorithm (EKF-ILNS). On the other hand, we consider two motion states, stable states (uniform linear motion with 0.6 $m/s$ ) and complex states as section IV-A.

From Fig. 9(a), it can be seen that in a stable motion state, the positioning accuracy of the FGO-ILNS and EKF-ILNS algorithms are relatively stable, and the difference is quite slight, which verifies the optimal state estimation of FGO-ILNS. In complex dynamic motion states, the localization performance of FGO-ILNS is significantly higher than that of EKF-ILNS. It can be seen from Fig. 9(b) that when the AUV is sailing with high maneuverability, the state estimation of the EKF-ILNS will have large drift errors, but when the motion state is stable, the EKF-ILNS can still output a considerably accurate state estimation. This is due to two reasons. Firstly, the estimation error of the filter-based algorithm will increase in places with intense motion. This is because high dynamics may cause changes in the dynamic characteristics of the system, thereby affecting the performance of the filter-based algorithm. In addition, factors such as sensor noise and nonlinearity during sharp turns may also affect the accuracy of the filter-based algorithm. Therefore, when the AUV performs high-dynamic complex movements, the estimation error of the EKF-ILNS may increase. Secondly, the optimization-based algorithm can effectively suppress outlier errors. Factor graph optimization is a method for solving nonlinear optimization problems and can achieve better accuracy compared to traditional EKF techniques. One reason is that factor graph optimization-based solution methods can consider more historical information, which can further enhance the robustness of the estimator against outliers. On the other hand, factor graph optimization-based solution methods can perform multiple iterations to reduce errors caused by linearization and obtain a better-optimized solution.

TABLE II: Position RMS Error of EKF-ILNS and FGO-ILNS with Different AUV State

States	Method	Direction
States	Method	E ( $m$ )	N ( $m$ )	U ( $m$ )
Stable	EKF-ILNS	0.58	0.99	0.04
Stable	FGO-ILNS	0.52	0.86	0.01
Complex	EKF-ILNS	17.21	16.36	0.35
Complex	FGO-ILNS	0.73	0.34	0.01

From a specific numerical analysis, in Table II, compared with EKF-ILNS, the accuracy of FGO-ILNS in the ENU direction is improved by 10.3%, 4.0%, and 75.0% respectively in stable states, and by 99.8%, 97.9%, and 97.1% in complex states.

Experiment I verified the effectiveness of the proposed FGO-ILNS, and its horizontal positioning accuracy reached the decimeter level as shown in Table II. Compared with the EKF algorithm, the FGO algorithm has more advantages when AUV is in high dynamic navigation.

IV-C Experiment II: Verifying high robustness of FGO-ILNS plus

To verify the high robustness of the FGO-ILNS, experiment II carried out a combination of SINS-LBL-GNSS-DVL-MCP-PS based on factor graph optimization — “FGO-ILNS plus”. The AUV trajectory and sensor error settings for this experiment are described in Section IV-A. Firstly, we will discuss the positioning performance when adding sensors on the basis of FGO-ILNS, and compare the positioning performance before and after adding DVL/MCP in Experiment II-a. Secondly, in Experiment II-b, we consider the situation that when the AUV goes out of the range of the LBL acoustic buoy, the LBL/DVL/MCP fails (only SINS and PS work), and then the AUV surfaces to receive the GNSS signal. In this situation, the positioning performance of FGO-ILNS plus is discussed. Note that the height direction positioning of the system has always been constrained by PS, so the following experiments only consider the horizontal direction positioning.

IV-C1 Experiment II-a

In Fig. 10, compared with FGO-ILNS, the stability and accuracy of FGO-ILNS plus are improved. When SW=20 and 30, in Fig. 10(a) and (b), FGO-ILNS plus can effectively eliminate the jump of positioning error caused by AUV high-dynamic states.

In factor graph optimization methods, as the number of fused sensors increases, the robustness of localization increases. This is because the fusion of multiple sensor information can provide more information and constraints, thereby better estimating the state of the system. For example, in our experiment II-a, FGO-ILNS plus, compared to FGO-ILNS, adds DVL and MCP sensors, that is, velocity and heading angle constraints are added, thereby improving positioning accuracy. In addition, factor graph optimization can effectively handle noise and uncertainty. In factor graph optimization, each sensor measurement is represented as a factor, and these factors can be connected by sharing variables. In this way, during the optimization process, the algorithm can adjust the state estimation according to the relationship between different sensor measurements, thereby suppressing noise and uncertainty. Therefore, in practical applications, using multiple sensors for fusion positioning can usually achieve better positioning results.

TABLE III: Position RMS Error of EKF-ILNS and FGO-ILNS PLUS with Different Sliding Window

Sliding Window Size	Method	Direction
Sliding Window Size	Method	E ( $m$ )	N ( $m$ )
SW=30	FGO-ILNS	0.87	0.52
SW=30	FGO-ILNS plus	0.55	0.45
SW=20	FGO-ILNS	0.67	0.28
SW=20	FGO-ILNS plus	0.55	0.45

From a specific numerical analysis, in Table III, compared with FGO-ILNS, the accuracy of FGO-ILNS plus in the horizontal direction is improved by 36.8% and 13.5% respectively when SW=30, and by 17.9% and -60.7% when SW=20. In Fig. 10(c), the four trajectory estimation results of different sensors and different sliding windows are analyzed. Compared with the FGO-ILNS with SW=30, the trajectory estimation of FGO-ILNS plus is closer to the ground truth during the period of high dynamic motion.

In short, after the sensor is increased, the accuracy of the system is generally improved and can reach the decimeter level under the FGO algorithm. And it also has a good suppression effect on error jumps, which reflects the highly robust characteristics of the multi-sensor combination.

IV-C2 Experiment II-b

Considering the situation: as shown in Fig. 11(a), AUV is in the initial stage during starting time to $t_{0}$ , and then starts to sink; During $t_{0}$ to $t_{1}$ (1683 $s$ ), SINS/LBL/DVL/MCP/PS sensors work normally, GNSS fails; AUV leaves the range of action of the LBL acoustic matrix at time $t_{1}$ and starts to float to the water surface. During $t_{1}$ to $t_{2}$ , LBL and DVL fail and last for 100s; After time $t_{2}$ (1783 $s$ ), AUV receives the GNSS signal.

We compared the positioning error and maximum drift distance of FKF-ILNS plus (the sub-filter is EKF) and FGO-ILNS plus (with different sliding window sizes), as shown in Fig. 11 (b)-(d). From the results of the maximum drift distance in Fig. 11(d), it can be seen that compared with the FKF-ILNS plus, the drifts of FGO-ILNS plus with SW=50, 100, 150, and 200 are smaller, and especially when SW=100, the drift is the smallest. The accuracy of an IMU decreases with continuous use due to the accumulation of errors caused by drift. Between moments $t_{1}$ and $t_{2}$ , the system loses constraints on position and velocity information, so the cumulative error of the system gradually increases. FKF-ILNS plus, a combined navigation system based on federated Kalman filtering, has sub-filters that are all EKF. Since EKF only considers information from the previous moment, it may not be able to fully utilize historical information to suppress IMU drift errors. In contrast, methods based on sliding window graph optimization may have smaller drift errors as the sliding window gets larger. This is because sliding window factor graph optimization can use more historical information for optimization, thereby better-suppressing drift errors. Choosing an appropriate window size can greatly improve positioning robustness in extremely challenging environments. When the number of available sensors increases, the amount of constraint measurement information increases and the system can quickly eliminate the cumulative error of the IMU. After time $t_{2}$ , GNSS constrains the AUV position. And in Fig. 11(b) and (c) we can see that the positioning errors decrease dramatically.

Therefore, the proposed FGO-ILNS is more robust than filter-based algorithms (federal Kalman filter and EKF) when sensor availability changes.

IV-D Experiment III: Public KAIST urban datasets

This part of the experiment will verify our proposed system in the real world. Due to the limitations of experimental conditions and the difficulty in obtaining large-scale position data and reference ground truth from public underwater datasets, we will use the public KAIST urban 38 datasets [42] for semi-physical experiment verification.

The datasets are collected by vehicles in a complex urban environment with a maximum speed of about 15 $m/s$ . The driving distance is about 11191 $m$ , and the total time is about 2154 $s$ . The sensors include an industrial-grade MEMS IMU MTi-300 with a gyroscope bias of 10° $/hour$ ; a VRS-RTK GPS with a horizontal accuracy is 10 $mm$ ; a magnetic rotary encoder with a resolution of 4096; an altimeter sensor with a resolution of 0.01 $hPa$ . We utilized the position result of VRS-RTK GPS as the trajectory of AUV, simulated the slant range of LBL, used the MEMS IMU MTi-300 to output gyroscope and accelerometer data, and used the altimeter sensor to offer elevation data. We calculated the speed information via magnetic rotary encoder data. We used the above data to verify FGO-ILNS plus.

We computed the estimated trajectories of FGO-ILNS plus (different SW) and SOTA algorithm ORB-SLAM3 [21] (fusing the monocular camera and inertial navigation) and compared them with the ground truth trajectory, as shown in Fig. 12. We can see that the estimated trajectory of FGO-ILNS plus matches well with the ground truth trajectory, but the results of ORB-SLAM3 show a lot of drift error. In the existing VI-SLAM algorithm, it is easy to generate cumulative errors when performing long-term and large-scale positioning, especially when it is difficult to achieve the loop condition. In the underwater SLAM problem, large-scale navigation is still an open research. The proposed algorithm in this paper integrates acoustic positioning without accumulative errors, which plays an important role in large-scale environment navigation. At the same time, it introduces a variety of state constraint information to improve the accuracy and stability of positioning.

TABLE IV: Absolute Trajectory Error in the KAIST Urban 38 Dataset

Method	ATE ( $m$ )
ORB-SLAM3	105.15
FGO-ILNS plus with SW=10	6.16
FGO-ILNS plus with SW=20	3.99
FGO-ILNS plus with SW=50	9.13
FGO-ILNS plus with SW=100	4.49

In Table IV, we calculated the absolute trajectory error (ATE), and the accuracy performance is relatively best in the case of FGO-ILNS plus with SW=20. When performing LBL simulation based on GPS data, there are some GPS-deny situations, so we set the LBL data quality to decline or even fail during this period. In the actual complex underwater environment, the meter-level positioning meets the requirements of AUV large-scale navigation.

From the analysis of the results of experiments I, II, and III, we believe that the FGO-ILNS has higher positioning accuracy and stability when the AUV is highly dynamic and the sensor availability changes compared with the filter-based algorithms (EKF and FKF) and SOTA optimization-based algorithms ORB-SLAM3.

V Conclusion

Aiming at the navigation and positioning problem of AUVs, we proposed an underwater multi-sensor fusion algorithm based on factor graph optimization named FGO-ILNS which tightly couples inertial and acoustic position system. Moreover, the FBPF method tightly coupled the IMU and other sensors to improve the utilization of measurements. In addition, the employment of sliding windows realized the real-time nature of the system. Compared with the EKF algorithm, FGO-ILNS has a higher positioning accuracy and robustness in highly dynamic motion states, which meets the positioning accuracy requirements of AUV navigation. Moreover, the extended version of FGO-ILNS is called FGO-ILNS plus, applied when more sensors are used for fusion positioning. Compared with the FKF algorithm and ORB-SLAM3, FGO-ILNS plus has higher positioning accuracy and stability. Additionally, FGO-ILNS plus is more robust when sensor availability changes. Consequently, the proposed FGO-ILNS provides a reference scheme for the navigation and positioning of AUVs in high-dynamic, long-term, and large-scale environments. Due to the limitation of experimental conditions, it is temporarily no conditions for sea trials. Future research work will focus on improving the underwater environment perception and global positioning capabilities of AUVs via adding visual information or imaging sonar information to FGO-ILNS. The next version of FGO-ILNS is called Acoustic-VINS, which tightly coupled acoustic, visual, and inertial information.

References

[1] J. Yan, Y. Gong, C. Chen, X. Luo, and X. Guan, “Auv-aided localization for internet of underwater things: A reinforcement-learning-based method,” IEEE Internet of Things Journal, vol. 7, no. 10, pp. 9728–9746, 2020.
[2] S. Song, J. Liu, J. Guo, C. Zhang, T. Yang, and J. Cui, “Efficient velocity estimation and location prediction in underwater acoustic sensor networks,” IEEE Internet of Things Journal, vol. 9, no. 4, pp. 2984–2998, 2022.
[3] W. Shi, J. Xu, D. Li, and H. He, “Attitude estimation of sins on underwater dynamic base with variational bayesian robust adaptive kalman filter,” IEEE Sensors Journal, vol. 22, pp. 10 954–10 964, 2022.
[4] W. Shi, J. Xu, H. He, D. Li, H. Tang, and E. Lin, “Fault-tolerant sins/hsb/dvl underwater integrated navigation system based on variational bayesian robust adaptive kalman filter and adaptive information sharing factor,” Measurement, vol. 196, p. 111225, 2022.
[5] P. Liu, B. Wang, Z. Deng, and M. Fu, “Ins/dvl/ps tightly coupled underwater navigation method with limited dvl measurements,” IEEE Sensors Journal, vol. 18, pp. 2994–3002, 2018.
[6] D. Yuan, X. Ma, Y. Liu, L. Yang, Y. Wu, and X. Zhang, “Reaearch on underwater integrated navigation system based on sins/dvl/magnetometer/depth-sensor,” in OCEANS 2017 - Aberdeen, 2017, pp. 1–6.
[7] S. Pan, X. Xu, L. Zhang, and Y. Yao, “A novel sins/usbl tightly integrated navigation strategy based on improved anfis,” IEEE Sensors Journal, vol. 22, pp. 9763–9777, 2022.
[8] Y. Xu et al., “Usbl positioning system based adaptive kalman filter in auv,” in OCEANS 2018 - MTS/IEEE Kobe Techno-Oceans, 2018, pp. 1–4.
[9] Y.Yao, X.Xu, D.Yang, and X.Xu, “An imm-ukf aided sins/usbl calibration solution for underwater vehicles,” IEEE Transactions on Vehicular Technology, vol. 69, pp. 3740–3747, 2020.
[10] M.-N. P.Otero, Á.Hernández-Romero, “Lbl system for underwater acoustic positioning: Concept and equations,” ArXiv, vol. abs/2204.08255, 2022.
[11] Y. S. Simamora, H. A. Tjokronegoro, E. Leksono, and I. S. Brodjonegoro, “Compensation of time-varying clock-offset in a lbl navigation,” Bulletin of Electrical Engineering and Informatics, vol. 9, no. 4, pp. 1364–1372, 2020.
[12] L. H.Li, S.Yan, “Underwater long baseline positioning algorithm based on double-parameter constraint,” in 2020 IEEE International Conference on Signal Processing, Communications and Computing, 2020, pp. 1–5.
[13] N. R.Almeida, J.Melo, “Characterization of measurement errors in a lbl positioning system,” in OCEANS 2016 - Shanghai, 2016, pp. 1–6.
[14] Y. S. M. Simamora, H. A. Tjokronegoro, E. Leksono, and I. S. Brodjonegoro, “Compensation of ins/lbl navigation errors in a polynomial sound-speed-profile,” Journal of Engineering and Technological Sciences, vol. 54, no. 2, 2022.
[15] T. Zhang, H. Shi, L. Chen, Y. Li, and J. Tong, “Auv positioning method based on tightly coupled sins/lbl for underwater acoustic multipath propagation,” Sensors, vol. 16, no. 3, p. 357, 2016.
[16] C. F.Chi, Y.Xiao, “Research on auv integrated navigation method based on improved particle filter algorithm,” in 2021 OES China Ocean Acoustics, 2021, pp. 299–304.
[17] J. Song, W. Li, X. Zhu, Z. Dai, and C. Ran, “Underwater adaptive height-constraint algorithm based on sins/lbl tightly coupled,” IEEE Transactions on Instrumentation and Measurement, vol. 71, pp. 1–9, 2022.
[18] W. Wen, T. Pfeifer, X. Bai, and L.-T. Hsu, “It is time for factor graph optimization for gnss/ins integration: Comparison between fgo and ekf,” arXiv, 2020.
[19] C. Cadena et al., “Past, present, and future of simultaneous localization and mapping: Toward the robust-perception age,” IEEE Transactions on Robotics, vol. 32, no. 6, pp. 1309–1332, 2016.
[20] J. Liu, W. Gao, and Z. Hu, “Optimization-based visual-inertial slam tightly coupled with raw gnss measurements,” in 2021 IEEE International Conference on Robotics and Automation, Xi’an, China, 2021, pp. 11 612–11 618.
[21] C. Campos, R. Elvira, J. J. G. Rodríguez, J. M. Montiel, and J. Tardós, “Orb-slam3: An accurate open-source library for visual, visual–inertial, and multimap slam,” IEEE Transactions on Robotics, vol. 37, no. 6, pp. 1874–1890, 2021.
[22] Q. Zeng, W. Chen, J. Liu, and H. Wang, “An improved multi-sensor fusion navigation algorithm based on the factor graph,” Sensors, vol. 17, 2017.
[23] H. Tang, T. Zhang, X. Niu, J. Fan, and J. Liu, “Impact of the earth rotation compensation on mems-imu preintegration of factor graph optimization,” IEEE Sensors Journal, vol. 22, pp. 17 194–17 204, 2022.
[24] T. Qin, S. Cao, J. Pan, and S. Shen, “A general optimization-based framework for global pose estimation with multiple sensors,” arXiv, 2019.
[25] W. Zhao, T. He, A. Y. M. Sani, and T. Yao, “Review of slam techniques for autonomous underwater vehicles,” in Proceedings of the 2019 International Conference on Robotics, Intelligent Control and Artificial Intelligence, 2019, pp. 384–389.
[26] S. Rahman, A. Q. Li, and I. Rekleitis, “Svin2: A multi-sensor fusion-based underwater slam system,” The International Journal of Robotics Research, vol. 41, no. 11-12, pp. 1022–1042, 2022.
[27] S. et al, “Visual slam for underwater vehicles: A survey,” Computer Science Review, vol. 46, p. 100510, 2022.
[28] F. Guth, L. Silveira, S. Botelho, P. Drews, and P. Ballester, “Underwater slam: Challenges, state of the art, algorithms and a new biologically-inspired approach,” in 5th IEEE RAS/EMBS international conference on biomedical robotics and biomechatronics. IEEE, 2014, pp. 981–986.
[29] Z. Huang, H. Cai, and M. Xiang, “Multi-source information fusion localization algorithm based on auv factor graph considering information delay,” Zhongguo Guanxing Jishu Xuebao/Journal of Chinese Inertial Technology, vol. 29, pp. 625–631, 2021.
[30] X. Ma, X. Liu, C.-L. Li, and S. Che, “Multi-source information fusion based on factor graph in autonomous underwater vehicles navigation systems,” Assembly Automation, vol. 41, no. 5, pp. 536–545, 2021.
[31] K. Watanabe, K. Utsunomiya, K. Harada, and Q. Shen, “Development of a floating lbl system and a lightweight rov for sky to water system,” in OCEANS 2019 MTS/IEEE SEATTLE. IEEE, 2019, pp. 1–6.
[32] C. Hu, S. Zhu, Y. Liang, and W. Song, “Tightly-coupled visual-inertial-pressure fusion using forward and backward imu preintegration,” IEEE Robotics and Automation Letters, vol. 7, no. 3, pp. 6790–6797, 2022.
[33] Y. S. M. Simamora, H. A. Tjokronegoro, E. Leksono, and I. S. Brodjonegoro, “Compensation of ins/lbl navigation errors in a polynomial sound-speed-profile,” Journal of Engineering and Technological Sciences, vol. 54, no. 2, 2022.
[34] R. Kümmerle, G. Grisetti, H. Strasdat, K. Konolige, and W. Burgard, “g2o: A general framework for graph optimization,” in 2011 IEEE International Conference on Robotics and Automation. IEEE, 2011, pp. 3607–3613.
[35] F. M.Kaess, A.Ranganathan, “isam: Incremental smoothing and mapping,” IEEE Transactions on Robotics, vol. 24, no. 6, pp. 1365–1378, 2008.
[36] C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On-manifold preintegration for real-time visual–inertial odometry,” IEEE Transactions on Robotics, vol. 33, no. 1, pp. 1–21, 2016.
[37] H.-P. Chiu, S. Williams, F. Dellaert, S. Samarasekera, and R. Kumar, “Robust vision-aided navigation using sliding-window factor graphs,” in 2013 IEEE International Conference on Robotics and Automation, 2013, pp. 46–53.
[38] T. Qin, P. Li, and S. Shen, “Vins-mono: A robust and versatile monocular visual-inertial state estimator,” IEEE Transactions on Robotics, vol. 34, pp. 1004–1020, 2018.
[39] S. Agarwal and K. Mierle, “Ceres solver: Tutorial & reference,” Google Inc, vol. 2, no. 72, p. 8, 2012.
[40] S. Liu, T. Zhang, J. Zhang, and Y. Zhu, “A new coupled method of sins/dvl integrated navigation based on improved dual adaptive factors,” IEEE Transactions on Instrumentation and Measurement, vol. 70, pp. 1–11, 2021.
[41] D. Wang, X. Xu, and L. Hou, “An improved adaptive kalman filter for underwater sins/dvl system,” Mathematical Problems in Engineering, vol. 2020, pp. 1–14, 2020.
[42] J. Jeong, Y. Cho, Y.-S. Shin, H. Roh, and A. Kim, “Complex urban dataset with multi-level sensors from highly diverse urban environments,” The International Journal of Robotics Research, vol. 38, no. 6, pp. 642–657, 2019.