We are running autonomous missions where the drone (quadrotor) takes off from an enclosure in offboard mode, performs a flight in AUTO.MISSION and then lands in the enclosure also in offboard mode. We use VISION_POSITION_ESTIMATE messages and EKF2_AID_MASK = 9 (GPS + vision) to obtain “sub-GPS-accuracy” positioning during takeoff and landing.
We use Here+ as an external GPS+Compass combo.
We’ve encountered cases, where the magnetic interferences from enclosure’s structure and actuators induces a significant (up to 20 degrees) offset of magnetic heading (which disappears as the drone takes off and moves away from the enclosure). This is obviously a problem: while the vision-based positioning does not, by itself, depend on magnetic heading, magnetic heading is used to rotate the solution so that it is based on a NED frame.
While this obviously a problem with the enclosure itself, it is impossible to fully eleminate its influence, because it is, by definition, an enclosure and contains actuators, that are used to open and close it.
Is it possible to use vision yaw fusion alongside the magnetometer to obtain “correct” heading estimate inside and in direct vicinity of enclosure? What will happen if vision-based and magnetic yaw drift away from each other (which is what will happen when interferences are present)?
So one thing to keep in mind is that you are probably not using the compass included with the GPS but the internal Pixhawk’s compass. At least according to this issue: https://github.com/PX4/Firmware/issues/11830
Would it help if you were actually using the external mag?
We are definitely using the external compass since we are using old here+ modules. We specifically check that external mag is available on every airframe.
Aha, Here+ is not Here 2, sorry about that…
I am pinging @Paul_Riseborough, he should be able to help. I know that he usually will ask for a log, so if you can provide one where we see the issue that will probably help.
we used vision and compass fusion last year. Not sure if there were any developments in PX4 about this in last 6 months.
We ended up setting wide variance for magnetic readings and narrow for vision for EKF2 to give more weight to vision.
But then your vision yaw has to match your magnetic one if both compass and magnetometer are functioning nominally. So initially we were measuring magnetic yaw and then keep adjusing vision yaw by this offset.
The problem was that built in magnetometers are very bad and can become pretty much random. Or at least drift 90 degrees from ‘initial’ North. I think this is your case of moving out of enclosure.
This is where problems can develop. If your vision pose becomes patchy /delayed then EKF2 may start to have hard time reconciling sporadic vision poses with constant stream of inconsistent magnetometer readings.
I think we have seen ‘toilet bowl’ syndrome a few times from that.
I suspect the solution is to re-initialise vision yaw externally. Like having an external ‘supervisor’ that catches inconsistency magnetometer/vision and re-initialises vision.
Happy to consult further on this. Send me a private message and I’ll say if I can be useful.
That’s what I am doing currently. I think that PX4 should reinitialize vision yaw every time vision reappears after few seconds of down time. I can make a PR if you want.
I think rationale for not doing it inside PX4 is because vision is external, it is not a simple sensor reading. And having code that would decide if it looks ‘suspicious’ is too complex/error prone to be inside autopilot.
But interested to see the opinion of px4 developers
We are using external magnetometer, but, yes, upon exiting the enclosure, yaw estimate changes by around 20 or so degrees (which is quite severe).
This one is easy for us, because our vision position estimate is produced by an onboard computer + camera, that deduce the position of the drone relative to the enclosure using the markers, that are part of the enclosure. Since the enclosure does not move while the mission is in progress, transforming “visual yaw” to NED frame is trivial, because the orientation of the enclosure relative to north is known apriori.
Unfortunately, current fusion logic in EKF2 is written so that if there is vision yaw source present and there is also a magnetometer, vision yaw is completely ignored if there is a GPS fix. Obviously, there is GPS fix present at all stages of the flight so vision yaw is never used. You can see for yourself: https://github.com/PX4/ecl/blob/master/EKF/control.cpp#L559
yeah, I see what you mean!
and that code was there for over a year.
we did some experiments last summer/autumn but could not come to definite conclusion how reliable GPS + vision fusion is.
my ‘feeling’ was that something weird was going on with yaw fusion, and it was not just quality of internal (or external) magnetometers.
but I did not dive deep into the code.
you need input from @Paul_Riseborough