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 it provides an optimal solution. Contrarily to the most famous EKFSLAM solution, this method provides a global solution and no linearization procedures are required. Furthermore, we show that the estimation error converges exponentially fast toward a neighbourhood around zero, where the width of this region is proportional to 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. In particular we show the behavior of the filter against peaks of zero mean uniform output noise. This can be the case of feature matching errors, when a landmark observation is associate to a wrong landmark.