Motion Tracking with Fixed-lag Smoothing: Algorithm and Consistency Analysis

This paper presents a fixed-lag smoothing algorithm for tracking the motion of a mobile robot in real time. The algorithm processes measurements from proprioceptive (e.g., odometry, inertial measurement unit) and exteroceptive (e.g., camera, laser scanner) sensors, in order to estimate the trajectory of the vehicle. Smoothing is carried out in the information-filtering framework, and utilizes iterative minimization, which renders the method well-suited for applications where the effects of the measurements' nonlinearity are significant. The algorithm attains bounded computational complexity by marginalizing out older states. The key contribution of this work is a detailed analysis of the effects of the marginalization process on the consistency properties of the estimator. Based on this analysis, a linearization scheme that results in substantially improved accuracy, compared to the standard linearization approach, is proposed. Both simulation and real-world experimental results are presented, which demonstrate that the proposed method attains localization accuracy superior to that of competing approaches.