Robust Sensor Fusion with Self-Tuning Mixture Models

A fundamental problem of non-linear state estimation in robotics is the violation of assumptions about the sensors' error distribution. State of the art approaches reduce the impact of these violations with robust cost functions or predefined non-Gaussian error models. Both require extensive parameter tuning and fail if the sensors' error characteristic changes over time, due to environmental changes, ageing or sensor malfunctions. We demonstrate how the error distribution itself can be part of the state estimation process. Based on an efficient approximation of a Gaussian mixture, we optimize the sensor model simultaneously during the standard state estimation. Due to an implicit expectation-maximization approach, we achieve a fast convergence without prior knowledge of the true distribution parameters. We implement this self-tuning algorithm in a least-squares optimization framework and demonstrate its real time capability on a real world dataset for satellite localization of a driving vehicle. The resulting estimation quality is superior to previous robust algorithms.