State Estimation and Control on Lie Group Manifolds

40 concepts30 papers1 proposal

When tracking or controlling a rigid body in 3D space—whether it is a quadrotor, a legged robot, or a spacecraft—we must represent its orientation. Traditionally, engineers have relied on representations like Euler angles or quaternions. However, these choices introduce severe mathematical artifacts. Euler angles suffer from gimbal lock (geometric singularities), while unit quaternions suffer from antipodal ambiguity (where $+q$ and $-q$ represent the exact same orientation), requiring artificial constraints during optimization. If we instead use raw $3 \times 3$ rotation matrices, we are forced to maintain 9 parameters constrained by orthogonality ($R^T R = I$), which standard unconstrained optimization algorithms will quickly violate.

To bypass these limitations, a powerful paradigm treats 3D rotations and poses not as flat vectors, but as elements of smooth geometric surfaces known as Lie groups. The rotation group is denoted as $SO(3)$ (the Special Orthogonal group), and the full 6-DoF pose group (position and orientation) is denoted as $SE(3)$ (the Special Euclidean group).

As argued by Lee et al., modeling orientations directly on the $SO(3)$ manifold eliminates coordinate-induced singularities and ambiguities entirely, ensuring global validity across all possible maneuvers.

The Core Intuition: Manifolds and Tangent Spaces

The fundamental reason why Lie group modeling works so effectively lies in the relationship between a curved manifold and its flat tangent space.

Imagine a sphere. If you stand at the equator, the space immediately around you looks and behaves like a flat 2D plane. You can walk in any direction using standard vector addition. This flat plane is the tangent space at your current position.

For a Lie group, the tangent space evaluated at the identity element is its Lie algebra (denoted as $\mathfrak{so}(3)$ for rotations). Crucially, the Lie algebra is a vector space. For $SO(3)$, this vector space is isomorphic to $\mathbb{R}^3$, meaning that any small rotation perturbation, velocity, or uncertainty can be represented as a simple, unconstrained 3D vector (often called a rotation vector).

   Curved Manifold (SO(3))          Flat Tangent Space (so(3) ~ R³)
     [ Valid Rotations ]               [ Unconstrained Vectors ]
            \                                     /
             \    <--- Exponential Map (exp) ----/
              \   ---- Logarithmic Map (log) --->

We bridge these two spaces using two mathematical operators:

  • The Exponential Map ($\exp$): Takes an unconstrained vector from the flat tangent space and "wraps" it onto the curved manifold, automatically producing a perfectly valid, orthogonal rotation matrix.
  • The Logarithmic Map ($\log$): Takes a rotation matrix on the manifold and pulls it back to a minimal vector representation in the tangent space, measuring the "difference" or error.

By operating on the tangent space, we can use minimal parameterizations (3 degrees of freedom for rotation, 6 for pose) without any redundancy or singularities. As Jia et al. demonstrate, utilizing these exponential and logarithmic mappings allows for highly efficient state estimation that avoids both the singularities of Euler angles and the numerical redundancies of quaternions.

On-Manifold Optimization and Filtering

How do we run an estimator or an optimizer if the state lives on a curved surface? The trick is to perform all linear operations, updates, and calculus inside the flat tangent space, and then project the results back onto the manifold.

The Error-State Mechanism

In classical state estimation, like an Extended Kalman Filter (EKF), we maintain an absolute state estimate and update it linearly. On a manifold, we instead use an Error-State Kalman Filter (ESKF) or an Invariant Extended Kalman Filter (InEKF).

Instead of tracking the absolute orientation linearly, the filter tracks a small error vector $\delta \theta \in \mathbb{R}^3$ in the tangent space. The true state $R$ is modeled as the composition of the current estimate $\hat{R}$ and the exponentiated error vector:

$$R = \hat{R} \exp(\delta \theta)$$

Because $\delta \theta$ lives in a flat vector space, we can compute standard linear Jacobians, propagate covariance matrices, and apply standard Kalman gain updates to it. Once the optimal error $\delta \theta^*$ is found, it is mapped back to the manifold via the exponential map to update the main estimate, and the error state is reset to zero.

Kok et al. explain that linearizing orientation deviations directly on the $SO(3)$ manifold provides significantly more stable and accurate results than treating quaternions or matrices as standard Euclidean vectors. Furthermore, He et al. show that formulating state estimation on a matrix Lie group can make the deterministic part of the error dynamics exactly linear, yielding faster convergence and superior robustness to large initialization errors.

Smoother Optimization Convergence

For batch optimization problems—such as Structure from Motion (SfM) or iterative point-cloud registration—optimizing directly on the manifold ensures that every intermediate step remains geometrically valid. Carceroni et al. and Song et al. demonstrate that direct optimization on $SO(3)$ and $SE(3)$ Lie manifolds prevents optimization paths from getting stuck in coordinate singularities, leading to smoother, more reliable convergence in the presence of heavy measurement noise.

Structure-Preserving Integration and Control

When physical systems evolve over time, we must integrate velocities to track states. If you integrate angular velocities using standard Euclidean addition ($\hat{R}_{t+1} = \hat{R}_t + \omega \Delta t$), the resulting matrix will quickly drift away from orthogonality, causing the simulated or estimated body to lose its rigid structure.

IMU Preintegration

In robotics, Inertial Measurement Units (IMUs) provide high-frequency angular velocities and accelerations. To avoid re-integrating these raw measurements every time an optimizer adjusts a historical state, researchers use "preintegration."

Förster developed a mathematically rigorous framework for integrating inertial measurements directly on the $SO(3)$ manifold. By accumulating high-frequency motion constraints into a single relative rotation on the manifold, the representation remains invariant under rigid body transformations and keeps uncertainty stable. This on-manifold formulation was shown by Förster et al. to eliminate estimator overconfidence and maintain consistency even during aggressive, high-pitch maneuvers.

Geometric Control and MPC

In control theory, we want to plan trajectories that respect the true geometry of the workspace. For instance, Jang shows that by linearizing error dynamics within the Lie algebra of $SE(3)$, a highly nonlinear Model Predictive Control (MPC) problem for marine vehicles can be transformed into a fast, convex Quadratic Program (QP) without sacrificing tracking accuracy.

For highly articulate systems, the open-source Crocoddyl framework introduced by Mastalli et al. models robot states on Lie group manifolds to plan dynamic multi-contact maneuvers (like jumps and flips). By respecting the underlying topology of the configuration space rather than using simplified Euclidean approximations, the optimization maintains mathematical rigor and solves within milliseconds.

Extending to Particles and Deep Learning

The elegance of the tangent-space abstraction has allowed the Lie group framework to expand beyond classical filtering into probabilistic sampling and deep learning.

Manifold-Aware Particle Filters

In Monte Carlo Localization (MCL), robots track their positions by maintaining a cloud of candidate poses (particles). Adding standard Gaussian noise to the orientation coordinates of these particles can result in invalid rotation matrices or distorted distributions due to gimbal lock. Jo et al. solved this by sampling particle noise directly within the flat tangent space of the $SE(3)$ manifold before projecting it back via the exponential map. This ensures that every generated hypothesis represents a physically valid rigid body motion, dramatically improving filter stability.

Physics-Informed Neural Networks

When training neural networks to learn physics or control policies, forcing the network to output raw matrices or Euler angles often leads to unstable trajectories. To combat this, Atanasov et al. embedded Hamiltonian mechanics directly onto the $SE(3)$ manifold within a Neural ODE network, ensuring that the learned models remain globally valid and stable across all orientations.

Similarly, in generative robotics, Yu et al. designed diffusion policies that operate directly within the tangent space of the $SO(3)$ manifold. By defining the noise-clearing diffusion process in this mathematically consistent vector space, the model learns smooth, stable, and highly precise rotational trajectories for complex robotic manipulation tasks.

Key Foundational Work

Deterministic Global Attitude Estimation (2006) — contributed 2 concept elements to this direction.
Global Attitude Estimation using Single Direction Measurements (2006) — contributed 1 concept elements to this direction.
Attitude Maneuvers of a Rigid Spacecraft in a Circular Orbit (2005) — contributed 1 concept elements to this direction.
History & citationVersion 1 · 2026-10-04

Current · Permanent link

View frozen version 1

Cite this direction

Latest version (a living page):

@misc{lacuna_lie_group_rotation_modeling_1508a5b6,
  title = {Lie group rotation modeling},
  author = {{Lacuna}},
  howpublished = {Lacuna research direction},
  year = {2026},
  url = {https://lacuna.tiptreesystems.com/direction/lie-group-rotation-modeling/rdr_1508a5b66a4f426cacd4f1deebac3e6b},
  note = {Latest version of a living research direction; cite a version for a fixed text. Accessed 2026-11-20},
}

This exact version (version 1, frozen text):

@misc{lacuna_lie_group_rotation_modeling_1508a5b6_v1,
  title = {Lie group rotation modeling},
  author = {{Lacuna}},
  howpublished = {Lacuna research direction},
  year = {2026},
  url = {https://lacuna.tiptreesystems.com/direction/lie-group-rotation-modeling/rdr_1508a5b66a4f426cacd4f1deebac3e6b/v/1},
  note = {Version 1, frozen 2026-10-04. Accessed 2026-11-20},
}
About

These directions overlap with this one through shared papers.

See something wrong? Report a mistake