Convergence and Consistency Analysis for A 3D Invariant-EKF SLAM

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.Source: §I · p. 1

Related work

• Earlier nonlinear convergence proofs required the restrictive assumption that motion and observation Jacobians are evaluated at the ground truth.Source: §I · p. 1

Contributions

• RI-EKF is analyzed for 3D SLAM without requiring Jacobians to be evaluated at the ground truth.Source: §I · p. 1
• RI-EKF output is invariant under stochastic rigid body transformations, whereas SO(3)-EKF is invariant only under deterministic rigid body transformations.Source: §I · p. 1
• 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.Source: §I · p. 1

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.Source: §III · p. 2

Algorithm

• The algorithm takes the current estimate, covariance, control input, and observation, then outputs the propagated and updated estimate and covariance.Source: §III · p. 2

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.Source: §III · p. 2, 2) Jacobian matrices · p. 2

RI-EKF

• RI-EKF models the SLAM state on a Lie group and uses the general EKF propagation and update procedure.Source: A. RI-EKF · p. 2, §III · p. 2
• 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.Source: A. RI-EKF · p. 2

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.Source: 3) Landmark initialization · pp. 2–3
• The new landmark estimate is initialized as f_hat_(N+1) = p_hat + R_hat z.Source: 3) Landmark initialization · p. 3

B. Discussion

• SO(3)-EKF and SE(3)-EKF arise from alternative state-space and retraction choices within the same general EKF framework.Source: B. Discussion · p. 3
• Different retractions can produce identical Jacobians but substantially different filter behavior, showing that retraction choice affects performance.Source: B. Discussion · p. 3

Convergence analysis

• RI-EKF convergence is proven for stationary and one-step-motion scenarios when Jacobians are evaluated at the latest estimate.Source: §IV · p. 3

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.Source: A. Scenario A: Robot remains stationary · p. 3
• 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.Source: A. Scenario A: Robot remains stationary · p. 3
• The stationary single-new-landmark experiment uses repeated noisy observations and compares orientation error and det(Pθ) for RI-EKF and SO(3)-EKF.Source: A. Scenario A: Robot remains stationary · p. 3, Figure 1 · p. 3
• The result extends directly to multiple new landmarks: RI-EKF's stationary pose estimate remains unchanged when those landmarks are observed repeatedly.Source: A. Scenario A: Robot remains stationary · p. 3

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.Source: B. Scenario B: Robot takes a step… · p. 4
• With three landmarks non-coplanar with the robot position, the limiting covariance is characterized after repeated observations at point B.Source: B. Scenario B: Robot takes a step… · p. 4
• The theorem matches the expected behavior that observations at B reduce only the odometry-induced robot uncertainty, not landmark uncertainty already reduced at A.Source: B. Scenario B: Robot takes a step… · p. 4
• 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.Source: B. Scenario B: Robot takes a step… · p. 4
• RI-EKF pose uncertainty remains unchanged while stationary, increases after the step because of odometry noise, and gradually decreases with later landmark observations.Source: B. Scenario B: Robot takes a step… · p. 4, Figure 2 · p. 4
• SO(3)-EKF pose uncertainty falls below its initial value, indicating incorrect information injection and an overconfident uncertainty estimate.Source: B. Scenario B: Robot takes a step… · p. 4, §V · p. 4

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.Source: A. Unobservability and invariance… · pp. 4–5
• The paper defines filter-output invariance by requiring identical estimated observations from transformed initial estimates driven by the same odometry and observations.Source: A. Unobservability and invariance… · p. 5
• Stochastic rigid-body invariance combines deterministic rigid-body invariance with stochastic-identity invariance.Source: A. Unobservability and invariance… · pp. 4–5
• RI-EKF is invariant under stochastic rigid-body transformations, whereas SO(3)-EKF is invariant only under deterministic rigid-body transformations.Source: A. Unobservability and invariance… · p. 5, Table I · p. 5
• Pseudo-RI-EKF is invariant only under stochastic identity transformations, while SE(3)-EKF is invariant under neither listed transformation class.Source: A. Unobservability and invariance… · p. 5, Table I · p. 5
• For RI-EKF, stochastic-identity invariance depends directly on the propagation and observation Jacobians F_n and H_n.Source: A. Unobservability and invariance… · p. 5

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.Source: B. Results and analysis · p. 6
• 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.Source: B. Results and analysis · p. 6
• 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.Source: B. Results and analysis · p. 6, Figure 3 · p. 6, A. Settings · p. 6
• 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.Source: B. Results and analysis · p. 6
• 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.Source: B. Consistency and invariance · p. 5, §VII · p. 6

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.Source: A. Lie Group G(N) · p. 6
• Table II is identified as the paper’s performance-evaluation table.The supplied passage provides the table caption but no cell values.Source: Table II · p. 7

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.Source: B. Proof of Theorem 1 · p. 7
• 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.Source: B. Proof of Theorem 1 · p. 7
• 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.Source: B. Proof of Theorem 1 · p. 7
• 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.Source: B. Proof of Theorem 1 · p. 7

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.Source: C. Proof of Theorem 2 · p. 7
• 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.Source: C. Proof of Theorem 2 · p. 7
• 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.Source: C. Proof of Theorem 2 · p. 7
• 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.Source: C. Proof of Theorem 2 · p. 7

• 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.Source: D. Proof of Theorem 3 · p. 7
• 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^⊤.Source: D. Proof of Theorem 3 · p. 8
• 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.Source: D. Proof of Theorem 3 · pp. 7–8
• 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 Σ̄.Source: D. Proof of Theorem 3 · p. 8
• 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.Source: D. Proof of Theorem 3 · p. 8