Metric velocity and landmark distance estimation utilizing monocular camera images and IMU data

In this paper we present a novel approach for the estimation of metric velocities and metric distances to landmarks utilizing monocular images and inertial measurements only. The proposed algorithm is based on an Extended Kalman Filter and is closely related to the well known Simultaneous Localization and Mapping (SLAM). In contrast to standard SLAM formulations the state of an agent is expressed in the body frame instead of the inertial frame. This formulation results in direct observability of the velocity and landmark distances for dynamic trajectories and the ability to maintain a consistent estimate for non-dynamic trajectories.