Efficient Invariant Kalman Filter for Inertial-based Odometry with Large-sample Environmental Measurements
Xinghan Li, Haoying Li, Guangyang Zeng, Qingcheng Zeng, Xiaoqiang Ren, Chao Yang, Junfeng Wu
TL;DR
This work tackles the problem of globally optimal pose estimation for inertial-based odometry under non-linear measurements by developing an Efficient Invariant Kalman Filter (EIKF) that operates on the Lie group $SE_2(3)$. It combines a Gauss-Newton on Lie groups (LGN) update with a $\ oot{n}$-consistent initialization derived from large environmental measurements, yielding an update with $O(n)$ complexity and asymptotic MMSE optimality. The approach provides $\ oot{n}$-consistent pose estimators for camera (PnP) and LiDAR (ICP), enabling robust initialization and improved convergence in large-sample environmental measurement scenarios. Extensive simulations and real-data experiments across VIO, LIO, and LVIO demonstrate superior accuracy and runtime efficiency compared to state-of-the-art filters like IEKF and InEKF, with open-source code for replication.
Abstract
A filter for inertial-based odometry is a recursive method used to estimate the pose from measurements of ego-motion and relative pose. Currently, there is no known filter that guarantees the computation of a globally optimal solution for the non-linear measurement model. In this paper, we demonstrate that an innovative filter, with the state being $SE_2(3)$ and the $\sqrt{n}$-\textit{consistent} pose as the initialization, efficiently achieves \textit{asymptotic optimality} in terms of minimum mean square error. This approach is tailored for real-time SLAM and inertial-based odometry applications. Our first contribution is that we propose an iterative filtering method based on the Gauss-Newton method on Lie groups which is numerically to solve the estimation of states from a priori and non-linear measurements. The filtering stands out due to its iterative mechanism and adaptive initialization. Second, when dealing with environmental measurements of the surroundings, we utilize a $\sqrt{n}$-consistent pose as the initial value for the update step in a single iteration. The solution is closed in form and has computational complexity $O(n)$. Third, we theoretically show that the approach can achieve asymptotic optimality in the sense of minimum mean square error from the a priori and virtual relative pose measurements (see Problem~\ref{prob:new update problem}). Finally, to validate our method, we carry out extensive numerical and experimental evaluations. Our results consistently demonstrate that our approach outperforms other state-of-the-art filter-based methods, including the iterated extended Kalman filter and the invariant extended Kalman filter, in terms of accuracy and running time.