A Robust INS/USBL/DVL Integrated Navigation Algorithm Using Graph Optimization

The Autonomous Underwater Vehicle (AUV) is usually equipped with multiple sensors, such as an inertial navigation system (INS), ultra-short baseline system (USBL), and Doppler velocity log (DVL), to achieve autonomous navigation. Multi-source information fusion is the key to realizing high-precision underwater navigation and positioning. To solve the problem, a fusion scheme based on factor graph optimization (FGO) is proposed. Due to multiple iterations and joint optimization of historical data, FGO could usually show a better performance than the traditional Kalman filter. In addition, considering that USBL and DVL are usually heavily influenced by the environment, outliers are often present. A robust integrated navigation algorithm based on a maximum correntropy criterion and FGO scheme is proposed. The proposed algorithm solves the problem of multi-sensor fusion and non-Gaussian noise. Numerical simulations and field tests demonstrate that the proposed FGO scheme shows a better performance and robustness than the traditional Kalman filter. Compared with the traditional Kalman filtering, the positioning accuracy is improved by 5.3%, 9.1%, and 5.1% in the east, north, and height directions. It can realize a more accurate navigation and positioning of underwater multi-sensors.


Introduction
Underwater navigation has always been a challenging research hotspot [1]. Navigation based on satellite signals is often unavailable due to the limited underwater environment. An inertial navigation system (INS) is an autonomous navigation model [2], free from environmental interference. It can be applied to underwater navigation. However, inertial navigation has the problem of a cumulative divergence of errors over time. Error suppression of inertial navigation is always a difficult problem in underwater navigation and positioning.
Acoustic navigation is an effective underwater navigation scheme. The ultrashort baseline system (USBL) is an alternative navigation method. Therefore, inertial navigation assisted by an ultra-short baseline [3,4] has been a research hotspot of underwater navigation in recent years. Back in 2006, Morgado first proposed an INS/USBL integrated navigation model [5], in which a simplified INS mechanization is used to conduct the state model. In 2013, experiments verified the positioning accuracy of integrated navigation [6]. However, the mathematical model of INS is simplified, and the accuracy of integrated navigation is limited. To improve the accuracy of underwater navigation based on USBL, Zhang proposed a loosely coupled integrated navigation model based on a high-precision strap-down inertial navigation mechanical model [7]. In [8], Zhang also proposed a tightly coupled model based on relative measurement information, which has a better performance than the loosely coupled model. However, the positioning range of the USBL is limited.
In addition to USBL, another common method of underwater navigation is the Doppler velocity log (DVL). The research on INS/DVL focuses mainly on error calibration [9], initial (1) An optimization-based INS/USBL/DVL integrated navigation model is proposed.
Measurement factors such as the IMU factor, USBL factor, and DVL factor are designed. It could play a superior performance to the traditional filter method. (2) A robust estimation method combining the maximum entropy criterion and FGO is proposed. It can overcome the influence of outliers and improve the robustness of the integrated system. (3) Experiments have been conducted to show that it can achieve a more accurate and robust localization than the traditional methods for underwater positioning.

System Model
The coordinate systems used in the paper include a body frame (b-frame), navigation frame (n-frame), USBL frame (u-frame), and geographic frame (g-frame). Detailed definitions can be found in [8].
The AUV is equipped with a USBL and DVL, as shown in Figure 1.

System Model
The coordinate systems used in the paper include a body frame (b-frame), navigation frame (n-frame), USBL frame (u-frame), and geographic frame (g-frame). Detailed definitions can be found in [8].
The AUV is equipped with a USBL and DVL, as shown in Figure 1.

USBL DVL
Transponder Sea Bed The transducer of USBL on AUV sends an acoustic signal to the underwater transponder, which receives the signal and returns it to the transducer. The slant distance (r) is calculated from the round-trip time of the acoustic signal. The bearing angles ( , β) are measured according to the angle-of-arrival.
Thus, the relative position of the transponder in the u-frame is calculated by: The transponder position ( = [ ℎ ] ) is known. Thus, the position of AUV ( ) can be obtained: where denotes the installation error matrix from the u-frame to b-frame. It can be calibrated in advance [23].
is the attitude matrix. denotes the coordinate transformation matrix from the n-frame to g-frame, which is represented as follows: where the definitions of and can be seen in [2]. h denotes the height. The DVL also emits sound waves to the ocean floor, as Figure 1 shows. Assume that the frequency of transmitting acoustic signals is 0 . The angle between the beam and the horizontal plane is a. The frequency of sound waves as they reach the seafloor is 1 . The frequency is 2 when it reaches AUV again after reflection, and the angle between the beam and the horizontal plane is b. According to the principle of Doppler frequency shift, 1 can be obtained: where 0 is the sound speed. is the moving speed in the y direction in the b-frame.
2 can be obtained: The transducer of USBL on AUV sends an acoustic signal to the underwater transponder, which receives the signal and returns it to the transducer. The slant distance (r) is calculated from the round-trip time of the acoustic signal. The bearing angles (α, β) are measured according to the angle-of-arrival.
Thus, the relative position of the transponder in the u-frame is calculated by: The transponder position (t g = λ t L t h t T ) is known. Thus, the position of AUV (p g ) can be obtained: where C b u denotes the installation error matrix from the u-frame to b-frame. It can be calibrated in advance [23]. C n b is the attitude matrix. C g n denotes the coordinate transformation matrix from the n-frame to g-frame, which is represented as follows: where the definitions of R N and R M can be seen in [2]. h denotes the height. The DVL also emits sound waves to the ocean floor, as Figure 1 shows. Assume that the frequency of transmitting acoustic signals is f 0 . The angle between the beam and the horizontal plane is a. The frequency of sound waves as they reach the seafloor is f 1 . The frequency is f 2 when it reaches AUV again after reflection, and the angle between the beam and the horizontal plane is b. According to the principle of Doppler frequency shift, f 1 can be obtained: where c0 is the sound speed. v y is the moving speed in the y direction in the b-frame. f 2 can be obtained: Thus, the velocity of AUV in the b-frame can be obtained [24]: where

FGO-Based Integrated Navigation Algorithm
In this section, we will propose a robust integrated navigation algorithm based on FGO and the maximum entropy criterion.
In an integrated navigation system, the state estimation problem can be formulated as: wherex denotes the optimal state estimation. x k represents the system state at time k. z k represents the measurement vector at time k.
In FGO, we combine the measurements of all historical nodes, and the optimal estimation problem can be treated as a maximum likelihood estimation (MAP) problem. It can be obtained:x where f j x j is the j-th factor. It can be formulated as: where h j x j is the measurement function and z j is the j-th measurement. Σ j is the covariance matrix. Thus, (8) is equal to the following [25]: Based on the above FGO theory, we organized the graph model of the integrated navigation as Figure 2 follows: Sensors 2023, 23, x FOR PEER REVIEW 5 of 16  The factors involved in the graph model will be analyzed in detail in the following. The algorithm is designed in two parts. One is the design of the measurement factors, including the IMU pre-integration factor, USBL factor, and DVL factor. With the design of the factors, the graph model of FGO can be conducted. The other is the design of the outlier elimination model. It can suppress the influence of outliers and improve the robustness of the system.

IMU Pre-Integration Factor
The output of IMU can be modeled as follows: The factors involved in the graph model will be analyzed in detail in the following. The algorithm is designed in two parts. One is the design of the measurement factors, including the IMU pre-integration factor, USBL factor, and DVL factor. With the design of the factors, the graph model of FGO can be conducted. The other is the design of the outlier elimination model. It can suppress the influence of outliers and improve the robustness of the system.

IMU Pre-Integration Factor
The output of IMU can be modeled as follows: whereω b ib and f b are the actual output of IMU. b g is the gyroscope bias, and b a is the accelerometer bias. ω b ib and f b are the ideal outputs of IMU. n g is the white noise of the gyroscope, and n a is the white noise of the accelerometer.
Assume that the interval of pre-integration is [t k , t k+1 ]. The pre-integration measurements using the output of IMU can be expressed as follows: where: The position's pre-integration is expressed as: where P n k denotes the position at epoch m. V n k denotes the velocity at epoch m. g n denotes the gravity vector in the n-frame. ∆t k,k+1 is the time interval from t k to t k+1 . ω n en and ω n ie are expressed as follows: where ω ie is the earth rotation rate. The velocity pre-integration with respect to the states is expressed as: The attitude's pre-integration with respect to the states is expressed as: where q nm+1 nm is updated as follows: If the estimation of bias changes, the pre-integration measurements can be expressed as follows: where J α bg , J α ba , J β bg , J β ba , J γ bg are the sub-matrix, which can be seen in [18]. Thus, the corresponding IMU pre-integration residuals can be expressed as:

USBL Factor
The USBL measurements contain the slant distance and bearing angles. According to (2), the slant distance can be calculated as: The bearing angles can be calculated as: Thus, the USBL residuals can be expressed as:

DVL Factor
The DVL measures the speed in the b-frame. It has the following relationship with the AUV velocity: where n dvl is the measurement noise, which obeys the Gaussian distribution. Thus, the residuals of the DVL factor can be expressed as: Based on (20), (23) and (25), the optimization problem can be expressed as: where Σ imu,k , Σ usbl,k , and Σ dvl,k are the measurement covariance. r p − H p X 2 Σ p is the prior factor, and Σ p is the prior covariance.

Outlier Elimination Algorithm
To reduce the influence of outliers, the residual processing method based on the maximum correlation entropy criterion is used in FGO.
The correlation entropy between two random variables X and Y can be defined as: where E[·] is the expectation. κ(X, Y) denotes the kernel function. F(X, Y) denotes the joint probability density function of the random variables X and Y.
The Gaussian function is selected as the Kernel function, which is as follows: where {x i , y i } N i=1 means the N samples satisfying the joint probability density F(X, Y). e i = x i − y i . σ denotes the bandwidth of the kernel function.
The estimated value of correlation entropy can be expressed as: In (26), r usbl (Z usbl,k , X) and r dvl (Z dvl,k , X) can be formulated as: where Z usbl,k and Z dvl,k are measurements. H usbk,k and H dvl,k are the Jacobian matrix. The optimization function can be expressed as: Let e usbl,k = Σ −1/2 usbl,k (Z usbl,k − H usbk,k X), e dvl,k = Σ −1/2 dvl,k (Z dvl,k − H dvl,k X). Then, (31) can be reformulated as follows: where l and m are the dimensions of the measurement.
The Gaussian Kernel of the maximum entropy criterion is introduced into the measured part of the cost function.
In (36), the residual function r imu (Z imu,k , X) is from the IMU factor, as (20) shows. The residual function Z usbl,k − H usbk,k X is from the USBL factor, as (23) shows, and Z dvl,k − H dvl,k X is from the DVL factor, as (25) shows.
The integrated navigation algorithm is implemented by (36). It is a typical MAP problem. When the measurements of DVL, USBL, and IMU are obtained, the residual function can be formulated as (36). The object is to obtain the optimal solution of X by solving Equation (36). The state X contains the position, velocity, and attitude information of the vehicles.
The Ceres Solver [26] is used to solve the MAP problem. The Jacobian matrix of USBL and DVL are computed by the automatic derivative function of Ceres.

Simulation and Field Test
Simulation and field tests will be conducted in this section. First, a simulation test with different measurement noise values is conducted. The simulation test is necessary as the sensor data is ideal, and it can verify the accuracy of the model and algorithm. Second, a field test is conducted. The field test was carried out in the Yangtze River, and it can verify the effectiveness of the algorithm in the real world and prove its practicality. Thus, both tests are necessary.
All algorithms run on a laptop equipped with AMD Ryzen 7 5800U CPU, whose Radeon Graphics is 1.90 GHz and 16 G RAM.

Simulation Test
In this section, a series of simulation experiments are designed to verify the effectiveness and improvement of the proposed method.
Symbols used in the simulation experiment are listed as follows: 'KF' represents the traditional Kalman filter. 'HKF' denotes the robust Kalman filter based a Huber M-estimation. 'FGO' denotes the traditional FGO algorithm as described in [27]. 'RFGO' denotes the proposed robust FGO algorithm based on the maximum correlation entropy criterion.
In the simulation, the bias of the gyroscope is 0.01 • /h, and its random walk noise is 0.005 • / √ h. The bias of the accelerometer is 100 µg, and its random walk noise is 50ug/ √ Hz. The measurement noise of the system is set as follows: where V 1 , V 2 , and V 3 are the bearing, slant distance noise, and DVL velocity noise, respectively, which obey the Gaussian distribution. w.p. 0.95 denotes the normal data, which appears with probabilities of 95%, and w.p. 0.05 denotes the outlier's probability, which is 5% [28].
In the integrated navigation system, the initial attitude error is set as 0.05 The measurement covariance matrixes of USBL and DVL are set to be the same in these methods, as shown below: The simulation trajectory of the vehicle is shown in Figure 3.  The velocity of the AUV is set as 2 m/s, and the transponder is shown as the purple circle in Figure 3.
The position errors of different algorithms are shown in Figure 4. The velocity of the AUV is set as 2 m/s, and the transponder is shown as the purple circle in Figure 3.
The position errors of different algorithms are shown in Figure 4. In Figure 4, the KF method without a robust algorithm suffers from the influence of outliers. The HKF method can suppress the influence of outliers. The Huber-based approach is limited in its ability to handle large outliers. Thus, it shows a worse performance than the proposed method. Even the traditional FGO method shows a worse performance than the KF method, as the yellow line shows. This is because the INS mechanization in [27] is simplified, with the angular rate ω n en being ignored. The estimation accuracy is not as high as that of the Kalman filtering method in high-precision inertial navigation. The proposed RFGO method shows a superior performance to the Kalman-filter-based method due to multiple iterations and a joint optimization of historical data. The velocity of the AUV is set as 2 m/s, and the transponder is shown as the purple circle in Figure 3.
The position errors of different algorithms are shown in Figure 4. In Figure 4, the KF method without a robust algorithm suffers from the influence of outliers. The HKF method can suppress the influence of outliers. The Huber-based approach is limited in its ability to handle large outliers. Thus, it shows a worse performance than the proposed method. Even the traditional FGO method shows a worse performance In order to quantitatively analyze the estimation results, RMSE is used as the performance metric.
The RMSE results of different methods are listed in Table 1. In Table 1, RFGO performs best among these methods. The position accuracy is improved by 56.9%, 11.8%, and 70.4% compared with the KF, HKF, and FGO in the east direction, respectively. It is also improved by 62.4%, 35.4%, and 67.8% in the north direction and by 23.4%, 22.2%, and 28.8% in the up direction.
To make the results more convincing, we conducted statistics on the results of 50 independent experiments.
The RMSE of the Monte Carlo simulation test is used as the performance metric. The error bar of different algorithms is shown in Figure 5. In Figure 5, the proposed method has the highest accuracy and the most obvious performance improvement. In the case of nonlinearity and outliers, the advantage of our method is more obvious. direction, respectively. It is also improved by 62.4%, 35.4%, and 67.8% in the north direction and by 23.4%, 22.2%, and 28.8% in the up direction.
To make the results more convincing, we conducted statistics on the results of 50 independent experiments.
The RMSE of the Monte Carlo simulation test is used as the performance metric. The error bar of different algorithms is shown in Figure 5. In Figure 5, the proposed method has the highest accuracy and the most obvious performance improvement. In the case of nonlinearity and outliers, the advantage of our method is more obvious.

Field Test
In this section, a field test is designed to verify the effectiveness and improvement of the proposed method. The field test was carried out in the Yangtze River.
The equipment used in the test includes the USBL, DVL, IMU, and RTK GPS/PHINS integrated system, where the output of the RTK GPS/PHINS integrated system is used as the true value. As Figure 6 shows:

Field Test
In this section, a field test is designed to verify the effectiveness and improvement the proposed method. The field test was carried out in the Yangtze River.
The equipment used in the test includes the USBL, DVL, IMU, and RTK GPS/PHIN integrated system, where the output of the RTK GPS/PHINS integrated system is used the true value. As Figure 6 shows: In the field test, the transponder position and the installation error of USBL [29], well as the scale factor of DVL [30], are calibrated in advance.

GPS
The performance of the sensors is as Table 2 follows.  In the field test, the transponder position and the installation error of USBL [29], as well as the scale factor of DVL [30], are calibrated in advance.
The performance of the sensors is as Table 2 follows. In the field test, the initial navigation errors are set as follows: The measurement covariance matrixes of USBL and DVL are set to be the same in these methods, as shown below: R usbl = diag( 1 7e − 4 7e − 4 ) and R dvl = 0.1I 3×3 . The vehicle trajectory in the field test is shown in Figure 7. The position error of different algorithms is shown in Figure 8. In Figure 8, due to multiple iterations, the joint optimization of historical data, and the robust module based on the maximum correntropy criterion, the RFGO method out- The position error of different algorithms is shown in Figure 8. The position error of different algorithms is shown in Figure 8. In Figure 8, due to multiple iterations, the joint optimization of historical data, and the robust module based on the maximum correntropy criterion, the RFGO method outperforms the traditional Kalman-filter-based method. The traditional FGO method performs the worst due to the simplified INS mechanization and lack of a robust module. To In Figure 8, due to multiple iterations, the joint optimization of historical data, and the robust module based on the maximum correntropy criterion, the RFGO method outperforms the traditional Kalman-filter-based method. The traditional FGO method performs the worst due to the simplified INS mechanization and lack of a robust module. To quantitatively analyze the estimation results, RMSE is used as the performance metric.
The RMSE results of different methods are listed in Table 3. The position errors of the RFGO method are significantly smaller than those of the Kalman filtering algorithm and the traditional FGO method. The proposed method has a higher positioning accuracy.
Computational burden is also one of the factors that reflect the performance of the algorithm. The execution time of different algorithms is an indicator of the computational burden, as paper [22] shows. To show the computational load of these methods, a comparison of the execution time among different algorithms is given in Figure 9.
It can be seen from Figure 9 that since only the current state is processed, the computational burden of the KF and HKF methods is minimal. The proposed FGO method is more complex than the FGO method, as the fault detection module is added. Although the proposed method is the most computationally intensive, it has the best localization performance and is an optional solution as long as the computer's performance is adequate. The RMSE results of different methods are listed in Table 3. The position errors of the RFGO method are significantly smaller than those of the Kalman filtering algorithm and the traditional FGO method. The proposed method has a higher positioning accuracy.
Computational burden is also one of the factors that reflect the performance of the algorithm. The execution time of different algorithms is an indicator of the computational burden, as paper [22] shows. To show the computational load of these methods, a comparison of the execution time among different algorithms is given in Figure 9. It can be seen from Figure 9 that since only the current state is processed, the computational burden of the KF and HKF methods is minimal. The proposed FGO method is more complex than the FGO method, as the fault detection module is added. Although the proposed method is the most computationally intensive, it has the best localization performance and is an optional solution as long as the computer's performance is adequate.

Conclusions
Underwater multi-sensor fusion is always the key to achieving a high-precision nav-

Conclusions
Underwater multi-sensor fusion is always the key to achieving a high-precision navigation and positioning of AUV. Different from the traditional filtering-based fusion scheme, we propose a factor graph optimization scheme, which is suitable for underwater INS/USBL/DVL integrated navigation systems. Multiple iterations and the re-linearization of FGO enable it to better solve the nonlinear problems of the system and improve the positioning accuracy. Considering the influence of outliers in the integrated navigation system, a robust fusion algorithm based on the maximum correlation entropy criterion in the FGO scheme is proposed. Numerical simulations and field tests demonstrate that the proposed scheme has a higher positioning accuracy and better robustness.