/Factor Graph Based Incremental Smoothing in Inertial Navigation Systems

Factor Graph Based Incremental Smoothing in Inertial Navigation Systems

Vadim Indelman, Stephen Williams, Michael Kaess and Frank Dellaert
Conference Paper, Intl. Conf. on Information Fusion, pp. 2154-2161, July, 2012

Download Publication (PDF)

Copyright notice: This material is presented to ensure timely dissemination of scholarly and technical work. Copyright and all rights therein are retained by authors or by other copyright holders. All persons copying this information are expected to adhere to the terms and constraints invoked by each author’s copyright. These works may not be reposted without the explicit permission of the copyright holder.


This paper describes a new approach for informa- tion fusion in inertial navigation systems. In contrast to the commonly used filtering techniques, the proposed approach is based on a non-linear optimization for processing incoming measurements from the inertial measurement unit (IMU) and any other available sensors into a navigation solution. A factor graph formulation is introduced that allows multi-rate, asyn- chronous, and possibly delayed measurements to be incorporated in a natural way. This method, based on a recently developed incremental smoother, automatically determines the number of states to recompute at each step, effectively acting as an adaptive fixed-lag smoother. This yields an efficient and general framework for information fusion, providing nearly-optimal state estimates. In particular, incoming IMU measurements can be processed in real time regardless to the size of the graph. The proposed method is demonstrated in a simulated environment using IMU, GPS and stereo vision measurements and compared to the optimal solution obtained by a full non-linear batch optimization and to a conventional extended Kalman filter (EKF).

BibTeX Reference
author = {Vadim Indelman and Stephen Williams and Michael Kaess and Frank Dellaert},
title = {Factor Graph Based Incremental Smoothing in Inertial Navigation Systems},
booktitle = {Intl. Conf. on Information Fusion},
year = {2012},
month = {July},
pages = {2154-2161},
keywords = {Navigation, information fusion, factor graph, filtering},