Map Aided Localization and vehicle guidance using an active landmark search

In this paper, we address the problem of robust data association in active search for simultaneous vehicle localization and path tracking. We show that the classical landmarks active search approach, which consists in focusing processing resources on windows-of-interest where landmarks are supposed to be, is weak when faced with wrong data association. In some cases, the system will break down and will not be able to estimate the vehicle’s pose. This is a consequence of the focusing principle of this method. We propose a probabilistic framework to manage all matching hypotheses that incorporate a confidence level regarding the estimation of the vehicle’s pose that will evolve as a function of the probability that a right data association has been made. The system can also undo a previously made matching hypothesis when the confidence level on the vehicle’s pose estimation is too low. We illustrate the practicality of this approach by guiding an experimental vehicle in a real outdoor environment.