Visual Odometry Drift + “EKF3 IMU0/1/2 Stopped Aiding” After Switching to LOITER (Jetson Orin NX + RealSense + RTAB-Map)

ArduCopter
1

Hi everyone,

I am working on a Jetson Orin NX + Intel RealSense setup, using RTAB-Map to generate odometry and sending it to the flight controller through MAVROS vision_pose/odometry.

Initially everything works fine — I get good vision odometry, and the EKF starts aiding normally.
But as soon as I perform my first takeoff in Stabilize → switch to LOITER, the vehicle drifts heavily, and in the logs I repeatedly get:

EKF3 IMU0 stopped aiding
EKF3 IMU1 stopped aiding
EKF3 IMU2 stopped aiding
IMU X is using external nav data

So EKF3 keeps accepting and rejecting the vision data in a cycle.

My setup

  • Jetson Orin NX running ROS2

  • RealSense camera (D455)

  • RTAB-Map odom → /odom

  • MAVROS → /mavros/vision_pose/pose → FCU

  • Built indoor map + outdoor tests

Parameters I set on ArduPilot

VISO_TYPE = 1
EK3_ENABLE = 1

EK3_SRC1_POSXY = 6   (VISION)
EK3_SRC1_POSZ  = 2   (RANGEFINDER)
EK3_SRC1_YAW   = 6   (VISION)
EK3_SRC1_VELXY = 6
EK3_SRC1_VELZ  = 6

BRD_IMU_MASK = 7

I am using rangefinder for altitude and Vision for XY + yaw.

What I have already checked

  • ROS2 publishes in ENU, so I converted signs to match ArduPilot’s NED frame.

  • Verified static transforms for camera → base_link.

  • Confirmed MAVROS is receiving odometry correctly.

Current Problem

After switching to LOITER:

  • The drone drifts, as if EKF is rejecting VIO.

  • Messages keep repeating:
    “EKF3 IMU0/1/2 stopped aiding”

  • Local position keeps resetting.

It seems like EKF3 is unable to continuously fuse the vision data even though the data stream looks correct at MAVROS side.

What I need help with

  1. Are my EKF3 vision fusion parameters correct for this configuration?

  2. Do I need to publish full odometry (pose + twist) instead of only pose?

  3. Is it safe to use:

    • VISION for XY

    • RANGEFINDER for Z

    • VISION for yaw

    or does this create inconsistencies?

  4. How should I correctly handle ENU → NED conversion in ROS2 for ArduPilot?

  5. Is there a recommended way to confirm if EKF rejects due to:

    • covariance issues

    • timestamp mismatch

    • TF/frame issues

    • velocity inconsistency

Any guidance or example configurations for RealSense + RTAB-Map + ArduPilot will help a lot.
I am stuck on this “stopped aiding” loop for days.

Thanks in advance!