Mainline PX4 or voxl-px4 bug causing inconsistent Vz and z estimates by EKF2?

hmlow

Hi all

Im encountering an issue where the Vz and z estimated by QVIO is accurate but the fused Vz and z estimated by PX4's EKF2 is not, and in fact, inconsistent and moving in different directions. Im not sure if its a mainline PX4 or ModalAI PX4's bug, but i would appreciate if someone could take a look?

I posted the whole issue in detail on mainline PX4's github here:
https://github.com/PX4/PX4-Autopilot/issues/22196

Im on SDK1.0 and voxl-px4 is voxl-px4_1.14.0-2.0.43.

Many thanks

Moderator

have you configured your extrinsics?

https://docs.modalai.com/configure-extrinsics/

You may have a rotation that is affecting PX4's position estimate

Or, PX4 has parameters that effect the vision input https://gitlab.com/voxl-public/voxl-sdk/utilities/voxl-px4-params/-/blob/master/params/v1.14/platforms/Starling_V2.params?ref_type=heads#L104

hmlow

@Moderator yes I have configured the extrinsics.
I should also add that I have been flying this configuration for the last 6 months entirely indoors without any problems. I would imagine that extrinsics should not be the issue here since it affects only the qvio-server outout. Qvio's Vz and z are correct; it's the fused EKF2 outputs that are problematic (refer to the post I made in GitHub above).

I only realized this issue recently when I increased the various forward and side velocities from 1m/s to 3m/s.

Can I trouble you guys to try it on the Starling in an indoor environment with say at least 15-20m of empty space? Let the aircraft move forward or sidewards at full speed.. does it climb (or sink) while moving and visibly corrects the altitude after it has stopped (by as much as 0.5m)

Cliff Wong

Hey there, if you are flying solely with VIO (no GPS), suggest loading our indoor estimator params to add to the current setup on PX4. Outdoor vio+gps is here. These set the vio sensitivity that is in line with the suggestions mention on your PX4 forum's thread. Also we're assuming that extrinsics.conf values have not changed and are same as the factory settings.

Secondly, if you run voxl-inspect-pose vvhub_body_wrt_local on voxl2 it will apply the extrinsics and output pose that should be inline w/PX4's local position. If those values are correct, then we're looking at a PX4 tuning/config issue (we can still help).

One thing that looks weird on your PX4 flight log is Roll and Yaw rates are not keeping up w/respective setpoints (noisy). [Hence, thirdly, suggest an] IMU calibration on the PX4 side.

Lastly in /etc/modalai/voxl-vision-hub.conf there's a parameter called en_reset_vio_if_initialized_inverted. It is set to true as it's to account if vio was accidentally initialized upside down (while carrying the drone for example). You can set it to false and if there's a mismatch on VIO-PX4, you'll see it immediately.

hmlow

@Cliff-Wong

Amended the EVV and EVP noise and gate size values..

Doesnt seem to have made any diff; EKF2 local position Vz and z gradient are still in different directions during full left and right roll, whereas VIO reported Vz and z are seemingly correct instead. I have uploaded this log on the github post as well.

Extrinsics are correct because the VIO outputs are correct as mentioned.
I should also add that this is not based on the Starling but a smaller customized drone.
Have clocked at least 50hrs of indoor flight from SDK 0.9 thru 1.0 but only recently did i realize this behavior as i adjusted the max velocities up to 3m/s from 1m/s.
At 1m/s this issue is barely noticeable.

Moderator

@hmlow there are multiple sets of extrinsics

if you run voxl-inspect-pose vvhub_body_wrt_local on voxl2 it will apply the extrinsics and output pose that should be inline w/PX4's local position. Please verify those outputs match what px4 is reporting.

PX4 has its own rotations as well

hmlow

@Moderator Yes the extrinsics are correct and the VIO pose matches PX4 EKF2 estimates well except during the segments max velocity rolling maneuvers at 3m/s where VIO and EKF2 Vz and z deviates (but VIO's estimates is correct and matches with visual observation), which is what this post was about in the first place.

Comparison of x y z between VIO and EKF2 local position estimates:

Comparison of Vx between VIO and EKF2 estimates:

Comparison of Vy between VIO and EKF2 estimates:

Comparison of Vz between VIO and EKF2 estimates:

Im attaching the voxl-logger VIO logs and PX4 ulg file here. The 2 files are already time synced so you can plot them directly.

VIO log
PX4 log

Thanks

hmlow

@Cliff-Wong

While the logs and graphs above are based on my own customized aircraft, I just tried the same maneuver on the Starling (with shipped params and configured for indoor VIO) and i can confirm that the same observation is repeatable.

Im curious if you guys would see the same uncommanded climb/sink if you would roll your own Starling aircraft left (or right) for 10 - 20m (3m/s, with CP turned off) and then in the opposite direction. The logs would also show that the EKF2 estimates having the same issues as i have described here and on the github post..

hmlow

Hi did anyone manage to verify this?

hmlow

Any devs?

I was able to produce this issue on two separate Starlings and a customized 250g drone

wilkinsaf

@hmlow I was seeing the same thing on my starling. Have you tried completely disconnecting the GPS/baro unit on top and turning off the parameter HAS_MAG?

hmlow

@wilkinsaf That solved the issue for you?

wilkinsaf

@hmlow yes. Ideally you would adjust the EKF2 parameters to increase the gps/mag threshold so they wouldn't affect your local position. But, quick and dirty is to unplug the gps unit. See if that solves your issue. If it does, then worry about EKF2 parameters

hmlow

@wilkinsaf actually no, I'm using neither GPS nor mag (disabled both) as my flights are entirely indoors

wilkinsaf

@hmlow

So, basically the main issue is that in the logs the qvio and ekf2 are reporting different position estimates? But, everything is flying correctly?

hmlow

@wilkinsaf Flying correctly for the most part yes.

Except during specific maneuvers as i have described in my first post (details in the github link) and the third post.
Its challenging to summarize the issue in a few lines..

wilkinsaf

@hmlow I was flying over the weekend, and saw some inconsistencies as well tbh. The quality of the QVIO was reporting not as high as it was in version 0.9.5 SDK release. I will ask the devs and see what they say.
First, I need to compile some logs from voxl-qvio.