From 397ff4a102053eeca697423b2def56985ab214d1 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 12 Jul 2024 13:29:23 -0400 Subject: [PATCH] ekf2: sideslip symforce increase epsilon to avoid 1/e^2 numerical issues --- .../ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) 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 6a9ab34422..f7365b7333 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);