A post-processing algorithm based on factor graph optimization for autonomous underwater vehicle integrated navigation

Abstract

Aiming at the requirements of offline high-precision navigation and positioning in the mapping of ocean floor, survey, and other fields for autonomous underwater vehicle (AUV)integrated navigation system, a post-processing algorithm based on factor graph optimization is proposed in this paper. First, the factor graph model is established by the factor graph formulation to address the asynchronous and heterogeneous problem of multi-source information fusion. Then the equivalent inertial measurement unit (IMU) factor is introduced to replace several consecutive IMU factors between two adjacent measurements to further aid high-rate navigation solutions. Second the forward and backward message passing processes are performed independently using the sum-product algorithm in the factor graph model. Finally, the optimal navigation solution is obtained by weighted smoothing the results of the forward and backward message passing processes. Simulation results show this algorithm proposed in this paper provides more accurate navigation accuracy and smoothness, especially in the case of signals loss or sensors fault. Compared with the federal Kalman filtering method, the horizontal positioning errors of the un-simplified factor graph, the simplified factor graph, and the factor graph optimization are improved by about 30%, 34%, and 80%, respectively. The semi-physical environment results also clearly verify the reliability and effectiveness of the proposed method, and its horizontal positioning accuracies are kept within −2∼2 m.

Keywords

autonomous underwater vehicle multi-source information fusion sum-product algorithm factor graph optimization integrated navigation system

Introduction

Autonomous underwater vehicle (AUV) has wide application prospects in military and civilian fields as an important tool for human exploration and exploitation of the ocean (Sahoo et al., 2019; Sanchez et al., 2020). Energy, communication, navigation, and control are difficulties and key problems of AUV (Wang et al., 2021a, 2021b; Zhong, 2017). With the increasing of the variety and number of underwater navigation sensors/systems, there are more and more underwater navigation devices can be equipped with, such as Strapdown Inertial Navigation System (SINS), Doppler Velocity Log (DVL), Long/Short/Ultra Short Baseline, Global Positioning System (GPS), Terrain-Aided Navigation (TAN), and Magnetic Compass Pilot (MCP) (Allotta et al., 2016; Jalal and Nasir, 2021). SINS is an autonomous navigation technology that does not rely on external signals. By integrating the specific force and the angular acceleration measured by inertial sensors with the time in the body frame and transforming them into the navigation frame, the position, velocity, and attitude of the vehicle can be calculated in real time (Liu et al., 2021). However, the positioning error grows rapidly over time due to the inertial drift (Salavasidis et al., 2018). At present, AUV integrated navigation system is most commonly used to reduce the cumulative error of INS by integrating the auxiliary information provided by various underwater navigation devices. Therefore, how to fuse various sensors to improve the navigation solution of AUV integrated navigation system is an important research direction in the field of multi-source information fusion (Wu et al., 2019).

Sensor measurement mechanism, seawater medium, and complex underwater environment impose great challenges on multi-source information fusion (Prasanth, 2021; Song et al., 2022). The well-known federated Kalman filter has always been mostly used for the multi-source information fusion among the stand-alone INS and aiding sensors of an underwater vehicle (Guo et al., 2019). However, it also has inevitable drawbacks in the practical application. The federal Kalman filter can only perform the information fusion function until all available measurements have been collected, while the update frequency of each measurement is generally different and asynchronous. This will result in the poor measurement and navigation accuracy degradation at the time epoch of information fusion. When signal loss or sensors fault for a short time, the federal Kalman filter method is necessary to isolate the corresponding sub-filter separately and reconstruct the system structure, which greatly increases the computation and reduces the flexibility. This is bad for highly dynamic environments (Ma et al., 2019a).

In recent years, with the development of All Source Positioning and Navigation (ASPN), the factor graph approach provides an all-in-one navigation solution for multi-source information fusion (Zhou et al., 2015). The factor graph allows us to visualize the relationship between variables in abstract mathematical problems, and transforms complex computational processes into message passing and update, with more readability. Thus, the factor graph can be used to construct the highly applicable factor graph model using all available information sources to realize the information fusion among the SINS and asynchronous heterogeneous sensors.

In general, we use the message passing algorithm (the sum-product algorithm) to pass and update messages in the factor graph model to deduce the probability distribution of all variable nodes accurately (Loeliger et al., 2007; Loeliger, 2004). However, the improvement of the navigation accuracy is constrained by the less data available and limited solution time in the AUV underwater navigation system. And the poor measurement information, sensor failure or signal loss will make the navigation solution worse. Because the mapping data of ocean floor can be analyzed and processed forward or backward chronologically in the post-processing, some high-precision post-processing algorithms can be used to meet high positioning requirements, such as smoothing technology.

Therefore, this paper proposed the post-processing algorithm based on factor graph optimization for AUV integrated navigation system. Compared with the traditional Kalman filtering methods and others, the proposed method has the following contributions:

The factor graph model supports a plug and play capability by simply adding or rejecting adding related factors to overcome deficiencies of the federated Kalman filtering algorithm mentioned above (Ma et al., 2020; Indelman et al., 2013).

The equivalent inertial measurement unit (IMU) factor is introduced into the factor graph model to reduce the number of variables and factor nodes drastically. It also reduces the computation and storage to gain a high-rate navigation solution.

The proposed post-processing algorithm based on factor graph optimization algorithm provides more accurate navigation accuracy and smoothness in the post-processing of navigation data with asynchronous heterogeneous sensors, especially in the case of signals loss or sensors fault.

The related work will be discussed in the remainder of this paper. Section “Structure of AUV integrated navigation system” describes the structure of AUV integrated navigation system. The multi-source information fusion based on factor graph is addressed in section “Multi-source information fusion based on factor graph.” Then section “Factor graph optimization algorithm” introduces the factor graph optimization scheme in detail. Simulation and semi-physical experiments are provided in section “Experiments.” Finally, conclusions are presented in section “Conclusion.”

Structure of AUV integrated navigation system

The quite mature AUV integrated navigation system at home and abroad is selected in this paper, as shown in Figure 1 (Hagen et al., 2007; Mu et al., 2021). There are several kinds of available navigation information in this figure: the position, velocity, and attitude information provided by SINS; the velocity measurement information with respect to the ground provided by DVL; the heading measurement information provided by MCP; the best fitting position measurement information searched in the digital chart by TAN; the depth measurement information provided by Depth Meter (DM) about AUV distance from the seabed. SINS can work all the time to provide the complete navigation information without any external information. Therefore, SINS is usually used as the main navigation system, and other high-precision aiding navigation information is used to correct the accumulation errors of SINS.

Figure 1.

Structure of AUV integrated navigation system.

Due to the limited time of real-time navigation solution and the limited amount of available measurement information, the improvement of navigation accuracy is restricted to a certain extent. In addition, these available sensors equipped with an AUV usually work at multiple different frequencies and are often asynchronous. IMU sensor measures the specific force and the angular acceleration at a higher rate, while other aiding sensors/systems such as DVL, TAN, MCP, and DM generate measurements at much slower rates. And the interference of complex underwater environment and noise, some underwater navigation sensors/systems may fail or lose signal loss for a short time. All these will also lead to rapid growth of navigation positioning error. Therefore, it is necessary to use smoothing technology to obtain higher navigation solution where post-processing of navigation data is allowed, such as the mapping of ocean floor, survey and other fields.

Multi-source information fusion based on factor graph

This section establishes the strong flexible and expandable factor graph model of AUV multi-source integrated navigation system and realizes the information fusion between SINS and asynchronous heterogeneous sensors. A good system framework is the foundation and premise of the post-processing smoothing.

Factor graph formulation

A factor graph (Loeliger et al., 2007) is a bipartite graph $G = (F, X, E)$ representing the joint probability distribution function (pdf) of random variables, which describes not only the relationship between a global function and local functions but also between variables and local functions in a factorization of a global function. We suppose the global function $g (X)$ was decomposed into the product of several local functions, it can be written as (Ma et al., 2019b)

g (X) = \underset{j}{Π} f_{j} (X_{j})

(1)

where $X_{j} \subseteq X$ is the set of all independent variables of the jth local function $f_{j} (\cdot)$ . The above factorization can be expressed by the factor graph with two types of nodes: each variable $x_{i} \in X$ corresponds to a unique variable node; each local function $f_{j} \in F$ corresponds a unique factor node. An edge $e_{ij} \in E$ exists if and only if the factor node $f_{j}$ involves the variable node $x_{i}$ (the variable $x_{i}$ is an independent variable of the local function $f_{j}$ ).

We now introduce the concept of the factor graph into the information fusion problem of AUV multi-source integrated inertial navigation system. The joint pdf $P (X_{k} | Z_{k})$ about all navigation states and measurements is given by (Cox et al., 2019; Indelman et al., 2013)

P (X_{k} | Z_{k}) = P (x_{0}) Π_{i = 1}^{k} P (x_{i} | x_{i - 1}, z_{i}^{IMU}) Π_{i = 1, z_{j} \in Z_{i} \ z_{i}^{IMU}}^{k} P (z_{j} | X_{i}^{j})

(2)

where $P (x_{0})$ is the prior information of initial state for all variables; $P (x_{i} | x_{i - 1}, z_{i}^{IMU})$ and $P (z_{j} | X_{i}^{j})$ , respectively, describe a general process model and a general measurement model; $X_{i}^{j} \subseteq X_{i}$ represents the all estimated states when the measurement $z_{j}$ is obtained.

Specifically, supposed the noise obeys a Gaussian distribution, each of such factors $f_{j}$ in the joint pdf (2) is given by (Ma et al., 2019b)

\begin{array}{l} f_{i} (x_{i}) \propto P (x_{i} | x_{i - 1}, z_{i}^{I M U}) = \exp (- \frac{1}{2} {‖ e r r_{i} (x_{i}, z_{i}^{I M U}) ‖}_{Λ_{i}}^{2}) \\ = \exp (- \frac{1}{2} {‖ g_{i} (x_{i - 1}, z_{i}^{I M U}) - x_{i} ‖}_{Λ_{i}}^{2}) \end{array}

(3)

\begin{array}{l} f_{i} (X_{i}^{j}) \propto P (z_{j} | X_{i}^{j}) = \exp (- \frac{1}{2} {‖ e r r_{i} (X_{i}^{j}, z_{j}) ‖}_{Σ_{i}}^{2}) \\ = \exp (- \frac{1}{2} {‖ h_{i} (X_{i}^{j}) - z_{j} ‖}_{Σ_{i}}^{2}) \end{array}

(4)

where this is the squared Mahalanobis distance defined as $‖ e ‖_{Γ}^{2} ≜ e^{T} Γ^{- 1} e$ ; $g_{i} (.)$ and $h_{j} (\cdot)$ denote some known functions about state vectors and measurements.

Factor graph models

We now introduce the concept of the factor graph into the multi-source information fusion problem of AUV integrated navigation system. The multi-source information fusion problem is the equivalent of calculating the joint pdf of all states over time, which can be represented by a probabilistic graphical model, the factor graph.

A 15-dimensional vector is selected as the error of navigation state, which consists of attitude, velocity, and position error, the constant drift and bias of the gyroscope and accelerometer in each axis for the inertial sensor, written as follows

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

(5)

The factor graph abstracts the error of navigation state at the ith time-step as variable node $x_{i}$ , the measurement information obtained by asynchronous heterogeneous sensors as the factors $f^{IMU}$ , $f^{DVL}$ , $f^{MCP}$ , $f^{TAN}$ , and $f^{DM}$ . The factor graph model of AUV multi-source integrated inertial navigation system covering common underwater sensors is shown as Figure 2.

Figure 2.

Factor graph model of AUV multi-source integrated inertial navigation system.

As seen in Figure 2, IMU factors are added to the factor graph model at a high frequency depended on the inertial sensors, while other factors at slow frequencies determined by other aiding sensors. These factors are defined by asynchronous heterogeneous measurements and connect corresponding variable nodes in a natural way: an IMU factor $f^{IMU}$ connecting two variable nodes $x_{k}$ and $x_{k + 1}$ at two consecutive time instances $t_{k}$ and $t_{k + 1}$ , a DVL factor connecting $f^{DVL}$ at time $t_{j}$ , an MCP factor connecting $f^{MCP}$ at time $t_{l}$ , a TAN factor connecting $f^{TAN}$ at time $t_{p}$ , and a DM factor connecting $f^{DM}$ at time $t_{n}$ . The factor graph model is able to provide a plug and play capability, which means the model simply adds or refrains from adding the associated factor nodes from sensors, with strong flexibility and expansibility.

The conventional IMU factor $f^{IMU}$ is described as follows. The given IMU measurement $z_{k}^{IMU} \dot{=} {f^{b}, ω^{b}}$ and the variable node $x_{k}$ are used to predict the next variable node ${\hat{x}}_{k + 1}$ from the time propagation $x_{k + 1} = h (x_{k}, z_{k}^{IMU})$

\begin{array}{l} f^{I M U} (x_{k + 1}, x_{k}) ≐ d (x_{k + 1} - h (x_{k}, z_{k}^{I M U})) \\ = \exp (- \frac{1}{2} {‖ {\hat{x}}_{k + 1} - h (x_{k}, z_{k}^{I M U}) ‖}_{Λ_{i}}^{2}) \end{array}

(6)

where the operator $d (\cdot)$ is defined as a certain cost function. The squared Mahalanobis distance refers to $‖ e ‖_{Λ}^{2} \overset{Δ}{=} e^{T} Λ^{- 1} e$ , with $Λ$ being the process noise covariance matrix.

The DVL measurement is introduced as a wonderful example to demonstrating how easily the factor graph model accommodates factors for DVL measurements. The DVL sensor can provide the corresponding velocity data of the AUV in the body frame, and the measurement equation is given by

z_{j}^{DVL} = h^{DVL} (x_{j}) + n^{DVL}

(7)

where $h^{DVL}$ is the prediction DVL measurement function, describing the difference between the measurement $z_{j}^{DVL}$ to the underwater vehicles’ velocity at the time $t_{j}$ . $n^{DVL}$ is the DVL measurement noise. Therefore, the above equation defined a unary factor $f^{DVL}$ , which only connects to the variable node $x_{j}$ written as

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

(8)

where $Σ$ is the measurement noise covariance matrix. In a similar manner, when the measurements information of other aiding navigation sensors is obtained at other times, operating at different rates, the corresponding factors are defined and connect to corresponding variable nodes. In this way, the factor graph supports a plug and play capability to incorporate multi-rate and asynchronous sensors by adding or refraining from adding appropriate factors and variable nodes.

In fact, there is no interaction between the SINS and DVL states because they are two different types of states. The physical attributes of the SINS states are determined by its basic working principle based on the Newton’s laws of motion, while the DVL states only contain geometric information of bias, such as the horizontal installation bias from the inertial frame to the body frame. Furthermore, there is a high degree of coupling inside or between adjacent epochs the SINS states themselves, whereas the relationship of the DVL states is much weaker. Therefore, a small improvement of the factor graph is expected by removing from the DVL states of corresponding variable nodes and without impacting the SINS states. What’s more, the observability of position, velocity, and attitude are critical priorities rather than that of DVL state in the navigation application. In conclusion, in our revised factor graph approach, the variable node just contains SINS states in common.

Equivalent IMU factor

However, this seems to be an expensive computation and storage that should be avoided because of the high dimension of variable nodes. Let us assume that the update frequency of IMU and other sensors is 200 and 1 Hz (only for statistical convenience), respectively. Thus, we need to add 200 IMU factor nodes and 200 variable nodes into the factor graph model within 1 second, which means 200 times navigation solution within 1 second, including 200 times high-dimensional matrix message passing for the state transition matrix and the error covariance matrix.

A reasonable initial value for $x_{k + 1}$ can be taken from the transition matrix $F$ only related to the motion between two consecutive time instance. The change of parameters caused by the gentle motion of AUV is small enough to consider that the translation matrix $F$ is a constant matrix before the next measurement.

Thus, an equivalent IMU factor $f^{IMU_equiv}$ is introduced to accommodate several consecutive conventional IMU factors between two time instances $t_{i}$ and $t_{j}$ , which can help further aid high-rate navigation solutions. The accumulated change in attitude, velocity, and position between two time instances $t_{i}$ and $t_{j}$ is computed as follows

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

(9)

where $T$ is the time between two adjacent measurements, and $Φ$ is the state transition matrix. In this method, only 200 times cumulative operations of 15-dimensional matrix and 1 update of the error covariance matrix are required within 1 second. The number of variables and factor nodes is reduced drastically by speeding up the estimation process of the accumulated errors of SINS.

The factor graph model using an equivalent IMU factor is shown in Figure 3. In contrast to Figure 2, variable nodes are introduced to the factor graph model only when a new factor $f^{IMU_equiv}$ is added at a much slow frequency determined by the other aiding sensors.

Figure 3.

Simplified factor graph model of Figure 2.

Factor graph optimization algorithm

The above factor graph model solves the multi-source information fusion problem of AUV integrated navigation system. This section builds a factor graph optimization algorithm using the combination of the forward and backward message passing in the factor graph model to further improve the navigation accuracy and smoothness.

Gaussian message passing in factor graph models

Assume that all messages obey multivariate Gaussian probability density distributions in the factor graph model of AUV multi-source integrated navigation system (Wu et al., 2015). We can pass and update the Gaussian message using the message passing algorithm (the sum-product algorithm) in the factor graph model of AUV multi-source integrated navigation system to obtain the optimal estimation of navigation solutions (Loeliger et al., 2007). The Gaussian message passing in the factor graph model of Figure 3 is indicated in Figure 4. The factor graph model passes messages through edges between nodes. Black solid arrow represents the direction of the forward message passing; black dotted arrow indicates the direction of the backward message passing.

Figure 4.

Message passing in Figure 3.

In the framework of Kalman filter, the linearized state-space error model of AUV integrated navigation system is discretized as

x_{k} = Φ_{k / k - 1} x_{k - 1} + Γ_{k / k - 1} w_{k}

(10)

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

(11)

where the subscript $k$ denotes the kth epoch; $x$ is the error vector of navigation state; $Φ$ denotes the state transition matrix of the dynamic model; $z$ denotes the measurement vector obtained by some auxiliary sensors; $H$ is the measurement matrix. The process noise vector $w$ and the measurement noise vector $v$ are assumed to be uncorrelated Gaussian white noise processes with both zero-mean and covariance matrices $Q$ and $R$ .

According to equations (10) and (11), a more detailed factor graph model is shown in Figure 5, where black solid and red dashed arrows represent the forward and backward propagation direction of the factor graph, respectively. Like the Kalman filtering, the whole factor graph can be divided into the following two processes: the time update and the measurement update.

Figure 5.

The detailed factor graph model.

In the factor graph model, a message can be described by the mean vector $m$ and the covariance matrix $V$ (or the weight matrix $W \overset{Δ}{=} V^{- 1}$ and the transformed mean $Wm$ ). The forward message of the variable node is represented by mean ${\overset{\leftarrow}{m}}_{x}$ , covariance matrix ${\overset{\leftarrow}{V}}_{x}$ , and weight matrix ${\overset{\leftarrow}{W}}_{x} = {\overset{\leftarrow}{V}}_{x}^{- 1}$ ; the backward message of the variable node is represented by mean ${\overset{\leftarrow}{m}}_{x}$ , covariance matrix ${\overset{\leftarrow}{V}}_{x}$ , and weight matrix ${\overset{\leftarrow}{W}}_{x} = {\overset{\leftarrow}{V}}_{x}^{- 1}$ . All messages in the factor graph model can be computed using Table 1. $(.)^{H}$ denotes the Hermitian transposition.

Table 1.

Gaussian message in elementary nodes.

An equality constraint node	${\vec{W}}_{Z} = {\vec{W}}_{X} + \overset{\leftarrow}{W}_{Y}$	(12)
	$\overset{\leftarrow}{W}_{X} = \overset{\leftarrow}{W}_{Z} + \overset{\leftarrow}{W}_{Y}$	(13)
	${\vec{W}}_{Z} {\vec{m}}_{Z} = {\vec{W}}_{X} {\vec{m}}_{X} + \overset{\leftarrow}{W}_{Y} \overset{\leftarrow}{m}_{Y}$	(14)
	${\overset{\leftarrow}{W}}_{X} {\overset{\leftarrow}{m}}_{X} = {\overset{\leftarrow}{W}}_{Z} \overset{\leftarrow}{m}_{Z} + {\overset{\leftarrow}{W}}_{Y} {\overset{\leftarrow}{m}}_{Y}$	(15)
A zero-sum constraint node	${\vec{V}}_{Z} = {\vec{V}}_{X} + {\vec{V}}_{Y}$	(16)
	${\overset{\leftarrow}{V}}_{X} = {\overset{\leftarrow}{V}}_{Z} + {\vec{V}}_{Y}$	(17)
	${\vec{m}}_{Z} = {\vec{m}}_{X} + {\vec{m}}_{Y}$	(18)
	${\overset{\leftarrow}{m}}_{X} = {\overset{\leftarrow}{m}}_{Z} - {\vec{m}}_{Y}$	(19)
A proportion constraint node	${\vec{V}}_{Y} = A {\vec{V}}_{X} A^{H}$	(20)
	${\vec{m}}_{Y} = A {\vec{m}}_{X}$	(21)
	${\overset{\leftarrow}{W}}_{X} = A^{H} {\overset{\leftarrow}{W}}_{Y} A$	(22)
	${\overset{\leftarrow}{W}}_{X} {\overset{\leftarrow}{m}}_{X} = A^{H} {\overset{\leftarrow}{W}}_{Y} {\overset{\leftarrow}{m}}_{Y}$	(23)

Combination of forward and backward message passing

In fact, the forward message passing process of the factor graph is a forward filtering process from the starting time epoch to the current time epoch. The current navigation state is estimated using all measurement information of the current moment and the previous moment. The backward message passing process of the factor graph is a backward filtering process from the ending time epoch to the current time epoch. The current navigation state is estimated using all measurement information of the current moment and future moment. The factor graph optimization algorithm is appropriate weighted smoothing of the forward and backward message passing processes

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

(24)

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

(25)

where the mean vector $m_{x_{k}}$ and the covariance matrix $V_{x_{k}}$ are the smoothed results of the factor graph optimization.

Equation (24) obviously indicates that the smoothed covariance of the factor graph optimization is not greater than the covariance of either forward or backward message passing process. Theoretically, the factor graph optimization algorithm realizes the message optimization of the mean and variance, and obtains the better accuracy and smoothness for navigation solution than both forward and backward message passing processes of the factor graph. It makes full use of all measurement information obtained within a fixed interval to estimate the navigation state at every epoch, reflecting the potential accuracy that system can achieve under ideal conditions. This technology hardly needs any hardware changes, and only needs to record the required navigation information during AUV underwater operations, which is a simple and practical post-processing technology.

In the case of short-term failure of measurement information, the calculation process of the post-processing algorithm based on factor graph optimization is shown as Figure 6.

Figure 6.

Calculation process in the case of a measurement outage.

In conclusion, the detailed steps of the factor graph optimization algorithm are as follows:

Step 1: forward initialization. At the starting time epoch, the initial message of the forward message passing process is set as a multivariate Gaussian probability density distribution with mean vector ${\vec{m}}_{x_{0}}$ and covariance matrix ${\vec{V}}_{x_{0}}$ , which is determined by the SINS initial alignment.

Step 2: forward message passing process. Pass and update Gaussian messages using the sum-product algorithm from the starting time epoch to the ending time epoch. All Gaussian messages of variable nodes at every time epoch are stored in time during the forward message passing process.

Step 3: backward initialization. Once the Gaussian message reaches the ending time epoch, the end message of the forward message passing process with ${\overset{\leftarrow}{m}}_{x_{N}} = {\vec{m}}_{x_{N}}$ and ${\overset{\leftarrow}{V}}_{x_{N}} = {\vec{V}}_{x_{N}}$ is set as the prior message of the backward message passing.

Step 4: backward message passing process. Pass and update Gaussian messages using the sum-product algorithm from the (N−1)th epoch to the starting time epoch. All Gaussian messages of variable nodes at every time epoch are stored in time during the backward message passing process.

Step 5: optimization estimation. In combination with the forward and backward message passing processes, equations (24) and (25) are used for information fusion to achieve the optimal estimation with mean $m_{x_{k}}$ and covariance $V_{x_{k}}$ .

Experiments

In this section, a simulation and a vehicle test were conducted to verify and evaluate the performance of the proposed post-processing algorithm based on factor graph optimization algorithm for AUV multi-source integrated navigation system.

Simulations and results

The effectiveness and reliability of the proposed post-processing algorithm based on factor graph optimization are verified by simulation on the MATLAB platform. The common sensors’ settings are as follows. IMU measurements were generated by the gyroscope and the accelerometer at 100 Hz. The gyroscope constant biases in each axis are all set as 0.02°/h and the random biases are white Gaussian noise with zero mean and standard deviation 0.01°/h; the accelerometer constant biases in each axis are all set as 100 µg and the random biases are white Gaussian noise with zero mean and standard deviation 10 µg. IMU measurements are kept active during the entire simulation period since SINS is working autonomously with strong anti-interference capability. Other aiding navigation sensors measure data at a relatively low frequency than IMU, and asynchronous. The DVL measurements are generated every 1 second, and inactive between 150 and 350 seconds. The TAN measurements are generated every 3 seconds, and inactive between 250 and 400 seconds. The MCP measurements are generated every 0.5 second, and inactive between 800 and 850 seconds. The DM measurements are generated every 2 seconds, and inactive between 650 and 700 seconds.

There are 100 times statistically independent realizations composed the Monte Carlo simulation. Only one of these simulations is shown in this paper. Comparisons of the navigation parameters are given in Figures 7 –9. The performance of the proposed method (the blue solid line) is compared to the federal Kalman filtering (the pink dashed line), the un-simplified factor graph (the green dot dash line), and the simplified factor graph (the red dotted line). The statistical Root Mean Squared Error (RMSE) of navigation parameters is shown in Table 2.

Figure 7.

Comparison of attitude errors.

Figure 8.

Comparison of velocity errors.

Figure 9.

Comparison of position errors.

Table 2.

Statistical RMSE of navigation parameters.

Navigation parameters	Federal Kalman filtering	Un-simplified factor graph	Simplified factor graph	Factor graph optimization
$ϕ_{x} / (')$	0.2489	0.1906	0.2303	0.1253
$ϕ_{y} / (')$	0.2485	0.2153	0.2425	0.0667
$ϕ_{z} / (')$	0.1230	0.1222	0.1228	0.1177
$δ V_{N} / (m / s)$	0.0253	0.0196	0.0212	0.0069
$δ V_{E} / (m / s)$	0.0463	0.0291	0.0315	0.0062
$δ V_{D} / (m / s)$	0.0065	0.0051	0.0060	0.0008
$δ X / (m)$	1.4903	1.1009	0.9962	0.3531
$δ Y / (m)$	2.0611	1.2888	1.3345	0.3455
$δ H / (m)$	0.0933	0.0664	0.0764	0.0456

In order to illustrate positioning performance in Figure 9, the performance metrics of latitude and longitude errors in meters are further defined as follows

{\begin{matrix} Δ X = (\bar{L} - L) (R_{n} + h) \\ Δ Y = (\bar{λ} - λ) (R_{e} + h) \cos L \end{matrix}

(27)

where $L$ and $λ$ indicate the reference latitude and longitude; $\bar{L}$ and $\bar{λ}$ represent the latitude and longitude obtained by the factor graph optimization and federal Kalman filter algorithm; and $h$ denotes the depth information provided by the DM sensor equipped on AUV.

From Figures 7 –9, it can be seen that in the entire simulation period, the proposed factor graph optimization algorithm can provide better accuracy and smoothness for the navigation solution compared with the other three methods. Its validity and reliability are verified. The covariance of the forward message passing process should converge from front to back, and that of the backward message passing process should converge from back to front. Therefore, the weighted smoothing covariance of the proposed factor graph optimization algorithm keeps small relatively. This advantage mainly lies in the clearance of navigation sensors update, which can effectively reduce the accumulation of errors in SINS prediction process.

The navigation accuracy of the federal Kalman filtering method is slightly lower than the un-simplified and simplified factor graph methods. This is because the information fusion of the federal Kalman filter can be only performed until all available measurement information had been collected. The asynchronous heterogeneous measurement information undoubtedly reduced the navigation accuracy at every time step of information fusion. However, the plug and play capability of the factor graph model can easily solved this problem by simply adding or rejecting adding related factors. The simplified factor graph method was almost coincided with the un-simplified factor graph method. It illustrated that the equivalent IMU factor reduced the computing load and improved computational efficiency on the premise of ensuring the navigation accuracy. Hence, the simplified factor graph method laid the foundation for the post-processing process.

When sensor fault or signal loss, the proposed factor graph optimization method greatly reduces the influence of on the navigation system. For example, during 250∼300 seconds, the horizontal velocity and position errors of the federal Kalman filtering, un-simplified factor graph, and simplified factor graph methods grow rapidly with time. The former is because the corresponding SINS/DVL and SINS/TAN sub-filters have to be separated to allow the system reconstruction, which is an expensive operation. The latter two is because the factor graph model refrains from adding the DVL and TAN factor nodes. Therefore, only forward processing of data is easy to deteriorate due to the quality and quantity of available measurement information. However, in this period, the horizontal velocity and position errors of the proposed factor graph optimization method are improved and smoothed obviously using the correction of the backward message passing process of the factor graph.

Table 2 showed quantitatively the RMSE of the navigation parameters of the four algorithms. The conclusion is consistent with the above mentioned. In the entire simulation period, compared with the federal Kalman filtering method, the latitude error in meters of the un-simplified factor graph, the simplified factor graph, and the factor graph optimization is improved by 26.13%, 33.15%, and 76.31%, respectively; the longitude errors in meters of these are improved by 37.47%, 35.25%, and 83.24%, respectively; the north velocity of these is improved by 22.53%, 16.21%, and 72.73%, respectively; the east velocity of these is improved by 37.15%, 31.97%, and 86.61%, respectively. Other navigation parameters have also been improved. Therefore, the proposed factor graph optimization is an effective post-processing algorithm.

Semi-physical experiment and results

In order to further verify the performance of the post-processing algorithm based on factor graph optimization algorithm in practical engineering, the semi-physical experiment based on the vehicle was carried out in this section. We used a data set as the reference ground truth recorded by the loosely coupled SINS/GNSS integrated inertial system, which was a combination of PHINS from iXblue Company and FlexPark6 GNSS receiver from NovAtel Company. The data set of the reference ground truth generated the attitude, velocity, and position information at a frequency of 200 Hz. The DVL, TAN, MCP, and DM measurements were simulated on MATLAB platform based on the reference navigation information, with sample intervals of 0.3, 7, 0.1, and 2 seconds, respectively.

The vehicle test route started from the parking lot of the north stadium, and drive about 1550 seconds along the road arbitrarily in the area of Jiulonghu Campus, Southeast University. The estimated trajectories in the horizontal position using the proposed factor graph optimization algorithm (the red line) against the reference (the blue line) are shown in Figure 10. Figures 11 –13 showed the estimated attitude, horizontal velocity, and horizontal position errors of the proposed factor graph optimization algorithm.

Figure 10.

Estimated trajectories of the proposed factor graph optimization algorithm against the reference.

Figure 11.

Attitude errors of factor graph optimization algorithm.

Figure 12.

Horizontal velocity errors of factor graph optimization algorithm.

Figure 13.

Horizontal position errors of factor graph optimization algorithm.

It can be seen from the above figures that, the proposed factor graph optimization method in this paper can achieve the relative high navigation accuracy in the process of data post-processing. And there was no obvious jump during the entire environment period. The smoothed heading angle error, the horizontal velocity error, and the horizontal position error were kept within the range of −10’∼10’, −0.1∼0.1 m/s, and −2∼2 m, respectively. Therefore, the validity and reliability of the proposed factor graph optimization algorithm were verified in the vehicle experiment, which was consistent with the conclusion in the simulation section.

Conclusion

This paper presented a post-processing algorithm based on factor graph optimization for AUV integrated navigation to improve the navigation accuracy and smoothness. In the framework of the factor graph model, the proposed method combined the forward and backward message passing processes to smooth the post-processing navigation state parameters, and make full use of all measurement information within a fixed interval to obtain the optimal navigation solution. The factor graph model as the framework provided a plug and play capability and addressed multi-source information fusion problem with asynchronous heterogeneous sensors. The results of the simulation and vehicle experiment show that the proposed factor graph optimization algorithm provides considerable accuracy and smoothness for navigation solutions, especially in the case of signal loss.

In the future, the work considered for cooperative fusion can be extended by adding factor nodes from the relative measurement information exchange among AUVs. The analysis done so far has many limitations. A fault detection approach based on the factor graph could be increased by considering the measurement information jumps. More research is required in simulation analysis to study the effect of systems as in sensors, algorithms, and scenarios where these methods are tested.

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, in part, by the National Natural Science Foundation of China (grant nos 51979041 and 61973079) and the Ministry of Education & Equipment Pre-research Joint Foundation (6141A02011906).

ORCID iDs

Xiao-Shuang Ma

Chen-Long Li

References

Allotta

Caiti

Cotanzi

, et al. (2016) A new AUV navigation system exploiting unscented Kalman filter. Ocean Engineering 113: 121–132.

Cox

Van

Laar

De Vries

(2019) A factor graph approach to automated design of Bayesian signal processing algorithms. International Journal of Approximate Reasoning 104: 185–204.

Guo

Feng

, et al. (2019) Fault tolerant multi-sensor federated filter for AUV integrated navigation. In: IEEE underwater technology (UT) conference, Kaohsiung, Taiwan, 16–19 April, pp. 1–4.New York: IEEE.

Hagen

Midtgaard

Hasvold

, et al. (2007) Making AUVs truly autonomous. In: 2007 oceans conference, Vancouver, BC, Canada, 29 September–4 October, pp. 1928–1931. New York: IEEE.

Indelman

Williams

Kaess

, et al. (2013) Information fusion in navigation systems via factor graph based incremental smoothing. Robotics and Autonomous Systems 61(8): 721–738.

Jalal

Nasir

(2021) Underwater navigation, localization and path planning for autonomous vehicles: A review. In: 2021 international Bhurban conference on applied sciences and technologies (IBCAST), Islamabad, Pakistan, 12–16 January, pp. 817–828. New York: IEEE.

Liu

Zhang

, et al. (2021) A new coupled method of SINS/DVL integrated navigation based on improved dual adaptive factors. IEEE Transactions on Instrumentation and Measurement 70: 8504211.

Loeliger

(2004) An introduction to factor graphs. IEEE Signal Processing Magazine 21(1): 28–41.

Loeliger

Dauwels

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

10.

Liu

, et al. (2019a) Multi-source information fusion based on factor graph in autonomous underwater vehicles navigation systems. Assembly Automation 41(5): 536–545.

11.

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.

12.

Liu

Zhang

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

13.

, et al. (2021) A practical INS/GPS/DVL/PS integrated navigation algorithm and its application on autonomous underwater vehicle. Applied Ocean Research 106(3): 102441.

14.

Prasanth

(2021) Certain investigations on energy-efficient fault detection and recovery management in underwater wireless sensor networks. Journal of Circuits Systems and Computers 30(8): 2150137.

15.

Sahoo

Dwivedy

Robi

(2019) Advancements in the field of autonomous underwater vehicle. Ocean Engineering 181: 145–160.

16.

Salavasidis

Munafo

Harris

, et al. (2018) Terrain-aided navigation for long-endurance and deep-rated autonomous underwater vehicles. Journal of Field Robotics 36(2): 447–474.

17.

Sanchez

PJB

Papaelias

Marquez

FPG

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

18.

Song

Liu

Guo

, et al. (2022) Efficient velocity estimation and location prediction in underwater acoustic sensor networks. IEEE Internet of Things Journal 9(4): 2984–2998.

19.

Wang

Gao

Zhang

(2021a) Data-driven performance-prescribed reinforcement learning control of an unmanned surface vehicle. IEEE Transactions on Neural Networks and Learning Systems 32(12): 5456–5467.

20.

Wang

Gao

Chen

, et al. (2021b) Reinforcement learning-based finite-time tracking control of an unknown unmanned surface vehicle with input constraints. Neurocomputing 484: 26–37.

21.

Wang

, et al. (2015) Distributed cooperative localization based on Gaussian message passing on factor graph in wireless networks. Science China-Information Sciences 58(4): 042305.

22.

Xiao

, et al. (2019) Survey of underwater robot positioning navigation. Applied Ocean Research 90: 101845.

23.

Zhong

(2017) Review and prospect of equipment and techniques for unmanned undersea vehicle in foreign countries. Journal of Unmanned Undersea Systems 25(3): 215–225.

24.

Zhou

Lai

Guo

, et al. (2015) A research on all source navigation and positioning and its critical technology. In: 6th China satellite navigation conference (CSCN), Xian, China, 13–15 May, pp. 801–808. New York: Springer.