💻 computer science

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

This paper proposes a novel relative L-extended pose representation and an iterated invariant extended Kalman filter (IterIEKF) that effectively incorporates joint kinematic constraints into multi-IMU articulated rigid-body systems, achieving superior convergence, consistency, and accuracy compared to existing baselines.

Original authors: Sven Goffin, Cédric Schwartz, Silvère Bonnabel, Olivier Brüls, Pierre Sacré

Published 2026-07-30

📖 4 min read☕ Coffee break read

CC BY 4.0

Original authors: Sven Goffin, Cédric Schwartz, Silvère Bonnabel, Olivier Brüls, Pierre Sacré

Original paper licensed under CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

Imagine trying to track the movement of a complex machine or a human body using only tiny sensors strapped to the skin or metal. This is the world of inertial navigation, a field where scientists use IMUs (Inertial Measurement Units)—tiny devices packed with accelerometers and gyroscopes—to figure out where something is, how fast it's going, and which way it's facing. Think of an IMU like a blindfolded dancer who can feel every spin and step but has no idea where they are in the room. To find their way, they need a smart "brain" to guess their position based on those feelings.

For single objects, like a drone flying in a straight line, we have excellent math to help this brain guess correctly. But things get messy when we deal with articulated systems—things made of multiple parts connected by joints, like a robot arm with elbows and wrists, or a human leg with a knee and ankle. These parts are linked; if the knee bends, the foot moves, and the whole chain shifts. The challenge is that the math used for single objects often breaks down when you try to apply it to these linked chains, leading to guesses that drift off course or get confused by the connections between the parts. Scientists have been looking for a better way to keep these "blindfolded dancers" in sync without needing external cameras to watch them.

This paper introduces a clever new way to solve that puzzle. The authors, a team of researchers, propose a fresh mathematical "language" called the relative L-extended pose. Instead of trying to track every single body part's position from a fixed starting point (like a global map), they decided to track how each part moves relative to the one right next to it. Imagine a line of people holding hands: instead of each person trying to remember their distance from the starting line, they only need to know how far they are from the person holding their hand. This simple shift in perspective allows the math to handle the "hand-holding" (the joints) perfectly.

By using this relative approach, the team built a new filter called the IterIEKF (Iterated Invariant Extended Kalman Filter). This filter is special because it treats the connections between body parts as "noise-free" rules. In the old methods, the computer had to guess how tight the joints were, often leading to small errors that added up. The new method treats the joints like rigid laws of physics that cannot be broken, forcing the estimate to stay true to the actual shape of the robot or human.

The researchers tested this idea on two very different things: a UR5e robot arm performing pick-and-place tasks and a human leg doing forward lunges. They compared their new method against several older, standard techniques. The results were striking. The new filter didn't just work; it worked much better. In the robot tests, it reduced errors in position by about 50% and errors in speed by nearly 88% compared to the second-best method. For the human leg, the improvement was even more dramatic, with position errors dropping by 85.6%.

Crucially, the new filter didn't just get the right answer; it got there faster and more consistently. While the older filters often stumbled at the start, showing large spikes in error before slowly recovering, the new method converged almost immediately and stayed steady, run after run. The paper shows that by changing how we represent the problem—focusing on the relationships between parts rather than their absolute positions—we can create a much more reliable way to track movement. This isn't just a theoretical win; it means robots could move more precisely and wearable sensors could track human motion with far greater accuracy, all without needing expensive external cameras to watch them.

Technical Summary: Invariant Kalman Filtering for Extended Pose Estimation in Multi-IMU Articulated Rigid-Body Systems

Problem Statement
Accurate extended pose estimation (orientation, velocity, and position) for articulated rigid-body systems instrumented with Inertial Measurement Units (IMUs) is a critical challenge in robotics and human motion analysis. While the Invariant Extended Kalman Filter (IEKF) offers convergence guarantees and consistency under unobservability for single rigid bodies, extending these properties to articulated systems has remained an open problem. Existing approaches face two primary limitations:

Free-segment models: Treating each body as having an independent pose relative to the inertial frame prevents the direct application of invariant filtering because inter-body pose coupling makes it impossible to express kinematic constraints in an invariant form. Enforcing these constraints as noisy pseudo-measurements leads to residual violations.
Kinematic-tree models: While these satisfy constraints by construction, they often rely on parameterizations (e.g., Euler angles) that result in poor dynamic approximations and highly nonlinear measurement models, hindering convergence.

Methodology
The authors propose a reformulation of the extended pose estimation problem using a specific Lie group representation that satisfies the requirements of the invariant filtering framework.

Relative L-Extended Pose: The core methodological contribution is the introduction of the "relative L-extended pose." Instead of representing the state as absolute poses of all bodies, the system state is defined as a block-diagonal matrix in the Lie group $SE^L_2(3)$ . Each diagonal block represents the extended pose of a successor body relative to its predecessor in the kinematic tree, expressed in the predecessor's frame.
Group-Affine Dynamics: The authors demonstrate that this relative representation yields group-affine dynamics. This property is essential for the IEKF, as it ensures the error dynamics are independent of the state estimate, preserving the filter's convergence guarantees.
Invariant Joint Constraints: By utilizing relative poses, kinematic constraints (such as spherical and hinge joints) can be expressed in an invariant form. Specifically, constraints that previously coupled absolute poses (e.g., $p_0^1 + R_0^1 \rho_1 = p_0^2 + R_0^2 \rho_2$ ) are transformed into constraints dependent solely on the relative block $(X_k)_j$ .
Iterated IEKF (IterIEKF): The proposed framework incorporates these invariant constraints as noise-free pseudo-measurements within an iterated IEKF. This allows the filter to enforce kinematic constraints exactly (up to numerical precision) without sacrificing the theoretical guarantees of invariant filtering. The absolute pose of each body is subsequently recovered by chaining the relative blocks along the kinematic tree.

Key Contributions
The paper outlines three primary contributions:

State Representation: The introduction of the relative L-extended pose, a Lie group state representation for multi-IMU articulated systems that admits group-affine dynamics and invariant joint constraints.
Filter Formulation: A specific IterIEKF formulation that integrates spherical and hinge joint constraints as noise-free pseudo-measurements, leveraging the proposed representation.
Experimental Validation: Comprehensive evaluation on two distinct tasks: a UR5e robotic arm performing pick-and-place motions and a human leg performing forward lunges.

Experimental Results
The proposed method was validated against standard EKF, Iterated EKF (IterEKF), and absolute-pose IterIEKF baselines.

UR5e Robot: In 100 trials, the relative-representation IterIEKF converged essentially immediately with negligible run-to-run variability. It achieved the lowest Root Mean Squared Error (RMSE) across all categories compared to baselines. Specifically, it reduced RMSE by 50% for orientation, 87.7% for velocity, and 59.7% for position compared to the second-best filter (relative-representation EKF).
Human Leg: Similarly, in human motion tracking, the relative-representation IterIEKF converged within a few time steps with minimal variance. It outperformed all other filters, achieving RMSE reductions of 60.7% (orientation), 92.4% (velocity), and 85.6% (position) compared to the second-best filter in each category.
General Observations: Across both scenarios, the relative representation proved decisive. Filters using absolute representations (even with invariant structures) exhibited slower convergence, larger transient errors, and higher variability.

Significance and Claims
The paper claims that the choice of state representation is the fundamental factor in successfully extending invariant filtering to articulated systems. By adopting the relative L-extended pose, the authors provide a principled framework that:

Preserves the convergence and consistency guarantees of invariant filtering.
Allows for the exact incorporation of kinematic constraints as noise-free pseudo-measurements.
Consistently outperforms existing EKF and IterEKF approaches in terms of accuracy, convergence speed, and stability.

The authors conclude that this approach establishes a solid foundation for IMU-based pose estimation in articulated systems. They note that while the current work addresses hinge and spherical joints, future work will extend the framework to other joint types (cylindrical, prismatic, planar) and investigate strategies to compensate for soft-tissue artifacts in human motion analysis.

Technical Summary: Invariant Kalman Filtering for Extended Pose Estimation in Multi-IMU Articulated Rigid-Body Systems

More like this