Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

Invariant Kalman Filter for Relative Dynamics

Tejaswi K. C., Maneesha Wickramasuriya, Silvere Bonnabel, Axel Barrau, Taeyoung Lee

TL;DR

This work tackles the challenge of estimating relative states between two dynamical systems on Lie groups by introducing state trajectory independence (STI) and the novel relative trajectory independence (RTI). By decomposing the system vector field into left-invariant, intrinsic, and right-invariant components, the authors show that RTI implies STI for the relative estimation error, enabling the design of invariant Kalman filters that preserve the Lie group structure and achieve exact error linearization. They develop both left- and right-invariant relative KF formulations with explicit propagation and correction steps, and validate them numerically on a vehicle-kinematics model, where the invariant filters outperform a quaternion EKF, especially when measurement and filter invariances are matched. The results offer a principled geometric framework for consistent, symmetry-preserving relative-state estimation with potential applications in multi-agent navigation, cooperative localization, and formation control.

Abstract

This paper develops a geometric framework for invariant filtering of relative dynamics on Lie groups. We first revisit the notion of state trajectory independence, under which the estimation error evolves autonomously, and derive new equivalent conditions by decomposing the system vector field into left-invariant, intrinsic, and right-invariant components. Building on this result, we introduce the concept of relative trajectory independence to characterize when the relative motion between two dynamical systems is autonomous. A key theoretical finding is that relative trajectory independence automatically ensures state trajectory independence for the corresponding estimation error. This connection provides the foundation for constructing invariant filters that preserve the Lie group structure, maintain exact linearization of the error dynamics, and enable consistent covariance propagation. These are illustrated with numerical examples.

Invariant Kalman Filter for Relative Dynamics

TL;DR

This work tackles the challenge of estimating relative states between two dynamical systems on Lie groups by introducing state trajectory independence (STI) and the novel relative trajectory independence (RTI). By decomposing the system vector field into left-invariant, intrinsic, and right-invariant components, the authors show that RTI implies STI for the relative estimation error, enabling the design of invariant Kalman filters that preserve the Lie group structure and achieve exact error linearization. They develop both left- and right-invariant relative KF formulations with explicit propagation and correction steps, and validate them numerically on a vehicle-kinematics model, where the invariant filters outperform a quaternion EKF, especially when measurement and filter invariances are matched. The results offer a principled geometric framework for consistent, symmetry-preserving relative-state estimation with potential applications in multi-agent navigation, cooperative localization, and formation control.

Abstract

This paper develops a geometric framework for invariant filtering of relative dynamics on Lie groups. We first revisit the notion of state trajectory independence, under which the estimation error evolves autonomously, and derive new equivalent conditions by decomposing the system vector field into left-invariant, intrinsic, and right-invariant components. Building on this result, we introduce the concept of relative trajectory independence to characterize when the relative motion between two dynamical systems is autonomous. A key theoretical finding is that relative trajectory independence automatically ensures state trajectory independence for the corresponding estimation error. This connection provides the foundation for constructing invariant filters that preserve the Lie group structure, maintain exact linearization of the error dynamics, and enable consistent covariance propagation. These are illustrated with numerical examples.

Paper Structure

This paper contains 20 sections, 9 theorems, 137 equations, 2 figures.

Table of Contents

  1. Introduction
  2. Invariant Kalman Filter
  3. Lie Group
  4. State Trajectory Independence of Estimation Error (ETI)
  5. State Trajectory Independence for Relative Motion
  6. Relative Motion Estimation Problem
  7. State Trajectory Independence of Relative Motion (RTI)
  8. Estimation Error for Left-Invariant Relative State ($L$-RETI)
  9. Estimation Error for Right-Invariant Relative State ($R$-RETI)
  10. Invariant Filter for Relative Dynamics
  11. Relative Dynamics
  12. Propagation with the Left-Invariant Error $f$
  13. Correction with the Left-Invariant Error $f$
  14. Propagation with the Right-Invariant Error $h$
  15. Correction with the Right-Invariant Error $h$
  16. ...and 5 more sections

Key Result

Lemma 1

Consider eqn:dot_g_G, rewritten as Without loss of generality, it can be decomposed as where $\tilde{X}_t : \mathbb{R} \times \mathsf{G} \to \mathsf{T} \mathsf{G}$ is a time-dependent vector field satisfying and $\xi, \zeta : \mathbb{R} \to \mathfrak{g}$ are trajectories in $\mathfrak{g}$ such that

Figures (2)

  • Figure 1: Summary of the results for state trajectory independence. The top row corresponds to a single trajectory, while the two shaded rows represent relative trajectories. The left column depicts the relative states, and the right column corresponds to their estimation errors. Thick lines indicate state trajectory independence of estimation errors. A solid double arrow denotes a logical implication, whereas a dotted arrow indicates that additional conditions are required for the implication.
  • Figure 2: Simulation results for different filter and measurement combinations.

Theorems & Definitions (35)

  • Definition 1
  • Lemma 1
  • proof
  • Example 1
  • Theorem 1
  • proof
  • Example 2
  • Corollary 1
  • proof
  • Example 3
  • ...and 25 more