This thesis presents a larger scale sum-product style inference solution for in-situ, nonparametric platform navigation called Multi-modal iSAM (incremental smoothing and mapping). Our method is built around estimating the marginal posteriors of any or all variables in a high dimensional system, as described by a joint probability factor graph model. We find that most existing inference solutions in simultaneous localization and mapping (SLAM) are limited by a common Gaussian error model assumption, resulting in complex front-end processes that attempt to deal with non-Gaussian measurements. Our approach relaxes the Gaussian only restriction on the front-end to allow ambiguities (such as data association) to be deferred into the back-end inference and let probabilistic consensus find and track the dominant modes. Our approach is related to multi-hypothesis inference, but differs in that low likelihood modes (hypotheses) are ignored through belief propagation on the Bayes (Junction) tree, which is an efficient, symbolic refactorization of the nonparametric factor graph. Like the predecessor iSAM2 max-product style algorithm, we retain the Bayes tree incremental update property, which allows for tractable recycling of previous computations. We also present several non-Gaussian measurement likelihood models for 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, second-order inertial odometry residual function. Inertial odometry uses preintegration to seemlessly incorporate pure inertial sensor measurements into a factor graph, while supporting retroactive (dynamic) calibration of sensor biases. By centralizing our approach around the factor graph---with the aid of modern starved graph database technologies---we can separate concerns from different elements of the navigation ecosystem. 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 parametric likelihoods to infer (multi-modal) marginal posterior belief estimates of system variables.
Prof. John Leonard (MIT), in collaboration with Prof. M. Kaess at CMU