Cooperative Navigation Algorithm For Human-vehicle System Based On Factor Graph

Pedestrian navigation system,also known as individual navigation system or personal navigation system,is a wearable navigation device.Pedestrian navigation systems applying the principle of inertial navigation solution are often implemented by an Inertial Measurement Unit / IMU which is developed from a Micro Electro Mechanical System.However,the accuracy of MEMS IMU is low and error will accumulate rapidly when using it navagating.Reducing error is a hot issue in the field of pedestrian navigation.Collaborative navigation is an emerging navigation method which is often used in multi-agent system / MAS.Through sharing navigation status among nodes,measuring information between nodes or storing and forwarding information of location information and uncertainties of nodes,collaborative navigation technology can improve accuracy of some or all nodes with low costs of devices.Pedestrian navigation algorithm based on ZUPT,cooperative navigation algorithm based on factor graph and observability analysis of cooperative navigation based on Lie derivative are researched in this paper.Around the pedestrian navigation algorithm,the error of pedestrian navigation system is analyzed and modeled,the extended Kalman filter algorithm based on ZUPT is designed and implemented.Aiming at the problem of height divergence,the barometer height information is fused to constrain the height error.Aiming at the problem of heading angle error,a comprehensive model of geomagnetic field measurement error is established.The outlier detection strategy is introduced in parameter calibration.By eliminating the gross error data,the calibration accuracy is improved.A heading correction algorithm based on magnetic distortion rate adaptive complementary filtering is proposed,which effectively reduces the heading angle error.Around the collaborative navigation algorithm based on factor graph,the model of collaborative navigation system based on factor graph theory is proposed,the information fusion framework based on factor graph is established,the cost function of navigation state optimization is derived,and the genetic algorithm is introduced to optimize the navigation state to complete the information fusion.Around the analysis of cooperative navigation observability based on Lie derivative,the model of cooperative navigation observability is proposed,and the distribution of observability based on distance measurement and angle measurement is analyzed respectively.The correctness of the analysis of observability distribution is verified by numerical simulation.