External vision velocity

[kamilritz]

This PR allows ECL to fuse velocity measurements from an external vision sensor such as the Realsense T265. In addition, it also improves on the external vision position fusion, such that the external vision measurement fusion is no usable with several EKF_AID_MASK configurations.

Supported configuration for the EKF_AID_MASK are:
1 - GPS
73 - GPS + EV_POS + ROTATE_EV (incremental ev pos fusion, not recommended, use EV_VEL instead)
321 - GPS + EV_VEL + ROTATE_EV (recommended)
24 - EV_POS + EV_YAW
72 - EV_POS + ROTATE_EV
272 - EV_VEL + EV_YAW
320 - EV_VEL + ROTATE_EV
280 - EV_POS + EV_VEL + EV_YAW
328 - EV_POS + EV_VEL + ROTATE_EV

If you use no GPS information, you are now free to decide if you want your EKF reference frame to be aligned with true ENU frame (use ROTATE_EV) or aligned with the external vision reference frame (use EV_YAW).

The EKF_AID_MASK = 9 (GPS + EV_POS) is not supported and should not be used.

All these configurations are tested in EKF replay on log: https://review.px4.io/plot_app?log=bd994025-2312-4ee9-a7b2-f6ab2d65c6b8 and the majority of the configurations were validated on real world test flights.
Make sure to have EVV_NOISE=0.1, EVV_GATE=3, EVP_NOISE=0.1 and EVP_GATE=5.

@priseborough Thank you very much for your great help and your valuable inputs.
#631 #397

[mhkabir]

If you use no GPS information, you are now free to decide if you want your EKF reference frame to be aligned with true ENU frame (use ROTATE_EV) or aligned with the external vision reference frame (use EV_YAW).

If you're not using GPS, the recommended way would still be to use EV_YAW, no? Since ROTATE_EV case still needs to depend on the magnetometer for heading.

@jkflying @priseborough @kamilritz Also along this thread, more generally, we should be able to handle two very important cases where we are configured to run GPS + vision:

1. Indoors to outdoors

Starting up in a GPS denied situation where mag cannot be relied on, and therefore you don't know the true NED frame alignment. In this case, we should probably do absolute EV position and EV yaw fusion (EVP, EVV, EV_YAW) till it is detected that we are outdoors. At that point the reference frame can be reset to true NED, and relative fusion continued (GPS, EVV, ROTATE_EV). This will cause a jump in the local frame's rotation and position.

2. Outdoors to indoors

Flying outdoors, then entering an area where only vision is viable, with both GPS and mag being unreliable. In this case, we would need to fuse the absolute EV pos and rotated EV Yaw to prevent excessive drifting from only vel fusion. This case differs from (1) in that the NED alignment is already known, and we may want to retain some of that information.
I see two ways to handle this:
(i) Only update the EV rotation quaternion and EV pos offset (see below) when global heading observations are available (outdoor mode), and use it to pre-rotate the EV yaw before absolute fusion. We would additionally need to track the position offset between EV and EKF, and use this to first transform EV meas into the EKF frame before absolute fusion. To be noted - while indoors, the local frame will drift slowly in rotation from true NED.
(ii) Do a hard reset to the EV frame (like proposed to do to NED at the end of (1)) when we switch to indoor mode. This will cause a jump in local frame rotation and position.

Both of these will require us to really robustify indoor mode detection, and good mag/gpsyaw handling to be really clear about when we have reliable global heading available. The initialization of the GPS origin for both the above also needs to be discussed.

Automatically handling these cases will get rid of a lot of the parameter combos which may be confusing for new users.

This is mostly just a collection of my thoughts. We should probably sit down and discuss this quickly so that we can implement.

[kamilritz]

@mhkabir Thank you very much for your feedback. It is definitely my goal to realize the two mentioned use cases.

If you're not using GPS, the recommended way would still be to use EV_YAW, no? Since ROTATE_EV case still needs to depend on the magnetometer for heading.

Yes, probably better to use EV_YAW. But when you want to fuse with GPS later, ROTATE_EV gives you a nice way to check if your aligned EV measurements fit your recorded GPS velocities.

[priseborough]

The default behaviour should be to use relative heading from EV_YAW when operating without GPS due to problems with magnetometer performance in an indoor environment, however we should still allow the user to use an absolute heading reference to aid with development testing if that is what they want to do.

[mhkabir]

I think it is now more appropriate for these to be renamed to pos and vel, without the NED postfixes.

[kamilritz]

Yes, that is true

[mhkabir]

Same as above

[priseborough]

I think we should do this in two stages. This PR adds the underlying fusion modes required and is the basis for better handover logic. I support merging this and them using specific use cases like the ones @mhkabir raised to further develop the logic and corner case handling.

[mhkabir]

@priseborough Thanks for merging.

Next time, for large PRs like this, please try to squash commits before you merge because otherwise we end up with this a bunch of these minor non-building commits which makes bisecting a pain. Thanks!