A method to enforce map constraints in a particle filter's position estimate

Abstract

When particle filters are used to estimate indoor position with floor plan constraints, it can happen that, even when all the particles lie in the corridor, the particles' mean is not in the corridor. Such a position estimate is perceived by the user as a mistake in the algorithm. Projecting the particles' mean to the nearest corridor location is an obvious ad-hoc solution, but it is not optimal and the trajectory may be discontinuous in time. Another solution is to use a maximum a-posteriori estimate for the particle cloud where the particles in an inaccessible region are eliminated. However, this optimal solution might also have discontinuous trajectory and so it is not ideal for the real time positioning. In this work, the following principled approach is taken. Given a particle cloud representation of a posterior distribution for position, the position estimate is defined as the solution of a least squares problem with linear inequality constraints. This problem can be solved efficiently and reliably using standard numerical optimization algorithms and codes. Results are presented for simulated data and real-world data.