  Planning under uncertainty is a key requirement for physical systems due to
the noisy nature of actuators and sensors. Using a belief space approach,
planning solutions tend to generate actions that result in information seeking
behavior which reduce state uncertainty. While recent work has dealt with
planning for Gaussian beliefs, for many cases, a multi-modal belief is a more
accurate representation of the underlying belief. This is particularly true in
environments with information symmetry that cause uncertain data associations
which naturally lead to a multi-modal hypothesis on the state. Thus, a planner
cannot simply base actions on the most-likely state. We propose an algorithm
that uses a Receding Horizon Planning approach to plan actions that
sequentially disambiguate the multi-modal belief to a uni-modal Gaussian and
achieve tight localization on the true state, called a Multi-Modal Motion
Planner (M3P). By combining a Gaussian sampling-based belief space planner with
M3P, and introducing a switching behavior in the planner and belief
representation, we present a holistic end-to-end solution for the belief space
planning problem. Simulation results for a 2D ground robot navigation problem
are presented that demonstrate our method's performance.
