Sensor-based simultaneous localization and mapping — Part II: Online inertial map and trajectory estimation

A novel sensor-based filter for simultaneous localization and mapping (SLAM), featuring globally asymptotically stable error dynamics, is proposed in a companion paper, with application to uninhabited aerial vehicles (UAVs). This paper presents the second part of the algorithm, detailing a computationally efficient and numerically robust method for online inertial map and trajectory estimation based on the estimates provided by the SLAM filter previously derived. Central to the solution is the formulation of an optimization problem, that of finding the translation and the rotation that best explain the transformation between two sets of landmarks, with known associations, for consecutive time instants. The validation, performance, and consistency assessment of the proposed SLAM algorithm is successfully performed with real data, which was acquired by an instrumented quadrotor.