💻 computer science

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

This paper presents a modular, invariant extended Kalman filter formulated on the Lie group SE(3) for state estimation of serial rigid manipulators with arbitrary link counts, featuring a physically separated noise model and a chain structure that ensures linear computational cost and provable exponential stability.

Original authors: S. Yaqubi, J. Mattila

Published 2026-07-02

📖 5 min read🧠 Deep dive

CC BY 4.0

Original authors: S. Yaqubi, J. Mattila

Original paper licensed under CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

Imagine a robotic arm as a chain of rigid sticks connected by joints, like a human arm or a crane. To control this robot perfectly, a computer needs to know exactly where every stick is and how fast it's moving at every single moment. This is called "state estimation."

However, the sensors on these robots (gyroscopes and accelerometers) are noisy, like trying to hear a whisper in a hurricane. Traditional methods for cleaning up this noise often treat the robot's movement as if it were happening on a flat, straight line. But in reality, a robot arm moves in 3D space, twisting and turning in complex ways that don't fit on a flat map.

This paper introduces a new, smarter way to clean up the sensor noise and track the robot's position. Here is the breakdown of their solution using simple analogies:

1. The "Curved Map" vs. The "Flat Map"

The Problem: Imagine trying to draw a map of the Earth on a flat piece of paper. If you try to draw a circle around the North Pole, it gets distorted. Traditional filters try to flatten the robot's 3D movements into a 2D list of numbers. When the robot makes big turns, this "flat map" breaks down, and the math gets confused.

The Solution: The authors use a "curved map" (mathematically known as the Lie group SE(3)). Instead of forcing the robot's movement into a straight line, they let the math live on the natural, curved surface of 3D space. This ensures that no matter how much the robot twists, the math stays consistent and doesn't get "gimbal locked" (a state where the math loses track of direction).

2. The "Modular Chain" of Filters

The Problem: A robot arm has many links. If you treat the whole arm as one giant, complicated puzzle, fixing the position of one link requires recalculating the position of every other link at the same time. As the robot gets longer, this calculation becomes impossibly slow.

The Solution: The authors built a modular chain of filters. Think of it like a relay race.

Link 1 runs its own race, calculates its position, and passes a "baton" (its estimated position and uncertainty) to Link 2.
Link 2 takes that baton, adds its own sensor data, and passes a new baton to Link 3.
The Benefit: This means the computer doesn't have to solve one giant puzzle. It just solves a small puzzle for each link. If you add a 100th link to the robot, the computer doesn't get slower; it just does one more small step. The cost grows linearly, not exponentially.

3. The "Two-Channel" Noise Model

The Problem: Robots have two main sensors: a gyroscope (which feels spinning) and an accelerometer (which feels movement).

The gyroscope is like a spinning top; its noise is constant.
The accelerometer is like a car's speedometer that you have to integrate (add up over time) to get speed. If you wait longer to check it, the error from the noise grows larger.

The Solution: The authors realized that these two sensors behave differently.

They treat the gyroscope noise as a steady hum.
They treat the accelerometer noise as a "snowball" that grows the longer you wait between checks.
They also added a special "Coriolis" term. Imagine the robot swinging a heavy weight; the noise from the gyroscope gets amplified by the speed of the swing. Their filter accounts for this: if the robot is still, the noise is low; if it's swinging wildly, the filter expects more noise and adjusts accordingly.

4. The "Stability Certificate"

The Problem: Even if a filter works well today, how do you know it won't go crazy tomorrow if the robot moves in a weird way?

The Solution: The authors didn't just test it; they proved mathematically that the error will always stay within a specific, predictable "bubble."

They used a "Lyapunov function" (think of it as a mathematical energy meter) to prove that the error will shrink exponentially over time.
They also proved that even if the error in Link 1 gets a little big, it won't blow up the whole chain. The error might get slightly larger as it travels down the chain (due to the length of the links), but it will never explode to infinity. They call this "Exponential Ultimate Boundedness."

5. The Results

They tested this on a simulated 3D robot arm with two links moving in complex patterns.

Comparison: They compared their new filter against a standard "flat map" filter and just using raw sensor data.
Outcome: Their filter was significantly more accurate (about 24% better than the standard filter and 33% better than raw data).
Key Finding: The new filter stayed accurate even when the robot made large, fast movements, whereas the standard filter started to drift and make mistakes.

Summary

In short, this paper presents a new "GPS" for robotic arms that:

Respects the curved nature of 3D space (no more flat-map distortions).
Works efficiently on long chains of robot links (like a relay race).
Understands that different sensors make different kinds of mistakes.
Mathematically guarantees that the robot won't lose its mind, no matter how it moves.

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

Problem Statement
Accurate state estimation for serial rigid manipulators requires simultaneous geometric consistency and stochastic rigor. The configuration of each link resides in the Lie group $SE(3)$, yet classical Kalman filters operate in flat vector spaces, leading to linearization errors at large angles and issues like gimbal lock or normalization constraints inherent in Euler-angle or quaternion formulations. Furthermore, existing geometric filters often treat single bodies or the entire chain as a monolithic state, incurring cubic computational costs with respect to link count. A critical gap also exists in modeling Inertial Measurement Unit (IMU) noise: standard approaches fail to account for the distinct physical origins of gyroscope and accelerometer noise, specifically the sample-interval scaling of accelerometer noise upon integration and the state-dependent propagation of gyroscope noise through nonlinear Coriolis dynamics.

Methodology
The authors develop an Invariant Extended Kalman Filter (IEKF) formulated entirely within the Lie group $SE(3)$ for manipulators with an arbitrary number of links. The methodology relies on four core components:

Geometric Error Dynamics: The filter maintains a strict separation between the estimate $(\hat{g}_i, \hat{V}_i)$ and the Lie algebra error vector $\tilde{\xi}_i$ . By exploiting the group-affine property of the kinematic equations ( $\dot{g}_i = g_i V_i$ ), the linearized error dynamics are shown to be autonomous. This ensures the Riccati equation governs the true error covariance rather than a local approximation.
Physically Separated Noise Model: The paper introduces a novel noise model that treats gyroscope and accelerometer channels independently.
- Accelerometer: The measurement of specific force is integrated to yield translational twist. The resulting measurement covariance scales with the sample interval $\Delta t$ , analogous to process noise discretization.
- Gyroscope & Coriolis: Gyroscope noise enters the rotational channel directly but also propagates through the nonlinear Newton-Euler dynamics via the Coriolis term. This creates a state-dependent process noise contribution that vanishes at rest and grows with twist magnitude.
Modular Chain Synthesis: The filter is structured as a chain of per-link IEKFs. The predicted covariance of link $i$ depends on the posterior of link $i-1$ solely through the Adjoint-transformed covariance. This propagation mechanism ensures the total computational cost scales linearly ( $O(n)$ ) with the number of links, avoiding the cubic cost of monolithic state estimation.
Stability Analysis: The authors establish Exponential Ultimate Boundedness (EUB) in the mean square sense. Using a Lie algebra Lyapunov function, they derive per-link bounds and chain them across the manipulator using the Adjoint operator norm. Crucially, the analysis corrects naive stability arguments by accounting for the fact that the Adjoint map is not an isometry under the standard Euclidean inner product, meaning uncertainty amplification depends on link geometry (lengths).

Key Contributions
The paper claims four specific contributions to geometric stochastic estimation:

(C1) Noise Modeling: A physically separated IMU noise model incorporating a state-dependent Coriolis noise term and sample-interval scaling for accelerometers.
(C2) Modular Architecture: A chain of per-link IEKFs with linear computational cost achieved via Adjoint covariance propagation.
(C3) Complete Derivation: The full derivation of the IEKF, including the mixed pose-twist observation Jacobian.
(C4) Stability Certificate: A chained exponential ultimate boundedness certificate derived via a Lie algebra Lyapunov function, providing a modular stability guarantee scalable to arbitrary chain lengths.

Results
The design is validated through numerical simulations of a 3-DOF two-link rigid serial manipulator executing large-amplitude 3D motion. The proposed IEKF is compared against a coordinate-based EKF (CEKF) and a raw encoder baseline.

Accuracy: The IEKF achieved a 33% reduction in RMSE relative to the raw encoder baseline and a 24% reduction relative to the CEKF across all joints.
Robustness: While the CEKF error grew with motion amplitude due to coordinate linearization errors, the IEKF error remained stable, demonstrating the amplitude-invariance of the $SE(3)$ geometric error model.
Consistency: Normalized Estimation Error Squared (NEES) analysis confirmed the statistical consistency of the filter, with mean values falling within the 95% chi-squared acceptance band.
Uncertainty Propagation: The results confirmed that the covariance band correctly widens for downstream links (e.g., Link 2) as it incorporates upstream estimation uncertainty through the Adjoint map.

Significance
The paper positions this work as a rigorous framework for state estimation that bridges the gap between geometric intuition and stochastic rigor. By ensuring the filter operates natively on $SE(3)$, it avoids the singularities and linearization inconsistencies of flat-space approaches. The modular structure allows the method to scale to complex, long-chain manipulators without prohibitive computational costs. Furthermore, the explicit handling of state-dependent noise and the derivation of a stability certificate that accounts for geometric amplification factors provide a more physically accurate and theoretically sound basis for controlling flexible and rigid manipulators compared to existing geometric filters. The authors suggest this framework could serve as a geometrically consistent prior for learning-based estimators, addressing the challenge of maintaining Lie group consistency under learned corrections.