GPS Denied Autonomy using ROS2 Humble, MAVROS, NAV2, RTABMAP SLAM

ArduCopter Copter 4.6
1

Hello ArduPilot Community,

I am experiencing an intermittent drifting issue with a GPS-denied autonomous quadcopter setup. The drone occasionally exhibits a classic circular drift (toilet bowling) during an autonomous takeoff, and also drifts after successfully completing a ROS 2 Nav2 goal.

Here are my system details and the exact symptoms.

Hardware & Software Setup

  • Flight Controller: Holybro Kakute H7 v2

  • Firmware: ArduPilot 4.6.2

  • Companion Computer: Orange Pi 5 Plus

  • Software Stack: ROS 2 Humble, MAVROS, Nav2, RTAB-Map

  • Sensors: * External Compass (Primary heading source, no internal compass)

    • Holybro PMW3901 Optical Flow

    • TFmini Plus LiDAR (Rangefinder)

EKF3 Configuration

I am generating odometry data from RTAB-Map, which is accurate down to ~0.01m. This is sent to ArduPilot via the /mavros/vision_pose/pose topic at 15Hz.

  • POSXY: Vision Position Estimate (EK3_SRC1_POSXY = 6)

  • VELXY: Optical Flow (EK3_SRC1_VELXY = 5)

  • POSZ: Rangefinder (EK3_SRC1_POSZ = 2)

  • YAW: External Compass (EK3_SRC1_YAW = 1)

The Symptoms

Issue 1: Inconsistent Takeoff Drift When initiating an autonomous takeoff using NAV_TAKEOFF (CommandToL) to a constant altitude, the drone frequently starts drifting in a circular direction (toilet bowling) while keeping the nose pointed straight ahead. Oddly, it sometimes takes off and hovers perfectly under the exact same conditions. I have not changed the physical testing location or the lighting, and I have ensured the ground is highly textured for the optical flow sensor.

Issue 2: Drift After Nav2 Navigation If the drone manages a stable autonomous takeoff and hover, I send a Nav2 goal pose. The drone responds correctly, navigates to the destination, and momentarily holds a stable hover upon arrival. However, after a few seconds at the destination, it begins to drift again. The destination area also has plenty of visual features on the ground.

Troubleshooting Done So Far

  • Frame Conversions: Verified that all ENU to NED conversions from ROS 2 to MAVROS are correct. I cross-checked the incoming MAVLink messages in the QGroundControl/Mission Planner MAVLink Inspector, and the vision position estimates look correct and track cleanly.

  • Lighting/Environment: Tested in consistent lighting with high-contrast ground features to rule out optical flow starvation.

Questions

  1. Could the 15Hz update rate from RTAB-Map be too slow and causing EKF fusion conflicts with the higher-rate optical flow data?

  2. Given the toilet-bowling behavior, I suspect a yaw alignment issue between the compass and the vision pose, but why would it hover perfectly sometimes and drift others, and specifically drift after reaching a Nav2 goal?

  3. Are there specific EK3_VIS_ or flow scaling parameters I should adjust to better harmonize the Optical Flow and Vision Pose inputs?

I have attached a .bin log of a flight where both the takeoff drift and post-navigation drift occur. Any insights on what the EKF is struggling with would be greatly appreciated!

MissionPlanner_1.log.zip (65.9 KB)

2 Likes