ekf2_test: test yaw variance after reset

src/modules/ekf2/EKF/ekf.h

@@ -441,6 +441,7 @@ public:
 
 	// rotate quaternion covariances into variances for an equivalent rotation vector
 	Vector3f calcRotVecVariances() const;
+	float getYawVar() const;
 
 	// set minimum continuous period without GPS fail required to mark a healthy GPS status
 	void set_min_required_gps_health_time(uint32_t time_us) { _min_gps_health_time_us = time_us; }

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -889,6 +889,15 @@ Vector3f Ekf::calcRotVecVariances() const
 	return rot_var;
 }
 
+float Ekf::getYawVar() const
+{
+	Vector24f H_YAW;
+	float yaw_var = 0.f;
+	computeYawInnovVarAndH(0.f, yaw_var, H_YAW);
+
+	return yaw_var;
+}
+
 // initialise the quaternion covariances using rotation vector variances
 // do not call before quaternion states are initialised
 void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var)

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();