Information Fusion in Navigation Systems via Factor Graph Based Incremental Smoothing

Abstract

This paper presents a new approach for high-rate information fusion in modern inertial navigation systems, that have a variety of sensors operating at different frequencies. Optimal information fusion corresponds to calculating the maximum a posteriori estimate over the joint probability distribution function (pdf) of all states, a computationally-expensive process in the general case. Our approach consists of two key components, which yields a flexible, high-rate, near-optimal inertial navigation system. First, the joint pdf is represented using a graphical model, the factor graph, that fully exploits the system sparsity and provides a plug and play capability that easily accommodates the addition and removal of measurement sources. Second, an efficient incremental inference algorithm over the factor graph is applied, whose performance approaches the solution that would be obtained by a computationally-expensive batch optimization at a fraction of the computational cost. To further aid high-rate performance, we introduce an equivalent IMU factor based on a recently developed technique for IMU pre-integration, drastically reducing the number of states that must be added to the system. The proposed approach is experimentally validated using real IMU and imagery data that was recorded by a ground vehicle, and a statistical performance study is conducted in a simulated aerial scenario. A comparison to conventional fixed-lag smoothing demonstrates that our method provides a considerably improved trade-off between computational complexity and performance.