ekf2_test: test yaw variance after reset

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp

@@ -304,3 +304,8 @@ void EkfWrapper::setDragFusionParameters(const float &bcoef_x, const float &bcoe
 	_ekf_params->bcoef_y = bcoef_y;
 	_ekf_params->mcoef = mcoef;
 }
+
+float EkfWrapper::getMagHeadingNoise() const
+{
+	return _ekf_params->mag_heading_noise;
+}

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h

@@ -124,6 +124,8 @@ public:
 	void disableDragFusion();
 	void setDragFusionParameters(const float &bcoef_x, const float &bcoef_y, const float &mcoef);
 
+	float getMagHeadingNoise() const;
+
 private:
 	std::shared_ptr<Ekf> _ekf;
 

src/modules/ekf2/test/test_EKF_initialization.cpp

@@ -84,6 +84,12 @@ public:
 		EXPECT_TRUE(quat_variance(3) > quat_variance_limit) << "quat_variance(3): " << quat_variance(3);
 	}
 
+	void yawVarianceBigEnoughAfterHeadingReset()
+	{
+		// The yaw variance is smaller than its reset value as we do not probe instantly after the reset
+		EXPECT_GT(sqrtf(_ekf->getYawVar()), _ekf_wrapper.getMagHeadingNoise() / 5.f);
+	}
+
 	void velocityAndPositionCloseToZero()
 	{
 		const Vector3f pos = _ekf->getPosition();
@@ -264,6 +270,7 @@ TEST_F(EkfInitializationTest, initializeHeadingWithZeroTilt)
 
 	initializedOrienationIsMatchingGroundTruth(quat_sim);
 	quaternionVarianceBigEnoughAfterOrientationInitialization(0.00001f);
+	yawVarianceBigEnoughAfterHeadingReset();
 
 	velocityAndPositionCloseToZero();
 
@@ -290,6 +297,7 @@ TEST_F(EkfInitializationTest, initializeWithTilt)
 
 	initializedOrienationIsMatchingGroundTruth(quat_sim);
 	quaternionVarianceBigEnoughAfterOrientationInitialization(0.00001f);
+	yawVarianceBigEnoughAfterHeadingReset();
 
 	velocityAndPositionCloseToZero();