Treatment of Biased and Dependent Sensor Data in Graph-based SLAM

A common approach to attack the simultaneous localization and mapping problem (SLAM) is to consider factor-graph formulations of the underlying filtering and estimation setup. While Kalman filter-based methods provide an estimate for the current pose of a robot and all landmark positions, graph-based approaches take not only the current pose into account but also the entire trajectory of the robot and have to solve a nonlinear least-squares optimization problem. Using graph-based representations has proven to be highly scalable and very accurate as compared with traditional filter-based approaches. However, biased measurements as well as unmodeled correlations can lead to a sharp deterioration in the estimation quality and hence require careful consideration. In this paper, a method to incorporate biased or dependent measurement information is proposed that can easily be integrated into existing optimization algorithms for graph-based SLAM. For biased sensor data, techniques from ellipsoidal calculus are employed to compute the corresponding information matrices. Dependencies among noise terms are treated by a generalization of the covariance intersection concept. The treatment of both biased and correlated sensor data rest upon the inflation of the involved error matrices. Simulations are used to discuss and evaluate the proposed method.