An Optimal Cooperative Navigation Algorithm based on Factor Graph for Pedestrians

摘要

In order to study the problem of pedestrian cooperative navigation based on foot-mounted Micro Inertial Measurement Unit (MIMU) and Ultra-Wide Banded (UWB) ranging module in the GNSS-denied environment, a cooperative navigation algorithm for pedestrians based on factor graph optimization is presented. Combined with the characteristics of Zero Velocity Update (ZUPT) algorithm, the walking model of pedestrian is modeled. The algorithm proposed establishes a local factor graph for each pedestrian participating in the cooperative navigation, and represents the update process of system state and multi-sensor data fusion based on factor graph model. Each pedestrian can get the optimal solution of his position after many iterations. The algorithm proposed does not need any changes and feedback correction to the bottom of the foot-mounted MIMU, and it is easy to achieve while ensuring the navigation accuracy. The experimental results show that the algorithm proposed can better improve the navigation accuracy of each pedestrian compared with the cooperative navigation algorithm based on Kalman filter. When the number of pedestrians changes in real time in the process of cooperative navigation, the algorithm proposed can still effectively correct the navigation error. Compared with the filtering algorithm, our algorithm can better integrate other sensors into the cooperative navigation system by adding factor nodes.