For a robot approaching another moving vehicle, the essential question is often “Where am I relative to you?” Estimating that relationship requires accounting for both vehicles’ motion, their rotating coordinate frames, and uncertain sensor measurements.
In Invariant Kalman Filter for Relative Dynamics, Tejaswi K. C., Maneesha Wickramasuriya, Silvère Bonnabel, Axel Barrau, and Taeyoung Lee develop a geometric framework for answering this question directly. The work connects mathematical conditions for relative motion to practical estimators, with numerical simulations and hardware experiments in the revised manuscript. The journal paper is submitted to IEEE Transactions on Automatic Control.
When can relative motion be estimated directly?
One approach is to estimate each vehicle’s absolute state, then calculate their relative state. But this maintains two absolute descriptions even when only their relationship matters.
The paper asks when the relative state has its own closed equations of motion: equations that depend on the relative state and the two vehicles’ inputs, without requiring their individual absolute trajectories. This property is called relative trajectory independence.
Using Lie groups, which describe rotations and rigid-body motion while preserving their geometric constraints, the authors derive conditions for this property. For the class of systems studied, they show that the resulting relative dynamics also possess the structure needed for invariant filtering. This provides a systematic route from two physical systems to a reduced relative-state estimator.
Why the geometry of the error matters
A conventional extended Kalman filter approximates nonlinear error dynamics around its current estimate. When that estimate is poor, the approximation can also be poor.
An invariant filter defines the estimation error through the geometry of the state space. Under the paper’s conditions, the deterministic error dynamics become independent of the estimated trajectory, and the error’s logarithmic coordinates satisfy an exact linear differential equation. The measurement frame also guides the choice of invariant correction, allowing a state-independent measurement Jacobian for the measurement models considered.
This is an exact structural result for the deterministic error. The noisy filter still uses a first-order covariance approximation and a Gaussian uncertainty model. The benefit is a principled propagation and correction structure, rather than an exact solution to every nonlinear estimation problem.
Recovering from a large initial heading error
The numerical study considers two independently driven planar vehicles. Across 100 paired Monte Carlo runs, a left-invariant relative Kalman filter (LRKF) and a conventional EKF receive the same inputs, measurements, and noise realizations.
With a 150° initial heading error, the LRKF corrects its orientation and position more rapidly in the illustrated test. Both filters start at the correct relative position, but the incorrect heading causes their position estimates to drift before the measurements correct them.
The broader parameter study shows that the advantage depends on initialization and measurement noise. For smaller initial heading errors, the preferred filter is not uniform across all tested conditions.
From the equations to a flying robot
The hardware experiment pairs a manually driven F1TENTH ground vehicle with an autonomous quadrotor. Each carries an inertial measurement unit. Two filters run onboard the quadrotor using identical data: a bias-augmented LRKF and a bias-augmented quaternion EKF (QEKF).
Both propagate at 200 Hz and receive relative-pose corrections at 5 Hz. The pose measurements are derived from Vicon motion capture and deliberately corrupted with noise; the uncorrupted poses provide ground truth. This isolates the relative-estimation problem while using real vehicle motion and inertial sensing.
The tests deliberately include a large initial velocity error and a mismatch between the injected pose noise and the filters’ assumed measurement uncertainty. Across three 60-second trials, the LRKF has lower root-mean-square errors in position, velocity, and attitude than the QEKF.
| Trial | LRKF position error | QEKF position error | LRKF attitude error | QEKF attitude error |
|---|---|---|---|---|
| 1 | 5.8 cm | 6.9 cm | 5.83° | 6.86° |
| 2 | 7.1 cm | 8.4 cm | 5.27° | 6.20° |
| 3 | 13.7 cm | 15.2 cm | 7.22° | 8.03° |
These values are RMS error norms over each full trial, including the initial transient.
The experiments demonstrate real-time operation under practical sensing and communication conditions. The exact theoretical error structure applies to the underlying bias-free kinematics; it is not claimed for the additional bias states used in the hardware implementation.
A foundation for cooperative navigation
The central contribution is a way to determine when relative estimation can be simplified without discarding its geometry. That foundation is relevant to cooperative localization, formation flight, rendezvous, and navigation relative to a moving platform.
The work complements FDCL’s vision-based maritime flight research: perception supplies relative observations, while geometric filtering combines those observations with motion information. Shipboard launch and recovery are future applications of this framework; the hardware validation reported here uses the indoor aerial–ground vehicle experiment.
Paper
Tejaswi K. C., Maneesha Wickramasuriya, Silvère Bonnabel, Axel Barrau, and Taeyoung Lee, Invariant Kalman Filter for Relative Dynamics, IEEE Transactions on Automatic Control, submitted, 2026.
Read the public preprint · Earlier IFAC conference paper
The simulation and hardware results presented here are from the revised journal manuscript. The public arXiv version, last revised in November 2025, predates the hardware results described above.
