Anticipating Degradation in State Estimation Accuracy via Online Nonlinear Observability Analysis

A methodology is proposed to enable real-time evaluation of the observability of local motions, and generate a local observability cost map to enable informed local motion planning in order to avoid potential degradation or degeneracy in state estimator performance. The proposed approach leverages efficient numerical techniques in nonlinear observability analysis and motion primitive-based planning technique to realize the local observability prediction with real-time performance. The degradation of the state estimation performance can be readily predicted with the local observability evaluation result. The proposed approach is specialized to a representative optimization-based monocular visual-inertial state estimation formulation and evaluated through simulation and experiments. The experimental results demonstrated the ability of the proposed methodology to correctly anticipate the potential state estimation degradation.