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

Abstract

<para xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"> In this paper, we show that <emphasis emphasistype="italic">all</emphasis> processes associated with the move-sense-update cycle of extended Kalman filter (EKF) Simultaneous Localization and Mapping (SLAM) can be carried out in time <emphasis emphasistype="italic">linear</emphasis> with the number of map features. We describe <emphasis emphasistype="italic">Divide and Conquer</emphasis> SLAM, which is an EKF SLAM algorithm in which the computational complexity per step is reduced from <formula formulatype="inline"><tex Notation="TeX">$O(n^2)$</tex></formula> to <formula formulatype="inline"><tex Notation="TeX"> $O(n)$</tex></formula>, and the total cost of SLAM is reduced from <formula formulatype="inline"><tex Notation="TeX">$O(n^3)$</tex> </formula> to <formula formulatype="inline"><tex Notation="TeX">$O(n^2)$</tex></formula>. 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. </para>