Dynamics on Lie groups with applications to attitude estimation

TL;DR

This work develops a principled framework for nonlinear filtering on Lie groups using concentrated Gaussian distributions (CGs) and a tangent-space filter (TSF) that leverages the Lie algebra to achieve Kalman-like updates on manifolds. A central insight is that group-affine vector fields are exactly the condition under which CGs remain invariant under propagation, enabling CGs to remain a natural choice for state uncertainty on Lie groups; the theory is complemented by the tangent-space SDE (TSSDE) that describes diffusion in the Lie algebra and a continuous-time unscented transform (CTUT) for moment propagation. The TSF combines CTUT-based propagation, a whitening step to restore zero-mean tangent-space coordinates, and UT-based measurement updates, yielding an efficient, consistent attitude-estimation framework demonstrated on SU$(2)$, SE$(3)$, and SE$(2,3)$ with gyro-bias considerations. Numerical experiments on attitude estimation with gyro bias show that TSF with the SE$(3)$-group law achieves near-ideal statistical consistency and superior robustness compared to alternative approaches like the unscented quaternion estimator (USQUE). The results offer a practical, geometry-aware alternative to EKF/UKF for navigation and attitude problems, maintaining coherence between dynamics, measurements, and uncertainty on nonlinear manifolds.

Abstract

The problem of filtering - propagation of states through stochastic differential equations (SDEs) and association of measurement data using Bayesian inference - in a state space which forms a Lie group is considered. Particular emphasis is given to concentrated Gaussians (CGs) as a parametric family of probability distributions to capture the uncertainty associated with an estimated state. The so-called group-affine property of the state evolution is shown to be necessary and sufficient for linearity of the dynamics on the associated Lie algebra, in turn implying CGs are invariant under such evolution. A putative SDE on the group is then reformulated as an SDE on the associated Lie algebra. The vector space structure of the Lie algebra together with the notion of a CG enables the leveraging of techniques from conventional Gaussian-based Kalman filtering in an approach called the tangent space filter (TSF). We provide example calculations for several Lie groups that arise in the problem of estimating position, velocity, and orientation of a rigid body from a noisy, potentially biased inertial measurement unit (IMU). For the specific problem of attitude estimation, numerical experiments demonstrate that TSF-based approaches are more accurate and robust than another widely used attitude filtering technique.

Figures (3)

• Figure 1: The RMS over $200$ MC runs of the (normalized) $\chi^2_G$ statistical consistency metric \ref{['def:chisquareG']} for the TSF-$\rm{SE}_{3}$ (blue) and TSF-DP (red), along with a similar statistical consistency metric for the USQUE (black). The magenta lines represent the theoretically expected statistical consistency value for all filters, with the solid line between $1$ (the expected value) and the dashed lines representing the $\pm 1 \sigma$ around this expected value.
• Figure 2: The RMS over $200$ MC runs of the roll, pitch, and yaw errors and $3 \sigma$ bounds for the TSF-$\rm{SE}_{3}$ (blue), TSF-DP (red), and USQUE (black). The $x$-axis only ranges over the first $1/2$ hour of the simulation in order to highlight the differences in the errors.
• Figure 3: The RMS over $200$ MC runs of the total gyro bias error and ($3$ times the square root of) the largest eigenvalue of the associated error covariance matrix for the TSF-$\rm{SE}_{3}$ (blue), TSF-DP (red), and USQUE (black).

Theorems & Definitions (3)

• Theorem 1
• Corollary 1
• Lemma 1