Improve unit -test coverage

Author: jonas

docs/en/advanced_features/vision_target_estimator_advanced.md

@@ -17,6 +17,7 @@ This guide expands on the [Vision Target Estimator module overview](../advanced_
   - [Development and debugging tips](#development-and-debugging-tips)
     - [Adding new measurement sources](#adding-new-measurement-sources)
     - [Regenerating the symbolic model](#regenerating-the-symbolic-model)
+  - [Unit test suites](#unit-test-suites)
 
 ## System architecture
 
@@ -288,3 +289,22 @@ The generated headers (`predictState.h`, `predictCov.h`, `computeInnovCov.h`, `g
 3. If you need to refresh the committed defaults, set `-DVTEST_UPDATE_COMMITTED_DERIVATION=ON` and commit the regenerated files in `Position/vtest_derivation/generated*/` once vetted.
 
 If the build fails during regeneration, inspect the CMake output for the SymForce invocation and rerun it manually inside `Position/vtest_derivation/` to catch Python errors. After regenerating, rebuild the module to ensure the Jacobians and code stay in sync.
+
+## Unit test suites
+
+- `TEST_VTE_KF_position`: Kalman filter math for the position state (prediction, NIS gating, bias-aware H, OOSM gold standard). Static model (state size 3) by default, moving-target tests (state size 5) build only when `CONFIG_VTEST_MOVING` is enabled.
+- `TEST_VTE_KF_orientation`: Kalman filter math for yaw/yaw-rate (wrap logic, process noise, dt edge cases, covariance symmetry).
+- `TEST_VTE_VTEPosition`: Module logic for vision/GNSS fusion, offsets, interpolation, ordering, and uORB innovation topics (static + moving gated by `CONFIG_VTEST_MOVING`).
+- `TEST_VTE_VTEOrientation`: Module logic for yaw fusion, noise models, resets, and OOSM handling.
+- `TEST_VTE_VTEOosm`: Generic OOSM manager behavior.
+
+Run locally:
+
+```sh
+make tests TESTFILTER=TEST_VTE*
+```
+
+Timing constraints to keep in mind when exercising OOSM paths:
+
+- OOSM activates when measurement lag exceeds 20 ms.
+- History accepts measurements up to 500 ms old before rejecting them.

src/modules/vision_target_estimator/test/TEST_VTE_KF_orientation.cpp

@@ -43,16 +43,20 @@
 
 #include <cmath>
 #include <matrix/math.hpp>
+#include <drivers/drv_hrt.h>
 
 #include "Orientation/KF_orientation.h"
 
 namespace vte = vision_target_estimator;
 
 namespace
 {
+using namespace time_literals;
+
 static constexpr float kTolerance = 1e-4f;
 static constexpr float kMinVar = 1e-9f;
 static constexpr float kPi = 3.14159265358979323846f;
+static constexpr hrt_abstime kNowOffset = 10_us;
 
 using StateVec = matrix::Vector<float, vte::State::size>;
 
@@ -128,6 +132,27 @@ TEST(KFOrientationTest, PredictAddsProcessNoise)
 	EXPECT_NEAR(cov_diag(vte::State::yaw_rate), expected_p11, kTolerance);
 }
 
+TEST(KFOrientationTest, PredictAddsOffDiagonalCovariance)
+{
+	// WHY: The white-noise yaw acceleration model introduces cross-covariance terms.
+	// WHAT: Ensure off-diagonal terms match dt^2/2 * var and covariance remains symmetric.
+	vte::KF_orientation filter;
+	filter.setState(makeState(0.f, 0.f));
+	filter.setStateCovarianceDiag(makeDiag(0.f, 0.f));
+	filter.setYawAccVar(0.4f);
+
+	const float dt = 0.5f;
+	filter.predict(dt);
+
+	const auto cov = filter.getStateCovariance();
+	const float expected_off_diag = 0.5f * dt * dt * 0.4f;
+
+	EXPECT_NEAR(cov(vte::State::yaw, vte::State::yaw_rate), expected_off_diag, kTolerance);
+	EXPECT_NEAR(cov(vte::State::yaw_rate, vte::State::yaw), expected_off_diag, kTolerance);
+	EXPECT_NEAR(cov(vte::State::yaw, vte::State::yaw_rate),
+		    cov(vte::State::yaw_rate, vte::State::yaw), kTolerance);
+}
+
 TEST(KFOrientationTest, PredictIgnoresInvalidYawAccVar)
 {
 	// WHY: Invalid noise parameters must not corrupt the covariance.
@@ -145,6 +170,56 @@ TEST(KFOrientationTest, PredictIgnoresInvalidYawAccVar)
 	EXPECT_NEAR(cov_diag(vte::State::yaw_rate), 2.f, kTolerance);
 }
 
+TEST(KFOrientationTest, PredictZeroDtNoChange)
+{
+	// WHY: Zero dt should not change state or covariance.
+	// WHAT: Predict with dt=0 and ensure state/cov stay unchanged.
+	vte::KF_orientation filter;
+	filter.setState(makeState(1.2f, -0.5f));
+	filter.setStateCovarianceDiag(makeDiag(0.3f, 0.2f));
+	filter.setYawAccVar(0.4f);
+
+	const StateVec state_before = filter.getState();
+	const auto cov_before = filter.getStateCovariance();
+
+	filter.predict(0.f);
+
+	const StateVec state_after = filter.getState();
+	const auto cov_after = filter.getStateCovariance();
+
+	EXPECT_NEAR(state_after(vte::State::yaw), state_before(vte::State::yaw), kTolerance);
+	EXPECT_NEAR(state_after(vte::State::yaw_rate), state_before(vte::State::yaw_rate), kTolerance);
+	EXPECT_NEAR(cov_after(vte::State::yaw, vte::State::yaw), cov_before(vte::State::yaw, vte::State::yaw), kTolerance);
+	EXPECT_NEAR(cov_after(vte::State::yaw_rate, vte::State::yaw_rate),
+		    cov_before(vte::State::yaw_rate, vte::State::yaw_rate), kTolerance);
+	EXPECT_NEAR(cov_after(vte::State::yaw, vte::State::yaw_rate),
+		    cov_before(vte::State::yaw, vte::State::yaw_rate), kTolerance);
+}
+
+TEST(KFOrientationTest, PredictNegativeDtReturnsEarly)
+{
+	// WHY: Negative dt is invalid and should be ignored.
+	// WHAT: Verify predict() does not change state or covariance for dt < 0.
+	vte::KF_orientation filter;
+	filter.setState(makeState(-1.0f, 0.7f));
+	filter.setStateCovarianceDiag(makeDiag(0.6f, 0.4f));
+	filter.setYawAccVar(0.4f);
+
+	const StateVec state_before = filter.getState();
+	const auto cov_before = filter.getStateCovariance();
+
+	filter.predict(-0.1f);
+
+	const StateVec state_after = filter.getState();
+	const auto cov_after = filter.getStateCovariance();
+
+	EXPECT_NEAR(state_after(vte::State::yaw), state_before(vte::State::yaw), kTolerance);
+	EXPECT_NEAR(state_after(vte::State::yaw_rate), state_before(vte::State::yaw_rate), kTolerance);
+	EXPECT_NEAR(cov_after(vte::State::yaw, vte::State::yaw), cov_before(vte::State::yaw, vte::State::yaw), kTolerance);
+	EXPECT_NEAR(cov_after(vte::State::yaw_rate, vte::State::yaw_rate),
+		    cov_before(vte::State::yaw_rate, vte::State::yaw_rate), kTolerance);
+}
+
 TEST(KFOrientationTest, InnovationWrapsAcrossPi)
 {
 	// WHY: Innovations across the wrap should be small, not ~2pi.
@@ -154,15 +229,35 @@ TEST(KFOrientationTest, InnovationWrapsAcrossPi)
 	filter.setStateCovarianceDiag(makeDiag(1.f, 1.f));
 
 	const StateVec H = makeH(vte::State::yaw);
-	const vte::KF_orientation::ScalarMeas meas = makeMeas(1'000'000, -3.13f, 0.1f, H);
-	const vte::FusionResult res = filter.fuseScalarAtTime(meas, 1'000'010, 100.f);
+	static constexpr hrt_abstime kMeasTime = 1_s;
+	const vte::KF_orientation::ScalarMeas meas = makeMeas(kMeasTime, -3.13f, 0.1f, H);
+	const vte::FusionResult res = filter.fuseScalarAtTime(meas, kMeasTime + kNowOffset, 100.f);
 
 	const float expected_innov = matrix::wrap_pi(-3.13f - 3.13f);
 
 	EXPECT_EQ(res.status, vte::FusionStatus::FUSED_CURRENT);
 	EXPECT_NEAR(res.innov, expected_innov, kTolerance);
 }
 
+TEST(KFOrientationTest, InnovationDoesNotWrapYawRateMeasurement)
+{
+	// WHY: Only yaw innovations should be wrapped; yaw-rate is linear.
+	// WHAT: Ensure yaw-rate innovation uses the raw difference.
+	vte::KF_orientation filter;
+	filter.setState(makeState(0.f, 3.13f));
+	filter.setStateCovarianceDiag(makeDiag(1.f, 1.f));
+
+	const StateVec H = makeH(vte::State::yaw_rate);
+	static constexpr hrt_abstime kMeasTime = 1500_ms;
+	const vte::KF_orientation::ScalarMeas meas = makeMeas(kMeasTime, -3.13f, 0.1f, H);
+	const vte::FusionResult res = filter.fuseScalarAtTime(meas, kMeasTime + kNowOffset, 100.f);
+
+	const float expected_innov = -3.13f - 3.13f;
+
+	EXPECT_EQ(res.status, vte::FusionStatus::FUSED_CURRENT);
+	EXPECT_NEAR(res.innov, expected_innov, kTolerance);
+}
+
 TEST(KFOrientationTest, RejectsOutlierNis)
 {
 	// WHY: NIS gating is critical to rejecting yaw outliers.
@@ -172,8 +267,9 @@ TEST(KFOrientationTest, RejectsOutlierNis)
 	filter.setStateCovarianceDiag(makeDiag(0.01f, 0.01f));
 
 	const StateVec H = makeH(vte::State::yaw);
-	const vte::KF_orientation::ScalarMeas meas = makeMeas(2'000'000, 3.14f, 0.01f, H);
-	const vte::FusionResult res = filter.fuseScalarAtTime(meas, 2'000'010, 0.1f);
+	static constexpr hrt_abstime kMeasTime = 2_s;
+	const vte::KF_orientation::ScalarMeas meas = makeMeas(kMeasTime, 3.14f, 0.01f, H);
+	const vte::FusionResult res = filter.fuseScalarAtTime(meas, kMeasTime + kNowOffset, 0.1f);
 
 	const StateVec state = filter.getState();
 
@@ -192,8 +288,9 @@ TEST(KFOrientationTest, ClampCovarianceToMin)
 
 	const StateVec H = makeH(vte::State::yaw);
 	// Keep innovation small to avoid NIS rejection while exercising the clamp.
-	const vte::KF_orientation::ScalarMeas meas = makeMeas(3'000'000, 0.f, 1e-4f, H);
-	const vte::FusionResult res = filter.fuseScalarAtTime(meas, 3'000'010, 100.f);
+	static constexpr hrt_abstime kMeasTime = 3_s;
+	const vte::KF_orientation::ScalarMeas meas = makeMeas(kMeasTime, 0.f, 1e-4f, H);
+	const vte::FusionResult res = filter.fuseScalarAtTime(meas, kMeasTime + kNowOffset, 100.f);
 	const StateVec cov_diag = filter.getStateCovarianceDiag();
 
 	EXPECT_EQ(res.status, vte::FusionStatus::FUSED_CURRENT);
@@ -206,15 +303,51 @@ TEST(KFOrientationTest, CorrectionWrapsYaw)
 	// WHY: Correction should normalize yaw after applying the gain.
 	// WHAT: Force a correction that would exceed pi without wrapping.
 	vte::KF_orientation filter;
-	filter.setState(makeState(3.0f, 0.f));
-	filter.setStateCovarianceDiag(makeDiag(1.f, 1.f));
+	filter.setState(makeState(3.13f, 0.f));
+	filter.setStateCovarianceDiag(makeDiag(0.5f, 0.2f));
 
 	const StateVec H = makeH(vte::State::yaw);
-	const vte::KF_orientation::ScalarMeas meas = makeMeas(4'000'000, 3.14f, 0.1f, H);
-	const vte::FusionResult res = filter.fuseScalarAtTime(meas, 4'000'010, 100.f);
+	static constexpr hrt_abstime kMeasTime = 4_s;
+	const vte::KF_orientation::ScalarMeas meas = makeMeas(kMeasTime, -3.13f, 0.2f, H);
+	const vte::FusionResult res = filter.fuseScalarAtTime(meas, kMeasTime + kNowOffset, 100.f);
 	const StateVec state = filter.getState();
 
+	const float expected_innov = matrix::wrap_pi(-3.13f - 3.13f);
+	const float expected_k = 0.5f / (0.5f + 0.2f);
+	const float expected_yaw = matrix::wrap_pi(3.13f + expected_k * expected_innov);
+
 	EXPECT_EQ(res.status, vte::FusionStatus::FUSED_CURRENT);
+	EXPECT_NEAR(res.innov, expected_innov, kTolerance);
+	EXPECT_NEAR(state(vte::State::yaw), expected_yaw, kTolerance);
 	EXPECT_LE(state(vte::State::yaw), kPi);
 	EXPECT_GE(state(vte::State::yaw), -kPi);
 }
+
+TEST(KFOrientationTest, CorrectionMaintainsSymmetricCovariance)
+{
+	// WHY: Corrections must keep covariance symmetric and bounded.
+	// WHAT: Perform a correction and validate off-diagonal symmetry and magnitude.
+	vte::KF_orientation filter;
+	filter.setState(makeState(0.2f, 0.1f));
+	filter.setStateCovarianceDiag(makeDiag(0.6f, 0.4f));
+	filter.setYawAccVar(0.3f);
+
+	filter.predict(0.2f);
+	const auto cov_before = filter.getStateCovariance();
+
+	const StateVec H = makeH(vte::State::yaw);
+	static constexpr hrt_abstime kMeasTime = 5_s;
+	const vte::KF_orientation::ScalarMeas meas = makeMeas(kMeasTime, 0.25f, 0.1f, H);
+	const vte::FusionResult res = filter.fuseScalarAtTime(meas, kMeasTime + kNowOffset, 100.f);
+	const auto cov_after = filter.getStateCovariance();
+
+	EXPECT_EQ(res.status, vte::FusionStatus::FUSED_CURRENT);
+	EXPECT_NEAR(cov_after(vte::State::yaw, vte::State::yaw_rate),
+		    cov_after(vte::State::yaw_rate, vte::State::yaw), kTolerance);
+
+	const float bound = sqrtf(cov_after(vte::State::yaw, vte::State::yaw) *
+				  cov_after(vte::State::yaw_rate, vte::State::yaw_rate)) + kTolerance;
+	EXPECT_LE(fabsf(cov_after(vte::State::yaw, vte::State::yaw_rate)), bound);
+	EXPECT_LE(fabsf(cov_after(vte::State::yaw, vte::State::yaw_rate)),
+		  fabsf(cov_before(vte::State::yaw, vte::State::yaw_rate)) + 1.0f);
+}