Factor graph weight particles aided distributed underwater cooperative positioning algorithm

The growing scale of marine exploration requires high-resolution underwater localization, which necessitates cooperation among underwater network nodes and consideration of the channel complexity and power efficiency. In this paper, we proposed a factor graph weight particle-aided distributed underwater node cooperative positioning algorithm (WP-DUCP). The algorithm capitalizes on the factor graph and sum-product algorithm to decompose the global optimization to the product of several local optimization functions. Combined with Gaussian parameters used to construct weighted particles and to realize belief transfer, the algorithm shows low complexity and high efficiency and is suitable for energy-restricted and communication distance-limited underwater networks. In terms of convergence, localization resolution, and computational complexity, we conducted simulations and real tests with comparisons to the existing colocalization methods. The results verified the higher resolution of the proposed method with no extra computational burden.

With applications in fishery monitoring, ocean current monitoring, oil drilling platforms, and other underwater systems, underwater node positioning is becoming more critical [2]. Most existing underwater positioning capitalizes on GNSSaided floating buoys and water pressure meters. However, it is impractical to deploy a buoy positioning system for every underwater node [3]. To this end, research on underwater colocalization based on high-precision underwater nodes has recently started.
According to the research results of our team, underwater positioning is mainly divided into two categories. The first category realizes underwater positioning through underwater acoustic signals, and the second category uses sonar buoys to receive satellite navigation signals. These two types of underwater positioning categories are the transplantation of wireless positioning at the technical level and for a single underwater node. Research on using the communication and range information between multiple underwater nodes to realize positioning is nonexistent.
Earlier underwater colocalization drew lessons from terrestrial wireless colocalization systems [4]. The colocalization method based on the minimum mean squared error (MMSE) was proposed in [5]. The colocalization method based on the maximum a posteriori (MAP) is presented in [6]. These two methods adopted the marginal probability density function (PDF) of the underwater node position. However, the lack of high ranging precision in underwater systems degraded the colocalization performance. The colocalization method based on the factor graph was proposed in [7]. The method mapped the statistical graph model into the network topology and obtained the probability function through messages passing over the network. If a sufficient number of nodes exist, the method will provide high localization precision. However, this centralized processing increased the computational burden and the communication transmission requirement. A mixed distributed message passing algorithm based on belief propagation and the mean field (MF) is proposed in [8]. The algorithm adopted belief propagation in the motion-related region and adopted mean field message passing in the measurement-related parts of the factor graph. Moreover, the algorithm further adopted the Gaussian approximation to decrease the computational burden. The colocalization method based on connectivity informationaided belief propagation (CIBP) was proposed in [9]. To improve the positioning accuracy, the semidefinite programming (SDP) method based on CIBP was proposed in [10]. Those methods integrated the connectivity information into the belief propagation and excluded the fake position to avoid the wrong message passing. The distributed colocalization method based on variational message passing (VMP) was proposed in [11]. It adopted a second-order Taylor expansion to model the nonlinear ranging function and then obtained the colocation with mean and covariance information. These above methods hypothesize that the precision of ranging is higher than that of self-positioning. It is difficult to ensure this hypothesis in an underwater environment. A Taylor expansion-based distributed positioning (Taylor-DP) method was proposed in [12]. However, in the Taylor expansion process, the high-order terms are omitted, which leads to low cooperative positioning precision. In this paper, we proposed a distributed colocalization method based on weighted particles. The method expresses the belief information using weighted particles and applies sum-product theory to pass the message within the factor graph.

Underwater colocalization model
Due to ocean currents, the positions of underwater nodes are uncertain [13]. Using a hydraulic pressure meter, we model the underwater collaborative positioning system into a twodimensional planar system, as shown in Fig. 1.
In this paper, we make the following assumptions: 1. The communication range, denoted as R, and the measurement distance are equal. 2. The ranging error of node i and node j, denoted as n j−>i N (0, σ 2 i j ), follows a Gaussian distribution. 3. The prior probability distributions of all underwater nodes are independent, and the prior probability distributions along the x-axis and y-axis of each node are independent. Under these assumptions, the range measurement between node i and node j ∈ S i can be expressed as where · denotes the Euclidean norm. Denote Z i = z j−>i | j ∈ S i , i.e., the set of range measurements from node i to all the corresponding neighboring nodes. Denote Z = {Z i |i ∈ N }, i.e., the set of range measurements of all undetermined nodes to its corresponding neighboring nodes. The probability of the range measurement between node i at position x i and node j at position x j is expressed as According to Bayes estimation based on the MMSE [14], the position estimation of the undetermined node i, i.e., x i , can be expressed through the posterior PDF p(x i |Z) aŝ The posterior PDF p(x i |Z) can be expressed as the marginal joint PDF as where the slash in the dX\x i is the setminus operator.
According to the independence assumption we made earlier, we can express the joint PDF as where p(x i ) and p(x a ) denote the PDF of undetermined node i and anchor node a, respectively.

Underwater colocalization method based on weighted particles
Due to the high computational complexity and communication costs of the existing underwater cooperative positioning technology, node location updating is quite slow [15][16][17], and it is difficult to alleviate the error accumulation. To reduce the computational complexity and realize the rapid update of the node location, we constructed the corresponding factor graph and applied the sum-product theory for message passing. In this factor graph, the local information is expressed with weighted particles, and the belief is represented with a Gaussian approximation. In this way, the belief of the undetermined node i, i.e., the posterior of the position x i , is expressed as where l denotes the iterative index, f (x i ) denotes the prior PDF of position x i , and m (l) j−>i (x i ) denotes the message passing from node j to node i. According to sum-product theory, the message is proportional to the likelihood function, the prior PDF of neighbor node j and the prod of the message at node j, as shown in Equation (7).
Equation (7) shows that to obtain the belief in Equation (6), we need different messages passing from each node to its neighboring nodes. This will definitely increase the communication costs. Additionally, the calculation of m increased the complexity. Then, we simply Equation (7) as For the anchor nodes at known positions and undetermined nodes at unknown positions, we adopt Gaussian approximation to express the belief information as It is difficult to calculate this integral because of the nonlinearity, so we expand the range information by first-order Taylor series as wherex j−>i =x i − μ μ μ j . Substituting (11) to (10), we have where j+z and I denotes the unit matrix. The weighted particle of node i is expressed as where w i [n] denotes that the weight satisfies K n=1 W i [n] = 1 and K denotes the number of particles.
The communication costs in belief propagation rely on the expression of belief information. Most existing methods adopt the broadcasting method including all weighted particles, which is attributed to the high costs within multiple iterations. When the belief is irregular or multimodal, a Gaussian distribution is adopted as the worst case. Therefore, the multivariate Gaussian distribution is used to represent the belief between underwater nodes with mean value μ μ μ i and covariance i as follows: K particles are regenerated according to Gaussian approximation, and the weight corresponding to each particle is expressed as follows: The algorithmic representation of the proposed weighted particle-based distributed underwater copositioning method, WP-DUCP, is given in Table 1.

Simulation and analysis
To analyze the performance of the WP-DUCP algorithm, we considered a 100m * 100m two-dimensional underwater scenario in which the number of underwater anchor nodes with high positioning accuracy is M = 13 and the number of underwater nodes to be located is N = 100. Anchor nodes are underwater nodes with known position coordinates with high positioning accuracy, and the underwater nodes to be located are randomly distributed in the area of interest. The communication distance between any two underwater nodes is equal to the ranging distance and is set as R. Suppose the ranging noise between any two underwater nodes follows a zero mean Gaussian distribution with a variance of σ 2 i−> j . The prior probability density function of the position of the undetermined underwater node i can be denoted as follows: On the assumption of independence between the two axes, . Set the maximum number of iterations as L = 20. The number of particles in local information representation is k = 500. The simulation evaluates the performance in terms of convergence, error and computational complexity.

Convergence
In the simulation, consider the standard deviation of the ranging error between underwater nodes to be σ i−> j = 1m and the standard deviation of the underwater node position to be σ x i = σ y i = 10m. Then, we compare the proposed WP-DUCP to the SDP algorithm [10] and Taylor-DP algorithm [12] in terms of convergence speed. The comparison results of positioning performance are shown in Fig. 2. Figure 2 shows that the positioning error decreased and converged with the iterations. The SDP algorithm tends to converge after 8 iterations, and the RMSE can reach approximately 1.4 m. The Taylor-DP method needs approximately 10 iterations, and the RMSE is more than 1.6 m. The proposed WP-DUCP method shows superiority with approximately 5-6 iterations, and the convergence error of the RMSE can

Positioning accuracy analysis
Positioning accuracy is the most important metric of underwater cooperative navigation. In this part, the positioning accuracy is simulated under the same simulation as in Sect. 4.1, and the results are shown in Fig. 3.
As shown in Fig. 3, the positioning performance of the Taylor-DP algorithm is relatively poor. When the positioning error is less than 3 m, the CDF can only reach 91%, indicating that nearly 10% of the positioning accuracy of cooperative  nodes has not been substantially improved. The SDP method is better than the Taylor-DP algorithm. When the positioning error is less than 3 m, the CDF can reach 96%, indicating that the positioning accuracy of most cooperative nodes has been improved. When the positioning error is less than 3 m, the WP-DUCP algorithm proposed in this paper can guarantee that the positioning performance of 98% of cooperative nodes is improved. This is attributed to the utilization of the summation and product theory of factor graphs to convert the global optimum into the local optimum product, and it can effectively improve the positioning accuracy of underwater collaborative nodes by spreading through all the cooperative positioning nodes with confidence.

Complexity analysis
The computational complexity and communication costs are the decisive factors for the real-time performance of the underwater positioning algorithm. In this paper, we denote the number of particle parameters as K and the number of neighboring nodes of the undetermined node to be localized as N i . The complexity comparison is shown in table 2.
In terms of the computational complexity, the SDP algorithms failed in the comparison as their complexity is proportional to the square of K and N i . The computational complexity of the Taylor-DP algorithm is proportional to the

Iteration analysis
In this simulation, we set the anchor nodes at the predefined positions and distribute undetermined nodes randomly in the considered 100 m*100 m two-dimensional underwater scenario, as shown in Fig. 4. In this figure, the red asterisks represent the anchor nodes while the green circles denote the undetermined nodes. The localization results after 10 iterations are shown as red asterisks. As this figure shows, the localization performance in the center is better than that in the corner since more anchor nodes are included.
The color temperature of the positioning error of the 100 undetermined nodes within each iteration is shown in Fig.  5. Most positioning errors are below 1.4 m while the worst error is nearly 12 m.

Experiment and analysis
An anechoic tank test is conducted to verify the performance of the WP-DUCP localization method. The tank is covered with a silencing structure, as shown in Fig. 6, to avoid echoes. The self-designed underwater modem shown in Fig. 7 is adopted as the communication node. It is configured to work in ranging mode, and the precision is within 0.1 meters. The experiment is conducted in three scenarios, each lasting 75 minutes and collecting 75,000 sets of data.

Scenario 1
In this scenario, four anchor nodes are placed at the four corners of a 30m * 50m square, and the two undetermined nodes are placed on the axis within the square, as shown in Fig. 8. The initial position estimation error is within 1 meter while the ranging error is within 0.1 meter. Adopting the proposed colocation method based on the collected ranging measurements, the obtained position estimation is shown in Fig. 9a, and the RMSE of the position estimation of each undetermined node is shown in Fig. 9b. The CDF of the entire network is shown in Fig. 10.
As Fig. 9 shows, the position estimation error approaches the ranging precision. That is, the method can alleviate the effect of the initial position ambiguity. This is attributed to the belief propagation within the factor graph. The positioning errors of M1 and M2 are almost equal due to the symmetric geometry setting. As Fig. 10 shows, for approximately 95% of the collected data, the collaborative positioning accuracy of the entire network in scenario 1 can be achieved within 0.2 meters after multiple iterations. The experimental results show that the WP-DUCP algorithm has low computational complexity and communication overhead. This is attributed to the local information exchange in the form of parameters, and the results are consistent with the theoretical analysis. The proposed method is predicted to have a better positioning performance when the underwater nodes are in a stable state.

Scenario 2
In scenario 2, to test the performance of the proposed method for nodes not on the axis, we add two more undetermined  Fig. 11.
Adopting the proposed colocation method based on the collected ranging measurements, the obtained position estimation is shown in Fig. 12a, and the RMSE of the position estimation of each undetermined node is shown in Fig. 12b. The CDF of the entire network is shown in Fig. 13.
As Fig. 12 shows, the positioning accuracy of the four nodes M1, M2, M3 and M4 after convergence reaches approximately 0.1 m, similar to that of scenario 1. This shows that the WP-DUCP algorithm can also realize fast convergence when the number of nodes increases. This is because the WP-DUCP algorithm participates in the positioning solution by selecting the nodes with high confidence in the leading node and eliminates the influence of the nodes with poor positioning accuracy. In the 75,000 sets of test data, approximately 90% of the positioning accuracy can be achieved, as shown in Fig. 13. The experimental results verified that the WP-DUCP algorithm is consistent with the theoretical analysis.

Scenario 3
In an underwater network, it is difficult for anchor nodes to be constantly sustained in the periphery. Sometimes, due to the mobile characteristics of ocean waves, undetermined nodes may appear outside the square. On the basis of the network setting of scenario 2, we move one undetermined node outside the square, as shown in Fig. 14. All undetermined nodes have a northward moving speed of 1m/s to simulate the ocean current and to further verify the performance of the WP-DUCP method.
Adopting the proposed colocation method based on the collected ranging measurements, the obtained position estimation is shown in Fig. 15a, and the RMSE of the position estimation of each undetermined node is shown in Fig. 15b. The CDF of the entire network is shown in Fig. 16.
As Fig. 16 shows, the positioning accuracy of the two inner nodes M1 and M2 reaches approximately 0.1 m while that of the two outer nodes M3 and M4 after convergence reaches approximately 0.12 m. WP-DUCP can effectively reduce the impact of node movement on positioning accuracy. This shows that the WP-DUCP algorithm can also realize fast convergence with slightly worse performance for outer nodes. This contributes to the data fusion conducted in the belief propagation of the WP-DUCP algorithm with high confidence in the leading node. In the 75,000 sets of test data, approximately 90% of the positioning accuracy can be (a) Position Estimation Result (b) RMSE vs Iteration achieved to 0.28 m, as shown in Fig. 16. The experimental results verified that the WP-DUCP algorithm is consistent with the theoretical analysis.

Conclusion
The colocalization method adopted from the wireless research field suffers from the problems of difficult communication and complex channel fading when the method is adopted in underwater scenarios. Considering these factors, we proposed a factor graph weighted particles distributed underwater copositioning (WP-DUCP) approach. The approach is verified by simulation using three aspects: the convergence