Tightly-Coupled Visual-Inertial Localization and 3-D Rigid-Body Target Tracking

Abstract

In this letter we present a novel method to perform target tracking of a moving rigid body utilizing an inertial measurement unit with cameras. A key contribution is the tightly-coupling of the target motion estimation within a visual-inertial navigation system (VINS), allowing for improved performance of both the processes. In particular, we build upon the standard multi-state constraint Kalman filter -based VINS and generalize it to incorporate three-dimensional (3-D) target tracking. Rather than representing the target object as a moving point particle (which is often the case in the literature), we instead utilize a dynamic, 3-D rigid-body model, wherein orientation, position, and their derivatives are estimated, as well as the structure of points on the object. We then leverage visual bearings to this set of features for target motion estimation, rather than requiring continuous observation of a single representative point over the tracking period. Moreover, we propose three motion models which capture most commonly-seen tracking scenarios in practice such as UAVs, fixed-wing aircraft, and ground vehicles over changing slopes and perform an observability analysis with geometric interpretation, providing insights into parameter initialization, and modes of estimation drift. The proposed estimator is validated with both Monte-Carlo simulations and real-world experiments where it is shown to offer accurate performance even for challenging trajectories that do not completely fit the selected model.