A LiDAR-inertial Odometry with Principled Uncertainty Modeling

Abstract

This paper proposes a LiDAR-inertial odometry that properly solves the uncertainty estimation problem, guided by the rules of designing a consistent estimator. Our system is built upon an iterated extended Kalman filter, with multiple states in an optimization window. To survive environments without distinctive geometric structures, we do not track features over time. We only extract planar primitives from the local map and use a direct point-to-plane distance metric as the measurement model. The realistic noise parameters are estimated online by modeling point distributions. We use nullspace projection to remove dependency on the feature planes, which is equivalent to transforming the pose-map measurement into relative pose constraints. To avoid reintegrating all the laser points in the local window after every state correction, we use the Schmidt Kalman update to consider the probabilistic effects of past poses while their values are left unaltered. A collection of octrees with an adaptive resolution is designed to manage measurement points and the map efficiently. The consistency and robustness of our system are verified in both simulation and real-world experiments.