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

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.