ekf2: add no gyro bias estimate test case

This makes the ekf unstable and creates NANs during initialization
This commit is contained in:
bresch 2024-03-20 12:12:27 +01:00 committed by Mathieu Bresciani
parent c9221b91ad
commit cb2bb2e098
3 changed files with 39 additions and 0 deletions

View File

@ -314,3 +314,13 @@ float EkfWrapper::getMagHeadingNoise() const
{ {
return _ekf_params->mag_heading_noise; 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);
}

View File

@ -128,6 +128,9 @@ public:
float getMagHeadingNoise() const; float getMagHeadingNoise() const;
void enableGyroBiasEstimation();
void disableGyroBiasEstimation();
private: private:
std::shared_ptr<Ekf> _ekf; std::shared_ptr<Ekf> _ekf;

View File

@ -191,6 +191,32 @@ TEST_F(EkfInitializationTest, initializeWithZeroTiltNotAtRest)
learningCorrectAccelBias(); 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) TEST_F(EkfInitializationTest, gyroBias)
{ {
// GIVEN: a healthy filter // GIVEN: a healthy filter