Heterogeneous Feature State Estimation with Rao-Blackwellized Particle Filters

In this paper we present a novel technique to estimate the state of heterogeneous features from inaccurate sensors. The proposed approach exploits the reliability of the feature extraction process in the sensor model and uses a Rao-Blackwellized particle filter to address the data association problem. Experimental results show that the use of reliability improves performance by allowing the approach to perform better data association among detected features. Moreover, the method has been tested on a real robot during an exploration task in a non-planar environment. This last experiment shows an improvement in correctly detecting and classifying interesting features for navigation purpose.