pith. sign in

arxiv: 2604.10057 · v1 · submitted 2026-04-11 · 💻 cs.RO

Natural Gradient Gaussian Approximation Filter on Lie Groups for Robot State Estimation

Tianyi Zhang , Wenhan Cao , Chang Liu , Yao Lyu , Shengbo Eben Li This is my paper

Pith reviewed 2026-05-10 16:24 UTC · model grok-4.3

classification 💻 cs.RO
keywords state estimationLie groupsnatural gradientlegged robotsmanifold filteringGaussian approximationinvariant observationsrobot localization
0
0 comments X

The pith

The NANO-L filter improves state estimation on Lie groups by optimizing a Gaussian increment variable with natural gradients, avoiding tangent-space linearization and delivering 40 percent lower error in robot tests.

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

The paper seeks to reduce accumulated errors in state estimation for robotic systems whose configuration lives on a Lie group manifold, such as the pose of a legged robot. Standard filters handle nonlinear observations by linearizing them locally in the tangent space, but repeated linearization drifts the estimate over time. Instead, the method treats the update as an optimization problem that finds the mean and covariance of an additive increment variable on the Lie algebra; the exponential map then multiplies this increment onto the prior state to produce the posterior. Natural gradients, which use the Fisher information matrix to respect the geometry of the tangent space, solve the optimization efficiently. For the invariant observation models common in localization, the covariance update collapses to an exact closed-form expression, so the algorithm runs at speeds comparable to classical filters while cutting error by roughly 40 percent on a Unitree GO2 quadruped across varied terrain.

Core claim

Manifold filtering is recast as parameter optimization over a Gaussian increment variable that is mapped to the group via the exponential map to update the state multiplicatively; the natural gradient of this objective, incorporating the Fisher information of the increment, produces an iterative filter whose covariance step admits an exact closed-form solution when the observation model is invariant.

What carries the argument

Natural gradient optimization over the mean and covariance of the Gaussian increment variable on the Lie algebra, followed by the exponential map to apply the update multiplicatively on the group.

If this is right

  • Estimation error no longer accumulates from repeated local linearization of nonlinear observations on the manifold.
  • Covariance updates become exact and non-iterative for any invariant observation model, removing a common source of extra computation.
  • The filter maintains real-time speed on embedded hardware while improving accuracy for legged-robot navigation on rough terrain.
  • The same increment-variable formulation can be reused for other Lie-group state estimators without redesigning the linearization step.

Where Pith is reading between the lines

These are editorial extensions of the paper, not claims the author makes directly.

  • The approach could be combined with learned dynamics models to handle cases where the Gaussian increment assumption begins to break.
  • Because the update is multiplicative on the group, the filter naturally preserves manifold constraints that Euclidean filters must enforce by projection.
  • Similar natural-gradient optimization might reduce linearization error in related tasks such as visual-inertial odometry or simultaneous localization and mapping on SE(3).

Load-bearing premise

The Gaussian distribution on the increment variable remains a good approximation to the true posterior under the nonlinear manifold dynamics, and the observation model is invariant so that covariance can be obtained in closed form without iteration.

What would settle it

Run NANO-L and a standard linearized filter on the same hardware platform but with a deliberately non-invariant observation model, such as raw image features under perspective projection; if the 40 percent error reduction disappears or the filter diverges, the central advantage does not hold.

Figures

Figures reproduced from arXiv: 2604.10057 by Chang Liu, Shengbo Eben Li, Tianyi Zhang, Wenhan Cao, Yao Lyu.

Figure 1
Figure 1. Figure 1: A legged robot equipped with an IMU, joint encoders, and contact [PITH_FULL_IMAGE:figures/full_fig_p001_1.png] view at source ↗
Figure 2
Figure 2. Figure 2: Relationship among a matrix Lie group G, its Lie algebra g, and the associated vector space Rdim G. The color-graded ellipse depicts the uncertainty distribution. where the wedge operator for SO(3) maps a vector to a skew￾symmetric matrix as ϕ ∧ =   0 −ϕ3 ϕ2 ϕ3 0 −ϕ1 −ϕ2 ϕ1 0   . Beyond orientation, state estimation in robotics often re￾quires representing additional quantities such as position, veloci… view at source ↗
Figure 3
Figure 3. Figure 3: The overall estimation procedure of our NANO-L filter at time step [PITH_FULL_IMAGE:figures/full_fig_p006_3.png] view at source ↗
Figure 4
Figure 4. Figure 4: Box plots of position and orientation RMSE, aggregated over 100 [PITH_FULL_IMAGE:figures/full_fig_p008_4.png] view at source ↗
Figure 6
Figure 6. Figure 6: Different environments of real-world experiments. [PITH_FULL_IMAGE:figures/full_fig_p009_6.png] view at source ↗
Figure 7
Figure 7. Figure 7: Estimated position, velocity, and orientation for InEKF and NANO-L on the unstable terrain. [PITH_FULL_IMAGE:figures/full_fig_p010_7.png] view at source ↗
Figure 8
Figure 8. Figure 8: Estimated trajectories from InEKF and NANO-L on the flat terrain with different distances and durations. From left to right: (9.25 m, 40 s), (10.25 [PITH_FULL_IMAGE:figures/full_fig_p010_8.png] view at source ↗
Figure 9
Figure 9. Figure 9: Estimation accuracy and computation time of NANO-L on flat terrain [PITH_FULL_IMAGE:figures/full_fig_p011_9.png] view at source ↗
read the original abstract

Accurate state estimation for robotic systems evolving on Lie group manifolds, such as legged robots, is a prerequisite for achieving agile control. However, this task is challenged by nonlinear observation models defined on curved manifolds, where existing filters rely on local linearization in the tangent space to handle such nonlinearity, leading to accumulated estimation errors. To address this limitation, we reformulate manifold filtering as a parameter optimization problem over a Gaussian-distributed increment variable, thereby avoiding linearization. Under this formulation, the increment can be mapped to the Lie group through the exponential operator, where it acts multiplicatively on the prior estimate to yield the posterior state. We further propose a natural gradient optimization scheme for solving this problem, whose iteration process leverages the Fisher information matrix of the increment variable to account for the curvature of the tangent space. This results in an iterative algorithm named the Natural Gradient Gaussian Approximation on Lie Groups (NANO-L) filter. Leveraging the perturbation model in Lie derivative, we prove that for the invariant observation model widely adopted in robotic localization tasks, the covariance update in NANO-L admits an exact closed-form solution, eliminating the need for iterative updates thus improving computational efficiency. Hardware experiments on a Unitree GO2 legged robot operating across different terrains demonstrate that NANO-L achieves approximately 40% lower estimation error than commonly used filters at a comparable computational cost.

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

2 major / 2 minor

Summary. The paper reformulates state estimation on Lie groups as optimization over a Gaussian-distributed increment variable (mapped to the manifold via the exponential map), proposes a natural-gradient scheme that incorporates the Fisher information matrix to handle tangent-space curvature, derives an exact closed-form covariance update for invariant observation models using Lie-derivative perturbations, and reports hardware results on a Unitree GO2 legged robot showing approximately 40% lower estimation error than standard filters at comparable computational cost.

Significance. If the closed-form derivation and the validity of the Gaussian-increment approximation hold under the robot's dynamics, the approach offers a principled way to reduce linearization-induced errors in manifold filtering while retaining efficiency through the closed-form update. The hardware validation on varied terrains provides practical evidence of utility for legged-robot state estimation, where accurate pose and velocity estimates are load-bearing for agile control.

major comments (2)
  1. [derivation of closed-form covariance update] The closed-form covariance update (derived via the Lie-derivative perturbation model for invariant observations) is proven only under the invariance assumption; the manuscript does not verify whether the IMU, encoder, and foot-contact observation models actually employed on the Unitree GO2 satisfy this condition exactly. Any deviation would invalidate both the closed-form result and the claimed computational advantage, directly affecting the central performance claim.
  2. [hardware experiments on Unitree GO2] The 40% error-reduction result in the hardware experiments rests on the assumption that the Gaussian approximation on the tangent-space increment variable remains accurate after propagation through the nonlinear SE(3) or SO(3)×R³ dynamics (including contact forces and terrain interactions). No supporting analysis (e.g., Monte-Carlo sampling of the true posterior or comparison against particle-filter baselines) is provided to quantify approximation error over the filter horizon.
minor comments (2)
  1. [method formulation] The objective function for the parameter optimization over the increment variable should be stated explicitly with all terms (prior, likelihood, and any regularization) in equation form early in the method section.
  2. [preliminaries] Notation for the exponential map, adjoint operators, and Fisher information matrix should include a brief reference to standard Lie-group references (e.g., Chirikjian or Barfoot) to aid readers unfamiliar with the conventions.

Simulated Author's Rebuttal

2 responses · 0 unresolved

We thank the referee for the constructive feedback and positive assessment of the paper's contributions. We address each major comment below with clarifications and proposed revisions.

read point-by-point responses

  1. Referee: [derivation of closed-form covariance update] The closed-form covariance update (derived via the Lie-derivative perturbation model for invariant observations) is proven only under the invariance assumption; the manuscript does not verify whether the IMU, encoder, and foot-contact observation models actually employed on the Unitree GO2 satisfy this condition exactly. Any deviation would invalidate both the closed-form result and the claimed computational advantage, directly affecting the central performance claim.

    Authors: We appreciate this observation. The IMU, encoder, and foot-contact models used in the Unitree GO2 experiments are standard invariant observation models under the SE(3) group action, as established in the invariant filtering literature for legged robots. To directly address the concern, we will add a new subsection that explicitly verifies the invariance property for each model employed, confirming that the closed-form covariance update holds exactly and that the computational advantage is preserved. revision: yes

  2. Referee: [hardware experiments on Unitree GO2] The 40% error-reduction result in the hardware experiments rests on the assumption that the Gaussian approximation on the tangent-space increment variable remains accurate after propagation through the nonlinear SE(3) or SO(3)×R³ dynamics (including contact forces and terrain interactions). No supporting analysis (e.g., Monte-Carlo sampling of the true posterior or comparison against particle-filter baselines) is provided to quantify approximation error over the filter horizon.

    Authors: We acknowledge that Monte-Carlo sampling of the true posterior or particle-filter comparisons would offer stronger quantification of the Gaussian approximation error. Such analysis is computationally prohibitive for real hardware data involving contact forces and terrain interactions. The reported 40% error reduction is empirical evidence from extensive hardware trials across varied terrains. In revision, we will expand the discussion to analyze the approximation's practical validity based on the experimental outcomes, explicitly note this as a limitation, and suggest directions for future theoretical validation. revision: partial

Circularity Check

0 steps flagged

No circularity: derivation follows from Lie-group perturbation model and invariant-observation assumption

full rationale

The paper reformulates state estimation as optimization of a Gaussian increment variable mapped via the exponential map, applies natural-gradient descent with the Fisher information matrix, and derives an exact closed-form covariance update by invoking the Lie-derivative perturbation model specifically for invariant observation models. This closed-form step is a direct algebraic consequence of the stated invariance assumption and the chosen perturbation representation; it does not reduce to a fitted parameter renamed as a prediction, nor does it rely on self-citations, uniqueness theorems imported from prior author work, or ansatzes smuggled via citation. Hardware performance figures are empirical outcomes, not theoretical predictions forced by the derivation itself. The chain therefore remains self-contained against external benchmarks.

Axiom & Free-Parameter Ledger

0 free parameters · 2 axioms · 0 invented entities

Abstract-only review limits visibility into parameters or entities; the reformulation assumes a Gaussian increment and relies on the invariant observation model for the closed-form result.

axioms (2)
  • domain assumption The increment variable follows a Gaussian distribution on the tangent space
    Central to reformulating the filtering problem as parameter optimization.
  • domain assumption Observation model is invariant, allowing Lie derivative perturbation analysis
    Enables the exact closed-form covariance update without iteration.

pith-pipeline@v0.9.0 · 5549 in / 1272 out tokens · 34348 ms · 2026-05-10T16:24:50.258425+00:00 · methodology

discussion (0)

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

Forward citations

Cited by 1 Pith paper

Reviewed papers in the Pith corpus that reference this work. Sorted by Pith novelty score.

  1. Natural Gradient Bayesian Filtering: Geometry-Aware Filter for Dynamical Systems

    cs.RO 2026-05 unverdicted novelty 5.0

    The NANO filter uses natural gradient descent to iteratively refine Gaussian state estimates while preserving covariance positive definiteness and exactly recovering the Kalman update in the linear-Gaussian case.

Reference graph

Works this paper leans on

32 extracted references · 32 canonical work pages · cited by 1 Pith paper

  1. [1]

    Contact- aided invariant extended Kalman filtering for robot state estimation,

    R. Hartley, M. Ghaffari, R. M. Eustice, and J. W. Grizzle, “Contact- aided invariant extended Kalman filtering for robot state estimation,”The International Journal of Robotics Research, vol. 39, no. 4, pp. 402–430, 2020

    work page 2020

  2. [2]

    Pose estimation for ground robots: On manifold representation, integration, reparameteriza- tion, and optimization,

    M. Zhang, X. Zuo, Y . Chen, Y . Liu, and M. Li, “Pose estimation for ground robots: On manifold representation, integration, reparameteriza- tion, and optimization,”IEEE Transactions on Robotics, vol. 37, no. 4, pp. 1081–1099, 2021

    work page 2021

  3. [3]

    T. D. Barfoot,State estimation for robotics. Cambridge University Press, 2024

    work page 2024

  4. [4]

    State estimation for legged robots: Consistent fusion of leg kinematics and IMU,

    M. Bloesch, M. Hutter, M. A. Hoepflinger, S. Leutenegger, C. Gehring, C. D. Remy, and R. Siegwart, “State estimation for legged robots: Consistent fusion of leg kinematics and IMU,”Robotics: Science and Systems VIII, p. 17, 2013

    work page 2013

  5. [5]

    Fully proprioceptive slip- velocity-aware state estimation for mobile robots via invariant kalman filtering and disturbance observer,

    X. Yu, S. Teng, T. Chakhachiro, W. Tong, T. Li, T.-Y . Lin, S. Koehler, M. Ahumada, J. M. Walls, and M. Ghaffari, “Fully proprioceptive slip- velocity-aware state estimation for mobile robots via invariant kalman filtering and disturbance observer,” in2023 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2023, pp. 8096–8103

    work page 2023

  6. [6]

    State estimation for legged robots on unstable and slippery terrain,

    M. Bloesch, C. Gehring, P. Fankhauser, M. Hutter, M. A. Hoepflinger, and R. Siegwart, “State estimation for legged robots on unstable and slippery terrain,” in2013 IEEE/RSJ International Conference on Intel- ligent Robots and Systems. IEEE, 2013, pp. 6058–6064. 12

    work page 2013

  7. [7]

    The invariant extended Kalman filter as a stable observer,

    A. Barrau and S. Bonnabel, “The invariant extended Kalman filter as a stable observer,”IEEE Transactions on Automatic Control, vol. 62, no. 4, pp. 1797–1812, 2016

    work page 2016

  8. [8]

    A code for unscented Kalman filtering on manifolds (UKF-M),

    M. Brossard, A. Barrau, and S. Bonnabel, “A code for unscented Kalman filtering on manifolds (UKF-M),” in2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020, pp. 5701–5708

    work page 2020

  9. [9]

    Convergence and consistency analysis for a 3-D invariant-EKF slam,

    T. Zhang, K. Wu, J. Song, S. Huang, and G. Dissanayake, “Convergence and consistency analysis for a 3-D invariant-EKF slam,”IEEE Robotics and Automation Letters, vol. 2, no. 2, pp. 733–740, 2017

    work page 2017

  10. [10]

    Design and evaluation of an invariant extended Kalman filter for trunk motion estimation with sensor misalignment,

    Z. Zhu, S. M. R. Sorkhabadi, Y . Gu, and W. Zhang, “Design and evaluation of an invariant extended Kalman filter for trunk motion estimation with sensor misalignment,”IEEE/ASME Transactions on Mechatronics, vol. 27, no. 4, pp. 2158–2167, 2022

    work page 2022

  11. [11]

    Attitude error representations for Kalman filtering,

    F. L. Markley, “Attitude error representations for Kalman filtering,” Journal of guidance, control, and dynamics, vol. 26, no. 2, pp. 311– 317, 2003

    work page 2003

  12. [12]

    Associating uncertainty with three- dimensional poses for use in estimation problems,

    T. D. Barfoot and P. T. Furgale, “Associating uncertainty with three- dimensional poses for use in estimation problems,”IEEE Transactions on Robotics, vol. 30, no. 3, pp. 679–693, 2014

    work page 2014

  13. [13]

    Discrete extended Kalman filter on lie groups,

    G. Bourmaud, R. M ´egret, A. Giremus, and Y . Berthoumieu, “Discrete extended Kalman filter on lie groups,” in21st European Signal Process- ing Conference (EUSIPCO 2013). IEEE, 2013, pp. 1–5

    work page 2013

  14. [14]

    Continuous- discrete extended Kalman filter on matrix lie groups using concentrated gaussian distributions,

    G. Bourmaud, R. M ´egret, M. Arnaudon, and A. Giremus, “Continuous- discrete extended Kalman filter on matrix lie groups using concentrated gaussian distributions,”Journal of Mathematical Imaging and Vision, vol. 51, pp. 209–228, 2015

    work page 2015

  15. [15]

    Unscented filtering and nonlinear estimation,

    S. J. Julier and J. K. Uhlmann, “Unscented filtering and nonlinear estimation,”Proceedings of the IEEE, vol. 92, no. 3, pp. 401–422, 2004

    work page 2004

  16. [16]

    Nonlin- ear bayesian filtering with natural gradient gaussian approximation,

    W. Cao, T. Zhang, Z. Sun, C. Liu, S. S.-T. Yau, and S. E. Li, “Nonlinear Bayesian filtering with natural gradient gaussian approximation,”arXiv preprint arXiv:2410.15832, 2024

    work page arXiv 2024

  17. [17]

    Unscented Kalman filtering on lie groups,

    M. Brossard, S. Bonnabel, and J.-P. Condomines, “Unscented Kalman filtering on lie groups,” in2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2017, pp. 2485–2491

    work page 2017

  18. [18]

    Lie algebraic unscented Kalman filter for pose estimation,

    A. M. Sjøberg and O. Egeland, “Lie algebraic unscented Kalman filter for pose estimation,”IEEE Transactions on Automatic Control, vol. 67, no. 8, pp. 4300–4307, 2021

    work page 2021

  19. [19]

    A nonlinear filter for pose estimation based on fast unscented transform on lie groups,

    Y . Jin, W.-A. Zhang, J. Tang, H. Sun, and L. Shi, “A nonlinear filter for pose estimation based on fast unscented transform on lie groups,”IEEE Robotics and Automation Letters, 2024

    work page 2024

  20. [20]

    Invariant filtering for legged humanoid locomotion on a dynamic rigid surface,

    Y . Gao, C. Yuan, and Y . Gu, “Invariant filtering for legged humanoid locomotion on a dynamic rigid surface,”IEEE/ASME Transactions on Mechatronics, vol. 27, no. 4, pp. 1900–1909, 2022

    work page 1900

  21. [21]

    Multi-IMU propri- oceptive odometry for legged robots,

    S. Yang, Z. Zhang, B. Bokser, and Z. Manchester, “Multi-IMU propri- oceptive odometry for legged robots,” in2023 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2023, pp. 774–779

    work page 2023

  22. [22]

    Fast-lio: A fast, robust lidar-inertial odometry package by tightly-coupled iterated Kalman filter,

    W. Xu and F. Zhang, “Fast-lio: A fast, robust lidar-inertial odometry package by tightly-coupled iterated Kalman filter,”IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 3317–3324, 2021

    work page 2021

  23. [23]

    Iterated in- variant extended Kalman filter (iiekf),

    S. Goffin, A. Barrau, S. Bonnabel, O. Bruls, and P. Sacr ´e, “Iterated in- variant extended Kalman filter (iiekf),”arXiv preprint arXiv:2404.10665, 2024

    work page arXiv 2024

  24. [24]

    An optimization-centric view on Bayes’ rule: Reviewing and generalizing variational inference,

    J. Knoblauch, J. Jewson, and T. Damoulas, “An optimization-centric view on Bayes’ rule: Reviewing and generalizing variational inference,” Journal of Machine Learning Research, vol. 23, no. 132, pp. 1–109, 2022

    work page 2022

  25. [25]

    Understanding stochastic natural gradient variational inference,

    K. Wu and J. R. Gardner, “Understanding stochastic natural gradient variational inference,” inForty-first International Conference on Ma- chine Learning, 2024

    work page 2024

  26. [26]

    Natural gradient works efficiently in learning,

    S.-I. Amari, “Natural gradient works efficiently in learning,”Neural computation, vol. 10, no. 2, pp. 251–276, 1998

    work page 1998

  27. [27]

    New insights and perspectives on the natural gradient method,

    J. Martens, “New insights and perspectives on the natural gradient method,”Journal of Machine Learning Research, vol. 21, no. 146, pp. 1–76, 2020

    work page 2020

  28. [28]

    Exactly sparse gaussian variational inference with application to derivative-free batch nonlin- ear state estimation,

    T. D. Barfoot, J. R. Forbes, and D. J. Yoon, “Exactly sparse gaussian variational inference with application to derivative-free batch nonlin- ear state estimation,”The International Journal of Robotics Research, vol. 39, no. 13, pp. 1473–1502, 2020

    work page 2020

  29. [29]

    Cubature Kalman filters,

    I. Arasaratnam and S. Haykin, “Cubature Kalman filters,”IEEE Trans- actions on automatic control, vol. 54, no. 6, pp. 1254–1269, 2009

    work page 2009

  30. [30]

    Leg state estimation for quadruped robot by using probabilistic model with proprioceptive feedback,

    J. Sun, L. Zhou, B. Geng, Y . Zhang, and Y . Li, “Leg state estimation for quadruped robot by using probabilistic model with proprioceptive feedback,”IEEE/ASME Transactions on Mechatronics, 2024

    work page 2024

  31. [31]

    A tutorial on quantitative trajectory eval- uation for visual (-inertial) odometry,

    Z. Zhang and D. Scaramuzza, “A tutorial on quantitative trajectory eval- uation for visual (-inertial) odometry,” in2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018, pp. 7244–7251

    work page 2018

  32. [32]

    Invariant smoother for legged robot state estimation with dynamic contact event information,

    Z. Yoon, J.-H. Kim, and H.-W. Park, “Invariant smoother for legged robot state estimation with dynamic contact event information,”IEEE Transactions on Robotics, vol. 40, pp. 193–212, 2023

    work page 2023