pith. sign in

arxiv: 2607.00026 · v1 · pith:4VMY33X2new · submitted 2026-06-21 · 💻 cs.RO

Invariant Stochastic Filtering on SE(3) for Inertial-Encoder State Estimation of Serial Rigid Manipulators

S. Yaqubi , J. Mattila This is my paper

Pith reviewed 2026-07-02 21:46 UTC · model grok-4.3

classification 💻 cs.RO
keywords invariant extended Kalman filterSE(3)state estimationserial manipulatorsinertial sensorsgroup-affine systemsLie group filtering
0
0 comments X

The pith

An invariant extended Kalman filter on SE(3) gives serial manipulators autonomous error dynamics and modular mean-square stability.

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

The paper develops an invariant extended Kalman filter formulated directly on the Lie group SE(3) for estimating the state of serial rigid manipulators with any number of links. Because the kinematic equations are group-affine, the linearised error dynamics do not depend on the current state, so the Riccati equation tracks the true error covariance rather than an approximation. A separated noise model handles gyroscope and accelerometer data independently, and the filter is built as a chain of per-link estimators whose covariances are linked only through the Adjoint operator. Exponential ultimate boundedness in mean square is shown using a Lie-algebra Lyapunov function whose bounds chain across links via the Adjoint norm, yielding a stability result that scales with chain length.

Core claim

The group-affine property of the manipulator kinematics on SE(3) renders the linearised error dynamics autonomous, allowing the Riccati equation to govern the true covariance; the resulting modular IEKF chain admits an exponential mean-square stability certificate obtained by chaining per-link Lyapunov bounds through the Adjoint operator norm.

What carries the argument

The group-affine property of the kinematic equations on SE(3), which makes the error dynamics state-independent and permits exact covariance propagation via the Riccati equation.

If this is right

  • The computational cost grows linearly with the number of links.
  • The stability certificate remains valid for manipulators of arbitrary length.
  • The accelerometer measurement covariance scales exactly with the sample interval in the same manner as the discretised process noise.
  • The gyroscope noise contribution to the dynamics vanishes at rest and increases with twist magnitude.

Where Pith is reading between the lines

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

  • The same group-affine structure could be exploited for other rigid-body systems whose kinematics live on SE(3).
  • If the group-affine property holds only approximately, the autonomy of the error dynamics would degrade gracefully rather than collapse.
  • Real-time implementation on embedded hardware becomes feasible for long chains because each link's update depends on its predecessor only through a fixed matrix transformation.

Load-bearing premise

The kinematic equations of the serial rigid manipulator are group-affine when expressed on SE(3).

What would settle it

A numerical experiment in which the empirical error covariance of the filter diverges from the Riccati-predicted covariance for a long manipulator chain would falsify the autonomy claim.

Figures

Figures reproduced from arXiv: 2607.00026 by J. Mattila, S. Yaqubi.

Figure 1
Figure 1. Figure 1: Body-frame twists — true (black) and IEKF (blue dashed) with 2 [PITH_FULL_IMAGE:figures/full_fig_p015_1.png] view at source ↗
Figure 2
Figure 2. Figure 2: Absolute joint-angle estimation error | ˆθi − θi | [deg], scenario S1 — no filter (grey), CEKF (orange dashed), IEKF (blue dash-dotted): (a) θ1y, (b) θ1z, (c) θ2. The IEKF achieves the lowest error on all channels; the advantage is largest on θ2 where upstream Adjoint covariance propagation and IMU fusion contribute information unavailable to the encoder-only CEKF. (d) End-effector trajectory in steady sta… view at source ↗
Figure 3
Figure 3. Figure 3: RMSE [deg] for S1 and S2 — no filter (grey), CEKF (orange), IEKF [PITH_FULL_IMAGE:figures/full_fig_p017_3.png] view at source ↗
Figure 4
Figure 4. Figure 4: NEES for Link 2, scenario S1. Solid: ideal ( [PITH_FULL_IMAGE:figures/full_fig_p018_4.png] view at source ↗
read the original abstract

An invariant extended Kalman filter (IEKF) is developed for state estimation of serial rigid manipulators with an arbitrary number of links, formulated entirely within the Lie group SE(3). The group-affine property of the kinematic equations makes the linearised error dynamics autonomous, so the Riccati equation governs the true error covariance rather than a local approximation. A physically separated noise model treats gyroscope and accelerometer channels independently: the accelerometer provides translational twist via gravity-compensated integration, yielding a measurement covariance that scales with the sample interval in exact analogy with process noise discretisation; a state-dependent Coriolis noise term captures gyroscope noise propagating through the nonlinear dynamics, vanishing at rest and growing with twist magnitude. The filter is structured as a modular chain of per-link IEKFs in which the predicted covariance of each link depends on its predecessor only through the Adjoint-transformed posterior, giving linear computational cost in link count. Exponential ultimate boundedness in mean square is established via a Lie algebra Lyapunov function, with per-link bounds chained through the Adjoint operator norm to yield a stability certificate that is modular and scalable to arbitrary chain length. Numerical results validate the design.

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

1 major / 1 minor

Summary. The paper develops an invariant extended Kalman filter (IEKF) for inertial-encoder state estimation of serial rigid manipulators with arbitrary link count, formulated on the Lie group SE(3). It asserts that the kinematic equations possess the group-affine property, rendering the linearized error dynamics autonomous so that the Riccati equation tracks the true covariance rather than a local approximation. A separated noise model is introduced (accelerometer-derived translational twist with interval-scaled covariance; state-dependent Coriolis term for gyroscope propagation). The filter is realized as a modular chain of per-link IEKFs whose covariances propagate via Adjoint-transformed posteriors, yielding linear complexity. Exponential ultimate boundedness in mean square is proven via a Lie-algebra Lyapunov function whose per-link bounds are chained by Adjoint operator norms. Numerical results are cited for validation.

Significance. If the group-affine property is shown to hold exactly for the composite chain kinematics (including the adjoint coupling and Coriolis noise), the work would deliver a scalable, modular IEKF with explicit mean-square stability certificates and linear cost, which is a meaningful advance for Lie-group filtering in robotics. The modular chaining via Adjoint norms and the physically motivated noise separation are concrete strengths that could be adopted in manipulator applications.

major comments (1)
  1. [Abstract (group-affine property paragraph)] Abstract (group-affine property paragraph) and the kinematic model section: the claim that the full kinematic equations (accelerometer integration, gyroscope propagation, and state-dependent Coriolis noise) remain group-affine on the product of SE(3) copies is load-bearing for autonomy of the error dynamics and for the Riccati equation governing true covariance. For a serial chain each link velocity depends on the preceding pose through the adjoint, and the Coriolis term is explicitly state-dependent; an explicit derivation confirming that the composite vector field is still group-affine (i.e., the error dynamics are state-independent) is required, otherwise the autonomy and the subsequent Lyapunov chaining argument do not apply to the implemented filter.
minor comments (1)
  1. The abstract states that 'numerical results validate the design' but supplies no information on the manipulator configuration, sensor noise levels, comparison filters, or quantitative metrics; adding these details in the results section would improve reproducibility and clarity.

Simulated Author's Rebuttal

1 responses · 0 unresolved

We thank the referee for the careful and constructive review. The single major comment is addressed below; we will revise the manuscript to incorporate the requested explicit derivation.

read point-by-point responses

  1. Referee: [Abstract (group-affine property paragraph)] Abstract (group-affine property paragraph) and the kinematic model section: the claim that the full kinematic equations (accelerometer integration, gyroscope propagation, and state-dependent Coriolis noise) remain group-affine on the product of SE(3) copies is load-bearing for autonomy of the error dynamics and for the Riccati equation governing true covariance. For a serial chain each link velocity depends on the preceding pose through the adjoint, and the Coriolis term is explicitly state-dependent; an explicit derivation confirming that the composite vector field is still group-affine (i.e., the error dynamics are state-independent) is required, otherwise the autonomy and the subsequent Lyapunov chaining argument do not apply to the implemented filter.

    Authors: We agree that an explicit derivation of the group-affine property for the composite kinematics on SE(3)^n is necessary. In the revised manuscript we will add a dedicated subsection in the kinematic model section that derives the vector field step by step. The deterministic kinematics are shown to satisfy the group-affine condition on the product Lie group, with adjoint couplings between consecutive links absorbed into the group action; the state-dependent Coriolis contribution is treated exclusively as multiplicative process noise and does not enter the deterministic vector field. This structure keeps the linearized error dynamics autonomous, so the Riccati equation tracks the true covariance and the subsequent Lyapunov chaining argument continues to apply. revision: yes

Circularity Check

0 steps flagged

No circularity: derivation rests on stated group-affine kinematics and independent Lyapunov analysis.

full rationale

The paper states the group-affine property of the kinematic equations as an input property of serial rigid manipulators on SE(3), then derives autonomous error dynamics and Riccati covariance from it. Stability follows from a Lie-algebra Lyapunov function with Adjoint-norm chaining; neither step is defined in terms of the output filter nor obtained by fitting parameters to the target covariance. No self-citation chain or ansatz smuggling appears in the abstract or described derivation. The central claims therefore remain independent of the results they produce.

Axiom & Free-Parameter Ledger

0 free parameters · 2 axioms · 0 invented entities

The central claims rest on the group-affine property of manipulator kinematics and the validity of the proposed physically separated noise models; these are standard domain assumptions in Lie-group robotics but unverified here.

axioms (2)
  • domain assumption The kinematic equations of the serial rigid manipulator are group-affine on SE(3).
    Invoked to make linearised error dynamics autonomous (abstract).
  • domain assumption Gyroscope and accelerometer noise can be treated as physically separated channels with the stated covariance scaling and state-dependent Coriolis term.
    Used to derive measurement and process noise models (abstract).

pith-pipeline@v0.9.1-grok · 5740 in / 1488 out tokens · 27838 ms · 2026-07-02T21:46:53.083402+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

28 extracted references · 27 canonical work pages

  1. [1]

    Wiley-Interscience

    Estimation with Applications to Tracking and Navigation: Theory, Algorithms and Software. Wiley-Interscience. doi:10.1002/0471221279. Barrau, A., Bonnabel, S.,

    work page doi:10.1002/0471221279

  2. [2]

    IEEE Transactions on Automatic Control 62, 1797–1812

    The invariant extended Kalman filter as a stable observer. IEEE Transactions on Automatic Control 62, 1797–1812. doi:10.1109/ TAC.2016.2594085. Barrau, A., Bonnabel, S.,

    work page arXiv 2016

  3. [3]

    Journal of Mathematical Imaging and Vision 13, 205–228

    The motor extended kalman filter: A geometric approach for rigid motion estimation. Journal of Mathematical Imaging and Vision 13, 205–228. doi:10.1023/A:1011293515286. Bhat, S.P., Bernstein, D.S.,

    work page doi:10.1023/a:1011293515286

  4. [4]

    Systems & Control Letters 39, 63–70

    A topological obstruction to continuous global stabilization of rotational motion and the unwinding phenomenon. Systems & Control Letters 39, 63–70. doi:10.1016/S0167-6911(99)00090-0. Chirikjian, G.S.,

    work page doi:10.1016/s0167-6911(99)00090-0

  5. [5]

    IEEE Transactions on Robotics 33, 1–21

    On-manifold preinte- gration for real-time visual-inertial odometry. IEEE Transactions on Robotics 33, 1–21. doi:10.1109/TRO.2016.2597321. Ge, Y., Delama, G., Scheiber, M., Fornasier, A., van Goor, P., Weiss, S., Mahony, R.,

    work page doi:10.1109/tro.2016.2597321 2016

  6. [6]

    Hall, B.C.,

    doi:10.1016/j.conengprac.2025.106656. Hall, B.C.,

    work page doi:10.1016/j.conengprac.2025.106656 2025

  7. [7]

    2nd ed., Springer

    Lie Groups, Lie Algebras, and Representations: An Elementary Introduction. 2nd ed., Springer. doi:10.1007/978-3-319-13467-3. Hartley, R., Ghaffari, M., Eustice, R.M., Grizzle, J.W.,

    work page doi:10.1007/978-3-319-13467-3

  8. [8]

    The International Journal of Robotics Research 39, 402–430

    Contact-aided in- variant extended Kalman filtering for robot state estimation. The International Journal of Robotics Research 39, 402–430. doi:10.1177/0278364919894385. Heo, S., Park, C.,

    work page doi:10.1177/0278364919894385

  9. [9]

    IEEE Sensors Journal 18, 3780–3788

    Consistent EKF-based visual-inertial odometry on ma- trix Lie group. IEEE Sensors Journal 18, 3780–3788. doi:10.1109/JSEN.2018. 2808330. Khalifa, M., Hashim, H.A.,

    work page doi:10.1109/jsen.2018 2018

  10. [10]

    Kumar, H., Parada-Mayorga, A., Ribeiro, A.,

    doi:10.1016/j.measurement.2026.121964. Kumar, H., Parada-Mayorga, A., Ribeiro, A.,

    work page doi:10.1016/j.measurement.2026.121964 2026

  11. [11]

    IEEE Transactions on Signal Processing 72, 2842–2857

    Lie group algebra convolutional filters. IEEE Transactions on Signal Processing 72, 2842–2857. doi:10.1109/TSP. 2024.3365950. Li, T., Jiao, W.,

    work page doi:10.1109/tsp 2024

  12. [12]

    Geometric algebra based kalman filter trajectory prediction, in: 2024 3rd International Symposium on Aerospace Engineering and Systems, ISAES 2024, Institute of Electrical and Electronics Engineers Inc.. pp. 103–108. doi:10.1109/ISAES61964.2024.10751074. Li, T., Wang, J.,

    work page doi:10.1109/isaes61964.2024.10751074 2024

  13. [13]

    Lu, W., Luo, Y., Guo, C., Jiang, W.,

    doi:10.1016/j.automatica.2024.111995. Lu, W., Luo, Y., Guo, C., Jiang, W.,

    work page doi:10.1016/j.automatica.2024.111995 2024

  14. [14]

    Lynch, K.M., Park, F.C.,

    doi:10.1109/TIM.2025.3565028. Lynch, K.M., Park, F.C.,

    work page doi:10.1109/tim.2025.3565028 2025

  15. [15]

    Cambridge University Press

    Modern Robotics: Mechanics, Planning, and Con- trol. Cambridge University Press. doi:10.1017/9781316661239. Mitikiri, Y.B., Mohseni, K.,

    work page doi:10.1017/9781316661239

  16. [16]

    Murray, R.M., Li, Z., Sastry, S.S.,

    doi:10.1016/j.automatica.2021.109494. Murray, R.M., Li, Z., Sastry, S.S.,

    work page doi:10.1016/j.automatica.2021.109494 2021

  17. [17]

    CRC Press

    A Mathematical Introduction to Robotic Manipulation. CRC Press. doi:10.1201/9781315136370. 19 M¨ uller, A.,

    work page doi:10.1201/9781315136370

  18. [18]

    Multibody System Dynamics 43, 37–70

    Screw and lie group theory in multibody kinematics. Multibody System Dynamics 43, 37–70. doi:10.1007/s11044-017-9582-7. Øksendal, B.,

    work page doi:10.1007/s11044-017-9582-7

  19. [19]

    6th ed., Springer

    Stochastic Differential Equations: An Introduction with Appli- cations. 6th ed., Springer. doi:10.1007/978-3-642-14394-6. Park, W., Liu, Y., Zhou, Y., Moses, M., Chirikjian, G.S.,

    work page doi:10.1007/978-3-642-14394-6

  20. [20]

    Robotica 26, 419–434

    Kinematic state estimation and motion planning for stochastic nonholonomic systems using the exponential map. Robotica 26, 419–434. doi:10.1017/S0263574708004475. Phogat, K.S., Chang, D.E.,

    work page doi:10.1017/s0263574708004475

  21. [21]

    Reif, K., G¨ unther, S., Yaz, E., Unbehauen, R.,

    doi:10.1016/j.automatica.2020.108812. Reif, K., G¨ unther, S., Yaz, E., Unbehauen, R.,

    work page doi:10.1016/j.automatica.2020.108812 2020

  22. [22]

    IEEE Transactions on Automatic Control 44, 714–728

    Stochastic stability of the discrete-time extended Kalman filter. IEEE Transactions on Automatic Control 44, 714–728. doi:10.1109/9.754809. Rigo, D., Sansonetto, N., Muradore, R.,

    work page doi:10.1109/9.754809

  23. [23]

    Thrun, S., Burgard, W., Fox, D.,

    doi:10.1016/ j.sysconle.2023.105568. Thrun, S., Burgard, W., Fox, D.,

    work page arXiv 2023

  24. [24]

    IEEE Transactions on Automatic Control 23, 395–404

    Computing integrals involving the matrix exponential. IEEE Transactions on Automatic Control 23, 395–404. doi:10.1109/TAC.1978. 1101743. Welch, G., Bishop, G.,

    work page doi:10.1109/tac.1978 1978

  25. [25]

    Geometrically-based adaptive kalman filtering for attitude estimation, in: 2025 5th International Conference on Sensors and Information Technology, ICSI 2025, Institute of Electrical and Electronics Engineers Inc.. pp. 206–210. doi:10.1109/ICSI64877.2025.11009328. Yue, C., Liu, J., Guan, L.,

    work page doi:10.1109/icsi64877.2025.11009328 2025

  26. [26]

    Zhang, C., Qin, J., Yan, C., Shi, Y., Wang, Y., Li, M.,

    doi:10.1016/j.mechmachtheory.2025.106297. Zhang, C., Qin, J., Yan, C., Shi, Y., Wang, Y., Li, M.,

    work page doi:10.1016/j.mechmachtheory.2025.106297 2025

  27. [27]

    doi:10.1016/j.automatica.2023.111408. A Proof of the Group-Affine Property Proposition 2.The pose kinematics˙g i =g iVi satisfy the group-affine property: the left-invariant error˜gi = ˆg−1 i gi evolves autonomously, independent ofˆg i. 20 Proof.Differentiating ˜gi = ˆg−1 i gi and substituting ˙ˆgi = ˆgi ˆVi, ˙gi =g iVi: ˙˜gi =−ˆg−1 i ˙ˆgiˆg−1 i gi + ˆg−1...

    work page doi:10.1016/j.automatica.2023.111408 2023

  28. [28]

    Atθ i →πthe skew-part formula is singular; the axis must be recovered from the symmetric part of ˜Ri as the eigenvector for eigenvalue +1. 21 SE(3)logarithm.The translation component requires the left Jacobian in- verse (Chirikjian, 2012): J−1 l (ϕ) = θ/2 tan(θ/2)I3 + 1− θ/2 tan(θ/2) ϕϕ⊤ θ2 − 1 2[ϕ]×.(68) The complete six-dimensional pose error vector is:...

    2012