Tightly-coupled vision-aided inertial navigation via trifocal constraints

A tightly-coupled vision-aided inertial navigation system (TC-VA-INS) is proposed in this work, as a synergistic incorporation of vision with other sensors. In order to avoid the loss of information possibly resulting by the preprocessing of visual information, a best set of tracked feature points and readings of a low cost IMU are directly fused together within a vehicle state estimator. Instead of using 3D reconstruction, a vision based model is derived by using the trifocal tensor to propagate feature points across time steps, so as to express geometric constraints among three consecutive scenes. A kinematic model is used to account for the vehicle motion, and a Sigma Point Kalman Filter (SPKF) is used to achieve a robust state estimation in the presence of non-linearities. The proposed formulation is tested and demonstrated with a real dynamic indoor dataset. Results show improved estimates than in the case of a classical visual odometry approach, even in GPS-denied conditions and when magnetometer measurements are not reliable.