Tightly-Coupled Factor Graph Formulation For Radar-Inertial Odometry

In this paper, we present a Radar-Inertial Odometry (RIO) method based on the nonlinear optimization of factor graphs in a sliding window fashion. Our method makes use of a light-weight, low-power, inexpensive and commonly available hardware enabling easy deployment on small Unmanned Aerial Vehicles (UAV)s. We keep the state estimation problem bounded by employing partial marginalization of the oldest states, rendering the method real-time capable. We compare the implemented approach to the state-of-the-art multi-state Extended Kalman Filter (EKF)-based method in a one-to-one fashion. That is, we implemented in a single custom C++ RIO framework both estimation back-ends with all other parts shared and thus identical for a fair direct comparison. In the real-world flight experiments, we compare the two methods and show that both perform similarly in terms of accuracy when the linearization point is not far from the true state. Upon wrong initialization, the factor graph approach heavily outperforms the EKF approach. We also acknowledge that the influence of undetected outliers can overwhelm the inherent benefits of the nonlinear optimization approach leading to the insight that the estimator front-end has an important (and often underestimated) role in the overall performance. The open source code and datasets can be found here: https://github.com/aau-cns/aaucns_rio.