diff --git a/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp index 6a9ab3442220..f7365b733370 100644 --- a/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp @@ -82,10 +82,10 @@ void Ekf::updateSideslip(estimator_aid_source1d_s &aid_src) const { float observation = 0.f; const float R = math::max(sq(_params.beta_noise), sq(0.01f)); // observation noise variance - + const float epsilon = 1e-3f; float innov; float innov_var; - sym::ComputeSideslipInnovAndInnovVar(_state.vector(), P, R, FLT_EPSILON, &innov, &innov_var); + sym::ComputeSideslipInnovAndInnovVar(_state.vector(), P, R, epsilon, &innov, &innov_var); updateAidSourceStatus(aid_src, _time_delayed_us, // sample timestamp @@ -130,10 +130,11 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) _fault_status.flags.bad_sideslip = false; + const float epsilon = 1e-3f; VectorState H; // Observation jacobian VectorState K; // Kalman gain vector - sym::ComputeSideslipHAndK(_state.vector(), P, sideslip.innovation_variance, FLT_EPSILON, &H, &K); + sym::ComputeSideslipHAndK(_state.vector(), P, sideslip.innovation_variance, epsilon, &H, &K); if (update_wind_only) { const Vector2f K_wind = K.slice(State::wind_vel.idx, 0);