Invariant Stochastic Filtering on SE(3) for Inertial-Encoder State Estimation of Serial Rigid Manipulators

Abstract:An invariant extended Kalman filter (IEKF) is developed for state estimation of serial rigid manipulators with an arbitrary number of links, formulated entirely within the Lie group $SE(3)$. The group-affine property of the kinematic equations makes the linearised error dynamics autonomous, so the Riccati equation governs the true error covariance rather than a local approximation. A physically separated noise model treats gyroscope and accelerometer channels independently: the accelerometer provides translational twist via gravity-compensated integration, yielding a measurement covariance that scales with the sample interval in exact analogy with process noise discretisation; a state-dependent Coriolis noise term captures gyroscope noise propagating through the nonlinear dynamics, vanishing at rest and growing with twist magnitude. The filter is structured as a modular chain of per-link IEKFs in which the predicted covariance of each link depends on its predecessor only through the Adjoint-transformed posterior, giving linear computational cost in link count. Exponential ultimate boundedness in mean square is established via a Lie algebra Lyapunov function, with per-link bounds chained through the Adjoint operator norm to yield a stability certificate that is modular and scalable to arbitrary chain length. The state is augmented with gyroscope biases without loss of the autonomous error dynamics or the modular structure. The design is validated in a three-dimensional simulation of a two-link chain with encoder and IMU fusion, in a software-in-the-loop study of an industrial hydraulic rammer-boom manipulator in which the encoder row is replaced by a gravity-referenced virtual encoder for IMU-only estimation.

This document is an arXiv preprint posted for open access and citation purposes. It is under review and subject to revision

Robotics (cs.RO)

arXiv:2607.00026 [cs.RO]

(or arXiv:2607.00026v2 [cs.RO] for this version)

https://doi.org/10.48550/arXiv.2607.00026 Focus to learn more arXiv-issued DOI via DataCite

arXivLabs is a framework that allows collaborators to develop and share new arXiv features directly on our website.

Both individuals and organizations that work with arXivLabs have embraced and accepted our values of openness, community, excellence, and user data privacy. arXiv is committed to these values and only works with partners that adhere to them.

Have an idea for a project that will add value for arXiv's community? Learn more about arXivLabs.