fix(vision_target_estimator): harden inputs and reset lifecycle (#29399)

* fix(vision_target_estimator): harden inputs and reset lifecycle

Retain acceleration until attitude is available and age it by sample time. Normalize vision attitude, invalidate published estimator states on reset, and enforce valid timeouts and delayed fallback scheduling across task transitions.

Assisted-by: Codex
Signed-off-by: jonas <jonas.perolini@rigi.tech>

* refactor(vision_target_estimator): remove runtime timeout clamping

Use configured timeouts directly and keep their permitted ranges in parameter metadata.

Assisted-by: Codex
Signed-off-by: jonas <jonas.perolini@rigi.tech>

---------

Co-authored-by: jonas <jonas.perolini@rigi.tech>
This commit is contained in:
Jonas Peroliniandjonas authored and GitHub committed 2026-10-25 10:09:58 -05:00
1 parent 34feba05b4
commit f26361d999
10 files changed
+115 -52

No files matched your search

+1 -1
View File
@@ -1,7 +1,7 @@
# Vision Target Estimator orientation state, exposing the full yaw filter output with covariances for logging and tuning.
#
# Published by: vision_target_estimator (VTEOrientation).
# Subscribed by: logger only. The orientation-related fields consumed elsewhere (precision landing) are exposed on landing_target_pose.
# Subscribed by: precision landing (navigator) and the logger.
uint64 timestamp # [us] Time since system start
@@ -72,14 +72,15 @@ void VTEOrientation::resetFilter(hrt_abstime min_sample_time)
_min_sample_time = min_sample_time;
}
_estimator_initialized = false;
_target_est_yaw.resetHistory();
if (min_sample_time != 0) {
// Tell consumers the last published yaw no longer holds.
if (_estimator_initialized || (min_sample_time != 0)) {
_orientation_msg = {};
_orientation_msg.timestamp = nowUs();
_target_orientation_pub.publish(_orientation_msg);
}
_estimator_initialized = false;
_target_est_yaw.resetHistory();
}
void VTEOrientation::update()
@@ -76,7 +76,7 @@ public:
*/
void update();
/** Drop the estimator state. A nonzero sample boundary also publishes invalid yaw. */
/** Drop the estimator state and publish an invalid yaw. A nonzero sample boundary drops older samples. */
void resetFilter(hrt_abstime min_sample_time = 0);
/** Timeout after which the whole filter is declared stale (see @c timedOut). */
@@ -97,6 +97,10 @@ void VTEPosition::resetFilter(hrt_abstime min_sample_time)
_velocity_offset_ned.valid = false;
}
if (_estimator_initialized) {
publishInvalidState();
}
_estimator_initialized = false;
_last_relative_meas_fused_time = 0;
_bias.set = false;
@@ -962,7 +966,9 @@ bool VTEPosition::processObsVision(TargetObs &obs)
return false;
}
const matrix::Quaternionf quat_att(report.q);
// The covariance rotation below assumes a unit quaternion.
matrix::Quaternionf quat_att(report.q);
quat_att.normalize();
const Vector3f vision_rel(report.rel_pos);
const Vector3f vision_ned = quat_att.rotateVector(vision_rel);
const float min_ev_pos_var = sq(fmaxf(_param_vte_ev_pos_noise.get(), kMinObservationNoise));
@@ -1213,6 +1219,16 @@ void VTEPosition::publishAidSource(const TargetObs &target_obs, const AxisResult
_vte_aid_pubs[obsIndex(target_obs.type)].publish(target_innov);
}
void VTEPosition::publishInvalidState()
{
// Mark the logged state invalid. landing_target_pose is not published: precision landing keeps
// approaching the last pose it received and judges its age with its own timeout.
_vte_state.timestamp = nowUs();
_vte_state.rel_pos_valid = false;
_vte_state.rel_vel_valid = false;
_target_estimator_state_pub.publish(_vte_state);
}
void VTEPosition::publishTarget()
{
_vte_state = {};
@@ -1220,12 +1236,7 @@ void VTEPosition::publishTarget()
_target_pose.timestamp = _last_predict;
_vte_state.timestamp = _last_predict;
#if defined(CONFIG_VTEST_MOVING)
_target_pose.is_static = false;
#else
_target_pose.is_static = true;
#endif // CONFIG_VTEST_MOVING
_target_pose.is_static = kTargetIsStatic;
Vector3f rel_pos;
Vector3f rel_pos_var;
@@ -81,7 +81,7 @@ public:
*/
void update(const matrix::Vector3f &acc_ned);
/** Drop the estimator state. A nonzero sample boundary also invalidates cached NED inputs. */
/** Drop the estimator state and publish it as invalid. A nonzero sample boundary also invalidates cached NED inputs. */
void resetFilter(hrt_abstime min_sample_time = 0);
/** Cache the task-specific absolute reference used to build the mission GNSS observation. */
@@ -185,6 +185,11 @@ private:
};
static constexpr size_t kObsTypeCount = static_cast<size_t>(ObsType::kTypeCount);
#if defined(CONFIG_VTEST_MOVING)
static constexpr bool kTargetIsStatic = false;
#else
static constexpr bool kTargetIsStatic = true;
#endif // CONFIG_VTEST_MOVING
static constexpr size_t obsIndex(ObsType type)
{
return static_cast<size_t>(type);
@@ -354,6 +359,7 @@ private:
void publishTarget();
/** Report an observation that seeded the state instead of being fused. */
void publishInitializedAid(const TargetObs &obs);
void publishInvalidState();
void resetObservations();
uORB::Subscription _vehicle_gnss_sub{ORB_ID(vehicle_gnss)};
@@ -134,24 +134,20 @@ void VisionTargetEst::Run()
ScheduleClear();
} else {
const uint32_t estimator_update_period_us = static_cast<uint32_t>(math::min(kPosUpdatePeriodUs,
kYawUpdatePeriodUs));
ScheduleOnInterval(estimator_update_period_us);
// Poll at the estimator rate. Without the delay the timer would fire again at once.
ScheduleOnInterval(static_cast<uint32_t>(kEstimatorUpdatePeriodUs), static_cast<uint32_t>(kEstimatorUpdatePeriodUs));
}
}
// Task is running, check if an estimator must be started or re-started
startEstIfNeeded();
const bool task_completed = isCurrentTaskComplete();
if (task_completed) {
if (isCurrentTaskComplete()) {
stopAllEstimators();
_current_task_ptr = nullptr;
updateAidMask();
return;
}
// Task is running, check if an estimator must be started or re-started
startEstIfNeeded();
restartTimedOutEstimators();
if (noEstRunning()) {
@@ -670,23 +666,23 @@ bool VisionTargetEst::pollEstimatorInput(matrix::Vector3f &vehicle_acc_ned, matr
vehicle_attitude_s vehicle_attitude{};
vehicle_acceleration_s vehicle_acceleration{};
const bool vehicle_attitude_updated = _vehicle_attitude_sub.update(&vehicle_attitude);
const bool vehicle_acceleration_updated = _vehicle_acceleration_sub.update(&vehicle_acceleration);
// Keep the acceleration sample even when the attitude has not updated yet.
if (_vehicle_acceleration_sub.update(&vehicle_acceleration)) {
_vehicle_acc_body.xyz = matrix::Vector3f(vehicle_acceleration.xyz);
_vehicle_acc_body.timestamp = vehicle_acceleration.timestamp_sample != 0
? vehicle_acceleration.timestamp_sample : vehicle_acceleration.timestamp;
}
if (!vehicle_attitude_updated) {
if (!_vehicle_attitude_sub.update(&vehicle_attitude)) {
return false;
}
// Check the same attitude sample that will rotate this cycle's inputs.
handleAttitudeReset(vehicle_attitude);
if (vehicle_acceleration_updated) {
_vehicle_acc_body.xyz = matrix::Vector3f(vehicle_acceleration.xyz);
_vehicle_acc_body.timestamp = vehicle_acceleration.timestamp;
}
_vehicle_acc_body.valid = (_vehicle_acc_body.timestamp != 0)
&& !hasTimedOut(_vehicle_acc_body.timestamp, kAccUpdatedTimeoutUs);
&& !hasTimedOut(_vehicle_acc_body.timestamp, kAccUpdatedTimeoutUs)
&& _vehicle_acc_body.xyz.isAllFinite();
acc_valid = _vehicle_acc_body.valid;
/* Transform FRD body acc to NED */
@@ -510,7 +510,9 @@ TEST_F(VTEOrientationTest, ResetsFilterOnStalePredictionGap)
// WHEN: the estimator updates after the stale gap and then sees a fresh measurement.
_vte->update();
EXPECT_FALSE(_aid_sub->update());
EXPECT_FALSE(_state_sub->update());
// THEN: consumers are told the last yaw no longer holds.
ASSERT_TRUE(_state_sub->update());
EXPECT_FALSE(_state_sub->get().orientation_valid);
pushValidYawSample(-0.25f);
@@ -2266,6 +2266,27 @@ TEST_F(VTEPositionTest, BiasSeedPropagatesGnssOverLongVisionLatency)
expectBiasNear(_vte_state_sub->get(), kZeroVec, 0.05f);
}
// WHY: The reported attitude may not be a unit quaternion, and the covariance rotation assumes one.
// WHAT: A 90 degree yaw quaternion scaled by two rotates the position but must not scale the variances.
TEST_F(VTEPositionTest, VisionQuaternionIsNormalized)
{
enableMask({FusionMaskOption::kVisionPos});
setLocalVelocity(kZeroVec, vte_test::advanceMicroseconds(kStepUs));
const matrix::Vector3f rel_pos(1.f, 2.f, 3.f);
const matrix::Vector3f cov(0.09f, 0.09f, 0.09f);
const float half = std::sqrt(2.f); // 2 * cos(45 deg) and 2 * sin(45 deg)
publishVisionPos(rel_pos, matrix::Quaternionf(half, 0.f, 0.f, half), cov, vte_test::advanceMicroseconds(kStepUs));
_vte->update(kZeroVec);
ASSERT_TRUE(_aid_fiducial_sub->update());
const matrix::Vector3f expected(-rel_pos(1), rel_pos(0), rel_pos(2));
for (int axis = 0; axis < 3; ++axis) {
EXPECT_NEAR(_aid_fiducial_sub->get().observation[axis], expected(axis), kTolerance);
EXPECT_NEAR(_aid_fiducial_sub->get().observation_variance[axis], cov(axis), kTolerance);
}
}
#if defined(CONFIG_VTEST_MOVING)
// Equal UAV and target speeds leave their relative GNSS position unchanged across vision latency.
TEST_F(VTEPositionTest, MovingTargetBiasAlignmentRequiresRelativeVelocity)
@@ -2313,7 +2334,9 @@ TEST_F(VTEPositionTest, ResetsFilterOnStalePredictionGap)
enableMask({FusionMaskOption::kVisionPos});
_vte->setMeasUpdatedTimeout(500_ms);
const matrix::Vector3f local_pos(10.f, 20.f, -5.f);
setLocalVelocity(matrix::Vector3f{}, vte_test::advanceMicroseconds(kStepUs));
setLocalPosition(local_pos, vte_test::nowUs());
publishVisionPos(initial_rel_pos, vte_test::identityQuat(), kSmallVisionCov,
vte_test::advanceMicroseconds(kStepUs));
@@ -2324,6 +2347,8 @@ TEST_F(VTEPositionTest, ResetsFilterOnStalePredictionGap)
EXPECT_NEAR(initial_state.rel_pos[0], initial_rel_pos(0), kTolerance);
EXPECT_NEAR(initial_state.rel_pos[1], initial_rel_pos(1), kTolerance);
EXPECT_NEAR(initial_state.rel_pos[2], initial_rel_pos(2), kTolerance);
ASSERT_TRUE(_target_pose_sub->update());
EXPECT_TRUE(_target_pose_sub->get().abs_pos_valid);
vte_test::flushSubscription(_aid_fiducial_sub);
vte_test::flushSubscription(_vte_state_sub);
@@ -2332,7 +2357,11 @@ TEST_F(VTEPositionTest, ResetsFilterOnStalePredictionGap)
_vte->update(matrix::Vector3f{});
EXPECT_FALSE(_aid_fiducial_sub->update());
EXPECT_FALSE(_vte_state_sub->update());
// The logged state is marked invalid. The pose is not republished: precision landing keeps
// the last one it received as the last known target location.
ASSERT_TRUE(_vte_state_sub->update());
EXPECT_FALSE(_vte_state_sub->get().rel_pos_valid);
EXPECT_FALSE(_target_pose_sub->update());
publishVisionPos(fresh_rel_pos, vte_test::identityQuat(), kSmallVisionCov,
vte_test::advanceMicroseconds(kStepUs));
@@ -297,10 +297,11 @@ protected:
ASSERT_TRUE(_attitude_pub->publish(msg));
}
void publishAcceleration(const matrix::Vector3f &accel, hrt_abstime timestamp)
void publishAcceleration(const matrix::Vector3f &accel, hrt_abstime timestamp, hrt_abstime sample_time = 0)
{
vehicle_acceleration_s msg{};
msg.timestamp = timestamp;
msg.timestamp_sample = sample_time;
msg.xyz[0] = accel(0);
msg.xyz[1] = accel(1);
msg.xyz[2] = accel(2);
@@ -571,21 +572,22 @@ TEST_F(VisionTargetEstTest, ComputeGpsVelocityOffsetRequiresOffsetAndData)
expectVectorNear(vel_offset, expected_offset);
}
// WHY: Attitude is required to transform body-frame data into NED.
// WHAT: Call pollEstimatorInput without attitude and expect it to fail.
TEST_F(VisionTargetEstTest, PollEstimatorInputRequiresAttitude)
// Acceleration arriving before attitude must remain available for the next poll.
TEST_F(VisionTargetEstTest, PollEstimatorInputKeepsAccelerationUntilAttitudeArrives)
{
// GIVEN: No vehicle attitude sample has been published.
matrix::Vector3f acc_ned{};
const hrt_abstime sample_time = vte_test::nowUs();
const hrt_abstime now = vte_test::advanceMicroseconds(1_ms);
publishAcceleration(matrix::Vector3f(1.f, 2.f, -kGravity), now, sample_time);
matrix::Vector3f acc_ned{}, gps_offset{}, vel_offset{};
matrix::Quaternionf q_att{};
matrix::Vector3f gps_offset{};
matrix::Vector3f vel_offset{};
const bool vel_offset_updated = true;
bool acc_valid = true;
bool acc_valid = false;
EXPECT_FALSE(_vte->pollEstimatorInput(acc_ned, q_att, gps_offset, vel_offset, false, acc_valid));
// WHEN: The estimator input is polled.
EXPECT_FALSE(_vte->pollEstimatorInput(acc_ned, q_att, gps_offset, vel_offset,
vel_offset_updated, acc_valid));
publishAttitude(vte_test::identityQuat(), now);
ASSERT_TRUE(_vte->pollEstimatorInput(acc_ned, q_att, gps_offset, vel_offset, false, acc_valid));
EXPECT_TRUE(acc_valid);
EXPECT_EQ(_vte->_vehicle_acc_body.timestamp, sample_time);
expectVectorNear(acc_ned, matrix::Vector3f(1.f, 2.f, 0.f));
}
// WHY: Valid inputs should be rotated into NED and include gravity compensation.
@@ -1346,7 +1348,7 @@ TEST_F(VisionTargetEstTest, UpdateEstimatorsDropsStaleAcceleration)
const hrt_abstime stale_timestamp =
timestamp - (VisionTargetEstTestable::kAccUpdatedTimeoutUs + 1_ms);
publishAcceleration(matrix::Vector3f(0.1f, -0.2f, 9.5f), stale_timestamp);
publishAcceleration(matrix::Vector3f(0.1f, -0.2f, 9.5f), timestamp, stale_timestamp);
// WHEN: Estimator updates run with stale acceleration input.
_vte->updateEstimators();
@@ -1471,6 +1473,22 @@ TEST_F(VisionTargetEstTest, PollEstimatorInputResetsCachedFrame)
}
}
// WHY: A non-finite acceleration sample must not reach the prediction, which would otherwise skip a step.
TEST_F(VisionTargetEstTest, PollEstimatorInputRejectsNonFiniteAcceleration)
{
matrix::Vector3f acceleration{}, position_offset{}, velocity_offset{};
matrix::Quaternionf attitude{};
bool acceleration_valid = true;
const hrt_abstime now = vte_test::advanceMicroseconds(kStepUs);
publishAcceleration(matrix::Vector3f(NAN, 0.f, 0.f), now);
publishAttitude(vte_test::identityQuat(), now);
ASSERT_TRUE(_vte->pollEstimatorInput(acceleration, attitude, position_offset, velocity_offset, false,
acceleration_valid));
EXPECT_FALSE(acceleration_valid);
EXPECT_TRUE(acceleration.isAllFinite());
}
TEST_F(VisionTargetEstTest, PollLocalPoseUsesSampleTime)
{
uORB::Publication<vehicle_local_position_s> publication{ORB_ID(vehicle_local_position)};
@@ -53,7 +53,7 @@ parameters:
type: float
default: 3.0
unit: s
min: 0.0
min: 0.1
max: 50
decimal: 1
increment: 0.5
@@ -66,7 +66,7 @@ parameters:
type: float
default: 2.0
unit: s
min: 0.0
min: 0.1
max: 50
decimal: 2
increment: 0.25
@@ -77,7 +77,7 @@ parameters:
type: float
default: 1.0
unit: s
min: 0.0
min: 0.1
max: 10
decimal: 2
increment: 0.05
@@ -88,7 +88,7 @@ parameters:
type: float
default: 0.1
unit: s
min: 0.0
min: 0.01
max: 5
decimal: 3
increment: 0.01