On-line MPC-based Stochastic Planning in the Non-Gaussian Belief Space with Non-convex Constraints ∗

A convex program to design a feedback policy in nonGaussian belief space is proposed for a class of systems with differentiable non-linear observation model. A Particle Filter is adopted for belief representation. The size of the optimization vector grows linearly with time horizon and dimension of the state. A new approach is proposed to deal with obstacles as a part of optimization problem. Included results show the efficacy of our method.