aiqbench analysis
This research advances state estimation for articulated robotic systems by adapting invariant Kalman filtering to handle joint kinematic constraints within multi-IMU setups. By formulating dynamics on Lie groups relative to the robot's kinematic tree, the proposed method ensures convergence guarantees and significantly reduces position/velocity error compared to standard extended filters. The approach demonstrates practical utility in both industrial robotics (UR5e) and biomechanics applications through rigorous validation showing up to 50% lower root-mean-square error.
Invariant Kalman filtering for extended pose estimation in multi-IMU articulated rigid-body systems
arXiv:2606.25083v2 Announce Type: replace Abstract: Accurate extended pose estimation (orientation, velocity, and position) for IMU-instrumented articulated rigid-body systems is a key challenge in robotics and human motion analysis. The invariant extended Kalman filter (IEKF) addresses this problem for a single rigid body with convergence guarantees and consistency under unobservability, but extending these properties to articulated systems is nontrivial: inter-body pose coupling prevents a direct application, and incorporating joint kinematic constraints within the invariant framework remains an open problem. To address this gap, we introduce the relative L-extended pose, a Lie group representation for kinematic-tree systems based on relative poses between bodies. With one IMU per body, it yields group-affine dynamics and allows joint constraints to be expressed in invariant form. We incorporate these constraints as noise-free pseudo-measurements within an iterated IEKF (IterIEKF), thereby preserving the convergence and consistency guarantees of invariant filtering. The absolute pose of each body is then recovered by chaining the relative blocks along the kinematic tree. Validated on both a UR5e robot and a human leg, the proposed IterIEKF outperforms all EKF, IterEKF, and absolute-pose IterIEKF baselines. It converges faster, exhibits lower run-to-run variability, and consistently achieves the lowest RMSE, with reductions of at least 50% compared to the second-best filter across all scenarios considered in this work.
Abstract:Accurate extended pose estimation (orientation, velocity, and position) for IMU-instrumented articulated rigid-body systems is a key challenge in robotics and human motion analysis. The invariant extended Kalman filter (IEKF) addresses this problem for a single rigid body with convergence guarantees and consistency under unobservability, but extending these properties to articulated systems is nontrivial: inter-body pose coupling prevents a direct application, and incorporating joint kinematic constraints within the invariant framework remains an open problem. To address this gap, we introduce the relative L-extended pose, a Lie group representation for kinematic-tree systems based on relative poses between bodies. With one IMU per body, it yields group-affine dynamics and allows joint constraints to be expressed in invariant form. We incorporate these constraints as noise-free pseudo-measurements within an iterated IEKF (IterIEKF), thereby preserving the convergence and consistency guarantees of invariant filtering. The absolute pose of each body is then recovered by chaining the relative blocks along the kinematic tree. Validated on both a UR5e robot and a human leg, the proposed IterIEKF outperforms all EKF, IterEKF, and absolute-pose IterIEKF baselines. It converges faster, exhibits lower run-to-run variability, and consistently achieves the lowest RMSE, with reductions of at least 50% compared to the second-best filter across all scenarios considered in this work.
Robotics (cs.RO)
arXiv:2606.25083 [cs.RO]
(or arXiv:2606.25083v2 [cs.RO] for this version)
https://doi.org/10.48550/arXiv.2606.25083 Focus to learn more arXiv-issued DOI via DataCite
arXivLabs is a framework that allows collaborators to develop and share new arXiv features directly on our website.
Both individuals and organizations that work with arXivLabs have embraced and accepted our values of openness, community, excellence, and user data privacy. arXiv is committed to these values and only works with partners that adhere to them.
Have an idea for a project that will add value for arXiv's community? Learn more about arXivLabs.
Full text under the publisher's open licence · 298 words
Related coverage
Invariant Extended Kalman Filtering with Partial Orientation Measurement Integration: Theoretical Derivations and Application to Autonomous Surface Vessels
arXiv · Jul 30, 2026
Equivariant Filter Design for Acoustic and Depth Aided Inertial Navigation Systems
arXiv · Sep 18, 2026
Deployment-Ready UWB Localization for Industrial Ground Robots with Automatic Anchor Calibration and Terrain-Aware Fusion
arXiv · Aug 4, 2026
Deployment-Ready UWB Localization for Industrial Ground Robots with Automatic Anchor Calibration and Terrain-Aware Fusion
arXiv · Jul 20, 2026