A Factor Graph Integrated Navigation Method Based on Inertial Navigation Pre-integration

Ocean gravity information is an important part of the Earth’s gravity field model, and its data processing process is divided into integrated navigation and gravity extraction. High-precision measurement of ocean gravity should firstly improve the integrated navigation accuracy. Kalman filter uses interpolation to process multi-sensor heterogeneous data with different frequencies, which will lead to inaccurate observation values. In this paper, a factor graph integrated navigation method based on inertial navigation pre-integration is proposed. Firstly, the multisensor data are abstracted into variable nodes and factor nodes in the factor graph, and the nodes are connected by using the state transition probability density function. Then, the access of new nodes at different times is realized by updating the joint probability density function step by step. Finally, the nonlinear optimization method is applied to solve the optimal estimation result under the maximum a posteriori condition. Experimental results show that the proposed method has higher robustness and positioning accuracy compared with Kalman filtering method.