Skip to content

GCS_MAVLink: utilize VISION_SPEED_ESTIMATE covariance #32478

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
peterbarker merged 3 commits into from
Mar 19, 2026
Merged

GCS_MAVLink: utilize VISION_SPEED_ESTIMATE covariance #32478

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
cb8fd96
GCS_MAVLink: utilize VISION_SPEED_ESTIMATE covariance
chobitsfan Mar 18, 2026
3e06b5a
AP_VisualOdom: explain newly added vel_err parameter
chobitsfan Mar 18, 2026
b44d4b3
AP_VisualOdom: clamp vel_err to VEL_M_NSE minimum
chobitsfan Mar 19, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions libraries/AP_VisualOdom/AP_VisualOdom.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -232,7 +232,7 @@ void AP_VisualOdom::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_
// general purpose methods to consume velocity estimate data and send to EKF
// velocity in NED meters per second
// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
void AP_VisualOdom::handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality)
void AP_VisualOdom::handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, float vel_err, uint8_t reset_counter, int8_t quality)
{
// exit immediately if not enabled
if (!enabled()) {
Expand All @@ -241,7 +241,7 @@ void AP_VisualOdom::handle_vision_speed_estimate(uint64_t remote_time_us, uint32

// call backend
if (_driver != nullptr) {
_driver->handle_vision_speed_estimate(remote_time_us, time_ms, vel, reset_counter, quality);
_driver->handle_vision_speed_estimate(remote_time_us, time_ms, vel, vel_err, reset_counter, quality);
}
}

Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_VisualOdom/AP_VisualOdom.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -104,7 +104,7 @@ class AP_VisualOdom
// general purpose methods to consume velocity estimate data and send to EKF
// velocity in NED meters per second
// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality);
void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, float vel_err, uint8_t reset_counter, int8_t quality);

// request sensor's yaw be aligned with vehicle's AHRS/EKF attitude
void request_align_yaw_to_ahrs();
Expand Down
4 changes: 2 additions & 2 deletions libraries/AP_VisualOdom/AP_VisualOdom_Backend.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@ class AP_VisualOdom_Backend

// consume vision velocity estimate data and send to EKF, velocity in NED meters per second
// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
virtual void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality) = 0;
virtual void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, float vel_err, uint8_t reset_counter, int8_t quality) = 0;

// request sensor's yaw be aligned with vehicle's AHRS/EKF attitude
virtual void request_align_yaw_to_ahrs() {}
Expand All @@ -69,7 +69,7 @@ class AP_VisualOdom_Backend
// Logging Functions
void Write_VisualOdom(float time_delta, const Vector3f &angle_delta, const Vector3f &position_delta, float confidence);
void Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float pos_err, float ang_err, uint8_t reset_counter, bool ignored, int8_t quality);
void Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, bool ignored, int8_t quality);
void Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, float vel_err, uint8_t reset_counter, bool ignored, int8_t quality);
#endif

// align position with ahrs position by updating _pos_correction
Expand Down
4 changes: 2 additions & 2 deletions libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -90,7 +90,7 @@ void AP_VisualOdom_IntelT265::handle_pose_estimate(uint64_t remote_time_us, uint

// consume vision velocity estimate data and send to EKF, velocity in NED meters per second
// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
void AP_VisualOdom_IntelT265::handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality)
void AP_VisualOdom_IntelT265::handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, float vel_err, uint8_t reset_counter, int8_t quality)
{
// rotate velocity to align with vehicle
Vector3f vel_corrected = vel;
Expand All @@ -110,7 +110,7 @@ void AP_VisualOdom_IntelT265::handle_vision_speed_estimate(uint64_t remote_time_
_last_update_ms = AP_HAL::millis();

#if HAL_LOGGING_ENABLED
Write_VisualVelocity(remote_time_us, time_ms, vel_corrected, reset_counter, !consume, _quality);
Write_VisualVelocity(remote_time_us, time_ms, vel_corrected, _frontend.get_vel_noise(), reset_counter, !consume, _quality);
#endif
}

Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@ class AP_VisualOdom_IntelT265 : public AP_VisualOdom_Backend

// consume vision velocity estimate data and send to EKF, velocity in NED meters per second
// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality) override;
void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, float vel_err, uint8_t reset_counter, int8_t quality) override;

// request sensor's yaw be aligned with vehicle's AHRS/EKF attitude
void request_align_yaw_to_ahrs() override { _align_yaw = true; }
Expand Down
4 changes: 2 additions & 2 deletions libraries/AP_VisualOdom/AP_VisualOdom_Logging.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ void AP_VisualOdom_Backend::Write_VisualPosition(uint64_t remote_time_us, uint32
}

// Write visual velocity sensor data, velocity in NED meters per second
void AP_VisualOdom_Backend::Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, bool ignored, int8_t quality)
void AP_VisualOdom_Backend::Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, float vel_err, uint8_t reset_counter, bool ignored, int8_t quality)
{
const struct log_VisualVelocity pkt_visualvel {
LOG_PACKET_HEADER_INIT(LOG_VISUALVEL_MSG),
Expand All @@ -57,7 +57,7 @@ void AP_VisualOdom_Backend::Write_VisualVelocity(uint64_t remote_time_us, uint32
vel_x : vel.x,
vel_y : vel.y,
vel_z : vel.z,
vel_err : _frontend.get_vel_noise(),
vel_err : vel_err,
reset_counter : reset_counter,
ignored : (uint8_t)ignored,
quality : quality
Expand Down
7 changes: 4 additions & 3 deletions libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,22 +68,23 @@ void AP_VisualOdom_MAV::handle_pose_estimate(uint64_t remote_time_us, uint32_t t

// consume vision velocity estimate data and send to EKF, velocity in NED meters per second
// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
void AP_VisualOdom_MAV::handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality)
void AP_VisualOdom_MAV::handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, float vel_err, uint8_t reset_counter, int8_t quality)
{
// record quality
_quality = quality;

// send velocity to EKF if quality OK
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);

@rmackay9 rmackay9 Mar 18, 2026

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

@chobitsfan chobitsfan Mar 19, 2026

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

@rmackay9 rmackay9 Mar 19, 2026 •
edited
Loading

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

@chobitsfan chobitsfan Mar 19, 2026

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

@rmackay9 rmackay9 Mar 19, 2026 •
edited
Loading

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

@chobitsfan chobitsfan Mar 19, 2026

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

2026-03-19 14-28-31.zip

@rmackay9 rmackay9 Mar 19, 2026 •
edited
Loading

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

@chobitsfan chobitsfan Mar 25, 2026

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hi @rmackay9 Sorry for the late reply. (the notification email was mixed in with PR merge notifications, so I missed it.)

I wonder if the EKF would actually be happier if it didn't get that velocity reading at al

I think this is already handled by the quality field. It is the responsibility of the visual odometry source to report the quality of its estimate. Unfortunately, the quality field is only supported in the Odometry message. I may open a PR in the MAVLink repository to add it to VISION_SPEED_ESTIMATE.

ardupilot/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp

Lines 77 to 81 in 2d14165

bool consume = (_quality >= _frontend.get_quality_min());
if (consume) {
vel_err = fmaxf(vel_err, _frontend.get_vel_noise());
AP::ahrs().writeExtNavVelData(vel, vel_err, time_ms, _frontend.get_delay_ms());
}

AP::ahrs().writeExtNavVelData(vel, vel_err, time_ms, _frontend.get_delay_ms());
}

// record time for health monitoring
_last_update_ms = AP_HAL::millis();

#if HAL_LOGGING_ENABLED
Write_VisualVelocity(remote_time_us, time_ms, vel, reset_counter, !consume, _quality);
Write_VisualVelocity(remote_time_us, time_ms, vel, vel_err, reset_counter, !consume, _quality);
#endif
}

Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_VisualOdom/AP_VisualOdom_MAV.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@ class AP_VisualOdom_MAV : public AP_VisualOdom_Backend

// consume vision velocity estimate data and send to EKF, velocity in NED meters per second
// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality) override;
void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, float vel_err, uint8_t reset_counter, int8_t quality) override;
};

#endif // AP_VISUALODOM_MAV_ENABLED
8 changes: 6 additions & 2 deletions libraries/GCS_MAVLink/GCS_Common.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4068,7 +4068,7 @@ void GCS_MAVLINK::handle_odometry(const mavlink_message_t &msg)
// convert velocity vector from FRD to NED frame
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);

@peterbarker peterbarker Mar 18, 2026

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

@rmackay9 rmackay9 Mar 18, 2026

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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"

@chobitsfan chobitsfan Mar 18, 2026

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hi @peterbarker and @rmackay9, thank you for the review and suggestions. I have added an explanation for the newly added parameter.

}

// there are several messages which all have identical fields in them.
Expand Down Expand Up @@ -4137,7 +4137,11 @@ void GCS_MAVLINK::handle_vision_speed_estimate(const mavlink_message_t &msg)
mavlink_msg_vision_speed_estimate_decode(&msg, &m);
const Vector3f vel = {m.x, m.y, m.z};
uint32_t timestamp_ms = correct_offboard_timestamp_usec_to_ms(m.usec, PAYLOAD_SIZE(chan, VISION_SPEED_ESTIMATE));
visual_odom->handle_vision_speed_estimate(m.usec, timestamp_ms, vel, m.reset_counter, 0);
float vel_err = 0;
if (!isnan(m.covariance[0])) {
vel_err = sqrtf(m.covariance[0]+m.covariance[4]+m.covariance[8]);
}
visual_odom->handle_vision_speed_estimate(m.usec, timestamp_ms, vel, vel_err, m.reset_counter, 0);
}
#endif // HAL_VISUALODOM_ENABLED

Expand Down
Loading