# Factor Graph based Incremental Smoothing in Navigation Systems

In this DARPA-funded project, we collaborate with SRI International ltd. to develop a plug and play framework for navigation. The goal is to produce the best possible solution in real time based on different multi-rate and asynchronous sensors that may become inactive and/or resurrected at any time. A factor graph formulation is used as a representation of the joint probability function, and an efficient inference algorithm is used to calculate the MAP estimate given measurements from different sensors.

Following a recently-developed IMU pre-integration theory, an equivalent IMU factor is introduced to summarize consecutive IMU measurements into a non-linear factor, which can be re-linearized if required. This factor is then incorporated into the optimization whenever measurements from other sensors are received, while high-rate navigation solution is contentiously obtained by composing the last navigation state in the factor graph with the current summarized IMU measurements. This is in contrast to the commonly used navigation-aiding approach where IMU measurements are processed outside of the estimator, without being able to perform re-linearization of past IMU measurements. The (left) figure below illustrates a factor graph that accommodates factors from different sensors, and in particular equivalent IMU factors. See RAS2013Fusion2012 for further details.

We also present a parallelized navigation architecture (right image above) that is capable of running in real-time and incorporating long-term loop closure constraints while producing the optimal Bayesian solution. This architecture splits the inference problem into a low-latency update that incorporates new measurements using just the most recent states (filter), and a high-latency update that is capable of closing long loops and smooths using all past states (smoother). See IJRR2014, Fusion2012.

Code is publicly available (Software page).

## Related Publications:

### Articles

• S. Williams, V. Indelman, M. Kaess, R. Roberts, J. Leonard, and F. Dellaert. Concurrent Filtering and Smoothing: A Parallel Architecture for Real-Time Navigation and Full Smoothing. International Journal of Robotics Research, 33:1544-1568, October 2014.
[BibTeX] [URL] [PDF]
@Article{Williams14ijrr,
author = {S. Williams and V. Indelman and M. Kaess and R. Roberts and J. Leonard and F. Dellaert},
title = {Concurrent Filtering and Smoothing: A Parallel Architecture for Real-Time Navigation and Full Smoothing},
journal = "International Journal of Robotics Research",
year = 2014,
volume = 33,
pages = "1544-1568",
month = "October",
url = "http://ijr.sagepub.com/content/33/12/1544",
pdf = "http://vindelman.technion.ac.il/Publications/Williams14ijrr.pdf",
researchtopic = {IncrementalSmoothingNav},
}

• V. Indelman, S. Wiliams, M. Kaess, and F. Dellaert. Information Fusion in Navigation Systems via Factor Graph Based Incremental Smoothing. Robotics and Autonomous Systems, 61(8):721-738, August 2013. doi:10.1016/j.robot.2013.05.001
[BibTeX] [URL] [PDF]
@Article{Indelman13ras,
author = {V. Indelman and S. Wiliams and M. Kaess and F. Dellaert},
title = {Information Fusion in Navigation Systems via Factor Graph Based Incremental Smoothing},
year = 2013,
volume = 61,
number = 8,
month = "August",
journal = "Robotics and Autonomous Systems",
issn = "0921-8890",
doi = "10.1016/j.robot.2013.05.001",
url = "http://www.sciencedirect.com/science/article/pii/S092188901300081X",
pages = {721-738},
pdf = "http://vindelman.technion.ac.il/Publications/Indelman13ras.pdf",
researchtopic = {IncrementalSmoothingNav},
}

### In Proceedings

• V. Indelman, A. Melim, and F. Dellaert. Incremental Light Bundle Adjustment for Robotics Navigation. In IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), November 2013.
[BibTeX] [PDF] [Slides]
@InProceedings{Indelman13iros,
author = {V. Indelman and A. Melim and F. Dellaert},
booktitle = "IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)",
year = 2013,
month = "November",
location = {Tokyo, Japan},
pdf = "http://vindelman.technion.ac.il/Publications/Indelman13iros.pdf",
slides = "http://vindelman.technion.ac.il/Publications/Indelman13iros_ppt.pdf",
researchtopic = {LBA, IncrementalSmoothingNav},
}

• M. Kaess, S. Williams, V. Indelman, R. Roberts, J. J. Leonard, and F. Dellaert. Concurrent Filtering and Smoothing. In International Conference on Information Fusion, July 2012.
[BibTeX] [PDF] [Slides]
@InProceedings{Kaess12fusion,
author = {M. Kaess and S. Williams and V. Indelman and R. Roberts and J. J. Leonard and F. Dellaert},
title = {Concurrent Filtering and Smoothing},
booktitle = "International Conference on Information Fusion",
year = 2012,
month = "July",
location = {Singapore},
pdf = "http://vindelman.technion.ac.il/Publications/Kaess12fusion.pdf",
slides = "http://vindelman.technion.ac.il/Publications/Kaess12fusion_ppt.pdf",
researchtopic = {IncrementalSmoothingNav},
}

• V. Indelman, S. Wiliams, M. Kaess, and F. Dellaert. Factor Graph Based Incremental Smoothing in Inertial Navigation Systems. In International Conference on Information Fusion, July 2012.
[BibTeX] [PDF] [Slides]
@InProceedings{Indelman12fusion,
author = {V. Indelman and S. Wiliams and M. Kaess and F. Dellaert},
title = {Factor Graph Based Incremental Smoothing in Inertial Navigation Systems},
booktitle = "International Conference on Information Fusion",
month = "July",
year = 2012,
location = {Singapore},
pdf = "http://vindelman.technion.ac.il/Publications/Indelman12fusion.pdf",
slides = "http://vindelman.technion.ac.il/Publications/Indelman12fusion_ppt.pdf",
researchtopic = {IncrementalSmoothingNav},
}