GTSAM's Four New Estimators Cut Legged Robot Drift with Foot Contacts

Legged robots rely on IMUs for state estimation, but consumer-grade sensors suffer from drift. A new paper by Dellaert, Noh, Agrawal, and Kim tackles this by leveraging foot contacts—when a foot touches the ground, it provides a zero-velocity update that corrects the floating-base state (attitude, position, velocity, and IMU biases). The authors propose four increasingly expressive estimators built on the contact-aided invariant EKF framework by Hartley et al., but with a reduced contact update rate. They then augment this with a small factor graph replacement for the measurement update, and finally introduce fixed-lag smoothers that incorporate contact-episode footholds—with and without evolving IMU bias. This progression allows researchers to trade off computational cost against accuracy.

All four variants are implemented in the GTSAM factor graph library (Dellaert et al.) and come with a ROS2-compatible implementation for easy integration into real robot platforms. The code is open-source, ensuring reproducibility and enabling direct comparison with other proprioceptive odometry methods. For robotics engineers, these estimators offer a simple yet powerful way to improve legged robot localization without relying on GPS or external cameras—critical for indoor, underground, or degraded environments. The work bridges the gap between classic filter-based approaches and modern factor graph smoothing, providing a practical toolkit for both research and deployment.