Batch Measurement Error Covariance Estimation for Robust Localization

The factor graph has become the standard framework for representing a plethora of robotic navigation problems. One primary reason for this adoption by the community is the fast and efficient inference that can be conducted over the graph when a unimodal Gaussian noise model is assumed. However, the unimodal Gaussian noise model assumption does not reflect reality in many situations, particularly measurements that may include gross outliers (e.g. feature tracking between images, place recognition, or GNSS multipath). To combat this issue, several methodologies have been proposed for conducting robust inference on factor graphs. These models work by reducing the contribution of constraints that do not adhere to the specified noise model by scaling the corresponding elements of the information matrix. A unifying assumption shared by the proposed robust graph inference algorithms is that the measurement noise model is known a priori and that the specified noise model does not vary with time. In the situation where the measurement model is not fully known, rejecting the outliers can become far more difficult. To overcome this issue, a novel method is proposed that utilizes a non-parametric soft clustering algorithm to iteratively estimate the measurement error covariance matrix. The estimated covariance mixture model is then used within the max-mixtures framework to mitigate the effect of false constraints. The proposed methodology provides robust optimization in the face of faulty measurements where little or no information is provided about the measurement uncertainty.