Pith. sign in

REVIEW 2 major objections 5 minor 22 references

Iterated Invariant EKF for 3D Landmark-Aided Inertial Navigation

T0 review · 2 major / 5 minor · reviewed 2026-07-13 · grok-4.5

Challenge this review Re-run · record.json Download PDF Read on arXiv ↗ Review JSON

Pith's one-line read An iterated invariant Kalman filter cuts position and velocity error by about 80% for 3D landmark-aided inertial navigation.

desk verdict Solid first multi-landmark IterIEKF derivation for SE2(3) MAV navigation; clear ~80% MAE gains over IEKF in controlled sim, but simulation-only under continuous perfect visibility. read the letter →

arxiv 2607.00145 v2 pith:JKHIMHYR submitted 2026-06-30 cs.RO

Hilton Marques Souza Santana , João Carlos Virgolino Soares , Marco Antonio Meggiolaro This is my paper · ORCID
classification cs.RO
keywords IteratedInvariantEKFlandmark-aidedinertialnavigationvisual-inertialodometryfalseobservabilityLie-groupfilteringSE2(3)MonteCarloconsistencyMAVlocalization
verification ladder T0 review T1 audit T2 compute T3 formal

The pith

A machine-rendered reading of the paper's core claim, the machinery that carries it, and where it could break.

The reading

This paper claims that the Iterated Invariant Extended Kalman Filter (IterIEKF), previously used on legged robots, works better than standard filters when a flying robot estimates its 3D pose and velocity from an IMU and known landmarks. Classical SO(3)-based EKFs become overconfident along directions that are actually unobservable (false observability), which hurts accuracy. The plain Invariant EKF fixes that by using Lie-group error dynamics, but its single linearization of the landmark measurement can still leave residual error. The IterIEKF repeatedly refines the update so that, in the low-noise regime, the estimate lands on the intersection of the observed manifolds while uncertainty stays in the tangent space. On Monte Carlo trials built from a EuRoC MAV trajectory with three always-visible landmarks, the authors report roughly 80% lower position and body-velocity mean absolute error than the plain IEKF, better consistency (NEES near the state dimension), and faster attitude convergence.

What carries the argument

The IterIEKF Gauss-Newton loop on the right-invariant error: each iteration re-linearizes the multi-landmark innovation on SE2(3) imes R^6 so the estimate is driven onto the intersection of the observed submanifolds while the covariance remains in the tangent space.

What would settle it

Re-run the same Monte Carlo suite but randomly occlude one or more landmarks for multi-second intervals (or inject occasional association swaps) and check whether the IterIEKF still retains an ~80% MAE advantage and NEES near 15 over the plain IEKF.

Watch

Extended reading notes

Core claim

Formulated for the first time for landmark-aided 3D inertial navigation, the IterIEKF outperforms SO(3)-EKF, IterSO(3)-EKF and the ordinary IEKF on accuracy and consistency. In 50 Monte Carlo runs on a EuRoC-derived trajectory it reduces position MAE from 0.511 m to 0.096 m and velocity MAE from 0.48 m/s to 0.095 m/s relative to the IEKF, while keeping average NEES near 15.

Load-bearing premise

All three landmarks stay continuously visible at 1 Hz with perfect data association and known map positions for the whole trajectory; if any landmark drops out or is mis-associated, the manifold-intersection guarantee and the reported error cuts no longer hold.

Share X Bluesky LinkedIn Reddit HN

Signed reviews

No signed human review yet.

Editorial analysis

A structured set of objections, weighed in public.

Desk editor's note, referee report, and a circularity audit.

Referee Report

2 major / 5 minor

Summary. The paper formulates and evaluates the Iterated Invariant Extended Kalman Filter (IterIEKF) for 3D landmark-aided inertial navigation of a MAV. The state is embedded in SE2(3)×R6 with a right-invariant error; IMU strapdown kinematics supply the prediction (Eqs. 11, 16–20), and relative 3D landmark positions supply the measurement model (Eqs. 12–15). The multi-landmark Gauss–Newton update (Eqs. 28–37) is derived so that, in the low-noise regime, the estimate is driven onto the intersection of the observed submanifolds while uncertainty remains in the tangent space. Monte-Carlo trials (N=50) on a EuRoC V2_01_easy trajectory with three always-visible known landmarks show that IterIEKF reduces position and body-velocity MAE by roughly 80% relative to the single-step IEKF (0.096 m vs 0.511 m; 0.095 m/s vs 0.48 m/s) while keeping average NEES near the state dimension 15, outperforming SO(3)-EKF, IterSO(3)-EKF and IEKF in accuracy and consistency. Accelerometer-bias observability limitations are acknowledged.

Significance. If the reported gains hold under the stated conditions, the work supplies a concrete, fully derived multi-landmark IterIEKF for a standard MAV localization setting and demonstrates that combining Lie-group invariance with iterated measurement updates yields large accuracy and consistency improvements over both classical multiplicative EKFs and the single-step IEKF. The complete algorithmic derivation (prediction and multi-landmark Gauss–Newton update), transparent Monte-Carlo protocol on a public trajectory, and joint reporting of MAE and NEES are clear strengths. The continuous full-visibility, perfect-association, known-map regime is a genuine limitation of scope, but it is stated explicitly and does not invalidate the internal comparison. The paper is a solid incremental contribution that makes the IterIEKF usable for landmark-aided inertial navigation.

major comments (2)
  1. Section 9 (and the measurement model of Eqs. 12–15) assumes K=3 landmarks remain continuously visible at 1 Hz with perfect data association and known world positions for the entire trajectory. The manifold-intersection guarantee that underpins the IterIEKF update (motivating example of Section 4 and Theorems cited from Goffin et al.) is operative only under this regime. The central claim of an ~80% MAE reduction is therefore demonstrated only for this idealized sensing schedule. The manuscript should either (i) add at least one ablation with intermittent visibility / partial landmark sets, or (ii) state more prominently in the abstract, contributions list and conclusion that the reported gains are conditioned on continuous full visibility of a known map.
  2. Section 9.3 and Fig. 6 candidly show that accelerometer biases remain poorly observed (relative error persistently above 1) for every filter, including IterIEKF, because appending biases as an R6 factor breaks the strict group-affine property. While this does not overturn the position/velocity claims, the abstract and contribution bullets emphasize overall superiority without mentioning this structural limitation. A short clarifying sentence in the abstract or contributions list would keep the claim proportionate to the evidence.
minor comments (5)
  1. Eq. (37) updates the covariance with the gain and Jacobian from the first iteration (K0, H0) rather than the final iteration used for the state. This is a common practical choice but should be stated explicitly so that readers can reproduce the NEES numbers.
  2. The stopping criterion δ=10−4 and the optional loss-increase check are given only in prose (Section 7.2). Adding them to a short algorithm box or pseudocode would improve reproducibility.
  3. Fig. 1 and Fig. 2 are helpful geometric illustrations but the caption of Fig. 1 does not define the green/blue/gray objects; a one-line legend would help.
  4. Table 1 reports relative errors to IEKF; adding absolute MAE for roll/pitch/yaw (or a short attitude MAE column) would make the attitude discussion in Section 9.2 fully quantitative.
  5. A few typographical inconsistencies appear (e.g., “Coordenação” encoding, occasional missing spaces around citations). A light copy-edit pass would clean them.

Circularity Check

1 steps flagged · score 1.0 of 10

Minor non-load-bearing self-citation of authors' prior IterIEKF iteration scheme; MAV Jacobians, multi-landmark adaptation, and Monte-Carlo accuracy/consistency claims are independently derived and simulated against ground truth.

  1. self citation load bearing [Section 7.2, Eqs. 28-34 and surrounding text]
    "For the right-invariant error, this Gauss-Newton sequence was obtained in [18]. We adapted this loop to be able to handle K visible landmarks, resulting in the following: Φj = ExpSO(3)(-ξ ji 1:3), au j k = [Exp(-ξ ji)dk - dk]1:3, au… ξ0i := 015,1."

    The core iterative update equations are imported from the authors' own prior arXiv [18] rather than re-derived ab initio; the present paper only adapts the stacking for K landmarks. This is a self-citation of the algorithmic skeleton. It is not load-bearing for the central claim (the ~80 % MAE reduction and NEES consistency on the MAV trajectory), which is established by independent Monte-Carlo runs using newly derived landmark Jacobians and does not rely on any numerical result from [18].

full rationale

The paper's derivation chain for the multi-landmark right-invariant prediction (Eqs. 16-20), measurement Jacobians (Eqs. 21-27), and IterIEKF Gauss-Newton loop (Eqs. 28-37) is written out explicitly for the SE2(3) imes R6 landmark-aided IMU model; the same holds for the SO(3) baselines. Performance claims (Table 1, Figs. 4-6) are obtained by forward Monte-Carlo simulation of independent noise realizations on a known EuRoC-derived trajectory with known landmarks, not by fitting parameters that are later re-presented as predictions. The sole self-citation of note is the reuse of the authors' own prior Gauss-Newton sequence from the quadruped paper [18]; this supplies the iteration skeleton but is not required for the new measurement matrices, the observability analysis, or the reported MAE/NEES numbers, which stand on the simulations alone. External citations ([2], [12]) supply the IEKF and IterIEKF foundations without circular reduction. No self-definitional identities, fitted-as-prediction steps, uniqueness theorems imported from the authors, or renamed known results appear. Score 1 reflects only the minor self-citation of the iteration template.

Assumptions & free parameters 4 free parameters · 4 assumptions · 0 invented entities

The central performance claim rests on standard Lie-group calculus, the classical strap-down IMU model, additive white-Gaussian noise assumptions, and the geometric claim (imported from Goffin et al.) that iterated right-invariant updates project onto the observed submanifold. Free parameters are the usual filter tuning quantities (noise covariances, initial covariance, iteration tolerance). No new physical entities are postulated.

free parameters (4)
  • process and measurement noise covariances (Qg, Qa, Qbg, Qba, Nk) = 1.6e-3 I3 (accel), 4e-6 I3 (gyro), 1e-6 I3 (biases), 1e-3 I3 (landmarks)
    Hand-chosen diagonal values used for all Monte-Carlo trials (Eq. 55); directly affect absolute MAE numbers.
  • initial covariance P0 = diag((π/4)^2 I3, 1^2 I3, 2^2 I3, 1e-6 I3, 1e-6 I3)
    Sets the severity of the initial-state corruption that the filters must recover from; chosen to stress the filters (π/4 rad attitude, 1–2 m position).
  • Gauss-Newton stopping tolerance δ = 1e-4
    Controls how many iterations are performed; fixed at 10^{-4} for both iterated filters.
  • landmark world positions b1,b2,b3 = [-2,1,1.6], [0,2,2], [1,0.5,1.5] m
    Artificially placed fixed points that define the observed manifolds; their geometry influences observability and convergence speed.
assumptions (4)
  • domain assumption Right-invariant error kinematics on SE2(3)×R6 remain independent of the absolute trajectory when the dynamics are group-affine (Barrau & Bonnabel).
    Invoked throughout Sections 3 and 7 to claim that false observability is structurally eliminated.
  • domain assumption In the low-noise regime, iterated right-invariant updates project the estimate onto the intersection of observed submanifolds while confining uncertainty to the tangent space (Goffin et al. Theorems 1–2).
    Core justification for why IterIEKF should outperform single-step IEKF; used in the motivating example (Section 4) and claimed to explain the Monte-Carlo gains.
  • domain assumption IMU measurements are piecewise-constant over each Δt and corrupted by additive white Gaussian noise plus random-walk biases.
    Standard strap-down model (Eqs. 7–10) that generates both the true trajectory and the filter process noise.
  • standard math Lie-group exponential, logarithm and right-Jacobian maps for SE2(3) and SO(3) are well-defined and admit the usual first-order approximations.
    Used for all error-state definitions and linearizations (Eqs. 2–6, 16–19, 38–41).

how reviews work

0 comments
Cite this review

Pith. "Pith review of Iterated Invariant EKF for 3D Landmark-Aided Inertial Navigation." pith.science (2026). https://pith.science/paper/JKHIMHYR

@misc{pith2026260700145,
  author       = {Pith},
  title        = {Pith review of: Iterated Invariant EKF for 3D Landmark-Aided Inertial Navigation},
  year         = {2026},
  howpublished = {\url{https://pith.science/paper/JKHIMHYR}},
  note         = {Machine review of arXiv:2607.00145}
}
read the original abstract

Inertial navigation systems aided by three-dimensional landmark measurements constitute a fundamental problem in robotic perception and state estimation. Classical SO(3)-based Extended Kalman Filter (SO(3)-EKF) approaches provide practical solutions, but suffer from the false observability problem, in which the filter becomes overconfident in unobservable directions, leading to degraded estimation performance. The Invariant EKF (IEKF) addresses this limitation by reformulating the system dynamics as a group-affine system on a Lie group, although its measurement update does not fully satisfy certain state compatibility properties. More recently, the Iterated Invariant EKF (IterIEKF) was proposed to further improve the IEKF by ensuring, in the low-noise regime, that the estimated state remains on the observed state manifold while the uncertainty is confined to its tangent space. In this work, we formulate and apply the IterIEKF to landmark-based inertial 3D localization for the first time. Through numerical simulations, we show that the proposed approach outperforms the classical SO(3)-EKF, the Iterated SO(3)-EKF, and the IEKF in terms of both estimation accuracy and consistency.

Figures

Figures reproduced from arXiv: 2607.00145 by the authors.

Figure 1
Figure 1. a) Setup of a simple localization problem with three landmarks and almost [PITH_FULL_IMAGE:figures/full_fig_p007_1.png] view at source ↗
Figure 2
Figure 2. V2 01 easy dataset trajectory (gray), MAV true state (blue), landmarks (diamonds), and a single MAV sample of the corrupted initial state configuration for the invariant estimators (green). of the EuRoC MAV repository [7] and reconstruct ideal IMU inputs that satisfy the discrete-time system kinematics: Xi+1 = WiΦ(Xi)Yi(ωI,i, aI,i) ⇒ (WiΦ(Xi))−1 Xi+1 = Yi(ωI,i, aI,i) ⇒  ExpSO(3)(a), b  =  ExpSO(3)(ωI,i∆t), LaI,i… view at source ↗
Figure 2
Figure 2. Two stabilizer subgroups of SE(3): {S(N)} ∼= SO(3) and {R(N, v)} ∼= SO(2) [14]. In this work, the nominal state is propagated through the system dynamics, while the error state is propagated through the linearized dynamics. The error state propagation is discussed in Section 7 for the invariant filters and Section 8 for the multiplicative filters. The system dynamics for all filters considered in this work are gover… view at source ↗
Figures from the paper (6 more)
Figure 3
Figure 3. Figure 3: Results of the numerical Monte Carlo experiment on the [PITH_FULL_IMAGE:figures/full_fig_p017_3.png]
Figure 3
Figure 3. Figure 3: V2 01 easy dataset trajectory (gray), MAV true state (blue), landmarks (diamonds), and a single MAV sample of the corrupted initial state configuration for the invariant estimators (green). of the EuRoC MAV repository [7] and reconstruct ideal IMU inputs that satisfy t…
Figure 4
Figure 4. Figure 4: Average attitude estimates obtained with different filters across all real [PITH_FULL_IMAGE:figures/full_fig_p017_4.png]
Figure 5
Figure 5. Figure 5: Relative error for the estimates of the IMU bias obtained with different [PITH_FULL_IMAGE:figures/full_fig_p019_5.png]
Figure 5
Figure 5. Figure 5: Average attitude estimates obtained with different filters across all real [PITH_FULL_IMAGE:figures/full_fig_p017_5.png]
Figure 6
Figure 6. Figure 6: Relative error for the estimates of the IMU bias obtained with different [PITH_FULL_IMAGE:figures/full_fig_p019_6.png]

Review history (2 revisions) →

Discussion (0). Continue with ORCID to comment.

Reference graph

Works this paper leans on

22 extracted references · 5 linked inside Pith

  1. [1]

    Cambridge Univ

    Barfoot, T.D.: State Estimation for Robotics. Cambridge Univ. Press (2024)

    2024

  2. [2]

    IEEE Trans

    Barrau, A., Bonnabel, S.: The invariant extended Kalman filter as a stable observer. IEEE Trans. Autom. Control62(4), 1797–1812 (2016)

    2016

  3. [3]

    IEEE Trans

    Barrau, A., Bonnabel, S.: The geometry of navigation problems. IEEE Trans. Au- tom. Control68(2), 689–704 (2022) 20 H. Santana et al

    2022

  4. [4]

    Bloesch, M., Burri, M., Omari, S., Hutter, M., Siegwart, R.: Iterated extended Kalman filter based visual-inertial odometry using direct photometric feedback. Int. J. Robot. Res.36(10), 1053–1072 (2017)

    2017

  5. [5]

    IEEE Trans

    Brossard, M., Barrau, A., Chauchat, P., Bonnabel, S.: Associating uncertainty to extended poses for on Lie group IMU preintegration with rotating earth. IEEE Trans. Robot.38(2), 998–1015 (2021)

    2021

  6. [6]

    In: Proc

    Brossard, M., Bonnabel, S., Barrau, A.: Invariant Kalman filtering for visual iner- tial SLAM. In: Proc. Int. Conf. Inf. Fusion (FUSION). pp. 2021–2028 (2018)

    2021

  7. [7]

    Burri, M., Nikolic, J., Gohl, P., Schneider, T., Rehder, J., Omari, S., Achtelik, M.W., Siegwart, R.: The EuRoC micro aerial vehicle datasets. Int. J. Robot. Res. 35(10), 1157–1163 (2016)

    2016

  8. [8]

    Sensors20(7), 2068 (2020)

    Debeunne, C., Vivet, D.: A review of visual-LiDAR fusion based simultaneous localization and mapping. Sensors20(7), 2068 (2020)

Show all 22 references
  1. [9]

    IEEE Trans

    Dissanayake, M.W.M.G., Newman, P., Clark, S., Durrant-Whyte, H.F., Csorba, M.: A solution to the simultaneous localization and map building (SLAM) problem. IEEE Trans. Robot. Autom.17(3), 229–241 (2001)

    2001

  2. [10]

    Navi- gation18(4), 386–401 (1971)

    Edwards Jr., A.: The state of strapdown inertial guidance and navigation. Navi- gation18(4), 386–401 (1971)

    1971

  3. [11]

    arXiv preprint arXiv:2208.03376 (2022)

    Elhousni, M., Huang, X.: A survey on visual map localization using LiDARs and cameras. arXiv preprint arXiv:2208.03376 (2022)

    2022 arXiv

  4. [12]

    IEEE Trans

    Goffin, S., Barrau, A., Bonnabel, S., Br¨ uls, O., Sacr` e, P.: Iterated invariant ex- tended Kalman filter (IterIEKF). IEEE Trans. Autom. Control71(5), 3380–3387 (2026)

    2026

  5. [13]

    arXiv preprint arXiv:2102.03804 (2021)

    He, D., Xu, W., Zhang, F.: Kalman filters on differentiable manifolds. arXiv preprint arXiv:2102.03804 (2021)

    2021 arXiv

  6. [14]

    Mechanism and Machine Theory34(5), 719–730 (1999)

    Herv´ e, J.: The lie group of rigid body displacements, a fundamental tool for mech- anism design. Mechanism and Machine Theory34(5), 719–730 (1999)

    1999

  7. [15]

    IEEE Trans

    Leonard, J.J., Durrant-Whyte, H.F., et al.: Mobile robot localization by tracking geometric beacons. IEEE Trans. Robot. Autom.7(3), 376–382 (1991)

    1991

  8. [16]

    Li, M., Mourikis, A.I.: High-precision, consistent EKF-based visual-inertial odom- etry. Int. J. Robot. Res.32(6), 690–711 (2013)

    2013

  9. [17]

    additive filtering for spacecraft attitude determi- nation quaternion estimation

    Markley, F.L.: Multiplicative vs. additive filtering for spacecraft attitude determi- nation quaternion estimation. J. Guid. Control Dyn.26(2), 311–317 (2003)

    2003

  10. [18]

    arXiv preprint arXiv:2604.15449 (2026)

    Santana, H.M.S., Soares, J.C.V., Goffin, S., Nistic` o, Y., Bonnabel, S., Semini, C., Meggiolaro, M.A.: Iterated invariant EKF for quadruped robot odometry. arXiv preprint arXiv:2604.15449 (2026)

    2026 arXiv

  11. [19]

    In: 2024 IEEE-RAS 23rd International Conference on Humanoid Robots (Humanoids)

    Santana, H.M.S., Soares, J.C.V., Nistic` o, Y., Meggiolaro, M.A., Semini, C.: Pro- prioceptive state estimation for quadruped robots using invariant kalman filtering and scale-variant robust cost functions. In: 2024 IEEE-RAS 23rd International Conference on Humanoid Robots (Hu...

    2024

  12. [20]

    arXiv preprint arXiv:1711.02508 (2017)

    Sol` a, J.: Quaternion kinematics for the error-state Kalman filter. arXiv preprint arXiv:1711.02508 (2017)

    2017 arXiv

  13. [21]

    arXiv preprint arXiv:1812.01537 (2018)

    Sol` a, J., Deray, J., Atchuthan, D.: A micro Lie theory for state estimation in robotics. arXiv preprint arXiv:1812.01537 (2018)

    2018 arXiv

  14. [22]

    Trawny, N., Mourikis, A.I., Roumeliotis, S.I., Johnson, A.E., Montgomery, J.F.: Vision-aided inertial navigation for pin-point landing using observations of mapped landmarks. J. Field Robot.24(5), 357–378 (2007)

    2007

Pith tools

Reviewed July 13, 2026 · model on record in the stance chip above.