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

Jianzhu Huai, Yukai Lin, Yuan Zhuang, Min Shi

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.

Introduction

Accurate state estimation is essential for the safe navigation of autonomous systems. In visual-inertial SLAM, where absolute position and yaw are unobservable, a consistent estimator must maintain high uncertainty in these directions. However, traditional real-time estimators like Fixed-Lag Smoothers (FLS) often suffer from "inconsistency," where they become over-confident in their position estimates. This occurs primarily during marginalization, where the use of varying linearization points introduces spurious information into the system's unobservable subspace. This paper addresses this fundamental flaw by introducing a Right-Invariant Fixed-Lag Smoother (RI-FLS).

Methodology

The proposed RI-FLS leverages the geometric properties of Lie Groups to maintain estimator consistency naturally.

Right-Invariant Error Formulation

We represent the robot's state (orientation, velocity, and position) using the extended Lie Group $SE_2(3)$. By adopting a right-invariant error model, the error becomes independent of the robot's global transformation. This ensures that the system's unobservable directions remain consistent even as the robot's estimated trajectory is updated, preventing the "information gain" that plagues standard additive error models.

Simplified Observability Analysis

Traditionally, proving estimator consistency requires analyzing a massive observability matrix, a task that is computationally prohibitive for smoothers. We demonstrate that for FLSs, observability can be analyzed much more simply by examining the nullspace of the linearized system Jacobian. This theoretical breakthrough allows us to prove that the RI-FLS preserves the correct unobservable subspace without the need for artificial Jacobian corrections like the First Estimate Jacobian (FEJ) method.

Experimental Evaluation

We validated the RI-FLS using both synthetic simulations and real-world benchmarks.

The simulated torus trajectory used to evaluate estimator consistency.

Synthetic Simulation

A platform was simulated traversing a torus-shaped trajectory for 300 seconds within a landmark-rich environment. This controlled setup allowed for precise measurement of the Normalized Estimation Error Squared (NEES), the primary metric for estimator consistency.

Real-world Benchmark

The method was also applied to the EuRoC MAV dataset, which provides synchronized IMU and stereo imagery from a drone in industrial environments. This allowed us to compare the accuracy of RI-FLS against state-of-the-art traditional smoothers.

Results and Discussion

Estimator Consistency

The primary advantage of the RI-FLS is its remarkable consistency. In simulation tests where an ideal NEES score is 6, the RI-FLS achieved a stable score of approximately 6.6.

NEES comparison showing the superior consistency of RI-FLS over traditional FLS methods.

As shown in the figure above, traditional incremental and batch FLS methods exhibit rapidly diverging NEES values, indicating severe over-confidence. In contrast, the RI-FLS (and the iSAM2 incremental batch smoother) maintain stable, low NEES values throughout the trajectory. This confirms that the right-invariant formulation effectively protects the unobservable directions from spurious information gain.

Accuracy

On the EuRoC dataset, RI-FLS demonstrated accuracy comparable to or exceeding traditional methods. For example, on the "MH_01" sequence, RI-FLS achieved a Mean Absolute Trajectory Error (ATE) of 0.53m, significantly better than the 0.88m recorded by a standard FLS. This proves that maintaining mathematical consistency also benefits overall estimation precision.

Conclusion

The RI-FLS provides a mathematically robust solution to the inconsistency problem in visual-inertial navigation. By utilizing the geometry of Lie Groups, it maintains an "honest" assessment of uncertainty without the accuracy trade-offs associated with traditional Jacobian hacks. Future work will investigate the impact of exact Jacobian formulations and the potential of Left-Invariant errors for alternative sensor configurations.

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