Inertial Navigation System/Topology Measurement Integrated Algorithm with Factor Graph for Indoor Cooperative Localization

To address the deficiencies of standard filters and low robustness in cooperative localization system, a novel factor graph algorithm is proposed, which can be applied to indoor multiple users by fusing topology measurement provided by cameras with inertial navigation system. A topology measuring cooperative localization method is further presented utilizing the objects detection algorithm from the images. To derive a flexible optimization model of the navigation solution with incorporating asynchronous sensors capabilities, the topology measuring factors and inertial navigation system factors are created in solving nonlinear optimization problems. To further enhance the robustness, an improved switch constraint algorithm is developed by introducing weights decision approach considering both residuals and detection scores, and it better suits to the topology measurements. Simulations and experiments show that the rising accuracy of topology measurements improves position and velocity estimations and reduces iteration times of the navigation solutions. The positioning accuracy of non-cooperative+non-robust approach is 79.8% higher than that of cooperative+improved switching constraint approach. The improved switch constraint algorithm has a higher prediction success rate than that of the original switch constraint algorithm, and when the ratio of measuring occlusion duration is 0.4, the former improves the prediction success rate of the latter for outliers from 89.35% to 97.4%. Compared with the switch constraint algorithm, the improved algorithm by introducing weights decision approach removes abnormal topology measurements of the same user with multiple detected rectangles, improves cooperative positioning accuracy and robustness time consumption in computation. © 2020, Editorial Office of Journal of Xi'an Jiaotong University. All right reserved.