iMHS: An Incremental Multi-Hypothesis Smoother
This work addresses computational bottlenecks in state estimation for robotics applications, but it is incremental as it builds on existing methods like iSAM.
The paper tackles state estimation for multi-modal hybrid systems, which is computationally challenging due to combinatorial growth, by presenting an incremental multi-hypothesis smoother that unifies multiple discrete mode histories with a Bayes tree, demonstrating its generality across 1D, 2D, and 3D problem domains.
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).