ABSTRACT
This note describes a Sliding Window Filter that is as an on-line constant-time solution to the feature-based 6-degree-of-freedom full Batch Least Squares Simultaneous Localization and Mapping (SLAM) problem. We contend that for SLAM to be useful in large environments and over extensive run-times, its computational time complexity must be constant, and its memory requirements should be at most linear. Under this constraint, the "best" algorithm will be the one that comes closest to matching the all-time maximum-likelihood estimate of the full SLAM problem, while also maintaining consistency. We start by articulating the SLAM problem as a Batch Least Squares state estimation problem, and then show how to modify the Batch estimator into an approximate SlidingWindow Batch/Recursive framework that achieves constant time complexity and linear space complexity. We argue that viewing SLAM from the Sliding Window Least Squares perspective is very useful for understanding the structure of the problem. This perspective is also very general, capable of subsuming a number of common estimation techniques such as Bundle Adjustment and Extended Kalman Filter SLAM. By tuning a few parameters, the sliding window algorithm can scale from exhaustive Batch solutions to fast incremental solutions. The Sliding Window Filter enables other interesting properties, like continuous sub-mapping, lazy data association, undelayed or delayed landmark initialization, and incremental robust estimation. We test the algorithm in simulations using stereo vision exterioceptive sensors and inertial measurement proprioceptive sensors. Experiments show that the Sliding Window Filter comes close to matching the performance of the optimal batch estimator, even for small windows. |