Vision-Aided Inertial Navigation Using Planar Terrain Features

The idea is to implement a vision-aided inertial navigation system (INS) for estimating inertial measurement unit (IMU)-camera ego-motion. The system consists of a ground facing monocular camera mounted on an IMU that is observing ground plane feature points. The motion estimation procedure is through tracking detected corresponding feature points between two successive image frames. The main contribution of this paper is a novel closed-form measurement model based on the image data and IMU output signals. In contrast to existing methods, our algorithm is independent of the underlying vision algorithm such as image motion estimation or optical flow algorithms for camera motion estimation. Additionally, unlike the visual-SLAM based methods, our approach is not based on data association. The algorithm has been implemented using an Extended Kalman filter (EKF), which propagates the current and the last state of the system updated in the previous measurement state. Simulation results show that the introduced method is persistent to the level of the noise and works well even with few numbers of features.