Undelayed Initialization of Inverse Depth Parameterized Landmarks in UKF-SLAM with Error State Formulation

Abstract

In this paper we present an approach to combine error state estimation with total state monocular simultaneous localization and mapping (SLAM) in a single Unscented Kalman Filter (UKF). The map features use the inverse depth parametrization for undelayed initialization and for the ability to use low-parallax features with unknown depth information. Furthermore, a new map feature initialization method is presented using the Unscented transform (UT). This method allows to capture all correlations between the map Features and the error state variables without the necessity to calculate any Jacobian matrices.