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

Jul 16, 2026

Upload by Aurora

An invariant extended Kalman filter (IEKF) is developed for state estimation of serial rigid manipulators with any number of links, formulated entirely on the Lie group SE(3). The group-affine kinematics yield autonomous linearised error dynamics, so the Riccati equation captures the true error covariance. A physically separated IMU noise model treats gyroscope and accelerometer channels independently, including state-dependent Coriolis noise. The filter is a modular chain of per-link IEKFs with linear cost, and exponential ultimate boundedness is proved via a Lie-algebra Lyapunov function; simulations validate the approach.

LanguageEnglish

CategoryResearch & Report

FormatPDF

Pages22

Reading Time55 min

Introduction

Background and motivation

Related work

IMU noise modelling gap

Main contributions

arXiv :2607 .00026v1 [ cs .RO] 21 Jun 2026 Invariant Stochastic Filtering on SE(3) for Inertial-Encoder State Estimation of Serial Rigid Manipulators S. Yaqubi∗†‡ J. Mattila 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. Numerical results validate the design. Keywords: Invariant extended Kalman filter; Lie group SE(3); State estimation; Serial rigid manipulators; Stochastic filtering; Exponential ultimate boundedness. 1 Introduction State estimation for serial rigid manipulators requires geometric consistency and stochastic rigour simultaneously: the configuration of each link is an element of ∗ This work was supported by the Research Council of Finland under the project “Nonlinear PDE-model-based control of flexible manipulators”(Grant No. 355664) . (Corresponding author: S. Yaqubi.) †S. Yaqubi and J. Mattila are with the Department of Automation Technology and Mechanical Engineering, Tampere University, Korkeakoulunkatu 6, 33720 Tampere, Finland (e-mails: [sadeq.yaqubi@tuni.fi](sadeq.yaqubi@tuni.fi), [jouni.mattila@tuni.fi](jouni.mattila@tuni.fi)). ‡This document is an arXiv preprint posted for open access and citation purposes. It is under review and subject to revision. the Lie group SE(3) (M¨uller, 2018), yet the classical Kalman filter operates in a flat vector space. Precise closed-loop control depends on accurate knowledge of the body-fixed twist and pose of each link, estimated from noisy IMU and joint encoder data in real time. The nonlinear geometry of SE(3) invalidates coordinatebased linearisation at large angles, and the heterogeneous physical origins of IMU and process noise require a treatment that most existing filter formulations do not provide. Euler-angle and quaternion EKFs are well established (Thrun et al. , 2005; Welch and Bishop, 1995) but introduce unavoidable deficiencies: gimbal lock, normalisation constraints, and a linearisation consistency error that does not vanish at convergence (Barrau and Bonnabel, 2017; Bhat and Bernstein, 2000) . Geometric approaches have been pursued, including multiplicative quaternion filters (Wu and Jin, 2025; Mitikiri and Mohseni, 2021), dual-quaternion formulations (Khalifa and Hashim, 2026), motor-algebra EKFs (Bayro-Corrochano and Zhang, 2000), and Liegroup signal processing (Kumar et al. , 2024; Li and Jiao, 2024), but these address single bodies and do not propagate uncertainty modularly across chains. Filters on Lie groups place geometric intuition within a rigorous stochastic framework. Probability densities on Lie groups and kinematic state estimation via the exponential map were developed in Chirikjian (2012); Park et al. (2008) . The invariant EKF (IEKF) (Barrau and

FAQs

Why use an invariant extended Kalman filter formulated on SE(3)?

The method leverages the group-affine property of SE(3) kinematics so the linearised error dynamics become autonomous. This makes the Riccati equation govern the true error covariance instead of relying on a local approximation.

How is IMU noise modelled in the proposed filter?

Gyroscope and accelerometer noise are treated independently. Accelerometer integration introduces sample-interval scaling in the translational-twist measurement covariance, while gyroscope noise produces a state-dependent Coriolis noise term that vanishes at rest and grows with twist magnitude.

How does the modular per-link design improve computational cost for long chains?

Each link uses a per-link IEKF, and the predicted covariance of a link depends on its predecessor only through the Adjoint-transformed posterior. This yields linear computational cost with respect to the number of links.

Jul 16, 2026

Upload by Aurora