Abstract
Estimator inconsistency remains a critical challenge in robotic navigation, particularly in Visual-Inertial SLAM (VINS) where absolute position and heading are unobservable. Traditional Fixed-Lag Smoothers (FLS) often become overconfident due to spurious information gain during marginalization. This paper introduces the Right-Invariant Fixed-Lag Smoother (RI-FLS), a novel framework leveraging Lie group theory to ensure natural consistency. By formulating errors as right-invariant, the estimator maintains the correct observability properties of the SLAM problem without the need for complex heuristics like First Estimate Jacobians (FEJ).
Introduction
In Visual-Inertial SLAM, robots fuse camera images and inertial measurement unit (IMU) data to estimate their trajectory. A fundamental property of this system is that the absolute position and the yaw (heading) relative to gravity are mathematically unobservable. A consistent estimator should reflect this by maintaining high uncertainty in these directions. However, standard linearization-based smoothers often suffer from "spurious information gain." During the marginalization of old states to maintain a fixed-lag window, approximations lead the system to believe it has more information about its absolute location than it truly does. This overconfidence can lead to divergence and navigation failure.
Methodology
The proposed RI-FLS addresses inconsistency by redefining the state error using a Lie group formulation.
Lie Group Representation and Invariant Errors
Rather than treating the robot's state as a simple vector, the authors represent orientation, velocity, and position as elements of the $SE_2(3)$ Lie group. By defining a Right-Invariant Error, the error state becomes independent of the linearization point (the current estimate of the trajectory). This geometric property ensures that the unobservable directions of the system are naturally preserved during the marginalization process.
Simplified Observability and RI-FLS Algorithm
Traditional consistency analysis requires examining complex observability matrices. The authors demonstrate that for FLS, consistency can be verified more simply by checking the nullspace of the linearized system's Jacobian matrix. The resulting RI-FLS algorithm integrates these invariant errors into a sliding-window optimizer, providing a mathematically grounded alternative to the "hacks" typically required by First Estimate Jacobian (FEJ) methods.
Experimental Evaluation
The RI-FLS was validated through both synthetic simulations and real-world benchmarks.
- Synthetic Simulation: A platform followed a 3D torus trajectory for 300 seconds, surrounded by point landmarks. This setup allowed for the calculation of the Normalized Estimation Error Squared (NEES) to precisely measure consistency.
- EuRoC MAV Benchmark: The algorithm was tested on real-world micro aerial vehicle data, featuring synchronized stereo images and IMU data in challenging indoor environments.
Key Results and Discussion
Consistency and Accuracy
In simulations, the standard FLS exhibited significant inconsistency, with its pose NEES skyrocketing as it gained spurious information. In contrast, the RI-FLS maintained a NEES near the theoretical ideal, performing similarly to a full batch optimizer (like iSAM2) that retains all historical data.
On the EuRoC benchmark, RI-FLS demonstrated superior accuracy. For the "MH_01" sequence, it achieved a Mean Absolute Trajectory Error (ATE) of 0.53m, compared to 0.88m for the standard incremental FLS. The method also showed increased robustness when utilizing "smart factors" for landmark processing.
Insights
The primary advantage of the RI-FLS is its natural consistency. Because the geometry of the Lie group handles the preservation of unobservable directions, the marginalization step no longer introduces the mathematical drift that plagues standard filters. Furthermore, the research shows that landmark parameters and sensor biases do not interfere with the system's nullspace, simplifying the underlying mathematics.
Conclusion
The Right-Invariant Fixed-Lag Smoother provides a robust, geometrically consistent framework for VINS. While the current implementation uses identity approximations for certain IMU residual Jacobians to ensure perfect consistency—a common practice—future work will explore the implications of using exact Jacobians and the properties of left-invariant formulations. Additionally, the consistency of observable parameters like sensor biases remains an area for further investigation.
Summary updated