AP_NavEKF2: support VISION_SPEED_ESTIMATE

[chobitsfan]

fix #4498

It enables EKF2 fuse speed from VISION_SPEED_ESTIMATE just like speed from GPS
GCS/companion computer can use VISION_SPEED_ESTIMATE to send their velocity estimate.
It makes copters loiter performance a little better when using external navigation data (VISION_POSITION_ESTIMATE or ATT_POS_MOCAP)

below is the observation of motion capture system

[OXINARF]

• Commit needs to be split to comply with 1 commit per library rule
• Please add comments, similar to the existing ones

[chobitsfan]

Thank you. I have changed it

I think @chobitsfan has fixed the issue raised by @OXINARF so dismissing the review.

[rmackay9]

I think we need this rebased but more importantly it would be good to get @priseborough's opinion on whether this is the right way to do this.

[amilcarlucas]

@priseborough can you take a look at this ?

[priseborough]

First pass review doesn't show any structural issues. However there is an issue with the use of the GPS velocity noise parameter to set the velocity observation noise variance.

The https://mavlink.io/en/messages/common.html#VISION_SPEED_ESTIMATE message provides a covariance matrix for the velocity vector so we should be taking the diagonals from that message and using them as the observation noise variance for each element.

Edit: Also I would like to inspect a log with this feature operating so can you please provide a link to a log

[chobitsfan]

Hi @priseborough thank you

There are 2 logs in zip file, copter fly with the same firmware. In one log, gcs do not send VISION_SPEED_ESTIMATE, in another log gcs send VISION_SPEED_ESTIMATE
vis_speed_test.zip

The covariance in VISION_SPEED_ESTIMATE is a MAVLink v2 only extension and not available in MAVLink v1. Maybe we can use EK2_VELNE_M_NSE if covariance is not available in the message? Just like EK2_POSNE_M_NSE also used for external navigation system position observation noise.

[amilcarlucas]

This needs a rebase

[amilcarlucas]

Would be great to get this rebased

[rmackay9]

@amilcarlucas, I'm not sure it's great to ask the developer for a rebase unless someone is going to actively test and push it through the peer review (i.e. raise it on the dev call etc) to be sure it's merged. I think it could just be frustrating for the developer to rebase it but then have it ignored again.

On the other hand, I think this PR is still important and I am getting closer to testing it and getting it merged.. but I'm probably still a week or two away from doing that.

[chobitsfan]

Thank you @amilcarlucas and @rmackay9

I am sorry that I am in middle of other matters and do not have time to rebase and test it right now. I will do it as soon as possible once my hand is free. Thank you

[chobitsfan]

I have rebase it and add a VISS log message to log VISION_SPEED_ESTMATE

I add useVisVertVel flag instead of reuse useGpsVertVel

flight log vis_speed_log.zip
vis_speed_log2.zip

[rmackay9]

@chobitsfan, great, thanks for this.

By the way, after this PR goes in, if you and other devs are happy with the approach I will likely merge the handling of this message into the AP_VisualOdom library. I've done this for the vision-position-estimate messages in this new PR: #13982

[chobitsfan]

Of course @rmackay9 Thank you

[rmackay9]

I just wanted to add that I haven't forgotten about this PR. We have a lot of EKF fixes and changes being made recently and although this hasn't gone in, it also hasn't been forgotten, it's just about ordering.

[chobitsfan]

Thank you @rmackay9
If you need help, please feel free to ask :)

[chobitsfan]

replaced by #14368