GCS_MAVLink: utilize VISION_SPEED_ESTIMATE covariance#32478
GCS_MAVLink: utilize VISION_SPEED_ESTIMATE covariance#32478peterbarker merged 3 commits intoArduPilot:masterfrom
Conversation
| Vector3f vel{m.vx, m.vy, m.vz}; | ||
| vel = q * vel; | ||
| visual_odom->handle_vision_speed_estimate(m.time_usec, timestamp_ms, vel, m.reset_counter, m.quality); | ||
| visual_odom->handle_vision_speed_estimate(m.time_usec, timestamp_ms, vel, 0, m.reset_counter, m.quality); |
There was a problem hiding this comment.
0 seems like a poor choice as a default value to send in as an error. If you don't know then NaN would be a rather better choice.
There was a problem hiding this comment.
I'm not a fan of sending NaNs around internally to indicate that something is unknown or unused. Normally we do use zero. We should add a comment though to the handle_vision_speed_estimate function though similar towhat we have for quality, "// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best"
There was a problem hiding this comment.
Hi @peterbarker and @rmackay9, thank you for the review and suggestions. I have added an explanation for the newly added parameter.
rmackay9
left a comment
rmackay9
left a comment
There was a problem hiding this comment.
with the exception of adding the comment for the new argument this looks good to me, thanks!
|
I've corrected the commit structure; GCS_MAVLink changes were mixed in with VisualOdom patches |
| bool consume = (_quality >= _frontend.get_quality_min()); | ||
| if (consume) { | ||
| AP::ahrs().writeExtNavVelData(vel, _frontend.get_vel_noise(), time_ms, _frontend.get_delay_ms()); | ||
| vel_err = constrain_float(vel_err, _frontend.get_vel_noise(), 5.0f); |
There was a problem hiding this comment.
Hi @chobitsfan, thanks again for this. I see we've added a new upper constraint of "5" here. That is a lot of error so perhaps the EKF will just ignore the velocity (which would be good with that high an error) but what do you think? The VISO_VEL_M_NSE parameter also has a suggested upper limit of 5 but it's not strictly enforced so we do have a small change in behaviour here
There was a problem hiding this comment.
Hi @rmackay9 Thank you for reviewing. As you mentioned, the upper constraint of vel_err at 5 is based on the suggested upper limit of the VISO_VEL_M_NSE parameter. I constrain vel_err to 5 because visual odometry may report very large standard deviations when it becomes unreliable or drifts. Alternatively, should we use vel_err = fmaxf(_frontend.get_vel_noise(), vel_err), similar to how we handle GPS speed accuracy? Thank you.
There was a problem hiding this comment.
Hi @chobitsfan,
Yes, I think we should use the MAX of the two and remove the upper limit of "5" unless we have a specific reason to change it.
So you mention that the external visual odometry system reports very large errors at times, does that cause problems? Do you find the EKF should be consuming the velocity but doesn't? Of course the external system could apply an upper limit but I'm not sure which is best.
Maybe @priseborough has an opinion
There was a problem hiding this comment.
Hi @rmackay9, Thank you for suggestions
Do you find the EKF should be consuming the velocity but doesn't?
No, not so far. When visual odometry reports very large errors, it indicates an unstable state, and the EKF should not consume its data.
There was a problem hiding this comment.
Hi @chobitsfan,
OK, so if we want to protect against that then instead of constraining the error we should perhaps add a VISO_VEL_ERR_MAX parameter (a bit similar to the VISO_QUAL_MIN param) and then simply throw away the velocity if the error is over this value
This doesn't need to be done as part of this PR though
There was a problem hiding this comment.
Hi @rmackay9
Two test flight logs are included using the latest modifications to vel_err. In both flights, visual odometry became unstable, reported large errors, and eventually triggered the EKF failsafe.
There was a problem hiding this comment.
Hi @chobitsfan, but with the previous code of clamping the vel error at 5 everything was OK?
I wonder if this means that the EKF loses confidence in its velocity estimate just because the incoming velocity value has come in with a high error value attached to it. I wonder if the EKF would actually be happier if it didn't get that velocity reading at all and was instead forced to estimate it from the position and acceleration values it's getting.
An expert like @priseborough might know the answer off the top of his head
Hi @peterbarker Thank you for correcting the commit structure. |
3 participants
Summary
This PR enables the EKF to utilize the covariance field provided by VISION_SPEED_ESTIMATE.
Testing (more checks increases chance of being merged)
Description
Similar to the existing handling of the covariance field in VISION_POSITION_ESTIMATE, this PR calculates velocity error from the VISION_SPEED_ESTIMATE covariance and feeds it into the EKF.
Two test flight logs are included. In the first flight, visual odometry becomes unstable toward the end of the flight, which causes the vehicle to land.
spd_covar.zip