A Novel Three-Dimensional Coordinate Positioning Algorithm Based on Factor Graph

In this paper, a novel three-dimensional coordinate positioning algorithm based on factor graph is proposed to improve the measurement accuracy of the indoor global positioning system (iGPS) under large scale conditions. Different from the traditional iGPS positioning algorithm based on the least squares estimation, which has the problems of fixed solution model, low confidence results and poor stability, this proposed algorithm utilized Bayesian filtering to solve the coordinate positioning problem. Aiming at the character of plug and play for iGPS, a factor graph model based on Bayesian network is built, and then a sum product algorithm is used to convert the fixed model into the form of the product of every node, which reduces the independence of measurement information, and improves the confidence of the results. Furthermore, to further improve the positioning accuracy of the algorithm, an idea of maximum posterior estimation is integrated into the proposed algorithm, which enhances the stability of the algorithm at the same time. In order to verify the effectiveness of the proposed algorithm, a series of simulation and prototype tests have been carried out. The results show that compared with the traditional positioning algorithm based on least squares estimation, the accuracy of the proposed algorithm is improved by about 50%, and the positioning accuracy can achieve 0.3mm within a range of 10 meters, which realizes a high-precision measurement under large scale conditions.


I. INTRODUCTION
There are many different indoor positioning technologies, such as wireless fiedelity (WiFi), bluetooth, radio frequency identification (RFID), Zigbee, inertial measurement units (IMU), visible light positioning (VLP) and so on, which are the hotspots research at home and abroad. Among them, WiFi, bluetooth, RFID and Zigbee have a positioning accuracy in meter level, and there are some problems such as low positioning accuracy, many signal access points and low system stability [1]- [4]; IMU has a high positioning accuracy in a short time, but due to the influence of gyro The associate editor coordinating the review of this manuscript and approving it for publication was Walter Didimo . drift and accelerometer zero deviation, the positioning error increases with the increase of time [5], [6]; VLP has the positioning accuracy of centimeters or even millimeters and has good long-term stability, which shows a bright futrue for VLP [7]- [9].
However, in the large equipment manufacturing projects, the three-dimensional (3D) positioning accuracy requirements usually can be reached at a level of millimeter or even sub-millimeter in a few tens of meters of space [10]- [12]. As a result, the indoor positioning technologies as mentioned above cannot be suitable for the precision measurement under such conditions. To solve this problem, laser tracker, total station, theodolite and other new optical measuring facilities have been developed and applied in many technological VOLUME 8, 2020 This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ stages such as parts assembly, tunneling and deformation measurement for aircraft, subway and shipbuilding [13]- [15]. However, limited to the working principle of the facilities, they need a long time to achieve one single point positioning, which is quite low-efficient. In order to realize the 3D coordinates output in millimeter level, high efficiency and large scale conditions, NiKon proposed a new spatial measurement and positioning system called indoor global positioning system (iGPS), also Tian-Jin University invented a similar system called workspace measurement positioning system (wMPS) [16]- [19]. These systems realize positioning through the principle of angle intersection measurement based on laser scan; Furthermore, with the help of high-speed rotating motor, the systems can scan the measured space with a high frequency, and carry out a high frequency position information output. Based on the above two points, the systems solve the problem of low measurement efficiency under the condition of high positioning accuracy. Now these systems have been used as a new measurement method in many measurement and positioning scenarios. From the above, to enhance the systems' positioning capability, improve the precision level of large-scale equipment manufacturing, it is of great significance to carry out in-depth research on these systems, especially the research on positioning algorithm.
According to Reference [20], these systems construct the constraint equations through the bundle adjustment method based on photogrammetry at first, and then use the least square estimation (LSE) to acquire the 3D coordinates. This positioning algorithm is quite simple and easy to implement, however, there are three problems for it.
(1) The algorithm mathematical model is fixed. Due to the plug and play characteristics of these systems, the number of transmitter may change during measurement and this will directly cause a change in the algorithm mathematical model, resulting in abnormal positioning solution of these systems and affecting the positioning effect; (2) The positioning results are independent of each other with poor correlation. According to the algorithm mathematical model, once the number of observation information meets the demand of LSE model, the positioning solution is carried out, and the 3D positioning coordinates are output. However, since the observation information is not related to each other, the solution result is independent, which reduces the confidence of the positioning result and causes the resource waste of observation information.
(3) The algorithm based on LSE has weak anti-interference ability. Because of the poor robustness of LSE, when the system is disturbed, such as external vibration or speed mutation, its observation information will be mixed with errors, and these mixed errors will accompany in the solution process. As a result, the final positioning accuracy is decreased.
Due to the specificity and particularity of iGPS and wMPS, few people have carried out relevant algorithm optimization research on above problems. In this paper, a 3D coordinate positioning method based on factor graph (FG) is proposed to solve these problems. At first, based on the idea of factor graph and sum-product, we regard this observation information as a series of independent local nodes, thus the fixed mathematical model can be changed into the product of local nodes [21]. Thanks to the feature of flexible configuration for local nodes, the proposed algorithm can realize plug and play of observation information, and can no longer be limited to fixed mathematical models; Then since each independent local node is connected by soft message, thus a Bayesian network which satisfies the Markov process is formed, which improves the correlation of positioning estimation and improves the confidence of measurement results [22]; Besides, based on the thought of maximum posteriori estimation criterion, the proposed algorithm updates the state information at first and then the measurement information, which improve the robustness of the algorithm.
The remainder of this paper is organized as follows: Section 2 introduces the working principle and the accurate mathematical model. Section 3 designs the 3D coordinate positioning method based on FG. A validity evaluation is presented in section 4, including simulation and prototype tests. At last, a conclusion is remarked in section 5.

II. WORKING PRINCIPLE A. SYSTEM COMPONENTS
The main components of iGPS and wMPS are shown as Fig.1. The receiver placed in the point to be measured is a photoelectric converter, and it can receive optical signals from the transmitters and send the converted electrical signals to the front-end processor. The working principle of transmitter is similar to a theodolite to some extent, which measures the receiver's azimuth and pitch angles relative to itself [23]. Specifically, two line lasers with a fixed angle (θ off ) emit two fanned laser planes, and the two fanned laser planes can sweep through the whole measured space at a constant speed (ω) with the help of a stepping motor. Once the laser plane 1 turns a circle, the pulse lasers deliver pulse singles to receivers as the transmitter's starting time (T 0 ). The front-end processor separates the received signals from the line lasers and the pulse lasers, and calculates the time (T 1 , T 2 ) when the receiver receives the signal from the two line laser. As a result, the scanning angles (θ 1 , θ 2 ) of the two laser planes can be calculated based on the rotating speed (ω), shown as Equation (1). Based on the scanning angles, the receiver's coordinates can be determined as long as the receiver receives signals from at least two transmitters. Finally, to display the 3D coordinates, the results are sent to the computer through WiFi.

B. MATHEMATICAL MODEL
The mathematical model is based on two important coordinate systems which are defined as follows ( Fig. 2 and Fig. 3).   Transmitter coordinate system (marked as t): Z-axis is the transmitter's rotation axis which two laser planes rotate around. Origin is the intersection of laser plane 1 and Z-axis. X-axis is the intersecting line between the laser plane 1 and the vertical plane of Z-axis when the laser plane 1 rotates to the starting time (T 0 ). Y-axis is determined according to the right-hand rule.
Global coordinate system (marked as g): The coordinate system is based on two transmitters. Its Z-axis and Origin are the Z-axis and Origin of transmitter 1's coordinate system respectively. Supposing that the projection point of transmitter 2 on the vertical plane of Z-axis is O , then X-axis is the connection between the Origin and O . Also Y-axis is determined according to the right-hand rule.
The transformation relation between the two coordinate systems can be described as: where R is the rotation matrix which can be represented by three Euler angles, V is the translation matrix which consists of the transmitter's coordinates in global coordinate system. We define R and V as external parameters and these external parameters can be determined by calibrating [24]. Besides, there are some basic parameters in the transmitter coordinate system. According to Fig. 2 and Fig. 3, ϕ 1 and ϕ 2 are the inclined angles of laser Plane1 and laser Plane2, and θ off is a fixed offset angle between the two laser planes. Since two laser plans and the Z-axis cannot intersect at one point (the Origin) in the transmitter coordinate system because of the assembly error, we suppose the laser Plane2 intersects the Z-axis at O , then z is defined as the distance between the Origin and the O . These parameters ( ϕ 1 , ϕ 2 , θ off , and z) can be regarded as internal parameters, because they only depend on the assembly process, and they can be treated as constants as soon as the transmitter is completed.
Based on the coordinate systems and external/internal parameters defined above, the 3D coordinates can be calculated according to the bundle adjustment based on the photogrammetry. The method is described in detail as below.
When the laser plane 1 rotates to the starting time T 0 , the normal vectors of the two planes can be expressed as Equation (3) and Equation (4) in transmitter coordinate system: When the two laser planes sweep over the receiver at time T 1 and T 2 in turn, the normal vectors change into: where θ 1 and θ 2 can be obtained by Equation (1). VOLUME 8, 2020 According to the bundle adjustment algorithm, the plane parameter equation in transmitter coordinate system can be written as Equation (7).
T are the two laser planes' parameters in the transmitter coordinate system when the laser plane sweep over the receiver, respectively, and they can be defined as follows based on Equation (5) and Equation (6). X r t x r t , y r t , z r t is the receiver coordinates in the transmitter coordinate system.
To calculate the receiver coordinates in the global coordinate system X r g x r g , y r g , z r g , according to Equation (2), trans- T are the two laser planes' parameters of transmitter in the global coordinate system. If there are at least two transmitters sweep over the receiver, the receiver coordinates X r g x r g , y r g , z r g can be calculated using the following formulas.
The symbol n is the number of transmitters. Equation (12) can be arranged in the following form if n ≥ 2 : Since Equation (13) is an over-determined equation, thus the optimal solution of receiver coordinates X r g can be calculated based on LSE as shown in Equation (14).
According to Equation (14), we can know that the condition for the coordinate calculating is that the receiver should receive signals from at least two transmitters. Therefore, during positioning, no matter the number of transmitters increases or decreases, as long as the number of working transmitters is not less than two, the system can work normally.
Section 1 has analyzed the strength and weakness of the above solution mathematical model in-depth. In order to further optimize the coordinate positioning algorithm, we start to study the factor graph and sum-product algorithm in the next section.

III. 3D COORDINATE POSITIONING ALGORITHM BASED ON FG A. FACTOR GRAPH AND SUM-PRODUCT ALGORITHM
Factor graph is a probabilistic graph model based on Bayesian network, which transforms the global function into the product of nodes function by factor decomposition [25]- [27]. Thus it has the characteristic of visualizing the complex variable relations. The FG is composed of variable nodes and function nodes. The variable nodes are connected with the corresponding function nodes by the edge line to realize the information transmission between them, and we define the information as soft information (SI). Due to the bidirectional nature of information, SI can be divided into two types, namely, information passed from the variable node to the function node and information passed from the function node to the variable node. SI is often calculated by the sum-product algorithm, and the algorithm's basic principle is that the SI passed from the variable node to the measured function node is equal to the product of all the function nodes' SI passed to the variable node except the measured function node' SI. Fig. 4 shows a typical FG model. In this model, x stands for the variable node; f stands for the function node; H stands for the set of all function nodes connected to x;H/f stands for the set of all function nodes connected to x except function node f ; Similarly, Y stands for the set of all variable nodes connected to f ; Y /x stands for the set of all variable nodes connected to f except variable node x. µ x→f (x) stands for the SI passed from the variable node x to the function node f ; µ f →x (x) stands for the SI passed from the function node f to the variable node x; Similarly, µ y 1 →f (y 1 ) stands for the SI passed from the variable node y 1 to the function node f ; stands for the SI passed from the function node h 1 to the variable node x. µ x→f (x) and µ f →x (x) is calculated as Equation (15). Where~x is the variable nodes connected with the function node f except the variable node x.
According to the characteristics of information transmission in iGPS and wMPS, a mathematical model based on distributed FG is constructed, as shown in  the position result is output with 2 times' iterative update at Time k. The system still works. However, as for the traditional LSE algorithm, according to Equation (14), for different number of transmitters, the mathematical model is different. This will cause a problem that once the transmitters' number changes, the LSE algorithm will take one cycle to determine the number of transmitters and reselect the mathematical model to position. Thus if the transmitters' number changes from 3 to 2 at Time k, then the LSE algorithm will not output the positioning result at Time k. Similarly, if the transmitters' number increases at Time k, the iterations number of FG algorithm increases correspondingly, and the results can be output at Time k, while LSE will not be output the results at Time k due to the changes in the mathematical model. According to Fig. 5, observations from different transmitters are unrelated with each other. From the perspective of statistics, the error of independent observation information is also random, and this error will be brought into the positioning results. For example, if the observation information has a large gross error at a certain time, the positioning results will also contain a large outlier and we can't handle it, resulting in a decrease in the confidence of the positioning results. At the same time, independent observation information can only affect the positioning result of a certain moment. If the observation information of a certain moment is particularly good, it can only obtain good positioning information at that moment, but fails to improve the positioning accuracy of the subsequent moments, resulting in a waste of observation information resources. So, in order to avoid these problems, reduce the influence of the random error, and improve the effect of good observation information, the proposed FG algorithm using the ideas of iteration, through using the positioning result at Time k-1 and observation information at Time k, obtains the positioning result at Time k. This process can be regarded as link all independent observation information and make the results relevant.

2) THE CALCULATION METHOD OF SI
We still take the X-direction as an example. At first, let's calculate the SI passed from the variable node θ n−1 k to the function node h θ x n−1 k , shown as Equation (16).
where  can calculate the expectation and variance ofx n−1 k according to Equation (18) and Equation (19).
If the function Y = µ h θ →x n−1 k (θ) has a first-order derivative in the neighborhood of θ = θ n−1 k , then let's expand to the first-order Taylor equation at θ = θ n−1 k [28].
So the expectation can be express as: And the variance can be express as: From the above, . So the SI in Equation (18) and Equation (19) can be written as Equation (23).
At last, the SI passed from the function node f x n−1 k to the variable node x n k can be obtained, shown as Equation (24).
The update node function f x n−1 k = 1. Since the product of functions which satisfy the Gaussian probability distribution still satisfies the Gaussian distribution, the state node x n k can be represented by Equation (25).
During updating, the transformation relation about expectation and variance are shown in Equation (26).
According to Fig. 5, we suppose that the initial coordinate , which can be gotten by LSE, and its noise ω k ∼ ω k , m ω k , σ 2 ω k , thus the SI passed from the function node f to the variable node x n k can be shown as Equation (27).
m x n k is the final updated expectation in X-direction, let x n k = m x n k , namely, the positioning coordinate in X-direction at Time k is shown as Equation (28).
Similarly, the positioning coordinate y n k , z n k in Y-direction and Z-direction at Time k is shown as Equation (29).
From Equation (27) -Equation (29), we can see that the update of the status node is related to the measurement information and the previous status node information, which reduces the independence of the positioning results. Thus the positioning results can be output optimally.

3) ALGORITHM PERFORMANCE ANALYSIS
In order to analyze the effectiveness of the proposed algorithm in theory, taking X-direction as an example, we study the relationship among the status update nodex k obtained by the measurement information, the previous status update node x k−1 and the status update node x k based on the FG, and the three SI are shown as Equation (30).
According to the principle of positioning algorithm based on FG, the relationship among the three SI can be written as follows.
At first, comparing the variance ofx k , x k−1 and x k . Arranging σ 2 x k as Equation (32).
x k−1 < 1, so we have: According to Equation (33), the fusion variance based on FG is smaller than the variance of the two kinds of fusion sources. Since σ 2 x k < σ 2 x k , the algorithm based on FG is convergent, thus the positioning results will gradually close to the true value with the increase of the number of iterations.
Then comparing the expectation ofx k , x k−1 and x k . Supposing the real positioning is m r x k , so the errors of the three status nodes εx k , ε x k−1 and ε x k can be confirmed as follows.
Discussing the relationship of εx k and ε x k by making a difference shown as follows.
So the comparison ofx k and x k turns into the comparison of ε and 0. Since m r x k is a constant, to simplify the calculation, let m r x k = 0, thus Equation (35) can be expressed as Equation (36).
Equation (38) is a quadratic equation of one variable about mx k , and a 2 − 2a < 0. So if we want ε < 0,we just need mx k < x 1 or mx k > x 2 , x 1 , x 2 are shown as Equation (39).
This means the expectation m x k is bounded and will converge to truth gradually.
Moreover, writing Equation (31) as follows: Since the variance σ 2 x k−1 is convergent, which means σ 2 will be a small quantity after several iterations, so if there is a gross error in mx k , it won't cause a big interference for the result because of the tiny weight. And when the measurement expectation mx k is gradually close to the real expectation, its measurement variance σ 2 x k will also become small. In this way, when the measurement expectation mx k is better than the estimated expectation m x k−1 , its variance σ 2 x k will be better than the estimated variance σ 2 x k−1 , and the updated result m x k will be closer to the measurement expectation m x k−1 , as a result, the positioning result will be further optimized.
According to the above analysis, when there is a gross measurement error in the update process, the algorithm based on the FG can reduce the impact of the gross error to a great extent and improve the robustness and stability of the system; and when the measurement information performs well, the system positioning result become closer to the real value. To sum up, the proposed algorithm based on FG is very robust in theory.

IV. RESULTS
To verify the effect of proposed algorithm based on FG, a series of simulations and prototype tests have been designed and carried out.

A. SIMULATION EXPERIMENT
The simulation environment is built at first. According to the coordinate system definition in Section 2, in order to simulate the system, we suppose that the system has been calibrated, and the relative relationship between the transmitter coordinate system and the global coordinate system are directly given, namely, we think the transmitters' external parameters and internal parameters are known. Meantime the receivers' true coordinates in the global coordinate system are also known as the true value, so the true scanning angles θ 1 , θ 2 of the transmitter sweeping over the receiver can be calculated. According to [29], the external errors can be uniformly converted into the scanning angle error, so the random scanning angle errors θ 1 , θ 2 are added into the obtained scanning angles to simulate the actual working state of this system. Based on the scanning angles with errors, the proposed positioning algorithm based on FG can be tested.

1) SINGLE-POINT POSITIONING EXPERIMENT
The experimental conditions of single-point positioning are shown in Table 1. To simulate the plug-and-play nature of this system, a three-transmitters positioning scheme has been used at first. The three transmitters are called T 1 , T 2 , and T 3 for short, and T 3 has been removed for a period of time during the experiment. For the convenience of analysis, the same parameter information has been used in the three transmitters, and the scanning angle errors of the transmitters also be set at a same level. We have carried 100 times positioning simulation experiment. To compare the performance of the algorithms based on FG and LSE, the two algorithms are both simulated, and the results are shown as Fig.6 to Fig. 9. The positioning error R( X , Y , Z ) in different axes is obtained by Equation (41), and the total positioning error d is obtained by Equation (42). In Fig. 9, there are three transmitters working from Time 1 to Time 39 and Time 71 to 100, and two transmitters working from Time 40 to Time 70. This simulates the process in which transmitters' number decreases from 3 to 2 and then increases back to 3. It can be seen from the test results that the positioning output of the two algorithms are not affected     by the change in the number of transmitters. It should be noted that for the LSE algorithm, the process of model selection is ignored during the whole simulation period. In other words, for the moment when the transmitters' number changes, the positioning results of the corresponding model at that moment are directly output.
According to the results, we can see that the positioning error based on FG does not jump significantly when the number of transmitters changes, however, the positioning error based on LSE has been affected dramatically when the positioning scheme changes. This shows a great plug-andplay performance for the positioning algorithm based on FG.
According to the four figures above, the positioning error based on FG is convergent in trend, but it is still influenced by the measurement information, such as Time 2 to Time 7 and Time 40 to Time 50 in Fig. 7 and Fig 8 and Time 70 to Time 100 in Fig. 6 and Fig. 9. However, the positioning error based on FG is not higher than the measurement error or the positioning error at the previous time. This is because the algorithm based on FG improves the correlation between the measurement information.
We can see the positioning error based on FG isn't disturbed by the gross errors such as Time 40 and Time 46 in Fig. 6, Time 12 in Fig. 7, which shows the proposed algorithm has very good robustness and stability. Besides, if the measurement information is quite good with a small error such as Time 4 in Fig. 6, Time 9 in Fig. 7 and Time 8 in Fig. 8, the positioning error will immediately approach to 0, which shows the algorithm based on FG makes full use of the optimal information. As a result, the accuracy has been further improved.
In summary, the experimental results show that the positioning effect of FG is better than that of LSE, what's more, the experimental results satisfy the theoretical analysis in Section 3, which shows the correctness of the theoretical analysis.

2) THE WHOLE SPACE POSITIONING EXPERIMENT
We have only chosen one random point to compare the effect of FG and LSE, which is not representative. In order to verify the accuracy of the proposed algorithm, this section discusses the positioning effect of the two algorithms in the whole measurement space.
The experiment conditions are the same as in Table 1, and We choose two transmitters' scheme, namely, T 1 and T 2 . The whole measurement space is set as 10m×10m×10m. We have carried 10 times simulation experiment. To compare the performance of these two algorithms, we randomly select one of the experiment, and the results are shown from Fig. 10 to Fig. 17.     FG performs well in the whole measurement. Furthermore, we calculate the average of the ten positioning results of LSE, and the average total error of LSE is shown as Fig. 18. Comparing Fig. 16-Fig. 18, we can see that the proposed algorithm is still excellent.
There is an interesting thing in the figures above that needs further analysis, namely, there is an extreme value line in the plane of Y-O-Z, and the inclined angle of this line is    nearly 45 • . Next, we will take Equation (18) as an example to study the formation of the extreme value line.
For the convenience of analysis, the parameters except ϕ 1 in Table 1 are substituted into Equation (18).
(y, z) is the coordinates of the receiver to be measured at the previous time. X is the X-axis coordinate of T 2 in the global coordinate system, namely, X = 10. Thus the positioning error in X-axis can be got by Equation (44). X true is the true value of the receiver in X-axis, and it's a constant.
To find the extreme value, take the derivative of Equation (44), and let ∂ X ∂θ x n−1 When Plane1 scans into the Y-O-Z plane, θ x n−1 k = π 2 , tan β = z y , rewriting Equation (45) as follows: According to Equation (46), we know the tangent value of the inclined angle of the extreme value line and the tangent value of the plane inclined angle are reciprocal of each other. Since ϕ 1 = π 4 , thus β = π 4 . In conclusion, the inclined angle of extreme value line is proved.
According to the above analysis, it can be found that the positioning error may be related to the placement position of the receivers, and the positioning errors may be different for different positions in the measurement space. Since this problem is irrelevant to the topic of this paper, we do not discuss it in detail in this paper.

B. VERIFICATION EXPERIMENT
A system called ship high precision measuring and positioning system (SHPMPS) which is similar to iGPS in principle has been developed to verify the validity of the proposed algorithm. The whole experiment scenario is shown as Fig. 19. A SHPMPS with three transmitters and 4 receivers are placed randomly in the place to be measured. The transmitters' parameters and coordinate system parameters have been calibrated in advance, and the transmitters' coordinates in the global coordinate system are shown as Table 2. To compare the anti-interference ability of LSE and FG, the random perturbations are added to the transmitters during the whole experiment. The receivers coordinates are measured by a Leica AT403 laser tracker whose accuracy is ±15µm + 6µm/m [30].The receivers' coordinates are shown as Table 2, and these coordinates are treated as the benchmark value. The rotate speed of the transmitters is set as 50 revolutions per second, thus there are 50 times measurement information output per second in theory. The experiment starts with two transmitters scheme(T 1 and T 2 ), and changes into three transmitters scheme at about 1 second, and the whole experiment lasts about 2.5 second. This is long enough to complete the measurement for a static point. The algorithms based on FG and LSE are both used to realize the coordinate measurement, and the total positioning results of the 4 receivers are shown as Fig. 20 to Fig. 23.     According to Fig. 20-Fig. 23, 52 measurements have been made in the two-transmitters scheme and 80 in the three-transmitters scheme. The test results show that the system works well whether the transmitters' number changes or not. This verified that the change of the transmitters' number does not affect the positioning result output again.
However, it should be noted that the positioning result of LSE in Time 52 and Time 53 are the same, this is caused by the change in the mathematical model of LSE. To ensure that there is a output at every moment, when the system does not work, we artificially take the result of the previous moment as the result output of this moment. On the grounds of the results above, we can get the following conclusions.
(1) The algorithm based on FG is more flexible than that of LSE. Due to the plug and play characteristics of the algorithm based on FG, when the number of the transmitters changes, the algorithm based on FG only needs to increase or decrease the computing nodes, which does not affect the algorithm's solution speed and output frequency. However, the algorithms based on LSE need to change the solution model, which will affect the solution efficiency, even it may cause the positioning results cannot be solved when there is no preset model in the algorithm. Such as the output in the 52nd and 53rd update.
(2) The proposed algorithm has higher accuracy. It is not difficult to see from the figures above that the proposed algorithm has higher accuracy compared with the traditional algorithm. Table 3 summarizes the average positioning error and standard error of the two algorithms under different transmitter scheme. It can be seen from the table that the accuracy of the proposed algorithm is improved by about 50% under the condition of the three-transmitters scheme, and the standard error in FG is much lower than that in LSE, which reflect the stability of the algorithm based on FG. As for the average positioning error of the proposed algorithm is not better than the traditional algorithm in the two-transmitters scheme, this is because the initial value errors of the proposed algorithm is too large, but as the number of iterations increases, the positioning error gradually stabilizes at a small amount (At this time, the error of the proposed algorithm is also smaller than that of the traditional algorithm), which reflects a good convergence of the proposed algorithm, also reflects the low requirement for the initial value of this algorithm. As a result, the calculation difficulty of the algorithm has been reduced.
(3) The stability of the proposed algorithm is much better. With the increase of the observation information, the proposed FG algorithm can quickly obtain a higher positioning accuracy through constant iteration, and in the subsequent process, the proposed FG algorithm is not subjected to the interference of outliers, thus the output is very stable. While the result of the LSE algorithm is very independent, its location accuracy is lower, and it has to bear the interference of outliers.
(4) The anti-interference ability is better for the proposed algorithm. The results in Fig. 20 compare the anti-interference ability of the two algorithms. For example, at Time 46, 76, 104, 108 in Fig. 20, the positioning results by LSE are significantly higher than those at other times, and the reason for these large errors is our random perturbations, however, the positioning results by FG have not been significantly affected. Thus the factor graph has better anti-interference ability than the LSE. Also the positioning results of Time 39, 40, 87,132 in Fig. 21, Time 30, 67, 70 in Fig. 22 and Time 36, 92, 95 in Fig. 23 can prove the better anti-interference ability of the factor graph.
In addition, there are two findings from the test results. One is that the positioning accuracy is significantly improved as the number of transmitters increase, and the other is that the positioning accuracy is various at different locations, which also corresponds to the simulation results.

V. CONCLUSION
This paper presents a novel 3D coordinate positioning algorithm based on factor graph suitable for iGPS under large scale conditions. Compared with the traditional least square method, the proposed algorithm has the characteristics of plug and play, good stability and high positioning accuracy. The proposed algorithm is no longer limited to a fixed mathematical model. By changing the number of factor graph nodes, a flexible solution of the positioning results is realized, making the algorithm easier to use.
Simulation experiments and prototype tests have proved that, compared with the traditional least square algorithm, the positioning accuracy of the proposed algorithm under the condition of three transmitters is improved by about 50%, and the highest positioning accuracy reaches about 0.3mm on average within 10 meters. At the same time, due to the good stability of the proposed algorithm, the confidence of the system output results is greatly improved. Thus the performance of the system has been optimized obviously.
This article only studies the application of factor graphs in iGPS, for other plug and play systems, we also believe that factor graph will also perform well. In the future, we will optimize the positioning algorithm based on factor graph from the number of transmitters and layout of the receivers, which is not discussed in detail in this paper.
PAN JIANG was born in 1993. He received the bachelor's degree from Harbin Engineering University, China, in 2015. He is currently pursuing the Ph.D. degree with the Harbin Institute of Technology. His main research interest includes high precision inertial navigation algorithm.
HONGLIANG YIN was born in 1984. He received the Ph.D. degree from Beihang University, Beijing, China, in 2013. He is currently a Senior Engineer with the China Ship Research and Development Academy. His main research interest includes high precision inertial navigation algorithm.
DINGJIE XU was born in 1966. He is currently a Professor and a Ph.D. Tutor with the Institute of Navigation Instrument, Harbin Institute of Technology. He is mainly engaged in teaching and research in navigation, guidance, and control.