Factor-Graph-Aided Three-Dimensional Faster Cooperative Positioning Algorithm

With the development of smart cities and 5G applications, there is an increasingly urgent need for cooperative positioning among all kinds of intelligent terminals. Existing cooperative positioning technology is primarily designed for two-dimensional positions, and the computing speed and positioning accuracy cannot meet the needs of smart cities. To solve these problems, this paper proposes a factor-graph-aided three-dimensional faster cooperative positioning algorithm (FG-3DCP) that combines a factor graph and sum product theory to establish a cooperative localization model with many nodes. To reduce computational complexity and describe fast positioning, the parameter independence of the factor graph is used, and the positioning data of each node coordinate axis are calculated independently. Then, the positioning result is obtained by fusion, and the computing speed is markedly improved. The proposed algorithm was simulated and analyzed in terms of ranging error, position ambiguity, network topology and the number of nodes. When the proposed algorithm was compared to the existing non-Bayesian estimation cooperative position methods, the position performance improved more than 20%, and the convergence rate was the fastest in the 3D environment.


I. INTRODUCTION
In smart cities, the number of intelligent terminals is growing geometrically, and high-precision cooperative positioning is the core of real-time terminal work. Cooperative positioning methods have been studied in terms of the limitations of traditional positioning algorithms; however, most methods currently only perform two-dimensional plane theoretical analyses, which makes it difficult to meet the practical needs of three-dimensional positioning in high-rise buildings and complex underground networks in smart cities. Positioning methods based on Bayesian estimation are proposed in [1] and [2]. These methods obtain better positioning performance by calculating the marginal posterior probability distribution function of position variables. However, the multiplication The associate editor coordinating the review of this manuscript and approving it for publication was Ahmed Mohamed Ahmed Almradi . of many probability distributions in the algorithm markedly increases the communication overhead and computational complexity of the system, resulting in limited applications. An unscented Kalman filter (UKF) cooperative positioning method based on sigma point belief information transfer was proposed in [3]. This method uses the idea of highdimensional reconstruction and combines the belief information transfer strategy to obtain an approximate solution of the edge posterior distribution function with the variable nodes on a factor graph. This method is suitable for distributed mobile node cooperative localization in a nonlinear Gaussian scenario. However, due to the excessive number of adjacent nodes of agents, the problems of high combined message dimensions and a nonpositive definite covariance matrix are prone to occur in static networks. A posterior linearization belief propagation (PLBP) transfer method was proposed in [1]. The core of this method is that the agent only considers VOLUME 9, 2021 This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ the cooperation between two nodes concurrently and obtains the linearization belief information transfer on the factor graph through the unscented transformation. But this method can only obtain good results in low dimensions. Semidefinite programming (SDP) cooperative positioning was proposed in [4] and converts the value function minimization problem with maximum likelihood (ML) estimation into a convex SDP problem to obtain the node position by relaxing the nonconvex optimization constraints. However, the method is centralized, and the computational complexity is high, which is not well suited to large-scale networks. A nonparametric belief information transfer cooperative positioning method is proposed in [5]. The method uses the knowledge of factor graph theory to construct a network factor graph and systemizes the cooperative location problem in the network into a multi-node position variable edge posterior probability density distribution estimation problem. Weighted particles are used to approximate the belief message of each variable node in the factor graph. When the number of particles is sufficiently large, the probability density distribution of the node position variable can be approximated more accurately. However, a large number of particles also increases the computational complexity of the network. [6] uses the covariance matrix to quantify the position estimation error and selects the reference node with a smaller variance as the cooperative node of the positioning target. This method suppresses the influence of reference nodes with large position errors, but ignores the influence of non-line-of-sight (NLOS) errors on the positioning results. A distributed particle filter algorithm for cooperative positioning and target tracking was proposed in [7]. This method is used to achieve target positioning and tracking in indoor environments. However, due to the use of particles to approximate the posterior probability distribution of node positions, the computational complexity and communication overhead of the system are high. The above methods are primarily designed for two-dimensional plane and cannot be used for three-dimensional positioning. The existing three-dimensional positioning algorithms primarily add height information to two-dimensional positioning algorithms. The least square (LS) three-dimensional cooperative positioning algorithm was proposed in [8], and the maximum likelihood estimation (ML) three-dimensional cooperative positioning algorithm was proposed in [9]. The two methods introduce height information into the two-dimensional cooperative positioning to describe threedimensional positioning; however, height information cannot be fused with the two-dimensional positioning plane, resulting in low cooperative positioning accuracy. A sumproduct-aided cooperative positioning algorithm over a wireless network was proposed in [10] and [11], this algorithm describes the three-dimensional cooperative positioning of an underwater large-scale network. However, the computational load of this method is too high to match the real-time demands of smart cities. In order to improve the positioning stability, a variety of Kalman filtering techniques are widely used in cooperative positioning [12]- [15]. For example, a new variational bayesian adaptive extended kalman filter is proposed in [12], distributed particle filteris proposed in [15]. However, the above method can not solve the impact of mutation error. Information-aided belief propagation is proposed in [16] to identify mutation error. Angle measurement is introduced into the cooperative positioning system to identify mutation error [17],but it has the problem of large amount of calculation.Our team proposed a weighted factor graph aided distributed cooperative position algorithm [18]. A variety of performance tests are used in [19] and [20] and found that the cooperative positioning based on factor graph has the advantage of fast positioning speed. However, topology network, ranging error and time synchronization will reduce the performance of the cooperative localization algorithm.A reference node selection method is proposed in [21] and [22], a method to eliminate ranging error is proposed in [23] and cooperative joint localization and clock synchronization based on gaussian message passing is proposed in [24] and [25]. These methods solve one problem in the cooperative positioning system, and do not consider the problem of three-dimensional positioning.To solve these problems, this paper proposes a factor-graph-aided three-dimensional fast cooperative positioning algorithm. First, this method uses factor graph theory to construct a mathematical model of cooperative network topology. Second, it derives the calculation method of each variable node and function node in the internal factor graph of the agent based on the mapping relationship between nodes. Finally, positioning is completed after the iterative update of the information loop. The first section of this paper is an introduction, which primarily introduces existing positioning algorithms and analyzes their shortcomings. The second section maps the topological graph of the cooperative positioning network to the factor graph and describes the three-dimensional cooperative positioning algorithm based on the factor graph in detail. The third section analyzes the simulation results of the algorithm and quantitatively analyzes factors such as ranging error and position ambiguity. The fourth section summarizes the content of this paper.

II. FACTOR-GRAPH-AIDED THREE-DIMENSIONAL FASTER COOPERATIVE POSITIONING ALGORITHM
To solve the problem of the cooperative localization [11]- [15] of agents in a wireless environment, this paper uses the cooperative positioning strategy based on a factor graph to provide positioning for all agents. The proposed method uses the knowledge of factor graph theory and the sum product algorithm to construct a belief information [16], [17] transfer model for all agents. Each agent calculates its own position coordinates and position ambiguity variance by receiving belief information from neighbor nodes within communication range. The network topology diagram is shown in Figure 1, which contains m n agents and a p anchors; a red link indicates that there is an obstacle between the anchor node and the agent, which is NLOS; a yellow link indicates a line-of-sight connection between the anchor and the agent; and a blue link indicates communication between the agents. The update mode of the belief information of each agent is the same in the proposed method [18]; therefore, this paper takes any agent m q in the network as an example. The set of neighbor nodes within communication range of node m q is where A q represents the set of M anchor nodes connected to agent m q within the communication range, and M q represents the set of N agents connected to m q within the communication range. The node factor graph of m q is shown in Figure 2. In Figure 2, all nodes are considered to be variable nodes, F i−t j represents the function node from the neighbor node i to the agent t j , and the function node is used for data processing of positioning information. Agent m q in the network is considered as an example, and i ∈ C q , t j = m q . A solid line in the Figure 2 represents the belief information provided by the variable node to the function node, and a dotted line represents the position estimation information provided by the neighboring nodes to the target node [18]- [20]. Based on the calculation criteria of the sum-product algorithm, the paper uses belief information to iterate, transfer and solve in the factor graph, and then achieves the positioning of the target node. The belief information in this study primarily refers to mean information and variance information.
The FG-3DCP algorithm first collects the coordinate estimation information of agent m q from the adjacent nodes. Second, each neighboring node provides the function node with the belief information required by the agent m q . This information includes distance information and the variance of the position ambiguity variance of neighboring nodes [21], [22]. Finally, the belief information is updated iteratively on the factor graph until convergence. When calculating the coordinates of the agent, each coordinate axis is independent of each other. To meet the real-time demand of smart cities, this paper converts the three-dimensional positioning problem into three one-dimensional problems on three branches (x coordinates, y coordinates, and z coordinates), which can reduce computational complexity and improve computational speed. Agent m q in the network is considered to be an example to establish its internal factor graph, as shown in Figure 3.  Figure 3 represents the distance information between agent m q and its neighboring nodes. The distance information enters the internal factor graph from the function node D i , which uses Euler's formula to link the coordinates of x q , y q , and z q [18]:

The variable noded
where x iq , y iq and z iq respectively represent the relative distances between agent m q and its neighboring nodes on each coordinate axis; and d iq indicates the relative distance between nodes. The constraint relationship formula is shown in (2), where X i , Y i , Z i is the coordinate value of the neighbor node i and x q , y q , z q is the coordinate value of agent m q : In the proposed method, belief information is iteratively transmitted between variable nodes and function nodes. The information transmission method can be divided into upstream and downstream iterations. Considering agent m q as an example, the information calculation method between nodes is as follows.
First, in the upstream iteration, the function node E i is used to transform the belief information between the x q coordinate, the y q coordinate and the z q coordinate, the formulas are expressed as: where k represents the number of iterations. The function nodes A i , B i , and C i are used to describe the conversion between relative position and absolute position information.
Based on the type of neighbor nodes, this conversion can be divided into two cases: the neighbor node is an anchor node, or the neighbor node is a cooperative node with position ambiguity. These situations can be expressed as: (the neighbor node's anchor node) (6) (the neighbor node is other agent) (7) When the neighbor node is an anchor node with an accurate location, the belief information from function nodes A i , B i and C i to variable nodes x k q , y k q , z k q is respectively expressed as follows: When the neighbor node is a cooperative node with position ambiguity, the belief information is similar to the formula when the neighbor node is an anchor node. In Equations (8)∼(10), σ 2 x k q , σ 2 y k q , and σ 2 z k q are the variances of the Gaussian belief information from the variable nodes x k q , y k q , and z k q , respectively. After an upstream iteration ends, the next information update is prepared using the downstream iteration. The variable nodes x q , y q , and z q update the belief information of function nodes A i , B i , and C i based on the sum-product algorithm calculation criterion are expressed as Based on the principle that the multiplication result of multiple Gaussian distributed variables still obeys the Gaussian distribution, and the positioning of the agent in the collaborative network can be described. Considering the belief information update of variable node x as an example, variable x follows Gaussian distribution with mean m u and variance σ 2 u . When the number of variables x is U, the distribution of its product is proportional to the Gaussian distribution with mean m and variance σ 2 . The entire update process can be expressed as follows: 1 Equations (11)∼(13) can be calculated based on equations (14)∼ (16). In the downstream iteration, the function nodes A i , B i and C i update the belief information of variable nodes x iq , y iq and z iq based on Equations (17), (18) and (19) respectively, when the neighbor node is an anchor node: where σ 2 x k iq , σ 2 y k iq and σ 2 z k iq are the variances of the Gaussian belief information from the variable nodes x k iq , y k iq , and z k iq respectively. When the neighbor node is an agent of m q . Finally, the position updates of the variable nodes x q , y q , z q in the kth iteration is expressed as follows:

III. SIMULATION RESULTS AND ANALYSIS A. SINGLE POSITIONING EXPERIMENT RESULTS
In the proposed algorithm, the agents describe positioning through information interaction with neighboring nodes within communication range. The simulation environment is a 100 × 100 × 100m 3 cooperative positioning network space containing 13 anchors and 200 agents. Agents are randomly and evenly distributed in the three-dimensional space, and the network topology is shown in Figure 4, where a red solid triangle represents an anchor node, and a green solid circle represents an agent. Communication between agents and their adjacent anchors is indicated with a black line in the Figure 4, and communication with other agents is indicated with a blue line. To ensure ranging accuracy, agents only communicate and measure the distance with nodes within 40 m [23]- [24]. Assuming that the ranging noise is Gaussian white noise with a mean value of 0 and a variance of σ 2 n , and given that the ranging error is σ n = 1m and the initial position ambiguity variance of agents is 5 m, the positioning result is shown in Figure 5. The contour map of the  positioning error of 200 agents after 30 iterations is shown in Figure 6.
In Figure 5, a black circle indicates the true position of an agent, the red-to-blue gradient indicates the positioning result, and a blue straight line indicates the positioning error. Figure 5 shows that the locations of most agents are described with high positioning accuracy. However, the positioning errors of certain nodes distributed on the space boundary are larger because the number of neighboring nodes (anchor nodes and cooperative nodes) is small, and the received belief information has a large deviation, which reduces the positioning accuracy of the nodes. Figure 6 also shows that after 10 iterations, the positioning accuracy of most nodes is between 0 and 2 meters.

B. ANALYSIS OF THE INFLUENCE OF RANGING ERROR
In cooperative positioning [27], [28], the ranging accuracy between neighboring nodes and the positioning target will affect the accuracy of the entire cooperative positioning system [18]. Positioning errors will accumulate in the iterative process, resulting in a decrease in positioning performance. Therefore, this paper selects the same simulation conditions as in Figure 6 and performs simulation analysis on the standard deviations of ranging errors σ n of 1, 1.5, and 2 m. The results are shown in Figures 7 and 8.    Figure 7 shows that as the standard deviation of the ranging error σ n decreases, the convergence speed increases. When the standard deviation of the ranging error is 1 m, the convergence speed is maximized, and the positioning error is 1.483 m. Figure 8 shows that when the ranging error is 1 m, the accuracy of approximately 99% of the nodes can reach 3 m because when the ranging error decreases, the measured distance between the agent and its neighboring nodes is near the true distance, which allows the neighboring nodes to transmit more accurate belief information to the agent. Concurrently, the FG-3DCP can effectively reduce the impact of cooperative node information with large ranging errors, thereby improving system positioning performance.

C. ANALYSIS OF THE INFLUENCE OF POSITION AMBIGUITY
In a cooperative location network [29], the prior position ambiguity of neighboring nodes affects the positioning accuracy. The settings are the same as those used to produce Figure 6. The node with the positioning error ranking at 100 is analyzed with the position ambiguities σ p of 3, 4, and 5 m, and the results are shown in Figure 9. Figure 9 shows that the position ambiguity σ p of neighboring nodes affects system positioning performance: the smaller the position ambiguity σ p is, the faster the convergence speed. When the position ambiguity is σ p = 3m,  the positioning error is 1.12 m because prior information with a small position ambiguity σ p can provide more reliable belief information for agents.

D. ANALYSIS OF THE INFLUENCE OF NETWORK TOPOLOGY
In addition to the influencing factors of ranging error and position ambiguity, the collaborative network topology also limits the positioning accuracy of the system. Two topology structures are selected in this study, the simulation parameters are shown in Table 1, and the network topology is shown in Figure 10. In the two models, each agent has three anchors and three neighboring cooperative nodes within communication range. In model 1, agents are distributed on the eight vertices of a cube with a side length of 60 m, and anchors are distributed on the vertices of a regular quadrangular pyramid with a side length of 60 m. In model 2, the anchor position distribution is the same as that in model 1, and the agents are randomly distributed in the network. The simulation results of the two models are shown in Figure 11. Figure 11 shows that in the two topological networks, the positioning performance of model 1 is better than that of model 2 under the same simulation conditions. The positioning error can reach 0.98 m because the error vectors received from neighboring nodes by the agents in model 1 are not concentrated in the same direction but are relatively evenly distributed on the three axes x, y and z. Therefore, the convergence direction is more accurate in the iterative update, which makes positioning more accurate.

E. ANALYSIS OF THE NUMBER OF NEIGHBOR NODES
Compared to the traditional positioning method, the outstanding advantage of cooperative localization is that when the  number of anchor nodes is insufficient, positioning can be achieved by communicating with neighboring nodes. However, there are still certain problems with existing cooperative positioning systems. Expanding the communication range will increase the number of neighboring nodes of each agent in the network when the positions of the agents and the anchors in the network are fixed; these changes will affect positioning performance. The simulation conditions are the same as in Figure 6, and the communication range is set to 40, 50, and 60 m. The results are shown in Figures 12 and 13. Figure 12 shows that when the communication range is reduced, positioning performance decreases. Figure 13 also shows that when the communication range is 40 m, only 83.5% of the node positioning accuracy can reach 2 m. When the number of all agents in the network is constant, the number of neighbor nodes for each agent increases as the communication range increases, which can accept more comprehensive belief information and improve positioning accuracy.

F. COMPARATIVE ANALYSIS OF MULTIPLE ALGORITHMS
To comprehensively analyze the positioning performance of the factor-graph-aided three-dimensional faster cooperative positioning algorithm (FG-3DCP), the LS cooperative positioning algorithm, ML cooperative positioning algorithm and sum-product algorithm over a wireless network (SPAWN) are  compared to the proposed algorithm. The simulation conditions are as follows: the number of agents is 100, the number of anchors is 13, the ranging error σ n is 1 m, the position ambiguity σ p is 5 m, the communication range is 60 m and the number of particles of the SPAWN algorithm is 500. The results are shown in Figures 14 and 15. Figure 14 shows that the proposed method achieves better performance than the other methods due to its use of the factor graph. Under the same simulation conditions, the proposed method achieves a faster convergence speed and higher positioning accuracy, and the positioning error is approximately 0.97 m. FG-3DCP only needs 10 iterations to achieve positioning and meets the real-time requirements of smart cities. Figure 15 also shows that the positioning performance of the proposed method is significantly better than that of the LS and ML algorithms. Approximately 99% of nodes' positioning accuracies reach 2 m because the LS and ML methods do not consider the influence of the prior information of the nodes and the position ambiguity of the neighboring nodes. When there are insufficient anchor nodes within communication range, the position ambiguity of the cooperative nodes accumulates errors during each iterative update, resulting in poor positioning accuracy. The positioning accuracy of the SPAWN algorithm is slightly better than the proposed method because the SPAWN algorithm uses  many weighted particles to represent information. The more particles there are, the higher the accuracy of the SPAWN. However, this method also increases computational complexity and communication overhead. Conversely, the factorgraph three-dimensional coordinated positioning proposed in this paper achieves good positioning performance under the above mentioned conditions and reduces computational complexity, thus improving overall positioning performance.

G. FG-3DCP PERFORMANCE EXPERIMENT BASED ON UWB
Using the UWB modules to build the actual network with a size of 9m * 6m, and test the positioning performance of the FG-3DCP. There are 8 UWB modules in the network, four of which are anchor nodes. Set two nodes in the network as the agents to be located. Ranging between UWB modules is interfered due to the complex electromagnetic environment. The average ranging error between agent 1 and its neighboring nodes is 42.78cm, and the average ranging error between agent 2 and its neighboring nodes is 36.51cm. The position ambiguity of the agent node is 0.2m. Each agent node adopts a topology structure surrounded by neighboring nodes. The refresh time of the positioning result is 0.5s. The positioning error in a certain iteration is shown in Figure 16. Figure 16 shows that the RMSE of agent 1 after convergence is 0.51m and the RMSE of agent 2 after convergence is 0.42m. The positioning accuracy will further increase as the number of nodes in the network increases. Both agents have the ability to quickly converge, convergence can be completed after five iterations. Due to the small amount of calculation and fast convergence, FG-3DCP can achieve accurate realtime positioning in actual experiments. Carrying out further theoretical analysis, the computational complexity of FG-3DCP is O (N ) while N is the number of nodes, and the computational complexity of SPAWN is O K 2 · N while K represents the number of particles. Obviously, the computational complexity of SPAWN is much higher than FG-3DCP under similar calculation accuracy. It can be proved after experiment and analysis, FG-3DCP has the advantages of high positioning accuracy and low computational complexity. It can provide users with three-dimensional real-time highprecision positioning.

IV. CONCLUSION
To mitigate the two-dimensional limitation of existing cooperative localization technologies and their slow computational speeds that cannot meet the demands of real-time calculations. Based on the 2-D factor graph cooperative positioning algorithm, this paper proposes an FG-3DCP algorithm that is suitable for three-dimensional space. The proposed algorithm is based on the knowledge of factor graph theory; uses formulae of variable nodes and function nodes in the factor graph; determines the mathematical model and topological structure of the cooperative position; and solves the problem of multiagent cooperative position in practical application scenarios. Using simulations, this paper introduces the cooperative network topology and then simulates and analyzes factors such as ranging error, position ambiguity and the number of nodes. Finally, the proposed method is compared to other methods, the results show that the proposed method achieves a faster convergence speed than other methods and that approximately 99% of agents' positioning accuracy can reach 2 m.