Closed-form solution for attitude and speed determination by fusing monocular vision and inertial sensor measurements

Abstract

This paper considers the problem of data fusion when the adopted sensors are a monocular camera and inertial sensors (i.e. one tri-axial accelerometer and one tri-axial gyrometer). The investigation starts by performing an observability analysis to analytically derive all the observable modes, i.e. all the physical quantities that the information contained in the sensor data allows us to estimate. They are the position of the features in the camera frame, the vehicle speed in the same local frame and the absolute roll and pitch angles. The main contribution of the paper is a new algorithm to simultaneously estimate all the previous physical quantities. In particular, the algorithm is based on a closed-form solution which analytically expresses the vehicle speed and attitude in terms of the sensor measurements. In this algorithm the camera only needs to observe four times a single point feature in the environment. This allows performing the overall estimation in a very short time interval and without the need of any initialization or a priori knowledge. This is a key advantage since allows eliminating the drift on the scale factor and on the vehicle orientation. In addition, the algorithm can be easily extended in order to deal with biased inertial measurements and to deal with multiple features, in which case only three distinct camera poses are required (instead of four). Specifically, with three camera poses and two features, the vehicle speed and attitude together with the scale factor can be determined. The performance of the proposed approach is evaluated via Monte Carlo simulations and by using real data.