Yaqubi and Mattila's IEKF for robot arms scales linearly with link count

State estimation for robot arms with many joints is notoriously difficult because errors propagate nonlinearly. Yaqubi and Mattila's new invariant extended Kalman filter (IEKF) tackles this by formulating the entire estimation problem on the Lie group SE(3) – the mathematical space of rigid motions. The key insight is that the kinematic equations are group-affine, which means the linearised error dynamics become autonomous (input-independent). Consequently, the Riccati equation drives the true error covariance, not just a local approximation. The filter also introduces a physically motivated noise model that treats gyroscope and accelerometer channels separately: accelerometer readings provide translational twist via gravity-compensated integration with a covariance that scales with sample time, while a state-dependent Coriolis term captures gyroscope noise that vanishes at rest and grows with twist magnitude.

To scale to manipulators with many links, the filter is structured as a chain of per-link IEKFs where each link's predicted covariance depends on its predecessor only through an Adjoint-transformed posterior. This modular design yields linear computational cost in the number of links – a dramatic improvement over traditional methods that grow quadratically. The authors prove exponential ultimate boundedness in mean square using a Lie algebra Lyapunov function, with per-link bounds chained through the Adjoint operator norm. Numerical experiments validate the design, showing robust performance even under high twist rates. This work promises practical, real-time state estimation for complex robotic arms, from industrial manipulators to surgical robots.