Enabling Robust State Estimation Through Measurement Error Covariance Adaptation

Accurate platform localization is an integral component of most robotic systems. As these robotic systems become more ubiquitous, it is necessary to develop robust state-estimation algorithms that are able to withstand novel and noncooperative environments. When dealing with novel and noncooperative environments, little is known a priori about the measurement error uncertainty; thus, there is a requirement that the uncertainty models of the localization algorithm be adaptive. In this paper, we propose the batch covariance estimation technique, which enables robust state estimation through the iterative adaptation of the measurement uncertainty model. The adaptation of the measurement uncertainty model is granted through nonparametric clustering of the residuals, which enables the characterization of the measurement uncertainty via a Gaussian mixture-model. The provided Gaussian mixture model can be utilized within any nonlinear least squares optimization algorithm by approximately characterizing each observation with the sufficient statistics of the assigned cluster (i.e., each observation's uncertainty model is updated based on the assignment provided by the nonparametric clustering algorithm). The proposed algorithm is verified on several Global Navigation Satellite System collected datasets, where it is shown that the proposed technique exhibits some advantages when compared to other robust estimation techniques when confronted with degraded data quality.