A globally exponentially stable filter for bearing-only simultaneous localization and mapping in 3-D

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 a set of realistic simulations.