Factor graph aided distributed multi-navigation cooperative positioning algorithm

The development of smart city urgently needs the enhanced accuracy of navigation and positioning service, but the existing navigation technology is difficult to provide further improvement of positioning accuracy. The distributed cooperative positioning technology could increase the navigation and positioning accuracy, but the ranging/positioning error from any cooperative node will lose the effect. This paper presents a distributed cooperative positioning algorithm with factor graph-assisted. It constructs the belief information model by ranging error and positioning error of cooperative nodes, then fuse the positioning information through the factor graph theory. The cooperative nodes can access or disconnect the cooperative network at any time, which effectively avoids the influence of the positioning/ranging error of the cooperative node. The algorithm proposed in this paper is compared to several existing methods from four aspects: ranging error, positioning error of cooperative nodes, convergence speed and mutation error. The simulation results show that the proposed algorithm has 30% - 60% improvement in positioning accuracy compared to other methods under the same ranging error and positioning error. The convergence rate and mutation error elimination times are only one-third to one-fifth of the other methods.