Multi-modal and inertial sensor solutions for navigation-type factor graphs

Abstract

This thesis presents a sum-product inference algorithm for in-situ, nonparametric platform navigation called Multi-modal iSAM (incremental smoothing and mapping), for problems of thousands of variables. Our method tracks dominant modes in the marginal posteriors of all variables with minimal approximation error, while suppressing almost all low likelihood modes (in a non-permanent manner) to save computation. The joint probability is described by a non-Gaussian factor graph model. Existing inference algorithms in simultaneous localization and mapping assume Gaussian measurement uncertainty, resulting in complex front-end processes that attempt to deal with non-Gaussian measurements. Existing robustness approaches work to remove "outlier" measurements, resulting heuristics and the loss of valuable information. Track different hypotheses in the system has prohibitive computational cost and and low likelihood hypotheses are permanently pruned. Our approach relaxes the Gaussian only restriction allowing the frontend to defer ambiguities (such as data association) until inference. Probabilistic consensus ensures dominant modes across all measurement information. Our approach propagates continuous beliefs on the Bayes (Junction) tree, which is an efficient symbolic refactorization of the nonparametric factor graph, and approximates the underlying Chapman-Kolmogorov equations. Like the predecessor iSAM2 max-product algorithm [Kaess et al., IJRR 2012], we retain the Bayes tree incremental update property, which allows for tractable recycling of previous computations. Several non-Gaussian measurement likelihood models are introduced, such as ambiguous data association or highly non-Gaussian measurement modalities. In addition, keeping with existing inertial navigation for dynamic platforms, we present a novel continuous-time inertial odometry residual function. Inertial odometry uses preintegration to seamlessly incorporate pure inertial sensor measurements into a factor graph, while supporting retroactive (dynamic) calibration of sensor biases. By centralizing our approach around a factor graph, with the aid of modern starved graph database techniques, concerns from different elements of the navigation ecosystem can be separated. We illustrate with practical examples how various sensing modalities can be combined into a common factor graph framework, such as: ambiguous loop closures; raw beam-formed acoustic measurements; inertial odometry; or conventional Gaussian-only likelihoods (parametric) to infer multi-modal marginal posterior belief estimates of system variables.