Divide and Conquer: EKF SLAM in $O(n)$

In this paper, we show that all processes associated with the move-sense-update cycle of extended Kalman filter (EKF) Simultaneous Localization and Mapping (SLAM) can be carried out in time linear with the number of map features. We describe Divide and Conquer SLAM, which is an EKF SLAM algorithm in which the computational complexity per step is reduced from $O(n^2)$ to $O(n)$, and the total cost of SLAM is reduced from $O(n^3)$ to $O(n^2)$. Unlike many current large-scale EKF SLAM techniques, this algorithm computes a solution without relying on approximations or simplifications (other than linearizations) to reduce computational complexity. Also, estimates and covariances are available when needed by data association without any further computation. Furthermore, as the method works most of the time in local maps, where angular errors remain small, the effect of linearization errors is limited. The resulting vehicle and map estimates are more precise than those obtained with standard EKF SLAM. The errors with respect to the true value are smaller, and the computed state covariance is consistent with the real error in the estimation. Both simulated experiments and the Victoria Park dataset are used to provide evidence of the advantages of this algorithm.