Source-linked AI summary
Convergence and Consistency Analysis for A 3D Invariant-EKF SLAM
Teng Zhang, Kanzhi Wu, Jingwei Song, Shoudong Huang, Gamini Dissanayake
TL;DR
The paper studies how to obtain convergent and consistent 3D point-feature SLAM with an invariant EKF despite known inconsistency in traditional EKF SLAM. It analyzes RI-EKF convergence and invariance, then evaluates it against alternative filters. RI-EKF is invariant under stochastic rigid body transformations and outperforms the compared filters in the reported 3D simulations.
Problem
Traditional EKF point-feature SLAM can underestimate uncertainty and produce overconfident estimates, motivating analysis of convergence and consistency in 3D SLAM.
Method
The paper analyzes RI-EKF convergence, stochastic rigid-body invariance, and the relationship between invariance and estimator consistency.
Results
RI-EKF is invariant under stochastic rigid body transformations and outperforms SO(3)-EKF, SE(3)-EKF, Robocentric-EKF, and FEJ-EKF in the reported 3D SLAM simulations.
Takeaways & Limitations
The reported analyses and simulations indicate that invariance has an important effect on SLAM estimator consistency and accuracy.
Abstract
from arXiv · showhide
In this paper, we investigate the convergence and consistency properties of an Invariant-Extended Kalman Filter (RI-EKF) based Simultaneous Localization and Mapping (SLAM) algorithm. Basic convergence properties of this algorithm are proven. These proofs do not require the restrictive assumption that the Jacobians of the motion and observation models need to be evaluated at the ground truth. It is also shown that the output of RI-EKF is invariant under any stochastic rigid body transformation in contrast to $\mathbb{SO}(3)$ based EKF SLAM algorithm ($\mathbb{SO}(3)$-EKF) that is only invariant under deterministic rigid body transformation. Implications of these invariance properties on the consistency of the estimator are also discussed. Monte Carlo simulation results demonstrate that RI-EKF outperforms $\mathbb{SO}(3)$-EKF, Robocentric-EKF and the "First Estimates Jacobian" EKF, for 3D point feature based SLAM.
I. INTRODUCTION
The paper addresses inconsistency and convergence concerns in EKF-based 3D point-feature SLAM by analyzing RI-EKF and its invariance properties. It introduces a convergence analysis without ground-truth Jacobian evaluation and compares RI-EKF with alternative EKF formulations.
- Motivation: Traditional EKF point-feature SLAM can underestimate estimation uncertainty, producing overconfident and potentially inconsistent results.Prior work linked this inconsistency to the partial observability of SLAM.
- Related work: Earlier nonlinear convergence proofs required the restrictive assumption that motion and observation Jacobians are evaluated at the ground truth.
- Contributions: RI-EKF is analyzed for 3D SLAM without requiring Jacobians to be evaluated at the ground truth.
- Contributions: RI-EKF output is invariant under stochastic rigid body transformations, whereas SO(3)-EKF is invariant only under deterministic rigid body transformations.
- Contributions: The study investigates how these invariance properties affect estimator consistency and evaluates RI-EKF against SO(3)-EKF, Robocentric-EKF, and First Estimates Jacobian EKF.
III. THE INVARIANT EKF SLAM ALGORITHM
RI-EKF instantiates a general EKF framework using Lie-group state representations and a retraction-based uncertainty model. Its algorithm propagates and updates the state and covariance through Jacobians tailored to that representation.
- General EKF framework: The general EKF framework represents uncertainty as X = X_hat ⊕ e, with Gaussian error e and mean estimate X_hat.The retraction has an inverse mapping that recovers the error from the state and estimate.
- Algorithm: The algorithm takes the current estimate, covariance, control input, and observation, then outputs the propagated and updated estimate and covariance.
- Jacobians: Its propagation and update Jacobians are derived from the chosen uncertainty representation, with observation Jacobians computed by stacking feature terms or applying the chain rule.
- RI-EKF: RI-EKF models the SLAM state on a Lie group and uses the general EKF propagation and update procedure.
- RI-EKF: RI-EKF chooses the retraction X = X_hat ⊕ e := exp(e) X_hat, applying the Lie-group exponential to the error before the mean estimate.
3) Landmark initialization:
Landmark initialization augments the RI-EKF state and covariance when a new feature is observed. The section also contrasts RI-EKF with other retraction choices and motivates convergence analysis in representative motion scenarios.
- 3) Landmark initialization:: When a new landmark is observed, RI-EKF augments the state and adjusts the covariance using the observation and its noise covariance.
- 3) Landmark initialization:: The new landmark estimate is initialized as f_hat_(N+1) = p_hat + R_hat z.
- B. Discussion: SO(3)-EKF and SE(3)-EKF arise from alternative state-space and retraction choices within the same general EKF framework.
- B. Discussion: Different retractions can produce identical Jacobians but substantially different filter behavior, showing that retraction choice affects performance.
- Convergence analysis: RI-EKF convergence is proven for stationary and one-step-motion scenarios when Jacobians are evaluated at the latest estimate.
A. Scenario A: Robot remains stationary
When the robot remains stationary and observes only new landmarks, RI-EKF preserves the pose estimate and its relevant uncertainty, unlike SO(3)-EKF.
- A. Scenario A: Robot remains stationary: RI-EKF leaves the robot pose, existing landmarks, and their related uncertainty unchanged while repeatedly observing a new landmark at a stationary point.
- A. Scenario A: Robot remains stationary: Observing a previously unseen landmark does not reduce RI-EKF's robot-pose covariance because the observation provides no new information about the stationary robot location.
- A. Scenario A: Robot remains stationary: The stationary single-new-landmark experiment uses repeated noisy observations and compares orientation error and det(Pθ) for RI-EKF and SO(3)-EKF.
- A. Scenario A: Robot remains stationary: The result extends directly to multiple new landmarks: RI-EKF's stationary pose estimate remains unchanged when those landmarks are observed repeatedly.
B. Scenario B: Robot takes a step after a stationary period
After a long stationary period, RI-EKF responds to a single robot step by adding odometry uncertainty, then reduces only that added pose uncertainty through later observations.
- B. Scenario B: Robot takes a step after a stationary period: Theorem 2 considers a robot observing new landmarks indefinitely at point A, then taking one control-driven step to point B and repeatedly observing the same landmarks.
- B. Scenario B: Robot takes a step after a stationary period: With three landmarks non-coplanar with the robot position, the limiting covariance is characterized after repeated observations at point B.
- B. Scenario B: Robot takes a step after a stationary period: The theorem matches the expected behavior that observations at B reduce only the odometry-induced robot uncertainty, not landmark uncertainty already reduced at A.
- B. Scenario B: Robot takes a step after a stationary period: The simulation keeps the robot at A for 200 observations, moves it one step to B, and observes the same ten landmarks for 200 further steps.
- B. Scenario B: Robot takes a step after a stationary period: RI-EKF pose uncertainty remains unchanged while stationary, increases after the step because of odometry noise, and gradually decreases with later landmark observations.
- B. Scenario B: Robot takes a step after a stationary period: SO(3)-EKF pose uncertainty falls below its initial value, indicating incorrect information injection and an overconfident uncertainty estimate.
A. Unobservability and invariance property
The SLAM system is unobservable under stochastic rigid-body transformations, motivating an invariance comparison among EKF variants that links transformation behavior to estimator consistency.
- A. Unobservability and invariance property: SLAM outputs are unchanged under any stochastic rigid-body transformation, so the formulation is unobservable with respect to that transformation.
- A. Unobservability and invariance property: The paper defines filter-output invariance by requiring identical estimated observations from transformed initial estimates driven by the same odometry and observations.
- A. Unobservability and invariance property: Stochastic rigid-body invariance combines deterministic rigid-body invariance with stochastic-identity invariance.
- A. Unobservability and invariance property: RI-EKF is invariant under stochastic rigid-body transformations, whereas SO(3)-EKF is invariant only under deterministic rigid-body transformations.
- A. Unobservability and invariance property: Pseudo-RI-EKF is invariant only under stochastic identity transformations, while SE(3)-EKF is invariant under neither listed transformation class.
- A. Unobservability and invariance property: For RI-EKF, stochastic-identity invariance depends directly on the propagation and observation Jacobians F_n and H_n.
B. Consistency and invariance
The paper links estimator consistency to invariance under stochastic rigid-body transformations and evaluates RI-EKF against several 3D SLAM filters. RI-EKF is reported as the best-performing and consistent method in the supplied experiments.
- Consistency and invariance: SE(3)-EKF diverges even at σod = 1%, σob = 1%, while Pseudo-RI-EKF performs poorly.The paper attributes these outcomes to missing deterministic or stochastic invariance properties.
- Consistency and invariance: Robocentric-EKF performs poorly at σod = 5%, σob = 5% because landmark coordinate transformations increase propagation linearization errors.Its stochastic rigid-body invariance does not prevent this high-noise degradation relative to SO(3)-EKF, FEJ-EKF, and RI-EKF.
- Consistency and invariance: RI-EKF is the best-performing filter in Table II and remains consistent within the 95% confidence bound in Figure 3.The evaluation uses RMS error for accuracy and average NEES for consistency, with 100 Monte Carlo simulations per noise condition.
- Consistency and invariance: FEJ-EKF also has stochastic-transformation invariance but performs worse than RI-EKF because it linearizes at less accurate estimates.RI-EKF can safely use the latest estimate for Jacobian evaluation.
- Consistency and invariance: RI-EKF is invariant under stochastic rigid-body transformations, whereas SO(3)-EKF is invariant only under deterministic transformations.The paper connects stochastic-transformation invariance with preserving SLAM’s fundamental unobservability.
APPENDIX
The appendix introduces the Lie-group representation and associated algebraic structures used by the RI-EKF formulation. It also includes the performance-evaluation table.
- APPENDIX: G(N) is introduced as the Lie group underlying the 3D SLAM state representation.The supplied appendix passages define its group operation, Lie algebra, exponential mapping, and related matrix structure.
- APPENDIX: Table II is identified as the paper’s performance-evaluation table.The supplied passage provides the table caption but no cell values.
B. Proof of Theorem 1
The proof of Theorem 1 proceeds by induction over repeated observations in a stationary-robot scenario. It tracks how the estimate and covariance evolve after initialization and subsequent updates.
- B. Proof of Theorem 1: The proof initializes the estimate and covariance, then augments them after the first landmark observation using Algorithm 2.This establishes the induction base for the subsequent observation updates.
- B. Proof of Theorem 1: After k observations, the proof assumes a structured estimate and covariance before analyzing the next propagation and update.The stationary-robot condition makes the propagated mean and covariance equal to their preceding values.
- B. Proof of Theorem 1: At the next update, only the newly added landmark components of Ky are nonzero, leaving the robot pose and old landmarks unchanged.This observation supports the inductive preservation of the estimate structure.
- B. Proof of Theorem 1: The covariance evolution is then used to complete the theorem’s limiting result, including the stated divergence-to-infinity case.The supplied proof passage explicitly states that the covariance matrix converges to infinity for the relevant result.
C. Proof of Theorem 2
The proof of Theorem 2 derives covariance behavior after motion and observations using Jacobian-based information terms. It relies on positive-definite noise covariance and geometric landmark conditions.
- C. Proof of Theorem 2: The proof substitutes Jacobian relations from earlier results into the covariance expressions to derive the theorem’s motion-related update.The derivation explicitly uses the result of Theorem 1 and the Jacobian matrices.
- C. Proof of Theorem 2: The odometry noise contributes incremental uncertainty through ΔP = ad ˆX_A ˜Φ adᵀ, with ˜Φ positive definite.The passage identifies ˆX_A as the incremental uncertainty caused by odometry noise.
- C. Proof of Theorem 2: With isotropic observation noise, the information contribution H_jᵀ Ψ̄^-1 H_j remains the same across repeated observations at point B.The proof uses this equality to characterize covariance after l observations.
- C. Proof of Theorem 2: The stacked observation information term is full rank when m > 3 and three landmarks are non-coplanar with the robot position.This geometric condition supports the covariance argument in the proof.
D. Proof of Theorem 3
The proof establishes deterministic rigid-body invariance for SO(3)-EKF and RI-EKF, then extends RI-EKF invariance to stochastic identity transformations through covariance propagation and induction.
- The paper proves invariance properties only for RI-EKF and SO(3)-EKF, while stating that analogous proofs or counterexamples can address the other algorithms.This defines the scope of the theorem proof.
- The deterministic proof uses transformed Jacobians and gains, with Ḡy0 = Q_T G0 and K_y = Q_T K preserving the transformed state and covariance relations.The covariance update becomes P̄y1 = Q_T P1 Q_T^⊤.
- SO(3)-EKF and RI-EKF outputs remain invariant under deterministic rigid-body transformations by induction over propagation and update steps.The transformed estimate satisfies Ȳ1 = T(X̂1) and P̄y1 = Q_T P1 Q_T^⊤ after one step, and the result extends inductively.
- RI-EKF is also invariant under stochastic identity transformations by adding ΔP = CΣ̄C^⊤ to the initial covariance.The transformation is parameterized by stochastic noise Θ with covariance Σ̄.
- Because F_n = I and H_1ΔP = 0, propagation and updating preserve the covariance offset, allowing induction to establish RI-EKF stochastic invariance.The posterior remains (X̂1, P1 + ΔP), and the same argument applies at subsequent steps.