Geometric-Manifold-Assisted Distributed Navigation Probabilistic Information Fusion Cooperative Positioning Algorithm

: Positioning information is the cornerstone of a new generation of electronic information technology applications represented by the Internet of Things and smart city. However, due to various environmental electromagnetic interference, building shielding, and other factors, the positioning source can fail. Cooperative positioning technology can realize the sharing of positioning information and make up for the invalid positioning source. When one node in the cooperative positioning network has error, the positioning stability of all nodes in the whole cooperative network will be signiﬁcantly reduced, but the positioning probability information technology can effectively reduce the impact of mutation error. Based on this idea, this paper proposes an information-geometry-assisted distributed algorithm for probabilistic cooperative fusion positioning (IG-CP) of navigation information. The position information of different types of navigation sources is utilized to establish a probability density model, which effectively reduces the inﬂuence of a single position error on the whole cooperative position network. Combined with the nonlinear ﬁtting characteristics of the information geometric manifold, mapping and fusion of the ranging information between cooperative nodes on the geometric manifold surface are conducted to achieve cooperative positioning, which can effectively improve the stability of the positioning results. The proposed algorithm is simulated and analyzed in terms of the node positioning error, ranging error, convergence speed, and distribution of the cooperative positioning network. The simulation results show that our proposed cooperative positioning algorithm can effectively improve the positioning stability and display better positioning performance.


Introduction
Cooperative positioning is the core foundation of 5G application technologies such as Internet of Things and smart city. Due to the inherent errors of satellite navigation, inertial navigation, and other navigation methods, some nodes in the cooperative network will have error mutation [1][2][3][4][5][6][7]. It is very important to study the stability of cooperative positioning accuracy. The cooperative position system has received much attention from the research community and has wide applications, such as regional unmanned driving and an unmanned distribution network for urban forests [8][9][10]. Unmanned aerial vehicles (UAVs) need the support of high-stability positioning in the above scenarios; fortunately, the cooperative positioning system has many advantages in improving the stability of positioning accuracy.
Early cooperative positioning technology mainly utilized ranging and direction finding to obtain the positions of nodes. The first generation of cooperative positioning slow convergence speed and high sensitivity to the cooperative positioning topology network structure.
In order to solve the above problems, a cooperative positioning fusion algorithm based on information geometry theory is proposed. Information geometry was first utilized in radar target detection [32][33][34]. All kinds of electromagnetic parameters are transformed into an information probability function, and an electromagnetic scene is constructed. When the parameters of the electromagnetic scene are changed, target detection can be realized. Because different types of electromagnetic parameters are transformed into a probability density function, multitype parameters can be fused. The cooperative positioning network environment is similar to the radar signal detection environment; thus, it greatly increases the positioning accuracy stability.
The positioning probability density function of the cooperative node is constructed using the multigroup positioning information instead of the single-group positioning information. Through the variance and mean value of the function, the positioning performance of the node can be clearly reflected, which is conducive to the fusion processing and improvement of the positioning accuracy stability in a cooperative network. Based on this idea, this paper proposes an information-geometry-assisted distributed navigation information probabilistic cooperative fusion positioning (IG-CP) algorithm. The algorithm utilizes the positioning information of each cooperative node to establish a positioning error probability model, which is mapped to the geometric manifold and combined with the distance information between cooperative nodes to achieve fusion of the positioning information. The optimal fusion estimation of the position distribution probability of cooperative nodes is utilized to replace the positioning result of the last instance, and the process has an iterative solution. Combined with the nonlinear characteristics of the geometric manifold, the stability of cooperative node positioning can be effectively improved.
The rest of this paper is organized as follows: the cooperative positioning system model and the information geometry model are presented in Section 2. Based on this model, the IG-CP algorithm is explained in Section 3. To reduce the influence of the node positioning error and ranging error, the phase interference positioning theory is combined with information geometry to suppress the ranging error and node positioning error. The simulation results are given in Section 4, which mainly include the positioning error, ranging error, node distribution, and computational complexity. Finally, this paper concludes with a brief summary in Section 5.

System Model
Some formulas and symbols used in this paper are defined as Table 1. sufficient statistics of the positioning information data δ the distance measurement error w the distance measurement error vector σ 2 resents the variance n the position error r the distance measurement f (r|d, σ) the probability density measurement function l() the logarithmic likelihood function In the cooperative positioning network, there are hundreds of thousands of cooperative nodes. The scale of the cooperative node network becomes larger than before, and the system model is shown in Figure 1.  In the cooperative positioning network, all nodes need to be able to utilize the positioning and ranging information of the surrounding nodes to improve its positioning stability. In our proposed IG-CP algorithm, node D in the cooperative positioning network is randomly selected, and the positioning information of the A, B, and C nodes and the ranging information between them are utilized to realize the cooperative positioning of node D, as shown in the virtual frame of Figure 1.

Information Geometry Probability Model
In the cooperative positioning network, the positioning information of each cooperative node is transmitted to other nodes in the cooperative position network. The positioning accuracy of cooperative networks is mainly determined by the positioning accuracy and ranging accuracy. The change in the positioning accuracy is a nonlinear variation with arbitrary jitter. The existing cooperative position fusion technology mainly adopts attenuation coefficient to modify it such that the longer the time, the smaller the attenuation coefficient. When the positioning result of the cooperative node changes, it can only slowly increase or decrease the influence of the cooperative node on the positioning accuracy of the whole cooperative positioning network. It is difficult to realize the rapid update of cooperative node positioning information, thus limiting the engineering application of cooperative positioning technology. The flat surface of the information geometry itself is a kind of surface manifold, which is more suitable and easier to implement for complete nonlinear estimation. The complete nonlinear estimation of the information geometry transformation model is shown in Figure 2. Figure 2. Information geometry transformation diagram.
In the standard Euclidean space, the positioning estimation of the cooperative node D can be expressed by the likelihood function p(x|u). x represents the positioning sampling data. u represents the position coordinate vector of the cooperative node D. The likelihood function p(x|u) of cooperative node D can form a statistical manifold S = {p(x|u)} in Euclidean geometry, and it can be represented by parameterizations with the natural parameter θ and expectation parameter η. The likelihood function p(x|u) can be smoothly embedded into the Riemann geometric manifold of the exponential distribution by mapping u → θ(u), that is, in the natural parameter space {θ} ∈ A; it becomes a curve in the space, and its parameter equation is {θ = θ(u)}. The coordinate estimation problem of cooperative node D can be solved by the curve θ = θ((u)) in the natural parameter space A. The right graph in the lower half of Figure 2 represents the expected parameter space η ∈ B. The expected parameter space B and the natural parameter space A are dual. Dots represent the positioning information data after conversion in the expected parameter space B, and the corresponding relationship between the natural parameter space and expected parameter space can be established by the Legendre transformation. The nonlinear likelihood function p(x|u) is transformed into a standard family of exponential distributions p(F(x)|θ) by parametric reconstruction, where F(x) represents sufficient statistics of the positioning information data. It can obtain the coordinate estimation natural parameter value θ of cooperative nodes by linear estimation of the sufficient statistics F(x); then, mapping can be conducted from θ to u to obtain the positioning result of the cooperative node D. Based on the theory of information geometry, the position estimation of cooperative nodes is fitted to a point on the exponential geometric manifold; among them, the nonlinear characteristics of the positioning information data are reflected in the geometric structure change of the manifold, and the nonlinear solution of the position result can make full use of the geometrical characteristics of the manifold. On the other hand, differential geometry can be applied to solve manifold problems, and a geodesic iteration instead of a state update in the Kalman filter can yield better positioning results in the cooperative network fusion positioning system.

Information-Geometry-Assisted Cooperative Positioning (IG-CP)
To calculate the positioning of the node D, two groups of distance differences are adopted to construct the phase interference positioning model, two nodes among A, B, C, and D are selected as transmitters, and two sinusoidal signals with a small frequency difference are transmitted to form a differential frequency interference signal [35]. The remaining two nodes act as receivers, and the distance between the four nodes can be calculated according to the phase difference of the received signal, which can be used to eliminate the positioning ambiguity of nodes. For example, if nodes A and B are assumed to be transmitters and nodes C and D are assumed to be receivers, the corresponding phase interference positioning measurements can be expressed as: where X A , X B , X C , and X D represent the position coordinates of four cooperative nodes, respectively. X D − X A represents the distance between A and D; thus, Equation (1) can be rewritten as follows: When positioning node D in the point wireframe of Figure 1, it is also possible to set nodes A and C as transmitters and nodes B and D as receivers, and the corresponding phase interference positioning measurement can be expressed as follows: The measurement vector k (u) of phase interference positioning is established by using two measurement sets of the cooperative node D, which is expressed as follows: where u = [u x , u y ] T represents the position coordinates of the cooperative node D, which are to be estimated using unknown parameters. δ ab and δ ac represent the distance measurement errors. Therefore, the general phase interference positioning model of a cooperative node can be expressed as follows: where w represents the distance measurement error vector, x represents the measurement data, and σ 2 represents the variance in x. However, the position ambiguity error of a node is not considered in the above cooperative positioning model, which is too ideal.
To fit a real cooperative positioning network, each cooperative node must be completely independent, and it is assumed that the positioning error of each cooperative node is an independent zero-mean Gaussian distribution error. Taking the cooperative nodes A, B, and C as examples, the position error is recorded as n a , n b , and n c . After the node position error is introduced, Equation (5) can be expressed as In the process of cooperative network positioning, the errors along each axis of every cooperative node are completely independent, being set as n i,x and n i,y , with i representing the cooperative node number. According to statistical theory, when the position of cooperative node A contains Gaussian noise, we utilized Rice distribution model to construct the distance measurement r between A and D, where d = (x a − u x ) 2 + (y a − u y ) 2 is the real distance from cooperative node A to cooperative node D. Its probability density function can be expressed as follows: where r = (x a − u x + n a,x ) 2 + (y a − u y + n a,y ) 2 , σ represents the standard deviation of the measurement noise, and function I 0 (z) represents a zero-order modified Bessel function of the first kind: Since the distance measurement between cooperative nodes is usually more than 100 times the standard deviation of noise, the last term of Equation 10 can be approximated as follows: Therefore, the probability density measurement function between cooperative nodes can be rewritten as: Furthermore, the measured distance r between cooperative nodes can be approximated by a Gaussian distribution [36] with a mean value µ = √ d 2 + σ 2 and standard deviation σ, namely: When the measurement distance between cooperative nodes obeys the Gaussian distribution and is more than 100 times less than the measurement distance, Taylor series expansion can be utilized to approximate the distance. Taking the measurement distance between A and D as an example, η and ξ represent the distance measurement components on the x-axis and y-axis, respectively. The measurement distance between A and D is expressed as a function of two variables η and ξ as follows: Since the position error is included in the cooperative node A, the distance measurement can be rewritten as r = f (η + n a,x , ξ + n a,y ) = (η + n a,x ) 2 + (ξ + n a,y ) 2 (16) Expanding the Taylor series of the function f (η + n a,x , ξ + n a,y ) at point (η, ξ) yields f (η + n a,x , ξ + n a,y ) = f (η, ξ) + n a,xḟη (η, ξ) + n a,yḟξ (η, ξ) + · · · (17) Due to the position error of cooperative node A being far less than the measurement distance, the influence of higher-order terms in the Taylor expansion is very small, and the distance between cooperative nodes A and D can be approximated as follows: The position errors n a,x and n a,y comprise a Gaussian distribution with zero mean; thus, the sum of the two position errors still obeys the Gaussian distribution, and the mean µ s and variance σ s are expressed as follows: where σ a represents the standard deviation of the position error of collaboration node A.
The distance between cooperative nodes A and D can be approximately expressed by the following Gaussian distribution: From Equation (22), it can be seen that the position error does not affect the distance measurement distribution between cooperative nodes. According to the approximate result of the Taylor-expanded distance measurement shown in Equation (17), d AD , d BD , d BC , and d AC represent the ideal measurement distances between cooperative nodes as follows: The distance measurement related to the positioning solution of cooperative node D is as follows: The phase interference positioning measurement with position error k A,B,C,D can be expressed as Because the position errors of cooperative nodes along each axis are completely independent, the variance σ 2 s1 in the sum of the six position error terms in (28) can be obtained as follows: Similarly, the variance σ 2 s2 in another measurement k A,C,B,D is as follows: When the cooperative nodes have positioning errors, the phase interference positioning distance measurement values k A,B,C,D and k A,C,B,D have approximately the following distributions.
In a real situation where the cooperative node has a position error, the general phase interference positioning model of the cooperative node can be expressed as follows: n(u) represents the equivalent distribution of cooperative positioning node position errors, which is fitted to a Gaussian distribution with a mean value of 0 and variance of Σ s = diag(σ 2 s1 , σ 2 s2 ) in our model. w represents the measurement error of the phase interference positioning distance. Due to the measurement of the distance being completely independent in the phase interference positioning, x represents the positioning sampling data, u represents the position coordinate vector of the cooperative node, Σ w represents the variance of noise, and Σ s represents the variance of position coordinate vector. T he conditional probability distribution between the measurement value and the optimal estimation value is as follows.
The natural gradient method based on the statistical manifold is adopted to realize optimal positioning coordinate estimation of the cooperative node D, and according to the phase interference positioning measurement distribution given in Equation (33), the likelihood function of the measured value can be expressed as follows: The Gaussian probability density function shown in Equation (34) can be arranged into a standard bending index distribution form: According to Equation (35), in information geometry, it is necessary to establish a new parametric representation of natural parameters on the geometric manifold, where the natural parameters of the cooperative positioning nodes are set to (θ, Θ). The relationship between the natural parameter and the local parameter u is as follows: To simplify the calculation, the sufficient statistics of the Gaussian distribution of the measured value are set as a linear model On the geometric manifold, the potential function distributed ϕ(θ, Θ) can be expressed by local parameters as follows: where n represents the dimension of µ(u) or the potential of a set. The maximum-likelihood estimationû of the local parameter u can be obtained by solving the following maximumlikelihood equation: where l(û) represents the logarithmic likelihood function and η represents the expectation function. According to the properties of the bending index distribution, the expected parameter η and the Fisher information matrix G(θ) of the natural parameter can be obtained from the derivative of θ by the potential function ϕ(θ) as follows: The Jacobian matrix of the natural parameter θ with respect to the local parameter u of the cooperative node positioning coordinate is expressed as: The Fisher information matrix of the local parameter u is On the geometric manifold of natural parameters, maximum-likelihood parameter estimation of the bending exponential distribution family is adopted to obtain the positioning of the cooperative node D, and estimation update of the positioning coordinate u is as follows: where λ represents the iterative step size. When the new local parameter u k+1 of the positioning coordinate is obtained, the Fisher information matrix G(u) of the information geometric plane needs to be updated, as shown below: We utilized the iterative calculation method proposed in reference [29]: when the difference value e k+1 between two iterations is less than a certain threshold th, the iteration terminates, which is expressed as follows: The estimated value u k+1 is considered as the actual coordinate of the collaboration node. The flow of our IG-CP algorithm is shown in Figure 3.  The natural gradient utilizes the local curvature of the geometric manifold to modify the iterative direction of the standard gradient, which can result in a faster convergence rate. In addition, the Fisher information matrix is updated at the same time in each iteration based on the natural gradient estimation, which can meet the real-time fitting of the nonlinear positioning error and ranging error of the cooperative positioning system. It can effectively improve the accuracy and stability of cooperative positioning.

Ideal Condition Simulation
The size of the cooperative positioning network is 1000 m × 1000 m. In the cooperative network, the coordinates of the known cooperative nodes are (200 m, 200 m), (800 m, 100 m), and (500 m, 900 m), and the variance of the positioning errors of nodes with known positions is 1 m. The true coordinates of the unknown position node are at (500 m, 500 m), and the variance of the positioning error of an unknown node is 5 m. The measurement distance between cooperative nodes is the real value, and the ranging error is 0 m. On the plane of the geometric manifold with natural parameters, the probability density distribution of locating nodes is fused, and the maximum estimation of the probability density distribution is considered as the positioning result of the unknown cooperative node. The simulation results are shown in Figure 4. From Figure 4, we can see that the ranging error is 0 under the ideal condition of cooperative positioning. After the iterative convergence is completed, the optimal position estimation value of the cooperative node with an unknown position is exactly the same as the real position value, both of which are (500 m, 500 m), and the distribution probability density of the position is the same as that of the cooperative node at the unknown position. This result shows that the IG-CP algorithm, which utilizes the information probability to achieve cooperative positioning, can reduce the positioning error of cooperative nodes.

Simulation under Different Ranging Errors
In the cooperative positioning network, the ranging error will have a great impact on the positioning accuracy of the cooperative node. Existing ranging technologies mainly include radio ranging, UWB ranging, laser ranging, radar ranging, and other methods, with accuracies ranging from the cm level to the 10 m level. Therefore, the variance values of the ranging error are 10 m, 5 m, and 1 m in the simulation, and the variance of the positioning error of the cooperative node is 1 m. The distribution of the cooperative positioning network is the same as that under the ideal condition, and the simulation results are shown in Figures 5-7.   From Figure 5, we can see that when the variance of the ranging error is 10 m, the unknown positioning node's maximum probability density of the positioning error is only 0.7, far less than that of the other known positioning cooperative nodes; however, the optimal position coordinate estimation of the unknown cooperative node is the same as that in a real situation, and it is still (500 m, 500 m). The simulation results show that the proposed cooperative positioning algorithm based on information geometry can reduce the influence of the ranging error by fusing the information probability of the cooperative node in the geometric manifold. It can be seen from Figures 6 and 7 that as the variance of the ranging error decreases, the maximum value of the positioning error probability density of a cooperative node with an unknown position is close to the ideal situation, and the optimal value of the positioning coordinate is kept at (500 m, 500 m). Moreover, the distribution range of the positioning error probability function also approaches the ideal situation. This outcome shows that our IG-CP algorithm can effectively reduce the influence of ranging errors between cooperative nodes. When the ranging error between nodes becomes larger, it can also ensure the stability of the positioning accuracy of the whole cooperative network.

Simulation of Cooperative Positioning under Extreme Distribution
In the application of the cooperative positioning network, there will be an extreme distribution of other cooperative nodes around some edge nodes in one direction, resulting in a

Integrated Positioning Simulation under a Multinode Network
In the development of the cooperative positioning network, the number of nodes increases exponentially. Because of the cost and load, most of the nodes have only one or no navigation source; thus, it is necessary to improve the accuracy by cooperative positioning. In this section, the scope of the cooperative positioning network is also set to 1000 m × 1000 m, and the total number of cooperative nodes is 20. Among them, the variance of the positioning error of any five nodes is 1 m, and that of the other nodes is 5 m. The variance of the ranging error between cooperative nodes is 1 m. The simulation result is shown in Figure 9.
After multiple iterations, the optimal positioning estimates of all cooperative nodes are close to the real positions. According to the positioning error probability density distribution of cooperative nodes, the positioning accuracies of all cooperative nodes are basically the same, and the maximum probability density is close to 0.9. The positioning accuracy of all cooperative nodes is similar to that of the optimal cooperative nodes. It is proved that our proposed position error probability function fusion technology can quickly realize the positioning of the whole cooperative positioning network, eliminate the influence of the ranging error, and improve the positioning accuracy of the whole cooperative network.

Simulation Analysis of the Convergence Rate
In the cooperative positioning network, the convergence rate is an extremely important index, as it is the key factor of the application of the cooperative positioning network. Among them, the variance of the positioning error of any five nodes is 1 m, that of other nodes is 6 m, and the variance of the ranging error between cooperative nodes is 3 m, after T = 100 Monte Carlo simulations. To compare with the performance of the existing cooperative positioning algorithm, the IG-CP algorithm proposed in this paper is compared with the SDP algorithm proposed in [6], the Taylor-DP algorithm proposed in [8], and the FGCP proposed in [9]. The simulation results are shown in Figure 10. In Figure 10, the positioning errors of the four algorithms decrease and tend to converge with the increase in the number of iterations. The Taylor-DP algorithm requires 15 iterations to complete the convergence, and the MMSE is 1.7 m. The convergence rate of the FGCP algorithm is better than that of the Taylor-DP algorithm. The FGCP needs 12 iterations to complete the convergence, while the MMSE requires approximately 1.3 m. The convergence rate of the SDP is lower than that of our IG-CP algorithm and better than that of the other algorithm, and it requires eight iterations to complete the iterative convergence. The MMSE is close to the FGCP algorithm, requiring approximately 1.3 m. Our IG-CP algorithm has the fastest convergence speed, with convergence being completed in approximately five iterations. The MMSE of IG-CP is close to 1 m, which is equivalent to the cooperative node with the highest positioning accuracy.

Real-Environment Test
The IG-CP algorithm was tested in a real environment by using sensor nodes to construct a cooperative positioning network. The DWM1000 module is adopted to construct the cooperative node, and the distance between the cooperative nodes is measured by the UWB communication of DWM1000 [37]. The range of the DWM1000 module is 3 km, and the measurement accuracy of the module is 0.1 m, roughly the size of a coin. The appearance is shown in Figure 11. To realize the positioning of the cooperative nodes, the STM32 development board designed by our team is utilized in the cooperative positioning system, as shown in Figure 12.  To show the positioning performance of a large-scale cooperative node network, the experimental area was a farm near our university. The area included a small village and farmland, as shown in Figure 13. Twenty cooperative nodes were randomly set in the range of 1000 m × 1000 m. The initial positioning error and ranging error depend on the node device. The simulation result is shown in Figure 14. As can be seen in Figure 14, when the iteration is complete, the maximum probability density is close to 0.85. The positioning accuracy of all cooperative nodes is similar to that of the optimal cooperative nodes with high accuracy in the cooperative network. the optimal positioning estimates of all cooperative nodes are close to the real positions. The experimental results are in agreement with the simulation results, and the village buildings have little influence on the positioning accuracy. A few cooperative nodes are at the edge of the cooperative location network and have large fluctuation due to the influence of the accumulation of ranging errors in the same direction. However, most of the cooperative nodes can improve the positioning accuracy through the IG-CP algorithm proposed in this paper. In the real-experiment test, the algorithm processing module is implemented by the STM32 development board and can realize a real-time response, which proves that the IG-CP algorithm has low computational complexity.

Conclusions
The existing distributed cooperative positioning methods generally suffer from high computational complexity and a slow convergence speed; thus, it is very difficult to apply the cooperative positioning technology in practice. The probability density model of positioning error information is established using the navigation information of each cooperative node in a distributed cooperative network; then, the positioning information fusion is carried out by combining the ranging information between cooperative nodes on the plane of the geometric manifold. In this paper, a simulation analysis is carried out in terms of the ranging error, node distribution, convergence speed, and communication overhead. The simulation results show that IG-CP can reduce the influence of the ranging error on the cooperative positioning node when the magnitude of the ranging error is the same as that of the positioning error of the cooperative node. Regarding the extreme distribution of a cooperative location network, the fusion of the location information probability model can avoid the accumulation of single-direction positioning errors and improve the positioning accuracy of cooperative location nodes at the edge. In the context of iterative computation, the iterative speed of IG-CP is more than 30% higher than that of the existing cooperative positioning algorithms, and the communication cost is lower than that of the other cooperative positioning algorithms. Our proposed IG-CP algorithm has lower computational complexity and a higher precision of cooperative positioning, which breaks through the shackles of existing cooperative positioning technology only from the perspective of location information fusion. It has better application value in the next generation of information technology, such as integrated space-based and ground-based networks, smart cities, driverless transport, and material distribution.