Manifold-Optimized Error-State Kalman Filter for Robust Pose Estimation in Unmanned Aerial Vehicles

B. Jia, Zongwen Bai, Yiqun Gao, Dong Wang, Meili Zhou, Pengfei Gao, Pei Zhang, Yang ZhangJournal of Electronic Research and Application 2025 · DOI

TL;DR

This paper introduces a manifold-optimized Error-State Kalman Filter (ESKF) that streamlines UAV pose estimation by treating rotations as elements of a manifold rather than flat Euclidean space. By combining an error-state approach with a clever adaptive Kalman gain strategy, the authors achieve higher accuracy and massive computational speedups—reducing processing time for high-dimensional data from seconds to milliseconds.

What Problem Does This Paper Address?

For Unmanned Aerial Vehicles (UAVs), knowing exactly where you are and how you are oriented (your "pose") is a matter of survival. Standard approaches rely on sensor fusion—combining "noisy" data from Inertial Measurement Units (IMUs) with "absolute" data from GPS or LiDAR.

However, two major hurdles persist:

  1. The Geometry of Rotation: Rotations are inherently non-linear. Traditional methods use Euler angles (which suffer from "gimbal lock" singularities), rotation matrices (which use 9 numbers to describe 3 degrees of freedom), or quaternions (which require constant re-normalization). These redundancies waste memory and can cause numerical instability.
  2. Computational Cost: Kalman filters require inverting matrices. When dealing with high-dimensional data like LiDAR, this inversion becomes a bottleneck that can prevent real-time flight control on low-power onboard computers.

Methods

The authors propose a framework that optimizes the Error-State Kalman Filter (ESKF) using manifold theory:

  • Manifold-Based Representation: Instead of forcing rotations into a standard vector space, they treat them as part of the $SO(3)$ manifold. They use exponential and logarithmic mappings to transform rotation vectors (3 degrees of freedom) into rotation matrices. This eliminates redundancy and avoids singularities like gimbal lock.
  • Error-State Formulation: The filter tracks a "nominal" state (the estimated trajectory) and an "error" state (the small deviations). Because the error state is kept very close to the origin, the authors can safely ignore complex second-order terms in their math. This simplifies the Jacobian matrices, making the filter more stable and efficient.
  • Adaptive Kalman Gain: This is the paper's "secret sauce" for speed. The authors introduce two different formulas for calculating the Kalman gain ($K$). The system automatically switches between them based on the dimension of the sensor data ($m$). If $m \le 18$ (like GPS), it uses one formula; if $m > 18$ (like LiDAR), it uses another. This ensures the filter always inverts the smallest possible matrix, drastically reducing the computational load.

Datasets

The researchers evaluated their algorithm using the GNSS-INS-SIM tool, a high-fidelity simulation environment used to generate IMU and GPS data. This allowed them to compare their filter's output against a perfect "ground truth" trajectory to measure exact errors in position and orientation.

Key Insights and Ideas

  • Minimalism is Efficiency: By using a minimal three-degree-of-freedom representation for rotation, the authors reduce the memory footprint and the number of operations required per update.
  • Linearization at the Origin: By ensuring the error state always stays near zero, the linear approximations used in the Kalman filter become much more accurate than they would be in a standard Extended Kalman Filter (EKF).
  • The Inversion Shortcut: The realization that you can mathematically rearrange the Kalman gain formula to avoid inverting large matrices is a game-changer for resource-constrained hardware.

Key Results

The proposed Manifold Space ESKF was compared against two industry standards: the Quaternion-based ESKF and the Multiplicative EKF (MEKF).

  • Higher Accuracy: The Manifold ESKF achieved a Root Mean Square Error (RMSE) of 3.546, significantly lower than the Quaternion-based (4.716) and Multiplicative (4.710) versions.
  • Massive Speedups: The adaptive gain formula showed its strength with high-dimensional data. For an observation dimension of 1532, the "old" standard formula took 1974 ms to compute, while the "new" adaptive formula took only 1.54 ms.
  • Robustness: Simulations confirmed the algorithm remains stable and accurate even during complex maneuvers where traditional Euler-based systems might hit a gimbal lock.

Limitations and Future Work

While the math is robust, the authors note a few areas for improvement:

  • Dynamic Switching: The current system selects a gain formula based on data dimensionality but doesn't yet support "hot-switching" logic for when a sensor fails (e.g., moving from GPS-denied environments to LiDAR-heavy ones seamlessly).
  • Real-World Hardware: The study relies on simulations. Future work will involve deploying the algorithm on physical, resource-constrained UAV platforms to test its performance against real-world sensor noise and vibration.

Summary updated

Abstract

This paper presents a manifold-optimized Error-State Kalman Filter (ESKF) framework for unmanned aerial vehicle (UAV) pose estimation, integrating Inertial Measurement Unit (IMU) data with GPS or LiDAR to enhance estimation accuracy and robustness. We employ a manifold-based optimization approach, leveraging exponential and logarithmic mappings to transform rotation vectors into rotation matrices. The proposed ESKF framework ensures state variables remain near the origin, effectively mitigating singularity issues and enhancing numerical stability. Additionally, due to the small magnitude of state variables, second-order terms can be neglected, simplifying Jacobian matrix computation and improving computational efficiency. Furthermore, we introduce a novel Kalman filter gain computation strategy that dynamically adapts to low-dimensional and high-dimensional observation equations, enabling efficient processing across different sensor modalities. Specifically, for resource-constrained UAV platforms, this method significantly reduces computational cost, making it highly suitable for real-time UAV applications.
BibTeX
@article{jia2025manifold,
  author = {B. Jia and Zongwen Bai and Yiqun Gao and Dong Wang and Meili Zhou and Pengfei Gao and Pei Zhang and Yang Zhang},
  title = {{Manifold-Optimized Error-State Kalman Filter for Robust Pose Estimation in Unmanned Aerial Vehicles}},
  journal = {Journal of Electronic Research and Application},
  year = {2025},
  doi = {10.26689/jera.v9i2.10093},
}
Figure