A Novel Imaging Measurement Model for Vision and Inertial Navigation Fusion with Extended Kalman Filtering

It is well-known that stand-alone inertial navigation systems (INS) have their errors diverging with time. The traditional approach for solving such incovenience is to resort to position and velocity aiding such as global navigation satellite systems (GNSS) signals. However, misalignment errors in such fusion architecture are not observable in the absence of maneuvers. This investigation develops a novel sighting device (SD) model for vision-aided inertial navigation for use in psi-angle error based extended Kalman filtering by means of observations of a priori mapped landmarks. Additionally, the psi-angle error model is revisited and an extended Kalman filter datasheet-based tuning is explained. Results are obtained by computer simulation, where an unmanned aerial vehicle flies a known trajectory with inertial sensor measurements corrupted by a random constant model. Position and velocity errors, misalignment, accelerometer bias, rate-gyro drift and GNSS clock errors with respect to ground-truth are estimated by means of INS/GNSS/SD fusion and tested for statistical consistency.