A Minimum Energy Solution to Monocular Simultaneous Localization and Mapping

In this paper we propose an alternative solution to the Monocular Simultaneous Localization and Mapping (SLAM) problem. This approach uses a Minimum-Energy Observer for Systems with Perspective Outputs and provides an optimal solution. Contrarily to the most famous EKF-SLAM algorithm, this method yields a global solution and no linearization procedures are required. Furthermore, we show that the estimation error converges exponentially fast toward a neighborhood of zero, where this region increases gracefully with the magnitude of the input disturbance, output noise and initial camera position uncertainty. For practical purposes, we present also the filter in both continuous and discrete time form. Moreover, to show how to integrate a new landmark in the state estimation, a simple initialization procedure is presented. The filter performances are illustrated via simulations.