This paper aims to present a multi-source information fusion algorithm based on factor graph for autonomous underwater vehicles (AUVs) navigation and positioning to address the asynchronous and heterogeneous problem of multiple sensors.
The factor graph is formulated by joint probability distribution function (pdf) random variables. All available measurements are processed into an optimal navigation solution by the message passing algorithm in the factor graph model. To further aid high-rate navigation solutions, the equivalent inertial measurement unit (IMU) factor is introduced to replace several consecutive IMU measurements in the factor graph model.
The proposed factor graph was demonstrated both in a simulated and vehicle environment using IMU, Doppler Velocity Log, terrain-aided navigation, magnetic compass pilot and depth meter sensors. Simulation results showed that the proposed factor graph processes all available measurements into the considerably improved navigation performance, computational efficiency and complexity compared with the un-simplified factor graph and the federal Kalman filtering methods. Semi-physical experiment results also verified the robustness and effectiveness.
The proposed factor graph scheme supported a plug and play capability to easily fuse asynchronous heterogeneous measurements information in AUV navigation systems.
