Enabling Robust State Estimation Through Measurement Error Covariance Adaptation

Abstract

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 non-cooperative environments. When dealing with novel and non-cooperative 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. Within 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 non-parametric 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 non-linear 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 upon the assignment provided by the nonparametric clustering algorithm). The proposed algorithm is verified on several GNSS collected data sets, where it is shown that the proposed technique exhibits some advantages when compared to other robust estimation techniques when confronted with degraded data quality.

Robust Error Estimation Based on Factor-Graph Models for Non-Line-of-Sight Localization

“…Most prior work on estimation methods, however, provided solutions assuming that noise covariances are known. Recently, methods based on the Expectation-Maximization (EM) framework have been proposed to estimate both the states and noise covariances in a factor graph framework [19,20,21,22]. While the objective of these papers was on making the factor graph or the estimation method robust to outliers, similar to our focus in the present paper, these approaches relied on sample covariances of the residuals (which maximizes the likelihood given the state estimates) as the direct estimators of the noise covariance matrices.…”

“…Equation ( 21) is what we needed in order to develop unbiased estimators of the scaling factors k 1 , ..., k s based on the method of moments approach. The scaling factors can be easily solved by writing out a matrix equation setting equal the observed values r T i r i to their expected values, i.e., the right hand side of (21), for all partitions i = 1, 2, ..., s as…”

“…In implementing the robust estimators in regression [27], [28] an Iteratively Reweighted Least Squares (IRLS) algorithm is often used to solve iteratively, for the best state estimate for a given set of noise variance values and the best variance values for a given state solution until convergence. These steps are analogous to the expectation and maximization steps of the methods in [19,20,21,22] where the residuals are used to compute a covariance (the M-step), and the estimated covariances are used to compute new residuals (the E-step). The proposed IRLS algorithm to implement the proposed unbiased variance estimators of the factor graph formulation and the M estimators for robust state estimation following this iterative framework is described in Algorithm 1.…”

Robust Incremental State Estimation Through Covariance Adaptation

“…The MM approach was extended within the batch covariance estimation (BCE) framework [5], [22] to enable the estimation of the multi-modal covariance models during optimization. The BCE approach enables the estimation of the multi-modal covariance model through the utilization of variational clustering [23] on the current set of state estimation residuals.…”

“…These GNSS data sets, as can be visualized through their ground traces, which are shown in Fig. 2, were made publicly available and are described within [5].…”