# Belief Space Planning for Mobile Robots with Range Sensors using iLQG

arXiv: https://arxiv.org/abs/2102.05466 · HTML: https://mlsanity.com/paper/2102.05466/

## Key facts

- Cited by 3 papers in the map (data through 14 Oct 2026).
- First posted on arXiv 10 Feb 2021.

## Built on by (most cited first, 3 of 3)

- [Belief-Conditioned One-Step Diffusion: Real-Time Trajectory Planning with Just-Enough Sensing](https://mlsanity.com/paper/2508.12166/) (2025)
- [IBBT: Informed Batch Belief Trees for Motion Planning Under Uncertainty](https://mlsanity.com/paper/2304.10984/) (2023)
- [MEXGEN: An Effective and Efficient Information Gain Approximation for Information Gathering Path Planning](https://mlsanity.com/paper/2405.02605/) (2024)

## Abstract (arXiv)

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.