From cb2bb2e0980e21a9cb4029aa036d2535d37c94b8 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Wed, 20 Mar 2024 12:12:27 +0100
Subject: [PATCH] ekf2: add no gyro bias estimate test case

This makes the ekf unstable and creates NANs during initialization
---
 .../test/sensor_simulator/ekf_wrapper.cpp     | 10 +++++++
 .../ekf2/test/sensor_simulator/ekf_wrapper.h  |  3 +++
 .../ekf2/test/test_EKF_initialization.cpp     | 26 +++++++++++++++++++
 3 files changed, 39 insertions(+)

diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp
index 513e6d057b..fe2c3a88ff 100644
--- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp
+++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp
@@ -314,3 +314,13 @@ float EkfWrapper::getMagHeadingNoise() const
 {
 	return _ekf_params->mag_heading_noise;
 }
+
+void EkfWrapper::enableGyroBiasEstimation()
+{
+	_ekf_params->imu_ctrl |= static_cast<int32_t>(ImuCtrl::GyroBias);
+}
+
+void EkfWrapper::disableGyroBiasEstimation()
+{
+	_ekf_params->imu_ctrl &= ~static_cast<int32_t>(ImuCtrl::GyroBias);
+}
diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h
index 43753e9bc5..e1d35f0934 100644
--- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h
+++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h
@@ -128,6 +128,9 @@ public:
 
 	float getMagHeadingNoise() const;
 
+	void enableGyroBiasEstimation();
+	void disableGyroBiasEstimation();
+
 private:
 	std::shared_ptr<Ekf> _ekf;
 
diff --git a/src/modules/ekf2/test/test_EKF_initialization.cpp b/src/modules/ekf2/test/test_EKF_initialization.cpp
index 6e2dae1b9e..eed2108641 100644
--- a/src/modules/ekf2/test/test_EKF_initialization.cpp
+++ b/src/modules/ekf2/test/test_EKF_initialization.cpp
@@ -191,6 +191,32 @@ TEST_F(EkfInitializationTest, initializeWithZeroTiltNotAtRest)
 	learningCorrectAccelBias();
 }
 
+TEST_F(EkfInitializationTest, initializeWithTiltNoGyroBiasEstimate)
+{
+	const float pitch = math::radians(30.0f);
+	const float roll = math::radians(-20.0f);
+	const Eulerf euler_angles_sim(roll, pitch, 0.0f);
+	const Quatf quat_sim(euler_angles_sim);
+
+	_ekf_wrapper.disableGyroBiasEstimation();
+	_sensor_simulator.simulateOrientation(quat_sim);
+
+	_sensor_simulator.runSeconds(_init_tilt_period);
+
+	EXPECT_TRUE(_ekf->control_status_flags().tilt_align);
+
+	initializedOrienationIsMatchingGroundTruth(quat_sim);
+	quaternionVarianceBigEnoughAfterOrientationInitialization(0.00001f);
+
+	velocityAndPositionCloseToZero();
+
+	positionVarianceBigEnoughAfterOrientationInitialization(0.00001f); // Fake position fusion obs var when at rest sq(0.5f)
+	velocityVarianceBigEnoughAfterOrientationInitialization(0.0001f);
+
+	_sensor_simulator.runSeconds(1.f);
+	learningCorrectAccelBias();
+}
+
 TEST_F(EkfInitializationTest, gyroBias)
 {
 	// GIVEN: a healthy filter