IMU-camera data fusion: Horizontal plane observation with explicit outlier rejection

In this paper, we address the problem of egomotion estimation using an inertial measurement unit and visual observations of planar features on the ground. The main practical difficulty of such a system is correctly determining the ground planar features from the visual observations. Herein, we propose a novel vision-aided inertial navigation system through simultaneous motion estimation and ground plane feature detection. We present a state-space formulation for the pose estimation problem and solve it via an augmented unscented Kalman filter. First, the predictions obtained by the Kalman filter are used to detect the ground plane features. Second, the detected features are fed back to the motion estimation algorithm to be used in the measurement update phase of the filter. The developed detection algorithm consists of two steps, namely homography-based and normal-based outlier rejection. The presented integration algorithm allows 6-DoF motion estimation in a practical scenario where the camera is not restricted to observe only the ground plane. Real-world experiments in an indoor scenario indicate the accuracy and reliability of our proposed method in the presence of outliers and non-ground obstacles.