Invariant Kalman filtering for extended pose estimation in multi-IMU articulated rigid-body systems

2026-06-23

AI Summary

This work addresses the challenge of achieving accurate, consistent, and convergent pose (orientation, velocity, and position) estimation in multi-IMU articulated rigid-body systems, where incorporating joint kinematic constraints without compromising the convergence and consistency of invariant filtering remains difficult. The authors propose a Lie group representation of relative L-extended poses, formulate a group-affine dynamic model, and embed kinematic constraints as noise-free pseudo-observations within an iterative invariant extended Kalman filter (IterIEKF). This approach introduces, for the first time in an invariant filtering framework, a Lie group–based state representation and constraint fusion mechanism that preserves both convergence and consistency for articulated systems. Evaluated on a UR5e robotic arm and a human leg, the method demonstrates faster convergence, lower inter-run variability, and at least 50% reduction in RMSE compared to various EKF and IterIEKF baselines, consistently achieving state-of-the-art estimation accuracy.

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

Problem

• extended pose estimation
• multi-IMU articulated systems
• invariant Kalman filtering
• kinematic constraints
• Lie group representation

Innovation

• Invariant Extended Kalman Filter
• Articulated Rigid-Body Systems
• Relative L-Extended Pose
• Group-Affine Dynamics
• Kinematic Constraints