New invariant filter cuts pose estimation error by 50% for articulated robots
Iterated IEKF with relative L-extended pose outperforms all baselines on UR5e and human leg.
Accurate pose estimation (orientation, velocity, position) for systems with multiple inertial measurement units (IMUs) on articulated rigid bodies is critical in robotics and human motion analysis. The invariant extended Kalman filter (IEKF) offers convergence guarantees for a single rigid body, but extending it to multi-body systems has been hindered by inter-body coupling and the difficulty of incorporating joint kinematic constraints into the invariant framework.
To address this, Sven Goffin and colleagues introduce the relative L-extended pose, a Lie group representation that yields group-affine dynamics for kinematic-tree systems with one IMU per body. This representation allows joint constraints to be expressed in invariant form and integrated as noise-free pseudo-measurements within an iterated IEKF (IterIEKF). Validated on a UR5e robot arm and a human leg, the proposed filter consistently achieves the lowest RMSE—at least 50% improvement over all EKF, IterEKF, and absolute-pose IterIEKF baselines—while converging faster and showing lower run-to-run variability.
- Novel relative L-extended pose representation enables group-affine dynamics for kinematic trees with one IMU per body.
- Joint constraints are encoded as noise-free pseudo-measurements within an iterated invariant EKF, preserving convergence guarantees.
- Validation on UR5e robot and human leg shows at least 50% lower RMSE than the second-best filter across all scenarios.
Why It Matters
Enables more reliable real-time tracking for surgical robots, exoskeletons, and human motion capture.