Hello,
I'm Dr. Julius Marshall with the Naval Air Warfare Center Aircraft Divsion. I would greatly appreciate some help from a dev team member about xy-drift in the EKF2 when MOCAP data is fused. It might be clearer to explain the issue over the phone, please email me at julius.a.marshall.civ@us.navy.mil so we could exchange contact information. We are pressed for time, we have a demonstration in a couple of weeks, so I would appreciate it if you contacted me this way. I'll explain the issue below, too.
We want to test a custom motion planner using MOCAP data as an input to the PX4's EKF2. I followed the instructions in this post (https://forum.modalai.com/topic/2118/send-motion-capture-system-pose-data-to-px4/4?_=1689004027042) to enable motion capture data fusion. The main issue is that the LOCAL_POSITION_NED xy-positions consistently track the motion capture solution for about 60 seconds then diverge. The z-position is unaffected. Furthermore, the quaternion (/mavros/local_position/pose) and ATTITUDE in QGC are showing a slow yaw rotation despite the vehicle being still. This happens from the moment voxl-px4 is started, but only when streaming MOCAP data to PX4.
We are using the VOXL 2 on a custom multirotor airframe. To give an idea of the software setup, I followed the forum post to enable dual IMX412 camera streams (https://forum.modalai.com/topic/1612/dual-imx412-connected-to-voxl-2/16?=1689004027034), modified the voxl-tag-detector to enable tag detection on HIRES images (https://forum.modalai.com/topic/2145/voxl-2-dual-imx412-april-tag-detection?=1689004027039), and modified the voxl-vision-hub (v.1.6.5) and voxl-mavlink-server (latest) to enable connections from multiple (custom) ground control stations from one or multiple computers.
The magnetometer is off (SYS_HAS_MAG = 0), and tested with the GPS/mag connected and without.
All EKF2 time delays for unused sensors are set to zero, consistently with (https://docs.px4.io/v1.9.0/en/advanced_config/tuning_the_ecl_ekf.html),
EKF2_AID_MASK = 24 (vision_position and vision_yaw),
MAV_ODOM_LP = 1 (/mavros/odometry/in closely follows MOCAP position).
The voxl-qvio-server is disabled, and the voxl-vision-hub.conf file has en_vio = false, and en_localhost... = true.
Commands are executed in separate terminals in the following order:
- VOXL 2 - roscore
- VOXL 2 - mavros (We implement a custom node to publish mavros/vision_pose/pose messages with timestamp of what we believe is the monotonic clock)
- HOST - Motion capture data stream
- QGroungControl is started on HOST
- VOXL 2 - voxl-mavlink-server
- VOXL 2 - voxl-px4 (prior to this we use rostopic echo to verify that MOCAP data can be seen from the VOXL 2)
From here we can observe the LOCAL_POSITION_NED message in QGC or equivalently the /mavros/local_position/pose topic. For the first 60 seconds or so, translational position is very close to the MOCAP data, however the quaternion's z and w components are changing. In the attitude message, the yaw angle is slowly moving, representing a counter-clockwise turn, this does not match the MOCAP data as the vehicle is perfectly still.
We observed the /mavros/imu/data_raw topic and the z component of the angular velocity (equivalent to the HIGHRES_IMU zgyro in QGC) is two orders of magnitude greater than the x or y components. When we stop streaming MOCAP data, and restart voxl-px4, the zgyro is much smaller while the vehicle is still, and the quaternion no longer drifts. At this point, it seems that the issue is with something in the MOCAP data. The /mavros/vision_pose/pose message populated by the mavros node is not publishing angular velocity/acceleration data. Any clues as to why zgyro behaves so much differently when the MOCAP data is streaming to PX4?
Alternatively, is there anyway to build the PX4 firmware used by voxl-px4 which can enable the LPE module? We have a setup with ODroid/Pixhawk using LPE that we use to fly with MOCAP frequently. We need the VOXL 2 however for the vision side of things (which is really impressive by the way).
We are slated for a demonstration in a couple of weeks, and I did not anticipate this issue with the EKF2. It is stalling testing.
Is there anyway I could get in contact with a developer today or tomorrow to fix this issue? I am available over the phone, but please email me at julius.a.marshall.civ@us.navy.mil
Very respectfully,
Julius Marshall