Amortized constant time state estimation in SLAM using a mixed Kalman-information filter

Abstract

The computational bottleneck in all information-based algorithms for SLAM is the recovery of the state mean and covariance. The mean is needed to evaluate model Jacobians and the covariance is needed to generate data association hypotheses. Recovering the state mean and covariance requires the inversion of a matrix of the size of the state. Current state recovery methods use sparse linear algebra tools that have quadratic cost, either in memory or in time. In this paper, we present an approach to state estimation that is worst case linear both in execution time and in memory footprint at loop closure, and constant otherwise. The approach relies on a state representation that combines the Kalman and the information-based state representations. The strategy is valid for any SLAM system that maintains constraints between robot poses at different time slices. This includes both Pose SLAM, the variant of SLAM where only the robot trajectory is estimated, and hierarchical techniques in which submaps are registered with a network of relative geometric constraints.