From 82ea544e8c48586d453ee99df2c012e381b322c9 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 20 Mar 2025 16:19:13 +0100 Subject: [PATCH] ekf2-test: add flow unit test for negative distance --- .../ekf2/test/test_EKF_flow_generated.cpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/src/modules/ekf2/test/test_EKF_flow_generated.cpp b/src/modules/ekf2/test/test_EKF_flow_generated.cpp index 58a0eabdeb..ee271df03a 100644 --- a/src/modules/ekf2/test/test_EKF_flow_generated.cpp +++ b/src/modules/ekf2/test/test_EKF_flow_generated.cpp @@ -71,3 +71,22 @@ TEST(FlowGenerated, distBottom0y) sym::ComputeFlowYInnovVarAndH(state.vector(), P, R, FLT_EPSILON, &innov_var, &H); EXPECT_GT(innov_var, 1e12); } + +TEST(FlowGenerated, distBottomNeg) +{ + // GIVEN: a small negative distance to the ground (singularity) + StateSample state{}; + state.quat_nominal = Quatf(); + state.pos(2) = 1e-3f; + + const float R = sq(radians(sq(0.5f))); + SquareMatrixState P = createRandomCovarianceMatrix(); + + VectorState H; + Vector2f innov_var; + sym::ComputeFlowXyInnovVarAndHx(state.vector(), P, R, FLT_EPSILON, &innov_var, &H); + EXPECT_GT(innov_var(0), 1e6); + EXPECT_GT(innov_var(1), 1e6); + sym::ComputeFlowYInnovVarAndH(state.vector(), P, R, FLT_EPSILON, &innov_var(1), &H); + EXPECT_GT(innov_var(1), 1e6); +}