iMHS: An Incremental Multi-Hypothesis Smoother

TL;DR

The paper introduces iMHS, an incremental multi-hypothesis smoother for state estimation in hybrid systems, efficiently managing discrete mode hypotheses through a unified Bayes tree structure, applicable across various robotics problems.

Contribution

It presents a novel incremental inference method that unifies multiple discrete hypotheses with continuous state estimation using a hybrid factor graph and Bayes tree, improving efficiency.

Findings

Demonstrated effectiveness in lane change detection, aircraft maneuver detection, and contact detection in legged robots.

Achieved computational efficiency by exploiting temporal structure and hypothesis pruning.

Validated the approach across diverse problem domains with promising results.

Abstract

State estimation of multi-modal hybrid systems is an important problem with many applications in the field robotics. However, incorporating discrete modes in the estimation process is hampered by a potentially combinatorial growth in computation. In this paper we present a novel incremental multi-hypothesis smoother based on eliminating a hybrid factor graph into a multi-hypothesis Bayes tree, which represents possible discrete state sequence hypotheses. Following iSAM, we enable incremental inference by conditioning the past on the future but we add to that the capability of maintaining multiple discrete mode histories, exploiting the temporal structure of the problem to obtain a simplified representation that unifies the multiple hypothesis tree with the Bayes tree. In the results section we demonstrate the generality of the algorithm with examples in three problem domains: lane change detection (1D), aircraft maneuver detection (2D), and contact detection in legged robots (3D).