Fast 3D pose estimation with out-of-sequence measurements

We present an algorithm for pose estimation using fixed-lag smoothing. We show that fixed-lag smoothing enables inclusion of measurements from multiple asynchronous measurement sources in an optimal manner. Since robots usually have a plurality of uncoordinated sensors, our algorithm has an advantage over filtering-based estimation algorithms, which cannot incorporate delayed measurements optimally. We provide an implementation of the general fixed-lag smoothing algorithm using square root smoothing, a technique that has become prominent. Square root smoothing uses fast sparse matrix factorization and enables our fixed-lag pose estimation algorithm to run at upwards of 20 Hz. Our algorithm has been extensively tested over hundreds of hours of operation on a robot operating in outdoor environments. We present results based on these tests that verify our claims using wheel encoders, visual odometry, and GPS as sensors.