Robust Tightly-Coupled Filter-Based Monocular Visual-Inertial State Estimation and Graph-Based Evaluation for Autonomous Drone Racing

The evolution of autonomous drone navigation has seen significant progress with the development of visual-inertial systems like VINS-Mono and OKVIS, which combine camera and IMU data for pose estimation. Early competitions utilized diverse sensors, including stereo cameras and LiDAR, but these setups were costly and less practical for micro-drones. Recent efforts focus on monocular solutions due to their simplicity and affordability, exemplified by systems like DeepVIO and OpenVINS. Despite these advances, high-speed flight introduces severe challenges such as motion blur, vibration, and environmental variability, which degrade feature detection and tracking. Traditional PnP algorithms require multiple features and are sensitive to noise, limiting their robustness. Moreover, the lack of standardized, quantitative benchmarks hampers consistent evaluation, especially in GNSS-denied environments. Overall, achieving real-time, robust, and precise state estimation during extreme maneuvers remains an open challenge, motivating the development of more resilient algorithms.

The core problem addressed is maintaining accurate, robust state estimation for high-speed, aggressive drone maneuvers in environments lacking external references. Existing methods depend heavily on multiple visible features and RANSAC-based outlier rejection, which introduce latency and are prone to failure under motion blur and occlusion. The rigid requirement of four or more features limits the system’s responsiveness and early correction capability, leading to drift and instability. Additionally, the absence of standardized evaluation metrics complicates performance benchmarking. The challenge is to develop a lightweight, fast, and robust estimator that can operate reliably with minimal visual information, supporting speeds over 20 m/s while ensuring safety and control precision.

This work introduces several key innovations: 1) integrating pixel reprojection residuals directly into an Error-State Kalman Filter, enabling valid updates with only two corners; 2) replacing RANSAC with a robust reweighting scheme, significantly reducing latency and improving outlier handling; 3) designing an offline full-batch factor graph optimization (ADR-FGO) that refines the entire trajectory, providing a high-accuracy reference for evaluation; 4) supporting high-speed, feature-sparse estimation without reliance on external systems like GNSS or MoCap. These innovations collectively improve robustness, computational efficiency, and accuracy, especially under extreme dynamics, marking a substantial advancement over prior methods that depend on multiple features and complex outlier rejection.

• �� Visual detection: Use deep learning models (RTMO) to detect gate corners, perform reordering based on camera pose to resolve corner switching.
• �� Association: Match detected corners to known gate models using spatial and scale consistency, minimizing a cost function.
• �� State prediction: Use high-frequency IMU data with kinematic models to propagate the system state.
• �� Error integration: Incorporate pixel reprojection residuals as innovation terms within the ESKF, supporting minimal two-corner updates.
• �� Outlier handling: Apply a Huber-based robust reweighting scheme to suppress outliers, replacing RANSAC.
• �� Offline optimization: Use a factor graph that integrates IMU pre-integration, corner reprojection, and extrinsics refinement, producing a globally consistent high-precision trajectory.
• �� Implementation: Real-time detection with TensorRT, factor graph optimization with SymForce, validated on TII-RATM dataset and real-world races.

The system was evaluated on the TII-RATM dataset, which provides synchronized IMU, monocular camera, and ground-truth data at 500Hz, 120Hz, and 275Hz respectively, in an indoor arena. Multiple sequences were used to tune parameters and compare against baselines like OpenVINS and PnP+EKF. The experiments measured translation, rotation, and velocity RMSE, demonstrating significant improvements. The ablation studies analyzed the impact of corner visibility and the number of corners used. The offline ADR-FGO was validated by comparing its trajectory to ground-truth, achieving an average error of 0.060 meters. The real-world deployment during the A2RL Season 2 confirmed the system’s robustness at speeds over 20 m/s, maintaining stable estimates despite noisy detections and aggressive maneuvers.

ADR-VINS achieved an average translation RMSE of 0.143 meters, outperforming traditional methods by over 65%. Supporting only two corners per update, it maintained low drift during partial gate visibility. The offline ADR-FGO refined the trajectory to an RMSE of 0.060 meters, an 88% improvement over MAPLAB. During high-speed flights exceeding 20 m/s, the system demonstrated stable, robust estimation, validating its practical utility. Ablation results showed that reducing the minimum corners to two still yielded high accuracy, highlighting the method’s efficiency. The combined real-time and offline approaches significantly advance the state of high-speed visual-inertial navigation.

This framework is suitable for high-speed drone racing, where rapid, accurate localization is critical. It can be adapted for search and rescue in GPS-denied environments, providing reliable navigation in complex terrains. The system’s robustness supports integration into commercial autonomous drones, enabling safe operation in urban or indoor environments. Future developments could include multi-sensor fusion, deep learning-based detection, and real-time global optimization, broadening its applicability to diverse autonomous systems.

Detection robustness may decline under extreme lighting or occlusion, affecting stability. Offline optimization requires high computational resources, limiting real-time deployment. Dependence on corner detection makes the system vulnerable to detection failures or mismatches. Future work should focus on improving detection robustness, reducing computational load, and integrating additional sensors to enhance environmental adaptability.