AP_NavEKF3/AP_VisualOdom/SITL: add support for vision-speed-estimates

[rmackay9]

This is built on top of PR #14368 from @chobitsfan (this original PR should be merged first).

This PR has the following enhancements (on top of the older PR):

• EKF3 is enhanced to consume the VISION_SPEED_ESTIMATE mavlink messages via the AP_VisualOdom library
  • Vision-position-estimate messages must also be received for it to function properly (this keeps the code similar to the GPS which can also provide position and velocity but not velocity alone)
  • ResetVelocity enhanced to reset to the external nav velocity
  • CorrectExtNavVelForSensorOffset() corrects the external navigation's velocity based on the sensor position
• AP_Logger's VISV message is used to log all the new data
• SITL's SIM_Vicon class is enhanced so it can send the vision-speed-estimate message and the following params are added:
  • SIM_VICON_TMASK (for "Type Mask") allows real-time control of which of the three supported messages (vision-position-estimate, vision-speed-estimate and vicon-position-estimate) are sent
  • SIM_VICON_VGLIT_X/Y/Z allows adding an earth-frame glitch (in m/s) to the simulated velocity
• AP_Math::get_vel_correction_for_sensor_offset is added so we can reduce some duplicate code in EKF2, EKF3 and the simulator that adjusts the velocity based on the sensor's offsets. In this PR the new function is only used for external nav velocities so the scope is kept small

Some non-blocking issues:

• if vision-speed-estimates are sent without also sending vision-position-estimates the EKF will not complete initialisation. Theoretically we should be able to provide only velocities but this does not work yet from some reason and is left for a future PR
• the vision-speed-estimate messages's reset_counter are logged but not used. In the short-term this is OK because Thien's T265 scripts will send the reset using the vision-position-estimate message but eventually we will want to support this especially if we resolve the issue directly above
• we should extend the AP_Math::get_vel_correction_for_sensor_offset to be used for the existing GPS code. This was not done in order to limit the scope of this PR's changes

Testing done so far:

• The EKF2 changes have been tested in SITL and on a real vehicle
• The EKF3 changes have been tested in SITL including
  • checking that the velocity corrections for sensor position are correct
  • vision sensor failure and glitches seem to be handled correctly

Some more SITL and actual flight tests (using EKF3) will be completed before merging

[rmackay9]

This has been successfully test flown and below are some shots of the position and velocity innovations which appear quite low.

[rmackay9]

Here's a flight video test of this PR
https://www.youtube.com/watch?v=jCY9-fWtA7c

[tridge]

be nice for vicon params to be in a child table

[priseborough]

The main issue is the lack of accuracy information being passed via this interface. If unavailable from the sensor, then it should be set to a default or parameter defined value in the sensor driver.

Use of specified minimum GPS velocity error to set observation noise for this sensor type should be avoided.

A recommended improvement is to apply the position offset correction to the delayed data immediately after it is retrieved from the buffer rather than apply it separately in the fusion and reset operations.

[chobitsfan]

Hi @rmackay9 I do some changes address @priseborough 's comments in #14368. Could you be able to take a look? Thank you very much

I do some flight test and it looks well.
22 2020-5-28 PM 02-34-22.zip

[rmackay9]

This has been rebased on @chobitsfan's PR #14368

[rmackay9]

Rebased on master and two commits added to address requests from @priseborough.