EKF SLAM updates in O(n) with Divide and Conquer SLAM

Abstract

In this paper we describe divide and conquer SLAM (D&C SLAM), an algorithm for performing simultaneous localization and mapping using the extended Kalman filter. D&C SLAM overcomes the two fundamental limitations of standard EKF SLAM: 1.) the computational cost per step is reduced from O(n <sup xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">2</sup> ) to O(n) (the cost full SLAM is reduced from O(n <sup xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">3</sup> ) to O(n <sup xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">2</sup> )); 2.) the resulting vehicle and map estimates have better consistency properties than standard EKF SLAM in the sense that the computed state covariance adequately represents the real error in the estimation. Unlike many current large scale EKF SLAM techniques, this algorithm computes an exact solution, without relying on approximations or simplifications to reduce computational complexity. Also, estimates and covariances are available when needed by data association without any further computation. Empirical results show that, as a bi-product of reduced computations, and without losing precision because of approximations, D&C SLAM has better consistency properties than standard EKF SLAM. Both characteristics allow to extend the range of environments that can be mapped in real time using EKF. We describe the algorithm and study its computational cost and consistency properties.