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.
Alomari A, Phillips W, Aslam N, et al., 2017, Dynamic Fuzzy-Logic Based Path Planning for Mobility-Assisted Localization in Wireless Sensor Networks. Sensors, 17: 1904.
Yurtsever E, Lambert J, Carballo A, et al., 2020, A Survey of Autonomous Driving: Common Practices and Emerging Technologies. IEEE Access, 8: 58443–58469.
Sabatini AM, 2011, Estimating Three-Dimensional Orientation of Human Body Parts by Inertial/Magnetic Sensing. Sensors, 11: 1489–1525.
Sola J, 2017, Quaternion Kinematics for the Error-State Kalman Filter. arXiv, https://doi.org/10.48550/arXiv.1711.02508
Zhao S, Zhang H, Wang P, et al., 2021, Super Odometry: IMU-centric LiDAR-Visual-Inertial Estimator for Challenging Environments. IEEE International Conference on Robotics and Automation (ICRA), 2021: 14193–14200.
Forster C, Carlone L, Dellaert F, et al., 2017, On-Manifold Preintegration for Real-Time Visual-Inertial Odometry. IEEE Transactions on Robotics, 33(1): 1–21.
Qin T, Li P, Shen S, 2018, Vins-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator. IEEE Transactions on Robotics, 34(4): 1004–1020.
Yu Z, Zhu L, Lu G, 2021, Vins-motion: Tightly-Coupled Fusion of Vins and Motion Constraint. 2021 IEEE International Conference on Robotics and Automation (ICRA), 71(6): 5799–5810.
Bloesch M, Burri M, Omari S, et al., 2017, Iterated Extended Kalman Filter Based Visual-Inertial Odometry Using Direct Photometric Feedback. International Journal of Robotics Research, 36(10): 1053–1072.
Xia R, Pei H, 2020, Ranging-Aided Aerobridge Navigation Using Dual Quaternion Based Multiplicative Extended Kalman Filter. 2020 International Symposium on Autonomous Systems (ISAS), 2020: 1–6.
Xiang G, 2019, 14 Lessons of Visual SLAM: From Theory to Practice (2019), Electronic Industry Press, Beijing.
Hertzberg C, Wagner R, Frese U, et al., 2013, Integrating Generic Sensor Fusion Algorithms with Sound State Representations Through Encapsulation of Manifolds. Information Fusion, 14(1): 57–77.
Crassidis JL, Junkins JL, 2011, Optimal Estimation of Dynamic Systems. CRC Press, New York.
Xiang G, 2023, SLAM in Autonomous Driving and Robotics: From Theory to Practice, Electronic Industry Press, Beijing.
Xu W, Zhang F, 2021, Fast-Lio: A Fast, Robust Lidar-Inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter. IEEE Robotics and Automation Letters, 6(2): 4675587.