Adaptive SLAM Methodology Based on Simulated Annealing Particle Swarm Optimization for AUV Navigation

: Simultaneous localization and mapping (SLAM) is crucial and challenging for autonomous underwater vehicle (AUV) autonomous navigation in complex and uncertain ocean environments. However, inaccurate time-varying observation noise parameters may lead to ﬁltering divergence and poor mapping accuracy. In addition, particles are easily trapped in local extrema during the resampling, which may lead to inaccurate state estimation. In this paper, we propose an innovative simulated annealing particle swarm optimization-adaptive unscented FastSLAM (SAPSO-AUFastSLAM) algorithm. To cope with the unknown observation noise, the maximum a posteriori probability estimation algorithm is introduced into SLAM to recursively correct the measurement noise. Firstly, the Sage–Husa (SH) based unscented particle ﬁlter (UPF) algorithm is proposed to estimate time-varying measurement noise adaptively in AUV path estimation for improving ﬁltering accuracy. Secondly, the SH-based unscented Kalman ﬁlter (UKF) algorithm is proposed to enhance mapping accuracy in feature estimation. Thirdly, SAPSO-based resampling is proposed to optimize posterior particles. The random judgment mechanism is used to update feasible solutions iteratively, which makes particles disengage local extreme values and achieve optimal global effects. The effectiveness and accuracy of the proposed algorithm are evaluated through simulation and sea trial data. The average AUV navigation accuracy of the presented SAPSO-AUFastSLAM method is improved by 18.0% compared to FastSLAM, 6.5% compared to UFastSLAM, and 5.9% compared to PSO-UFastSLAM.


Introduction
Autonomous underwater vehicles (AUVs) play a vital role in ocean information acquisition, requiring robust and reliable navigation performance [1,2]. With advances in autonomous robotics, the sensor configuration of autonomous vehicle provides many options for vehicle localization and navigation, such as the LiDAR/GNSS/IMU integration systems [3,4], the GNSS/on-board sensors fusion framework [5] and the GPS/INS/DVL combination systems. However, the marine environment is complex and changeable, and the navigation and positioning of AUV are very challenging. When the global navigation satellite system (GNSS) signal fails, simultaneous localization and mapping (SLAM) provides a feasible solution for AUV navigation and positioning in unknown marine environments. The acoustic ranging equipment, such as mechanical scanning imaging sonar (MSIS), equipped with the AUV can identify underwater environmental features for mapping [6,7]. The navigation devices are used for dead reckoning (DR).
The data-fusion methods in SLAM mainly include graph optimization and filtering. The former has higher precision than the latter but slower calculation speed. At present, Graph-SLAM has emerged as a promising method for land vehicles [8][9][10]. Additionally, some researchers have applied graph optimization to AUV SLAM. The novel graph-based Due to the complex and variable seawater environment, sensors such as the inertial navigation system (INS), Doppler velocity log (DVL), and MSIS equipped with AUV can be subject to noise interference [35]. The default noise covariance does not fully conform to the time-varying noise distribution, leading to false observations and deviations in state and feature estimates. In recent years, some researchers have also proposed innovative ideas for the SLAM algorithm. The adaptive smoothing variable structure filter (ASVSF) was proposed to estimate measurement noise for SLAM systems, which uses the maximum a posteriori probability creation and weighted exponent concepts [36]. However, as a suboptimal estimation algorithm, the SVSF may have a mismatch between the innovation covariance matrix and the theoretical value. The system accuracy based on this algorithm is not high. The AEKF-SLAM method based on maximum likelihood estimation (MLE) and expectation-maximization (EM) creation was proposed to recursively estimate noise mean and covariance [37]. However, the proposed AEKF-SLAM method is still based on the EKF algorithm, which struggles to solve the Jacobian matrix. The variational Bayesian (VB) based UFastSLAM algorithm was presented to estimate measurement noise in UPF and UKF [38]. However, the proposed UFastSLAM algorithm heavily depends on model prior information, in which the empirical noise characteristics might reduce the stability of the filter.
Therefore, it is essential to present a method that combines particle optimization and noise estimation for AUV navigation. In this paper, we present an innovative simulated annealing particle swarm optimization-adaptive unscented FastSLAM (SAPSO-AUFastSLAM) algorithm, which can solve the particle global optimization loss problem, while estimating measurement noise mean and covariance in real time. The proposed algorithm can improve the AUV SLAM accuracy in complex and variable marine environments. The contributions of this paper are as follows: • To improve AUV navigation accuracy, the SAPSO-AUFastSLAM algorithm is proposed to enhance resampling while estimating time-varying measurement noise; • The Sage-Husa (SH) based UPF algorithm is proposed to estimate time-varying measurement noise adaptively in SLAM for improving the filtering accuracy. Meanwhile, the SH-based UKF algorithm is proposed in SLAM to enhance mapping accuracy; • The SAPSO-based resampling algorithm is proposed to optimize posterior particles. The random judgment mechanism is used to update feasible solutions iteratively, which makes particles disengage the local extreme values and achieves optimal global effects; • The proposed SAPSO-AUFastSLAM algorithm was effectively verified in simulation and sea trials. Compared with traditional algorithms, the proposed algorithm has improved accuracy and stability.
The remainder of this paper is organized as follows. The AUV SLAM model and feature extraction are introduced in Section 2. A detailed description of the proposed SAPSO-AUFastSLAM algorithm is illustrated in Section 3. Section 4 presents our AUV platform and experimental validation, and conclusions are given in Section 5.

AUV SLAM Model and Feature Extraction
In this section, the AUV SLAM models is reviewed, and the sonar feature extraction process is described in detail.

The Review of AUV SLAM Model
The information observed by the sensor is based on the carrier coordinate system, while the state estimation and feature update is expressed in the navigation coordinate system. It is necessary to convert the information of the carrier coordinate system to the navigation coordinate system as shown in Figure 1. The pressure sensor (PS) equipped with AUV can accurately measure absolute depth based on the seawater properties. Therefore, this paper focuses on 2D SLAM navigation. The AUV SLAM models include the motion model and observation model, which are typical discrete nonlinear system models divided into two parts: state estimation and feature updating [39]. The 2D position and heading angle of the AUV are used to construct the SLAM state space, which can be defined as where (x k , y k ) represents the eastward and northward coordinates of the AUV in the navigation coordinate system at time k. ϕ k represents the yaw angle of AUV at time k.
In this research, we adopt point feature to denote features in the undersea environment. Feature points (i.e., landmarks) are extracted from sonar raw observations, which are detailed in Section 2.2. The feature space includes the 2D coordinates of each landmark as follows: where i indicates the index of particles, and (m k xi , m k yi ) represents the 2D features position observed at time k. The observation space includes the AUV distance and angle relative to environmental features. The formula can be defined as where r k represents the distance between AUV and features at time k, and θ k represents the angle between AUV and features in the navigation coordinate system at time k. The motion model can achieve recursive estimation of the AUV state. The formula can be defined as where X k represents the state space variable at time k, including the AUV position and heading. The state matrix is described as (5) The u k stands for AUV control information at time k, and (v x , v y ) represents the 2D velocity, measured by sensor DVL. t represents the time interval. ω k represents the system process noise, where ω k ∼ N(0, Q k ). σ k represents the angular velocity of the AUV. The observation model is used to implement the SLAM feature update. The formula is as follows: where Z k represents the observation space variable at time k, including the relative distance and angle between the AUV and feature. The observation matrix is described as (7) as follows: Here, the relative distance and angle are calculated based on the estimated AUV position and observed feature position. Each feature observed at time k must participate in the calculation, and v k represents the observation noise of the AUV at time k, where v k ∼ N(0, R k ).

The Fusion Feature Extraction Algorithm
We arrange experiments near the coast so that point features on the coast wall can be used for sonar observations. The MSIS sensor equipped with the AUV emits sound waves to the surrounding environment, and uses the echo intensity value to identify the position of obstacles ahead. The sonar configuration information is shown in Table 1. The measurement uncertainty is determined based on the observation covariance. In this paper, we set the initial observation noise covariance matrix to be R, which can be defined as follows: where σ 2 r indicates the distance error and is defined as σ r = 0.18. σ 2 p indicates the angle error and is defined as σ p = 0.052. Initial parameters are determined based on sonar observation deviation and multiple test evaluations.
Due to the underwater acoustic noise and sensor error, spurious features in sonar observations may impact the accuracy of locating the real features. Furthermore, a long scan cycle can lead to motion distortion. Therefore, we present the fusion feature extraction algorithm that integrates filtering, sparsification, and coordinate transformation [40].

Data Preprocessing Based on Threshold Segmentation
Data preprocessing is crucial to implement efficient sampling, which can be achieved through filtering and sparsification. Here, we set the noise distance threshold based on real experiment to preliminarily filter out spurious observations. Subsequently, the intensity threshold is determined based on the binarization threshold method to further reduce erroneous feature points, which can be defined as follows: where B Max indicates the maximum intensity value of per ping raw sonar datum. B Min indicates the minimum intensity value. Data within the threshold are retained, and data exceeding the threshold are discarded.
Considering the calculation speed, the filtered sonar data are sparsely processed in order to ensure that the target-related information is retained, followed by removing redundant features. We set the minimum inter-bin distance threshold according to the experimental situation and data volume. Neighboring features whose distance is less than the threshold are halved to achieve sparsification.

Motion Compensation Based on Coordinate Transformation
Since the AUV sails underwater, slow sonar scanning may cause data mismatch problems, resulting in the distortion of sonar images. Therefore, preprocessed features can be converted to the navigation coordinate system, compensating for the above problems. Based on current navigation data and sonar observation information, the feature coordinate transformation formula can be defined as follows: where x F,G and y F,G indicate the x-axis and y-axis feature position coordinates in the navigation coordinate system. x R,G and y R,G represent the x-axis and y-axis AUV position coordinates in the navigation coordinate system. ρ F and θ F indicate the relative distance and angle between the AUV and the feature in the carrier coordinate system. ψ R,G stands for the angle between the AUV and the east direction. It should be noted that the effects of the pitch and roll angle on the features are ignored in this experiment since the angles vary little during the mission phase.

Innovative SAPSO-AUFastSLAM Algorithm
To improve SLAM system accuracy and stability and then enhance AUV navigation and mapping capabilities in an unknown environment, an innovative SAPSO-AUFastSLAM algorithm is proposed in this paper. The proposed algorithm can achieve better performance than conventional methods.

Overview of the Proposed Algorithm
The proposed algorithm is based on the UFastSLAM framework [41]. The measurement noise in the importance sampling stage is estimated by the proposed SH-UPF algorithm. The measurement noise mean and covariance in the feature estimation stage are approximated by the proposed SH-UKF algorithm. At the same time, the SAPSO optimization algorithm is presented for optimal resampling. The overall flow chart is shown in Figure 2.

Importance Sampling
Sampling the suggested distribution is a crucial step in SLAM. In the traditional methods, the noise covariance of the observation model is uniquely determined and cannot meet the time-varying requirements. In response, an adaptive UPF algorithm is proposed for importance sampling. The mean and covariance of the measurement noise in the model are estimated in real time using the SH algorithm to meet the actual requirements of the SLAM systems. The improved UPF observation model is as follows: where Z k represents the observed variable. v k indicates the time-varying observation noise.
The noise mean and covariance can be defined as where δ kj represents a trigonometric function. r k indicates the measurement noise mean. R * k indicates the observation noise covariance. The improved adaptive UPF is described in detail in Section 3.2.

Feature Estimation
The feature estimation stage is implemented through the proposed SH-UKF algorithm in this paper, including the prediction and update. Like the noise estimation in importance sampling, the SH algorithm is used to construct an adaptive UKF model at this stage to generate a realistic noise distribution, improving the measurement model accuracy. The adaptive UKF is described in detail in Section 3.3.

Resampling
For the problem of particle diversity loss in traditional resampling methods [42,43], researchers proposed some optimization algorithms [44]. However, the particle "precocity" phenomenon in the existing methods may lead to premature convergence during the search process, resulting in filtering trajectory divergence. Therefore, we propose the SAPSO algorithm to replace traditional optimization algorithms, introducing random factors into the particle update stage to obtain the optimal global solution. The proposed optimal algorithm is detailed in Section 3.4.

Adaptive UPF Algorithm Based on Sage Husa
To eliminate the impact of inaccurate measurement noise covariance on AUV SLAM accuracy, we present an improved SH-UPF algorithm to optimize the SLAM navigation process. The updated proposed distribution is more consistent with the actual distribution by estimating noise mean and covariance in real time and using them in the importance sampling stage. Here, the time update steps of UPF will be described in Appendix A.
When external sensors are available, the observation prediction sigma point set is transferred through the AUV SLAM observation model mentioned in (11). The prediction and the observation prediction sigma point set are as follows: where subscript j indicates the index of the sampling point, and i stands for the index of particles. The left sides of (14) and (15) represent the symbols of predicted sigma point and the observed predicted sigma point. The predicted sigma point includes the AUV position components and the heading angle. The average and cross covariance of the observation prediction generated by weighted summation are represented as follows: where X k|k−1 indicates the predicted state variable at time k, and w j m and w j c are used to calculate the mean and covariance. The formula is expressed as follows: Here, λ = α 2 (m + κ) − m. We take κ = 0, α = 0.002. As seen above, the sampling of sigma points is divided into three stages, and the number of samples is determined by the state space dimension m, m = 5. Assuming that the observation noise at time k follows a Gaussian distribution with zero mean and covariance R k , the innovation covariance matrices can be expressed as follows: The filter gain is calculated based on the results obtained from (17) and (19) to update the AUV state and covariance. The formula can be expressed as follows: where K k represents the UPF filter gain, and ε k represents an innovation vector used to evaluate the deviation between the observed and predicted values.
To meet the system adaptability, the SH algorithm is introduced for the real-time correction of UPF. The SH algorithm is based on maximum a posteriori estimation and exponentially weighted attenuation and has the unbiased nature of constant value noise estimation [45]. Noise estimation can be implemented recursively. The noise value at time k + 1 can be estimated from the observation data at time k, and the mathematical formula is expressed as follows: where r k and r k−1 represent the mean value of estimated noise at k time and k − 1 time, and R * k and R * k−1 represent the covariance matrix of estimated noise. d k represents the weighted exponential factor of decaying memory. In noise estimation statistics, it is essential to emphasize the latest data and gradually forget older values. Here, we define d k as follows: The value of b must meet the requirements of 0.95 < b < 0.99. To meet the system requirements, we take b = 0.98. As seen from the above, the estimated value of observation noise can be obtained based on the SH-UPF algorithm at time k, which can be used in the observation update process next time. The updated formula for fusion estimation noise can be expressed as follows: where the calculation of the innovation vector needs to consider the estimated mean value of the observation noise, and the observation noise covariance in the innovation covariance needs to be updated to the estimated value at the current time.
During the SH-UPF filtering process, R * k and r k are updated simultaneously. Due to the difference between the SLAM system and pure navigation, more than one feature is observed each time, and each observation value participates in the update. Considering practical applications, noise estimation is performed once for each feature update in the proposed algorithm, and the final estimated value is transmitted to the next moment. The accuracy of AUV underwater SLAM navigation is improved through iterative filtering of the UPF system and fusion of recursive estimates of observation noise.

Adaptive UKF Algorithm Based on Sage Husa
In the AUV SLAM system, feature estimation is a crucial step that determines mapping accuracy and serves as AUV navigation. In traditional UFastSLAM, feature estimation is based on the UKF algorithm. In order to achieve adaptive mapping performance, we propose an enhanced SH-UKF algorithm for the AUV SLAM, which recursively estimates the time-varying measurement noise. After successful data association, the sigma points are determined to be sampled from the Gaussian distribution and transferred to the non-linear function. After weighted summation, the observation prediction mean and cross-covariance are obtained, with the formula as follows: where superscript f represents the feature estimation sign.ẑ where R k, f represents the observation noise covariance matrix at the feature estimation stage. The mean and covariance of the posterior estimation of features can be expressed as follows: Similar to the improved SH-UPF algorithm, this section integrates the SH noise estimator in the UKF algorithm to estimate the noise parameters at the next moment through the statistical data at the k moment and integrates them into the observation update process of the feature estimation to improve the accuracy of the posterior estimation. The noise estimation formula can be defined as where r k, f and r k−1, f represent the estimated noise mean at time k and k − 1 during the feature update, respectively. R * k, f and R * k−1, f represent the estimated noise covariance matrices at time k and k − 1. ε k, f represents the innovation vector of UKF. At this stage, the calculation method for d k is the same as in Section 3.2. The estimated noise data participate in the feature update process at the next moment, making the final estimated feature more approximate to the actual value.
The adaptive SH-UKF algorithm introduces noise estimation into the feature update process, which is simple and easy to implement, making the posterior probability estimation value of the improved algorithm more accurate. The proposed method can improve the AUV SLAM mapping accuracy and avoid the problem of the algorithm being too complex. The flowchart of the proposed algorithm is shown in Figure 3.

Optimal Resampling Based on Simulated Annealing Particle Swarm Optimization
To improve the global optimization loss of samples during the UFastSLAM resampling phase, we propose an enhanced SAPSO algorithm to achieve the optimal global effect for optimal resampling.
PSO is a sample parallel random optimization method based on iterative thinking, simulating birds foraging and fish clustering behavior [46,47]. Each particle in the sample represents a feasible solution to the problem and searches for the optimal solution in the sample space through information sharing and collaboration. Sample attributes include speed, position, and fitness. If a group of samples includes N particles, the position of each particle in the search space can be expressed as a set X = {X 1 , X 2 , X 3 , . . . , X N }. The speed of particles in the sample can be described as a set V = {V 1 , V 2 , V 3 , . . . , V N }. Then, the state update formula for the particles in each search for the optimal can be defined as where j represents the number of iterations, j = 1, 2, 3, . . . , T. c 1 and c 2 represent learning factors. To adapt to the system requirements, we take c 1 = c 2 = 2 while limiting the particle search speed V i to the range of [−4, 4]. r 1 and r 2 represent random factors that follow a random distribution of [0, 1]. pb i represents the optimal local value searched during sample iteration, and gb represents the optimal global value of the entire sample. wt i represents the inertial weight of the particle. In this paper, we present an improved weight calculation formula [47,48], which can be defined as where f i represents the fitness function. f mean represents the average fitness of the sample, and f min represents the minimum fitness of the sample. wt max represents the maximum weight, which is set to wt max = 1.2. wt min represents the minimum weight, which is set to wt min = 0.4. In this article, we adaptively determine the inertia weight of each particle for optimization based on the average fitness of the sample. The calculation formula for the fitness function is as follows: where the subscript x represents the lateral coordinates of the AUV position in the particle, the subscript y represents the longitudinal coordinates of the AUV position in each particle, and the target represents the mean value of the initial sample position. It is worth mentioning that there is a problem of premature convergence in the sample iteration process. After a few iterations, the particles' position in space remains unchanged, limited to a local range, resulting in low accuracy. To this end, the SA algorithm is introduced, a global optimization algorithm based on iterative thinking, using temperature as a control parameter and the value of the objective function as the internal energy [49]. Within the allowable temperature range, it allows samples to accept suboptimal solutions with a certain probability and ultimately obtain a relatively optimal solution through multiple iterations. In this article, according to the sampling principle of SA, particles in the sample space are transferred according to the following improved probability: When the fitness of the updated particles is less than the optimal value, the new particles are accepted with an exponential probability rather than completely deleting the new particles as is the case with traditional methods.
For optimal resampling, the SAPSO algorithm introduces a random judgment mechanism and combines with the probability jump feature in the search process. When iteratively updating a feasible solution, the improved probability is used instead of the determined judgment condition to accept a solution that may not be optimal. Therefore, it is possible to probabilistically jump out of the optimal local solution and ultimately tend to global optimization. The proposed method can improve the global search ability to a certain extent, thereby obtaining a more accurate AUV state estimation. The proposed SAPSO algorithm can be summarized in Algorithm 1.

Experimental Results and Analysis
In this section, we use a simulation platform and sea trial experiments to evaluate the proposed algorithm and verify its effectiveness. The proposed algorithm is compared with existing FastSLAM, UFastSLAM, and PSO-UFastSLAM algorithms. The navigation and mapping accuracy is comprehensively analyzed. The results are presented through result graphs and statistical tables.

Simulation
Here, we use the open-source simulation environment built by Tim [7] to generate robot trajectory and features. The trajectory path is specified by 17 waypoints with 35 landmarks and starts at (0,0). The experimental operation step size is 8779 steps, and the trajectory length is 658 meters. The initial simulation parameters can be summarized in Table 2. The effectiveness of the proposed algorithm is evaluated based on 300 Monte Carlo (MC) runs on the simulation case. The simulation results are shown in Figure 4.  We add loop closure detection to the initial simulation trajectory and further evaluate the performance of the proposed algorithm compared with other algorithms. This experiment is still based on 300 MC simulations, which is shown in Figure 5. Compared with the experimental results of simulation case 1, the performance of each algorithm is improved. It can be seen in Figure 5a,b that the proposed SAPSO-AUFastSLAM algorithm performs optimally. Figures 6 and 7 indicate the estimation error of different algorithms in an MC experiment. Figure 6a Table 3 gives the average statistical results of 300 MC experiments from simulation case 1 and simulation case 2. The proposed SAPSO-AUFastSLAM algorithm has the lowest average RMSE and the highest accuracy. In contrast, the FastSLAM algorithm has the highest average RMSE and the lowest accuracy, and the other two algorithms have values in the middle. The accuracy of the proposed algorithm is 32% higher than that of PSO-UFastSLAM and 51% higher than that of UFastSLAM, a 63% improvement over FastSLAM. Therefore, the proposed algorithm has the highest accuracy and the best positioning performance. To evaluate the robustness of the proposed algorithm in a non-stationary noise environment, we modified the initial observation noise parameters and conducted 100 MC experiments. We set the noise mutation probability P m and mutation gain G m in the program to simulate the real ocean environment. The non-stationary noise simulation process is shown in Figure 8. In this paper, we set G m to be a random integer at (1,8). P m is continuously increased to simulate the abnormal degree of dynamic noise. For each case, we performed 100 MC runs for validating. The average RMSE values of the different experiments are shown in Table 4. P-RMSE and F-RMSE indicate the average errors of the estimated positions and features in different algorithms. It can be seen that as the mutation probability increases, the proposed SAPSO-AUFastSLAM algorithm remains at a lower error level and is more robust.
As we all know, the number of valid particles is also an important criterion judging the performance of the SLAM algorithm. In this paper, the number of valid particles are referred to as N e f f , which can be defined as (39) where W k indicates the weight of the i-th particle at time k. Too many invalid particles can lead to particle depletion, reducing state and feature estimation accuracy. Therefore, we further evaluate the proposed algorithm by comparing the N e f f of different algorithms in a non-stationary noise environment. The average results of 100 MC runs are shown in Table 4. Obviously, the proposed algorithm has the most effective particles as the noise mutation rate increases.
The computational cost and positioning error of the proposed algorithm are further analyzed according to the number of particles. The localization error and running time of different algorithms under different particle numbers are used to evaluate the computational complexity and accuracy of the algorithms. For each test, the results are obtained over 150 Monte Carlo runs. As shown in Table 5 and Figure 9, the running time of the four algorithms increases while the positioning error decreases as the number of particles increases. From Figure 9a, it can be seen that the error of the proposed algorithm is always the smallest. Figure 9b shows the average running time of different algorithms. Because the noise statistics calculation and SAPSO iterations increase the computational cost, the running time of the proposed algorithm is relatively high compared to other algorithms. Considering the positioning accuracy and calculation cost, we choose 20 particles as reference in this paper.

Sea Trial
In order to fully evaluate the practicability of the proposed algorithm, we use two sets of sea test data to evaluate the algorithm performance. The two experiments were carried out at Nanjiang Wharf and Tuandao Bay, respectively.

The Experiment at Nanjiang Wharf
We used No.1 Sailfish-210 AUV (Underwater Vehicle Laboratory, Ocean University of China, Qingdao, China) as the experimental platform to verify the effectiveness of the proposed algorithm. The experiment was carried out at Nanjiang Wharf in Qingdao. Figure 10 shows the No.1 Sailfish-210 AUV during the sea trial and overall design block diagram, in which the global positioning system (GPS) provides real-time position information on the water surface. The IMU, DVL, and MSIS combine to complete the underwater SLAM task, and the IMU measures the heading, roll, and pitch angle. The DVL provides the velocity of the body coordinate system, and the MSIS measures the 2D position of the features in the environment. The detailed specifications of the sensors are shown in Table 6.   Figure 11. The estimated trajectory and features of the different algorithms are fully shown in Figure 11a. The enlarged local figure is shown in Figure 11b. It can be seen that the motion trajectory and features of the proposed SAPSO-AUFastSLAM algorithm are closer to the actual value.

The Experiment at Tuandao Bay
We used the No.2 Sailfish-210 AUV as the experimental platform to verify the accuracy of the proposed algorithm. The experiment was carried out at Tuandao Bay in Qingdao. Figure 13 shows the No.2 Sailfish-210 AUV during the sea trial and overall design block diagram, in which the INS, DVL, and MSIS combine to complete the underwater SLAM task, and the INS measures the heading, roll, and pitch angle. The detailed specifications of the sensors are shown in Table 7.   Figures 14 and 15 show the results of sea trial experiment on the No.2 Sailfish-210 AUV. Figure 14a,b represent the SLAM motion and feature estimations of the four algorithms. Figure 15b  The specific statistical results are shown in Table 8. The RMSE, accuracy, and running time of the different algorithms are summarized. Obviously, the errors of the proposed SAPSO-AUFastSLAM algorithm are smaller than conventional algorithms, and the accuracy is higher. The running time of the presented algorithm is increased to a certain extent, but they can meet the real-time performance. Therefore, the proposed algorithm can effectively improve the performance of AUV underwater SLAM and thus improve navigation accuracy. Step time represents the single-step running time of the AUV sea trial. Total time represents the total running time of the AUV sea trial. ERMSE indicates the eastern RMSE of AUV localization. NRMSE indicates the northern RMSE of AUV localization.

Conclusions
This paper proposes an innovative SAPSO-AUFastSLAM algorithm to perform the AUV SLAM process. The adaptive UPF and UKF algorithms are based on Sage-Husa noise estimation and the resampling algorithm based on SAPSO optimization in parallel to improve the accuracy of AUV navigation and mapping. The performance of the proposed algorithm is verified by simulation and sailfish AUV sea trial data and compared with the three conventional algorithms of PSO-UFastSLAM, UFastSLAM, and FastSLAM. The experimental results show the effectiveness of the proposed method. In the sea trial, the average accuracy of the proposed method is significantly improved compared with the conventional method.
Although the calculation time of the proposed algorithm can meet the needs of lowfrequency navigation, it is longer than that of conventional algorithms. In the future work, we will investigate increasing the calculation speed and adaptively estimating the system noise parameters to improve the performance of the proposed algorithm further. Subsequently, anomaly detection will be added to improve the coarse impact of the noise estimation algorithm, such as judging the divergence of the filtering process and introducing fading factors and adaptive coefficients for adjustment. Additionally, the impact of ocean currents and other factors on AUV navigation will be considered to improve accuracy.
where X

aug,[i]
k−1 represents the augmented AUV state matrix in the i-th particle at time k − 1. P aug,[i] k−1 represents the augmented AUV covariance matrix. Q is determined to the default value, as follows: where σ 2 vx indicates the horizontal speed error, and σ 2 vy represents the vertical velocity error. σ 2 wn stands for the angular velocity error. Based on the UT, we approximate the prior distribution by sampling sigma points from the augmented state space. The formula is described as follows: The corresponding weights for computing the predicted mean and covariance can be computed as (18). The value of the sigma point set is transferred through the AUV SLAM motion model shown in (4), and the nonlinear state function can be expressed as follows: The predicted mean and covariance can be obtained by weighted summation of the transformed sigma points, which can be defined as