Drift in Velocity Control using SITL

Dear Pixhawk Community,

We have been trying to get the iris-drone to fly stable in px4-SITL with velocity commands for two weeks now, but we have not yet succeeded.
Our Project outlines:

  • Ubuntu 20.04.6
  • ROS Noetic
  • PX4-Autopilot 1.14.0 as SITL
  • Gazebo 11.14.0
  • mavros 1.18.0
  • Simulating the IRIS Drone
  • QGroundControl 4.3.0

We followed these two guides: Ubuntu Dev. Env. and ROS with Gazebo Sim. to set up the SITL Simulation.
Then we use ROS to connect to the drone, get it into Offboard-Mode and to control it with poses and velocities.
For the position estimation we later are going to use Optitrack. To simulate it we extract the ground truth pose from gazebo (like described here) and publish it to the /mavros/odometry/out topic.

Now, controlling the drone with poses (/mavros/setpoint_position/local) works perfect. But when we send velocities (/mavros/setpoint_velocity/cmd_vel_unstamped) the drone is always drifting up. Especially when sending only angular velocities (neg. or pos., doesn’t matter) the iris-drone is always going up in the beginning (see image), after some time it starts to go down/up/down/up.
Where could this come from? We also did a rosbag and you can see the corresponding graph in the second image.

We believe that our problem sits (in front of the Computer, of course) somewhere in the EKF and the used sensors but we can’t figure out where. We turned off all (?) the other Sensors that the EKF uses: Barometer, Magnetometer, Range-Finder, GNSS/GPS. And turned on the Extrennal Vision Sensor Aiding except for the 3D Velocities option, because with this option the drone went completely nuts for angular velocities, probably because of some wrongly oriented reference frames. And also turned on Vision for the EKF2_HGT_REF parameter.

Turning off the EKF2_IMU_CTRL helped to reduce the drone’s upward drift, but we are still not satisfied with the behaviour of the drone. Could it also be possible that the IMU and Accelerometer is messing with us?

Thank you very much in advance for everyone who takes time looking at our problem!

That is what happens when the drone should just rotate in two meters hight around the Z-Axis:

For the graph:

:orange_circle: Orange = ground truth velocities by gazebo
:large_blue_circle: Blue = assumed velocities by px4
:green_circle: Green = desired velocities we are sendig to px4