  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).
