PaperlayerView exact revision on arXiv

Source-linked AI summary

Intrinsic filtering on Lie groups with applications to attitude estimation

Axel Barrau, Silvere Bonnabel

arXiv:1310.2539v2eess.SYcs.ROcs.SYmath.OC

TL;DR

Nonlinear filtering lacks a general efficient method, and engineering filters can have theoretical or computational drawbacks. This paper develops invariant filters on matrix Lie groups, analyzes their error distributions, and introduces discrete-time filters for attitude estimation. It proves stationary-distribution convergence for fixed-gain filters under stated noiseless convergence conditions and reports that the IEnKF captures error dispersion accurately under large noise.

  • Problem

    Nonlinear filtering has no general efficient method, and commonly used extended Kalman filters lack general theoretical properties and can be prone to divergence.

    Source: §1 · p. 1

  • Method

    The paper formulates stochastic filtering on matrix Lie groups and develops invariant filters, including a discrete-time IEKF and sample-based gain methods.

    Source: §1 · p. 3, §6 · p. 14

  • Results

    For fixed gains and almost-global noiseless convergence, the noisy error converges in distribution to a stationary law; under large noise, the IEnKF accurately captures error dispersion.

    Source: §1 · p. 4, §4.1 · p. 8

  • Takeaways & Limitations

    Invariant filtering on Lie groups provides a framework for studying error convergence and uncertainty in attitude-estimation applications.

    Source: §1 · p. 2, §6 · p. 14

    The paper leaves proof of IEKF convergence in distribution for future work and notes that its asymptotic gain optimization was performed naively.

    Source: §6 · p. 14

Abstract

from arXiv · show

This paper proposes a probabilistic approach to the problem of intrinsic filtering of a system on a matrix Lie group with invariance properties. The problem of an invariant continuous-time model with discrete-time measurements is cast into a rigorous stochastic and geometric framework. Building upon the theory of continuous-time invariant observers, we show that, as in the linear case, the error equation is a Markov chain that does not depend on the state estimate. Thus, when the filter's gains are held fixed, and the filter admits almost-global convergence properties with noise turned off, the noisy error's distribution is proved to converge to a stationary distribution, providing insight into the mathematical theory of filtering on Lie groups. For engineering purposes we also introduce the discrete-time Invariant Extended Kalman Filter, for which the trusted covariance matrix is shown to asymptotically converge, and some numerically more involved sample-based methods as well to compute the Kalman gains. The methods are applied to attitude estimation, allowing to derive novel theoretical results in this field, and illustrated through simulations on synthetic data.

1 Introduction

Nonlinear filtering lacks a general efficient method, while common engineering filters have theoretical, practical, or computational drawbacks. This paper develops invariant filters on Lie groups and studies their error behavior and attitude-estimation applications.

Motivation

  • General nonlinear filtering equations are not closed, and conditional densities usually cannot be computed through finite-dimensional equations.The extended Kalman filter is widely used but lacks general theoretical properties, can diverge, and linearizes around the estimated trajectory.Source: §1 · p. 1

Problem formulation

  • The paper rigorously formulates stochastic filtering on matrix Lie groups with continuous-time noisy dynamics and discrete-time observations.It develops a complete discrete-time model, where discretization must account for non-commutativity.Source: §1 · p. 3

Invariant filters

  • Invariant filters yield error evolution independent of the system trajectory, allowing gains to be computed offline and reducing onboard computation.The filters transpose linear observers to groups by replacing addition with group multiplication.Source: §1 · p. 3

Fixed-gain analysis

  • With fixed gains, the error becomes a homogeneous Markov chain; if the noiseless filter has almost-global convergence, the noisy error converges to a stationary distribution.The paper applies this analysis to attitude estimation with two vector measurements and gyroscope noise, and to an artificial horizon with non-Gaussian noise.Source: §1 · p. 3

Transient optimization

  • The discrete-time IEKF linearizes invariant error at the identity, and its Kalman gains and covariance converge to fixed values.This permits replacing the gain by its asymptotic value when onboard storage and computation are limited.Source: §1 · p. 3
  • For an autonomous error equation, the IEnKF outperforms other filters under large noise by accurately capturing error dispersion.Simulations also illustrate convergence properties absent from the compared MEKF and UKF.Source: §1 · pp. 3–4

2 Problem setting

The paper formulates noisy continuous-time dynamics on a matrix Lie group with discrete observations, illustrates the model through attitude and velocity estimation, and derives an exact discrete-time form. Its stochastic interpretation and assumptions specify the scope of that formulation.

2.1 Considered continuous-time model

  • The model uses a Lie-group state, Lie-algebra inputs, continuous white Langevin process noise, and discrete-time observations with measurement noise.The observation map is assumed left-right equivariant; a more restrictive rotation-matrix formulation is also supplied for readers unfamiliar with Lie groups and stochastic calculus.Source: §2.1 · p. 4

2.2.1 Attitude estimation on flat earth

  • Flat-earth attitude estimation uses gyroscope increments and two non-collinear earth-fixed vector measurements, with isotropic Gaussian gyroscope noise required for the stated model form.The vectors can represent magnetic field and gravity measurements; the passage notes the isotropy assumption is restrictive but relevant in practice.Source: §2.2.1 · p. 4

2.2.2 Attitude estimation on a round rotating Earth

  • On a rotating Earth, the attitude model uses a geographic frame with constant earth-rotation and gravity vectors, a vertical-direction observation, and isotropic gyroscope noise.Source: §2.2.2 · p. 5

2.2.3 Attitude and velocity estimation

  • The larger-group example models aircraft attitude and velocity on a flat Earth using gyroscope and accelerometer increments plus noisy earth-frame speed measurements.The state is represented using SE(3).Source: §2.2.3 · p. 5

2.3 Interpretation of Langevin noises on Lie Groups

  • The Lie-group Langevin equation is interpreted in the Stratonovich sense, since an embedded Itô interpretation for SO(3) almost surely leaves the group for any positive time.The paper also cites the Stratonovich interpretation as a better approximation for colored sensor noise than Itô's.Source: §2.3 · pp. 5–6

2.4 Exact discretization of the considered model

  • Exact discretization at observation times expresses the state update using deterministic input increments and a group-valued random variable whose law depends on the interval's process noise.The paper then uses this discrete-time model and allows the noise variable to be general, not necessarily generated by the continuous stochastic differential equation.Source: §2.4 · p. 6

3 A class of discrete-time intrinsic filters

The paper uses the linear Kalman filter as a template for discrete-time filters on Lie groups. The resulting invariant filters have autonomous error behavior and permit offline gain tuning under stated input conditions.

3.1 Preliminary: Kalman filter for linear stationary systems with drift

  • For a linear system with stationary drift, the Kalman filter's autonomous error allows gains to be optimized independently of dynamical inputs and computed offline.Under observability and detectability, the gains and error distribution converge, limiting onboard computation to the filter equations when only initial values are stored.Source: §3.1 · p. 7

3.2 Proposed intrinsic filters

  • The proposed intrinsic filters mimic linear prediction and update by replacing addition with group multiplication, with an isomorphic representation recovering the linear filter as a special case.The construction is motivated by continuous-time symmetry-preserving observers.Source: §3.2 · p. 7
  • The invariant error processes are Markov and independent of system inputs; when inputs are known or fixed, gains can be optimized offline independently of the system trajectory.The paper identifies multiple possible gain-tuning choices for the broad filter class.Source: §3.2 · p. 7

4 Fixed gains filters

With gains fixed, the paper studies when noisy invariant-filter errors converge in distribution rather than to a single state, then applies the framework to attitude estimation and artificial-horizon design. It also uses the resulting stationary error behavior to tune gains offline, including a comparison with the MEKF under outlier-contaminated noise.

4 Fixed gains filters

  • Holding gains fixed makes the error equation a homogeneous Markov chain, and under suitable conditions its noisy distribution forgets its initial law and converges to a stationary distribution.The deterministic filter may converge almost globally, but unpredictable noise prevents convergence to a single point.Source: §4 · pp. 7–8, §4.1 · p. 8

4.1 Convergence results

  • Under the stated confinement, noise, and noise-free convergence conditions, the error law converges in total variation to a unique stationary distribution.The confinement condition is automatically satisfied on compact groups such as SO(3).Source: §4.1 · p. 8
  • For compact groups, the convergence result holds globally without the confinement assumption; a further theorem permits weaker noise-free convergence under additional compactness and connectedness conditions.That theorem also states an isotropy-based invariance property for the stationary law.Source: §4.1 · p. 8

4.2 Application to attitude estimation

  • For attitude estimation on SO(3), the proposed discrete invariant observer is almost globally convergent without noise, and its noisy error distribution converges independently of its initial distribution.The discrete-time proof uses a Lyapunov function based on weighted squared differences between transformed reference vectors and those vectors.Source: §4.2 · p. 9

4.3 Learning robust gains: application to the design of an artificial horizon

  • Because the asymptotic error law depends on the gain but not the system trajectory, gains can be optimized offline and selected for speed or low error dispersion, reducing online computation.The paper presents this strategy in its artificial-horizon application.Source: §4 · p. 8, §4.3 · p. 9

4.3.1 Convergence results

  • For the artificial horizon, the gain limits the correction angle rather than rejecting large accelerometer deviations outright, while retaining noise-free convergence for the stated parameter range.The design addresses accelerometer outliers associated with aircraft acceleration; outright rejection could prevent convergence from a distant initialization.Source: §4.3 · pp. 9–10, §4.3.1 · p. 10
  • For the artificial-horizon filter, the noisy error converges in total variation to a stationary law independent of its initial distribution, while its noise-free observation error converges to zero.The paper evaluates asymptotic error dispersion over a grid of gain parameters.Source: §4.3.1 · p. 10, §4.3.2 · p. 10

4.3.2 Numerical asymptotic gain optimization

  • RMSE = 8.02 × 10−4 for the asymptotic invariant filter, versus RMSE = 4.3 × 10−3 for the MEKF under the tested outlier-contaminated noise.The invariant filter’s selected gains were k = 0.1202 and λ = 0.0029.Source: §4.3.2 · p. 10

5 Gaussian filters

The Gaussian-filter section develops invariant filters for transient estimation: an IEKF uses linearization and Gaussian approximations, while an IEnKF uses particle samples to estimate error statistics. Their simulations compare these approaches and show convergence of IEKF gains and stronger uncertainty capture by the IEnKF.

5 Gaussian filters

  • The section’s transient-phase filters compute gains at each step using Gaussian noise approximations and linearizations, and provide an approximation of estimation uncertainty.The section introduces the IEKF and particle-based IEnKF as two approaches.Source: §5 · p. 10

5.1 The discrete-time Invariant EKF

  • For the artificial-horizon example, the optimized invariant filter has k = 0.1202 and λ = 0.0029 and handles outliers while remaining efficient without them, unlike the MEKF.The figure compares the invariant filter with an outlier-ignoring MEKF and an MEKF with numerically adjusted observation-noise covariance.Source: Figure 1 · p. 11
  • The discrete-time IEKF linearizes the invariant error equation under small-error and small-noise assumptions, then applies Kalman gain tuning to the nonlinear model.Its gains and error covariance converge, allowing the gain eventually to be held at its limiting value.Source: §5.1 · pp. 10–11

5.1.2 Convergence of the gains

  • With fixed left inputs, an observable pair (Adϒn,Hξ), and full-rank Qw, the IEKF covariance Pn and gain Kn converge to fixed values.The limiting covariance is easier to interpret, and using the limiting gain yields a numerically cheap asymptotic IEKF.Source: §5.1.2 · p. 12

5.1.1 Linearization of the equations and IEKF formulas

  • The IEKF maps group-valued errors to the Lie algebra and uses first-order developments of the propagation and observation functions to form the Kalman calculations.The passage describes the Baker-Campbell-Hausdorff formula with only first-order terms retained.Source: §5.1.2 · p. 12, §5.1.1 · p. 12

5.2 The discrete-time Invariant EnKF

  • When the error equation is input-independent, the IEnKF samples the error density offline and uses Monte Carlo estimates of innovation and error covariance to compute gains.This replaces linearizations with recursively generated particle samples.Source: §5.2 · p. 12

5.3 Simulations

  • In simulations, the IEnKF gives a more satisfactory error-dispersion envelope than the linearized filters; the MEKF and IEKF perform comparably, and the IEKF gains converge.The IEnKF is recommended when extensive simulations are feasible and gains can be stored over the trajectory.Source: §5.3 · p. 13

6 Conclusion

The conclusion summarizes fixed-gain convergence in distribution, asymptotic covariance convergence for the IEKF, and uncertainty estimation by the invariant EnKF. Simulations show that the IEnKF captures error dispersion better than the compared filters, while several extensions and open questions remain.

6 Conclusion

  • The IEKF gain coefficients converge in the displayed comparison, and only 4 of 18 coefficients are nonzero in that case, enabling sparse implementation.The MEKF and USQUE coefficients evolve erratically in the same figure.Source: Figure 2 · p. 14
  • With fixed gains, the estimation error provably converges in distribution; the IEKF covariance converges asymptotically, and the invariant EnKF indicates estimate uncertainty.These are distinct properties of the paper’s fixed-gain and transient-phase approaches.Source: §6 · p. 14
  • The authors identify open directions including characterizing when IEKF gains are sparse and whether that sparsity saves computation or memory.They also point to unscented-transform and information-filter approaches for Lie groups.Source: §6 · p. 14
  • In 1000 simulations, only the IEnKF’s 3σ envelope captures error dispersion satisfactorily, containing 99% of simulated trajectories.The MEKF and IEKF give similar results; USQUE does slightly better but is outperformed by IEnKF.Source: Figure 3 · p. 14
  • Asymptotic-gain optimization was performed naively because efficient density simulation was beyond the paper’s scope; the authors identify MCMC acceleration as future work.The authors also leave proof of IEKF convergence in distribution for future work, describing it as conjectured and difficult.Source: §6 · p. 14

A Proof of Proposition 2

The proof introduces a matrix embedding for vectors and states that the transformed equations correspond to the original equations in R^n.

A Proof of Proposition 2

  • The appendix introduces an isomorphism φ_X that embeds a vector X in a structured square matrix.The displayed fragment specifies an (N+1)-dimensional matrix construction.Source: A Proof of Proposition 2 · p. 14
  • Using χn = φ_X(Wn), Ωn = φ_X(Bn), and ϒn = I, the text states that equations (7) become the displayed χ-equation.The supplied equation fragment is incomplete, so the summary does not infer additional algebraic steps.Source: A Proof of Proposition 2 · p. 15
  • The resulting expression for χ corresponds to equation (7) in R^n.Source: A Proof of Proposition 2 · p. 15

B Proofs of the results of Section 4.1

The proofs use Harris-chain results to show how noiseless convergence and sufficiently frequent small noise yield convergence of the error density to a stationary law. Applied to fixed-gain invariant filters, the argument also establishes initialization independence and, under equivariance, symmetry of the limiting law.

B Proofs of the results of Section 4.1

  • A local transition lower bound near the noiseless iterate, together with iterative dilation and pre-images covering C, supplies the mixing condition.The noise represents the combined model and observation disturbances.Source: B Proofs of the results of Section 4.1 · p. 15
  • If the sets U_n eventually cover C, the chain’s density converges in total variation to a limit independent of initialization.The theorem’s conclusion is convergence to a unique stationary distribution.Source: B Proofs of the results of Section 4.1 · pp. 15–16
  • The proof establishes a positive chance of moving from each U_{n+1} into U_n, then shows the first hitting time of U_1 is almost surely finite.The hitting-time argument uses repeated returns and the non-vanishing transition probability.Source: B Proofs of the results of Section 4.1 · pp. 15–16
  • The nested-set argument establishes Harris recurrence and aperiodicity, while local reachability supplies the needed controlled transitions within the central region.These properties permit the stationary-density convergence result to be applied.Source: B Proofs of the results of Section 4.1 · pp. 15–16
  • For fixed gains, the invariant-filter error satisfies the chain conditions, and its density converges in total variation to a stationary distribution when its support lies in C.The proof verifies eventual coverage using almost-everywhere convergence of the noiseless iterates.Source: B Proofs of the results of Section 4.1 · pp. 16–17
  • Under the equivariance conditions, left multiplication preserves the error-process law, so the asymptotic distribution inherits that invariance.Source: B Proofs of the results of Section 4.1 · p. 17

C.1 Proof of Proposition 4

The proof interpolates the discrete sequence with a continuous path along which the Lyapunov function decreases. Compactness and uniqueness of the path’s adherence value then establish convergence to the identity.

C.1 Proof of Proposition 4

  • Along each interval between integer times, the Lyapunov function is nonincreasing; bounded sublevel sets keep the interpolated trajectory compact.The argument uses continuity of E to extend the decrease across integer times.Source: §C.1 · p. 18
  • The interpolated trajectory remains in a compact set, and every adherence value is the identity, so the discrete sequence converges to Id.The proof first shows that E decreases and converges along the path.Source: §C.1 · p. 18

C.2 Proof of Proposition 3

For the SO(3) observer, the proof identifies the error feedback as the gradient of a Lyapunov function. That function decreases except at one excluded initial rotation, yielding almost-global convergence to Id.

C.2 Proof of Proposition 3

  • For every γ ∈ SO(3), the chosen feedback satisfies γ·k(γ) = gradE(γ), which the proof verifies as the first condition for applying Proposition 4.The proof defines k(γ) = k1(γᵀb1) × b1 + k2(γᵀb2) × b2 and E(γ) = k1||γᵀb1 − b1||2 + k2||γᵀb2 − b2||2.Source: §C.2 · p. 18
  • Except for the rotation about b1×b2 by angle π, E is strictly decreasing and Id is the only stationary point in the relevant sublevel set.Applying Proposition 4 then proves the discrete observer’s almost-global convergence.Source: §C.2 · p. 18
Loading 1310.2539v2…