A globally exponentially stable filter for bearing-only simultaneous localization and mapping with monocular vision

This paper proposes a novel filter for sensor-based bearing-only simultaneous localization and mapping in three dimensions with globally exponentially stable (GES) error dynamics. A nonlinear system is designed, its output transformed, and its dynamics augmented so that the proposed formulation can be considered as linear time-varying for the purpose of observability analysis. This allows the establishment of observability results related to the original nonlinear system that naturally lead to the design of a Kalman filter with GES error dynamics. The performance of the proposed algorithm is assessed resorting to real experiments based on the Rawseeds dataset as well as further realistic simulations.