EqVIO: An Equivariant Filter for Visual Inertial Odometry

TL;DR

EqVIO presents a novel VI-SLAM Lie group that couples SE$_2$(3) for IMU navigation with an $ ext{SOT}(3)^n$ landmark symmetry, yielding a fully symmetric VI-SLAM model. The Equivariant Filter (EqF) then achieves bias-free IMU propagation and a higher-order equivariant output approximation, producing a provably consistent estimator with reduced linearisation error. Empirical results on EuRoC and UZH FPV show EqVIO delivering superior trajectory accuracy and processing speed while supporting online extrinsics and IMU bias calibration. The approach demonstrates the practical impact of geometric symmetry in real-time VIO, with open-source release under a GPLv3 license for reproducibility and broader adoption.

Abstract

Visual-Inertial Odometry (VIO) is the problem of estimating a robot's trajectory by combining information from an inertial measurement unit (IMU) and a camera, and is of great interest to the robotics community. This paper develops a novel Lie group symmetry for the VIO problem and applies the recently proposed equivariant filter. The proposed symmetry is compatible with the invariance of the VIO reference frame, leading to improved filter consistency. The bias-free IMU dynamics are group-affine, ensuring that filter linearisation errors depend only on the bias estimation error and measurement noise. Furthermore, visual measurements are equivariant with respect to the symmetry, enabling the application of the higher-order equivariant output approximation to reduce approximation error in the filter update equation. As a result, the equivariant filter (EqF) based on this Lie group is a consistent estimator for VIO with lower linearisation error in the propagation of state dynamics and a higher order equivariant output approximation than standard formulations. Experimental results on the popular EuRoC and UZH FPV datasets demonstrate that the proposed system outperforms other state-of-the-art VIO algorithms in terms of both speed and accuracy.

Figures (19)

• Figure 1: A diagram showing the states of visual-inertial SLAM. Note that the IMU biases are excluded here.
• Figure 2: The true distribution of IMU positions at increments of 5 s obtained from integrating the dynamics \ref{['eq:bias_free_imu_dynamics']} compared with the estimated distributions from the EKF, MEKF, and EqF.
• Figure 3: The norm of linearisation error \ref{['eq:state_lin_error']} of the landmark dynamics \ref{['eq:visual_dynamics_landmark']} for the Euclidean \ref{['eq:visual_param_euclid']}, inverse-depth \ref{['eq:visual_param_invdepth']}, and polar \ref{['eq:visual_param_sot3']} parametrisations over the domain defined in \ref{['eq:linearisation_domain_U']}. The Euclidean parametrisation has zero dynamics linearisation error since the dynamics are exactly linear in these coordinates.
• Figure 4: The norm of linearisation error \ref{['eq:output_lin_error']} of the visual landmark measurement \ref{['eq:visual_measurement_landmark']} for the Euclidean \ref{['eq:visual_param_euclid']}, inverse-depth \ref{['eq:visual_param_invdepth']}, and polar \ref{['eq:visual_param_sot3']} parametrisations over the domain defined in \ref{['eq:linearisation_domain_U']}. The bottom-right subplot shows the linearisation error obtained when applying the equivariant output approximation 2021_vangoor_ConstructiveObserverDesign in the polar parametrisation.
• Figure 5: An illustration of the relationships between the true state $\xi$, estimated state $\hat{\xi}$, observer state $\hat{X}$, and global error $e$. The true and estimated state are related to the error and the origin by the transformation $\phi_{\hat{X}}$. The error $e$ is linearised through local coordinates to yield $\varepsilon = \vartheta(e)$.

Theorems & Definitions (13)

• Proposition 4.1
• Remark 5.1
• Lemma 5.2
• Lemma 5.3
• Lemma 5.4
• Lemma 5.5
• Lemma 6.1
• proof
• proof : Proof of Proposition \ref{['prop:invariance_action']}
• proof : Proof of Lemma \ref{['lem:simple_landmark_equivariance']}