Guaranteed simultaneous localization and mapping algorithm using interval analysis

To increase the autonomy of robots, it is necessary to have a precise and guaranteed localization. We propose a SLAM algorithm based on interval analysis and constraints propagation. The problem is casted into a constraint satisfaction which is solved in a guaranteed way via Interval Analysis. We define a new bounded landmark parameterization and an initialization method for monocular camera. Finally, we introduce the post-localization process which improves the localization accuracy using the future observations. Both simulations and experiments show guaranteed and consistent results of CP-SLAM.