A multi-sensor information fusion method based on factor graph for integrated navigation system

The current navigation systems used in many autonomous mobile robotic applications, like unmanned vehicles, are always equipped with various sensors to get accurate navigation results. The key point is to fuse the information from different sensors efficiently. However, different sensors provide asynchronous measurements, some of which even appear to be nonlinear. Moreover, some sensors are vulnerable in specific environments, e.g., GPS signal is likely to work poorly in interior space, underground, and tall buildings. We propose a multi-sensor information fusion method based on a factor graph to fuse all available asynchronous sensor information and efficiently and accurately calculate a navigation solution. Assuming the sensor measurements and navigation states in a navigation system as factor nodes and variable nodes in a factor graph, respectively, the update of the states can be implemented in the framework of the factor graph. The proposed method is experimentally validated using two different datasets. A comparison with Federated Filter, which has been widely used in integrated navigation systems, demonstrates the proposed method's effectiveness. Additionally, analyzing the navigation results with data loss verifies that the proposed method could achieve sensor plug and play in software.