Optimal Filtering for Non-Parametric Observation Models: Applications to Localization and SLAM

“An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization”ICRA 2008, Pasadena (California, USA), May 19-23, 2008. (PDF) – (Slides)

Abstract: The lack of a parameterized observation model in robot localization using occupancy grids requires the application of sampling-based methods, or particle filters. This work addresses the problem of optimal Bayesian filtering for dynamic systems with observation models that cannot be approximated properly as any parameterized distribution, which includes localization and SLAM with occupancy grids. By integrating ideas from previous works on adaptive sample size, auxiliary particle filters, and rejection sampling, we derive a new particle filter algorithm that enables the usage of the optimal proposal distribution to estimate the true posterior density of a non-parametric dynamic system. Our solution avoids approximations adopted in previous approaches at the cost of a higher computational burden. We present simulations and experimental results for a real robot showing the suitability of the method for localization.


2. Source code

The “optimal sampling” method described in this paper is implemented in the class mrpt::slam::CMonteCarloLocalization2D, which is the main class used in the ready-to-use application pf-localization.

In particular, the optimal sampling algorithm for (global) localization is implemented in the method prediction_and_update_pfAuxiliaryPFOptimal.

3. Reproducing the experiments

First, grab the configuration and data files of the experiments from icra2008optsamp.zip and decompress it.

Then, execute:

In both cases the results will be shown in real-time in a 3D window.

The video below is the result of running the “optimal” method proposed in this paper. It is remarkable that, given enough initial particles, it virtually always converges to the correct robot position in just 1 iteration.

4. Video


Results from the experiment: The proposed method converges to the correct robot location with fewer samples: