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

Sven Goffin, Cédric Schwartz, Silvère Bonnabel, Olivier Brüls, Pierre Sacré · 2026

Read Invariant Kalman filtering for extended pose estimation in multi-IMU articulated rigid-body systems by Sven Goffin, Cédric Schwartz, Silvère Bonnabel, Olivier Brüls, Pierre Sacré (2026) in the Tower of Babel, a free online library of public-domain works preserved in clean, searchable text. No sign-up. The full text is served as a plain-text file at a predictable URL, where text is present: https://antarctic-labs.com/tower-of-babel/arxiv-2606-25083.txt.

Introduction

Estimating the full pose of linked rigid bodies from inertial sensors is a standing problem in robotics and human motion analysis. Sven Goffin and four colleagues work with one inertial unit per body on a kinematic tree, a chain of connected segments. The invariant extended Kalman filter handles a single body with strong guarantees. Coupling between bodies blocks a direct extension, and joint constraints resist the invariant form.

The team introduces a relative extended pose representation on a Lie group. Its dynamics have the right shape, and joint constraints enter as noise free pseudo measurements inside an iterated invariant filter. Tested on a UR5e robot arm and a human leg, their filter beats every baseline tried. It converges faster, varies less between runs, and cuts the root mean square error by at least half against the second best filter.

Catalog record

CREATORSven Goffin, Cédric Schwartz, Silvère Bonnabel, Olivier Brüls, Pierre Sacré
YEAR2026
COLLECTIONPAPERS
FORMATTXT
FILE SIZE57 KB
LICENSEhttp://creativecommons.org/licenses/by/4.0/
RIGHTSCC BY
SOURCEarXiv

Source: https://arxiv.org/abs/2606.25083

Browse the full library · About the Tower of Babel

Full text

The opening of the work, from the library's hosted text:

Invariant Kalman filtering for extended pose estimation in multi-IMU articulated rigid-body systems Sven Goffin, Cédric Schwartz, Silvère Bonnabel, Olivier Brüls, Pierre Sacré 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 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 % 50\% compared to the second-best filter across all scenarios considered in this work. I Introduction Over the past decade, wearable inertial measurement units (IMUs) have become a method of choice for motion tracking in robotics and human movement analysis [ 11 ] . Unlike vision-based systems, which require bulky external infrastructure, operate within a finite capture volume, and are sensitive to occlusions, IMUs are small and self-contained, enabling large-scale motion capture in constrained or open environments. As a result, IMU-based pose estimation has found broad applications in exoskeletons [ 30 ] , legged robots [ 7 , 26 , 18 ] , and human motion analysis [ 12 , 25 , 1 ] . Stochastic filtering is the standard framework for IMU-based pose estimation, with the extended Kalman filter ( EKF ) [ 16 ] and its variants being the most widely used methods for articulated systems [ 23 ] . These methods, however, do not exploit the Lie group structure of rigid-body motion. Geometric filtering addresses this limitation [ 8 , 19 , 21 , 28 , 20 ] . In particular, the invariant extended Kalman filter ( IEKF ) [ 2 , 3 ] achieves convergence guarantees and consistency under unobservability for single-body extended pose (orientation, velocity, position) estimation [ 3 , 27 , 15 , 24 , 29 , 4 ] . Extending these properties to articulated rigid-body systems remains, however, an open problem: inter-body pose coupling prevents a direct application of the IEKF to such systems, and incorporating kinematic constraints while preserving its convergence and consistency guarantees is nontrivial. In this paper, we reformulate the problem of extended pose estimation for kinematic-tree articulated systems as an invariant filtering problem and we advocate the use of the relative L -extended pose as a Lie group state representation for such systems equipped with L IMUs (Fig. 1 shows L=2 ). This representation yields group-affine dynamics and allows joint constraints to be expressed in invariant form, enabling their incorporation as noise-free pseudo-measurements within an iterated IEKF ( IterIEKF ) [ 13 ] . The result is a principled extension of invariant filtering to articulated rigid-body systems, with the associated convergence and consistency guarantees. The proposed method is experimentally validated on two extended-pose estimation tasks: a UR5e pick-and-place sequence and a human-leg forward-lunge sequence. In both cases, the proposed IterIEKF outperforms an IterIEKF based on absolute body poses, as well as all EKF and iterated EKF ( IterEKF ) baselines under either representation. Fig. 1: Rigid-body system with a tree structure, composed of two movable bodies and a base fixed in \mathcal{F}\mskip-5.0mu_{\scriptstyle 0} . An IMU is mounted on each movable body. Our goal is to estimate the extended pose (position, velocity, orientation) of the full articulated system, from the IMU outputs only. The contributions of this paper are: (i) the introduction of the relative L -extended pose, a Lie group state representation for multi-IMU articulated rigid-body systems, which admits group-affine dynamics and invariant joint constraints; (ii) an IterIEKF formulation based on the proposed representation that incorporates spherical and hinge joint constraints as noise-free pseudo-measurements; and (iii) an experimental evaluation on a robotic arm and a human-leg sequence, validating the approach across diverse articulated systems. II Problem statement We consider an articulated system of L rigid bodies arranged in a kinematic tree, with one rigidly mounted IMU per body (Fig. 1 shows L=2 ). Bodies and joints follow Featherstone’s indexing [ 10 ] : body 0 is the fixed base, anchored in the inertial frame \mathcal{F}\mskip-5.0mu_{\scriptstyle 0} , and frame \mathcal{F}\mskip-5.0mu_{\scriptstyle i} denotes the frame of body i , which we take to coincide with its IMU frame without loss of generality. For joint J_{j} , p(j) and s(j) are the predecessor and successor body indices, and \prescript{\vphantom{|}\scriptstyle}{}{\mathrlap{\boldsymbol{\rho}}\hphantom{\boldsymbol{\varsigma}}}^{\vphantom{|}\scriptstyle j},\prescript{\vphantom{|}\scriptstyle}{}{\mathrlap{\boldsymbol{\varsigma}}\hphantom{\boldsymbol{\varsigma}}}^{\vphantom{|}\scriptstyle j}\in\mathbb{R}^{3} are the vectors from \mathcal{F}\mskip-5.0mu_{\scriptstyle p(j)} and \mathcal{F}\mskip-5.0mu_{\scriptstyle s(j)} to J_{j} . For any vector or matrix quantity, the bottom-right index is the time step and the top-right superscript identifies the frames involved. If specified, the top-left superscript indicates the expression frame. This way, \mathbf{R}_{k}^{\scriptstyle ij}\in SO(3) is the rotation from \mathcal{F}\mskip-5.0mu_{\scriptstyle j} to \mathcal{F}\mskip-5.0mu_{\scriptstyle i} at time k , and \prescript{\vphantom{|}\scriptstyle l}{\vphantom{k}}{\mathbf{v}}_{k}^{\vphantom{|}\scriptstyle ij},\prescript{\vphantom{|}\scriptstyle l}{\vphantom{k}}{\mathbf{p}}_{k}^{\vphantom{|}\scriptstyle ij}\in\mathbb{R}^{3} are the velocity and position of \mathcal{F}\mskip-5.0mu_{\scriptstyle j} with respect to (w.r.t.) \mathcal{F}\mskip-5.0mu_{\scriptstyle i} , expressed in \mathcal{F}\mskip-5.0mu_{\scriptstyle l} . Neglecting IMU biases and without motion priors, each body follows the inertial navigation dynamics \displaystyle\mathbf{R}_{k+1}^{\scriptstyle 0i} \displaystyle=\mathbf{R}_{k}^{\scriptstyle 0i}\exp_{SO(3)}((\prescript{\vphantom{|}\scriptstyle i}{\vphantom{k}}{\boldsymbol{\omega}}_{k}^{\vphantom{|}\scriptstyle i}+\mathbf{w}_{k}^{\scriptstyle i,\boldsymbol{\omega}})dt), (1a) \displaystyle\prescript{\vphantom{|}\scriptstyle 0}{\vphantom{k+1}}{\mathbf{v}}_{k+1}^{\vphantom{|}\scriptstyle 0i} \displaystyle=\prescript{\vphantom{|}\scriptstyle 0}{\vphantom{k}}{\mathbf{v}}_{k}^{\vphantom{|}\scriptstyle 0i}+\left(\mathbf{R}_{k}^{\scriptstyle 0i}(\prescript{\vphantom{|}\scriptstyle i}{\vphantom{k}}{\mathbf{a}}_{k}^{\vphantom{|}\scriptstyle i}+\mathbf{w}_{k}^{\scriptstyle i,\mathbf{a}})+\prescript{\vphantom{|}\scriptstyle 0}{\vphantom{|}}{\mathbf{g}}\right)dt, (1b) \displaystyle\prescript{\vphantom{|}\scriptstyle 0}{\vphantom{k+1}}{\mathbf{p}}_{k+1}^{\vphantom{|}\scriptstyle 0i} \displaystyle=\prescript{\vphantom{|}\scriptstyle 0}{\vphantom{k}}{\mathbf{p}}_{k}^{\vphantom{|}\scriptstyle 0i}+\prescript{\vphantom{|}\scriptstyle 0}{\vphantom{k}}{\mathbf{v}}_{k}^{\vphantom{|}\scriptstyle 0i}dt+\left(\mathbf{R}_{k}^{\scriptstyle 0i}(\prescript{\vphantom{|}\scriptstyle i}{\vphantom{k}}{\mathbf{a}}_{k}^{\vphantom{|}\scriptstyle i}+\mathbf{w}_{k}^{\scriptstyle i,\mathbf{a}})+\prescript{\vphantom{|}\scriptstyle 0}{\vphantom{|}}{\mathbf{g}}\right)\frac{dt^{2}}{2}, (1c) where \prescript{\vphantom{|}\scriptstyle i}{\vphantom{k}}{\boldsymbol{\omega}}_{k}^{\vphantom{|}\scriptstyle i},\prescript{\vphantom{|}\scriptstyle i}{\vphantom{k}}{\mathbf{a}}_{k}^{\vphantom{|}\scriptstyle i}\in\mathbb{R}^{3} are the IMU angular velocity and specific force in \mathcal{F}\mskip-5.0mu_{\scriptstyle i} , \prescript{\vphantom{|}\scriptstyle 0}{\vphantom{|}}{\mathbf{g}} is gravity in \mathcal{F}\mskip-5.0mu_{\scriptstyle 0} , dt is the time step, and \mathbf{w}_{k}^{\scriptstyle i}=(\mathbf{w}_{k}^{\scriptstyle i,\boldsymbol{\omega}},\mathbf{w}_{k}^{\scriptstyle i,\mathbf{a}})\sim\mathcal{N}(\mathbf{0}_{6\times 1},\mathbf{Q}_{k}^{\scriptstyle i}) is the process noise. Joints couple the body poses through kinematic constraints; for the spherical joints in Fig. 1 : \displaystyle J_{1}:\quad\prescript{\vphantom{|}\scriptstyle 0}{\vphantom{k}}{\mathbf{p}}_{k}^{\vphantom{|}\scriptstyle 01}+\mathbf{R}_{k}^{\scriptstyle 01}\prescript{\vphantom{|}\scriptstyle 1}{}{\mathrlap{\boldsymbol{\varsigma}}\hphantom{\boldsymbol{\varsigma}}}^{\vphantom{|}\scriptstyle 1} \displaystyle=\prescript{\vphantom{|}\scriptstyle 0}{}{\mathrlap{\boldsymbol{\rho}}\hphantom{\boldsymbol{\varsigma}}}^{\vphantom{|}\scriptstyle 1}, (2a) \displaystyle J_{2}:\quad\prescript{\vphantom{|}\scriptstyle 0}{\vphantom{k}}{\mathbf{p}}_{k}^{\vphantom{|}\scriptstyle 02}+\mathbf{R}_{k}^{\scriptstyle 02}\prescript{\vphantom{|}\scriptstyle 2}{}{\mathrlap{\boldsymbol{\varsigma}}\hphantom{\boldsymbol{\varsigma}}}^{\vphantom{|}\scriptstyle 2} \displaystyle=\prescript{\vphantom{|}\scriptstyle 0}{\vphantom{k}}{\mathbf{p}}_{k}^{\vphantom{|}\scriptstyle 01}+\mathbf{R}_{k}^{\scriptstyle 01}\prescript{\vphantom{|}\scriptstyle 1}{}{\mathrlap{\boldsymbol{\rho}}\hphantom{\boldsymbol{\varsigma}}}^{\vphantom{|}\scriptstyle 2}, (2b) where \prescript{\vphantom{|}\scriptstyle p(j)}{}{\mathrlap{\boldsymbol{\rho}}\hphantom{\boldsymbol{\varsigma}}}^{\vphantom{|}\scriptstyle j},\prescript{\vphantom{|}\scriptstyle s(j)}{}{\mathrlap{\boldsymbol{\varsigma}}\hphantom{\boldsymbol{\varsigma}}}^{\vphantom{|}\scriptstyle j}\in\mathbb{R}^{3} with j\in\{1,2\} are known from calibration. Our goal is to estimate the extended pose \prescript{\vphantom{|}\scriptstyle}{\vphantom{k}}{\mathbf{X}}_{k}^{\vphantom{|}\scriptstyle} (orientation, velocity, position) of the full articulated system, from the IMU outputs only . Two state representations are commonly used to address this problem, each with a fundamental limitation. In the free-segment model, each body has an independent pose w.r.t. \mathcal{F}\mskip-5.0mu_{\scriptstyle 0} and joint constraints are imposed as stochastic pseudo-measurements. This approach cannot properly handle the noise-free constraints ( 2 ): enforcing them as noisy measurements in an EKF update [ 25 ] leaves residual constraint violations [ 14 ] , while handling them via a particle filter [ 31 ] incurs substantially higher computational cost. The constrained-optimization approach [ 17 ] shares the same difficulty. The kinematic-tree model avoids this issue by combining the root pose w.r.t. \mathcal{F}\mskip-5.0mu_{\scriptstyle 0} with relative poses between adjacent segments in minimal coordinates,

Read the complete text