Analytically-selected multi-hypothesis incremental MAP estimation

In this paper, we introduce an efficient maximum a posteriori (MAP) estimation algorithm, which effectively tracks multiple most probable hypotheses. In particular, due to multimodal distributions arising in most nonlinear problems, we employ a bank of MAP to track these modes (hypotheses). The key idea is that we analytically determine all the posterior modes for the current state at each time step, which are used to generate highly probable hypotheses for the entire trajectory. Moreover, since it is expensive to solve the MAP problem sequentially over time by an iterative method such as Gauss-Newton, in order to speed up its solution, we reuse the previous computations and incrementally update the square-root informationmatrix at every time step, while batch relinearization is performed only periodically or as needed.