  In this work, we use iterative Linear Quadratic Gaussian (iLQG) to plan
motions for a mobile robot with range sensors in belief space. We address two
limitations that prevent applications of iLQG to the considered robotic system.
First, iLQG assumes a differentiable measurement model, which is not true for
range sensors. We show that iLQG only requires the differentiability of the
belief dynamics. We propose to use a derivative-free filter to approximate the
belief dynamics, which does not require explicit differentiability of the
measurement model. Second, informative measurements from a range sensor are
sparse. Uninformative measurements produce trivial gradient information, which
prevent iLQG optimization from converging to a local minimum. We densify the
informative measurements by introducing additional parameters in the
measurement model. The parameters are iteratively updated in the optimization
to ensure convergence to the true measurement model of a range sensor. We show
the effectiveness of the proposed modifications through an ablation study. We
also apply the proposed method in simulations of large scale real world
environments, which show superior performance comparing to the state-of-the-art
methods that either assume the separation principle or maximum likelihood
measurements.
