Information Fusion in Navigation Systems via Factor Graph Based Incremental Smoothing

View/ Open
Date
2013-08Author
Indelman, Vadim
Williams, Stephen
Kaess, Michael
Dellaert, Frank
Metadata
Show full item recordAbstract
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.