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(ImuCtrl::GyroBias); +} + +void EkfWrapper::disableGyroBiasEstimation() +{ + _ekf_params->imu_ctrl &= ~static_cast(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; 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