A Sliding Window Filter for Incremental SLAM

This work develops a sliding window filter for incremental simultaneous localization and mapping (SLAM) that focuses computational resources on accurately estimating the immediate spatial surroundings using a sliding time window of the most recent sensor measurements. Ideally, we would like a constant time algorithm that closely approximates the all-time maximum-likelihood estimate as well as the minimum variance Cramer Rao lower bound (CRLB) - that is we would like an estimator that achieves some notion of statistical optimality (quickly converges), efficiency (quickly reduces uncertainty) and consistency (avoids over-confidence). To this end we give a derivation of the SLAM problem from the Gaussian non-linear least squares optimization perspective.We find that this results in a simple, yet general, take on the SLAM problem; we think this is a useful contribution.

Our approach is inspired by the results from the photogrammetry community, dating back to the late 1950’s [1], and later derivatives like Mikhail’s least squares treatment [6], the Variable state dimension filter(VSDF) [5], visual odometry( VO) [4], modern bundle adjustment(BA) [10, 3] and of course extended Kalman filter (EKF) SLAM [9].

We apply the sliding window filter to SLAM with stereo vision and inertial measurements. Experiments show that the best approximate method comes close to matching the performance of the optimal estimator while attaining constant time complexity - empirically, it is often the case that the difference in their performance is indistinguishable.