pith. sign in

arxiv: 2607.00145 · v1 · pith:JKHIMHYRnew · submitted 2026-06-30 · 💻 cs.RO

Iterated Invariant EKF for 3D Landmark-Aided Inertial Navigation

Hilton Marques Souza Santana , Jo\~ao Carlos Virgolino Soares , Marco Antonio Meggiolaro This is my paper

Pith reviewed 2026-07-02 18:50 UTC · model grok-4.3

classification 💻 cs.RO
keywords invariant EKFinertial navigationlandmark measurementsstate estimationLie groupKalman filterconsistencyrobotics
0
0 comments X

The pith

The iterated invariant EKF improves accuracy and consistency for 3D landmark-aided inertial navigation.

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

This paper applies the iterated invariant extended Kalman filter to inertial navigation aided by three-dimensional landmark measurements. It claims this method resolves the false observability problem in classical filters by keeping the estimated state on the observed manifold and uncertainty in its tangent space. Numerical simulations show the approach outperforms the classical SO(3)-EKF, the iterated SO(3)-EKF, and the standard IEKF in both accuracy and consistency.

Core claim

By reformulating the system dynamics as a group-affine system on a Lie group and iterating the measurement update, the IterIEKF ensures in the low-noise regime that the estimated state remains on the observed state manifold while the uncertainty is confined to its tangent space, and this yields superior estimation performance when applied to landmark-based inertial 3D localization.

What carries the argument

The Iterated Invariant EKF (IterIEKF), which reformulates dynamics on a Lie group and iterates the update to maintain state compatibility.

Load-bearing premise

The low-noise regime property that keeps the state on the manifold and uncertainty in the tangent space transfers to the landmark-aided inertial navigation setting.

What would settle it

Simulations or tests where the IterIEKF does not outperform the SO(3)-EKF, iterated SO(3)-EKF, or IEKF in accuracy or consistency metrics.

Figures

Figures reproduced from arXiv: 2607.00145 by Hilton Marques Souza Santana, Jo\~ao Carlos Virgolino Soares, Marco Antonio Meggiolaro.

Figure 1
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. 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∆… view at source ↗
Figure 3
Figure 3. Figure 3: Results of the numerical Monte Carlo experiment on the [PITH_FULL_IMAGE:figures/full_fig_p017_3.png] view at source ↗
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] view at source ↗
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] view at source ↗
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.

Editorial analysis

A structured set of objections, weighed in public.

Desk editor's note, referee report, simulated authors' rebuttal, and a circularity audit. Tearing a paper down is the easy half of reading it; the pith above is the substance, this is the friction.

Referee Report

0 major / 2 minor

Summary. The manuscript formulates the Iterated Invariant EKF (IterIEKF) for landmark-aided inertial navigation in 3D and applies it for the first time to this setting. It builds on the group-affine structure of the dynamics on a Lie group to mitigate false observability in SO(3)-EKF while ensuring, in the low-noise regime, that the estimated state lies on the observed manifold with uncertainty confined to the tangent space. The central empirical claim is that numerical simulations demonstrate superior accuracy and consistency relative to the classical SO(3)-EKF, the Iterated SO(3)-EKF, and the standard IEKF.

Significance. If the reported simulation results are representative, the work supplies a concrete, first-of-its-kind demonstration that IterIEKF can be transferred to landmark-aided INS without loss of its consistency properties. This is a useful incremental contribution in the invariant-filtering literature for robotics, where consistency under partial observability remains a practical concern. The explicit comparison against three established baselines strengthens the case, though the magnitude of improvement and its sensitivity to landmark geometry or noise regimes are not yet quantified in the abstract.

minor comments (2)
  1. [Abstract] The abstract states that simulations show outperformance but provides no information on the noise levels, landmark density, trajectory length, or Monte-Carlo trial count. These parameters are needed to judge whether the low-noise regime assumption underlying the IterIEKF property is actually exercised and whether the consistency gains are statistically significant.
  2. [Method] The manuscript should include a brief derivation or reference to the specific state manifold and group-affine property used for the landmark-aided INS model (e.g., the precise definition of the state Lie group and the form of the measurement map) so that readers can verify that the IterIEKF update equations remain well-defined in this setting.

Simulated Author's Rebuttal

0 responses · 0 unresolved

We thank the referee for the positive assessment of the manuscript and the recommendation for minor revision. The work is recognized as a first-of-its-kind application of IterIEKF to landmark-aided 3D inertial navigation with demonstrated improvements in accuracy and consistency.

Circularity Check

0 steps flagged

No significant circularity

full rationale

The paper applies the previously proposed IterIEKF method to landmark-aided inertial navigation and evaluates it via numerical simulations against baselines. No derivation chain, fitted parameters, or self-referential definitions are present in the provided abstract or structure; the central claim rests on external simulation results rather than any reduction of outputs to inputs by construction. Self-citations, if any, are not load-bearing for the claimed outperformance.

Axiom & Free-Parameter Ledger

0 free parameters · 0 axioms · 0 invented entities

Only abstract available; no free parameters, axioms, or invented entities are described.

pith-pipeline@v0.9.1-grok · 5749 in / 1050 out tokens · 23379 ms · 2026-07-02T18:50:09.322231+00:00 · methodology

discussion (0)

Sign in with ORCID, Apple, or X to comment. Anyone can read and Pith papers without signing in.

Reference graph

Works this paper leans on

21 extracted references · 5 canonical work pages · 2 internal anchors

  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)

    2068

  9. [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

  10. [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

  11. [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)

    work page arXiv 2022

  12. [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

  13. [13]

    arXiv preprint arXiv:2102.03804 (2021)

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

    work page arXiv 2021

  14. [14]

    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

  15. [15]

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

    2013

  16. [16]

    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

  17. [17]

    Iterated Invariant EKF for Quadruped Robot Odometry

    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)

    work page internal anchor Pith review Pith/arXiv arXiv 2026

  18. [18]

    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 (Humanoids). pp. 213–220 (2024)

    2024

  19. [19]

    Quaternion kinematics for the error-state Kalman filter

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

    work page internal anchor Pith review Pith/arXiv arXiv 2017

  20. [20]

    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)

    work page arXiv 2018

  21. [21]

    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