VOXL 2 Motion Capture EKF2 Issue XY-Drift
-
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.milVery respectfully,
Julius Marshall
-
@majjam I don't have any insight into most of the issues you discuss but there are a couple of things that may or may not help. First, to completely disable magnetometer you have to set SYS_HAS_MAG to 0 and EKF2_MAG_TYPE to 5. And yes, you can build voxl-px4 and enable different modules. What version of voxl-px4 do you have now? If you are not using our latest we can give you access to our 1.14.0 based build that has a lot of updates since the 1.12.x based build and may be a better base for your testing.
-
@Eric-Katzfey Hi Eric, thanks for the reply.
I have completely disabled the magnetometer with SYS_HAS_MAG = 0 and EKF2_MAG_TYPE = 5 (None). I forgot to mention EKF2_MAG_TYPE in my first post.
I currently am using v1.12.31beta (listed on QGC under firmware) and voxl-vision-hub v.1.6.5. If you could give me access to your latest build that would be much appreciated. With the latest voxl-px4 I'll try to blend MOCAP with the EKF2 and hopefully the yaw/xy-position do not diverge. I'm not sure if the latest build of yours has LPE enabled, but if not I'll follow (https://docs.modalai.com/voxl2-px4-build-guide/) to build it in case the EKF2 still gives issues.
We have decided that for the time being we will be integrating the VOXL 2 with a Pixhawk board. This Pixhawk is running a modified version of PX4 v1.9.0 and the local position estimator. We are using a splitter to connect TELEM2 and Serial4/5 ports to the VOXL 2 using custom USB cables. We are leveraging the VOXL 2's sensors and using it as an NDAA compliant means to implement custom perception, estimation, and controls packages which are too computationally expensive for other NDAA compliant boards like ODroid XU4. This is a temporary setup, and I am describing this for others who may run into similar issues when integrating MOCAP. In the future we plan to remove the Pixhawk completely so I will certainly be reaching out for assistance. Thank you again.
Respectfully,
'
Julius Marshall -
@majjam Our latest SDK 1.0.0 is now available on our downloads page: https://developer.modalai.com/asset/2
-
@Eric-Katzfey Awesome, thank you.