Kalman Filter with Moving Reference for Jump-Free, Multi-Sensor Odometry with Application in Autonomous Driving

Control, tracking, and obstacle detection algorithms for mobile robots, including autonomous cars, rely on a jump-free estimate of the vehicle's pose. While one cannot completely avoid jumps in global solutions like INS/GNSS and SLAM, relative localization (i.e., odometry) does not suffer from this problem. Methods based on graph optimization are popular in that field, but they do not scale very well with high-frequency measurements. Kalman filters (KFs) are able to cope with those measurements, but they face the issue of a continuously growing covariance. This results in instabilities and eventually jumps in the state estimate. We present an approach to handle this problem by periodically moving the reference state forward in time, which is realized using two filters. The equations for implementing this in both the extended Kalman filter (EKF) and the unscented Kalman filter (UKF) are derived. The algorithm is evaluated using real-world datasets covering different scenarios of autonomous driving. We show that our method provides a smooth and stable estimate even over long time periods and that it achieves a better localization performance than the standard approach.