Estimator initialization in vision-aided inertial navigation with unknown camera-IMU calibration

This paper focuses on motion estimation using inertial measurements and observations of naturally occurring point features. To date, this task has primarily been addressed using filtering methods, which track the system state starting from known initial conditions. However, when no prior knowledge of the initial system state is available, (e.g., at the onset of the system's operation), the existing approaches are not applicable. To address this problem, in this work we present algorithms for computing the system's observable quantities (platform attitude and velocity, feature positions, and IMU-camera calibration) directly from the sensor measurements, without any prior knowledge. A key contribution of this work is a convex-optimization based algorithm for computing the rotation matrix between the camera and IMU. We show that once this rotation matrix has been computed, all remaining quantities can be determined by solving a quadratically constrained least-squares problem. To increase their accuracy, the initial estimates are refined by an iterative maximum-likelihood estimator.