An analysis of observability-constrained Kalman Filtering for vision-aided navigation

Significant improvements in the accuracy of navigation state estimation have been previously demonstrated through the fusion of inertial measurement unit (IMU) and visual sensor data in both GPS-denied and GPS-enabled scenarios. Despite this improved navigation state accuracy, several significant hurdles remain before widespread acceptance of vision-aided navigation can be achieved. One significant bottleneck is the lack of accurate information about the performance of the vision-aided navigation algorithms. Many vision-aided navigation algorithms are implemented using some form of the Kalman Filter, thereby returning a covariance estimate that should correspond with the accuracy of the current navigation estimate. Unfortunately, a well known problem with the Kalman Filter is that its covariance estimates are inconsistent, i.e. the Kalman Filter estimates of uncertainty are significantly smaller than the true uncertainty achieved by the Kalman Filter. Recently a set of papers has introduced the concept of “observability-constrained” Kalman filtering that helps solve the consistency problem. In this paper, we apply the observability-constrained Kalman Filter to a vision-aided navigation problem and analyze its results. Significantly more accurate state and uncertainty estimates are achieved using the observability-constrained Kalman Filter. Unfortunately, the it is still not consistent, so a comparison with a batch, bundle adjustment approach is also performed to verify the possibility of consistent uncertainty estimation.