Consistent Right-Invariant Fixed-Lag Smoother with Application to Visual Inertial SLAM

Jianzhu Huai, Yukai Lin, Yuan Zhuang, Min Shi

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.

The simulation setup involves a platform following a complex torus trajectory within a landmark-rich environment to test estimator consistency.
  • 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.

Trajectory error comparison showing that RI-FLS (green) significantly reduces drift compared to standard incremental and batch fixed-lag smoothers, particularly in the z-axis.

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

Abstract

State estimation problems without absolute position measurements routinely arise in navigation of unmanned aerial vehicles, autonomous ground vehicles, etc., whose proper operation relies on accurate state estimates and reliable covariances. Unaware of absolute positions, these problems have immanent unobservable directions. Traditional causal estimators, however, usually gain spurious information on the unobservable directions, leading to over-confident covariance inconsistent with actual estimator errors. The consistency problem of fixed-lag smoothers (FLSs) has only been attacked by the first estimate Jacobian (FEJ) technique because of the complexity to analyze their observability property. But the FEJ has several drawbacks hampering its wide adoption. To ensure the consistency of a FLS, this paper introduces the right invariant error formulation into the FLS framework. To our knowledge, we are the first to analyze the observability of a FLS with the right invariant error. Our main contributions are twofold. As the first novelty, to bypass the complexity of analysis with the classic observability matrix, we show that observability analysis of FLSs can be done equivalently on the linearized system. Second, we prove that the inconsistency issue in the traditional FLS can be elegantly solved by the right invariant error formulation without artificially correcting Jacobians. By applying the proposed FLS to the monocular visual inertial simultaneous localization and mapping (SLAM) problem, we confirm that the method consistently estimates covariance similarly to a batch smoother in simulation and that our method achieved comparable accuracy as traditional FLSs on real data.
BibTeX
@inproceedings{huai2021consistent,
  author = {Jianzhu Huai and Yukai Lin and Yuan Zhuang and Min Shi},
  title = {{Consistent Right-Invariant Fixed-Lag Smoother with Application to Visual Inertial SLAM}},
  booktitle = {AAAI Conference on Artificial Intelligence},
  year = {2021},
  doi = {10.1609/aaai.v35i7.16758},
}
Figure