diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp index 6b523fad28..630fef49d2 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -160,7 +160,7 @@ void Ekf::controlGnssVelFusion(estimator_aid_source3d_s &aid_src, const bool for const bool do_reset = force_reset || !_control_status_prev.flags.yaw_align; // Start fusing the data without reset if possible to avoid disturbing the filter - if (!do_reset && ((aid_src.test_ratio[0] + aid_src.test_ratio[1]) < sq(0.5f))) { + if (!do_reset && aid_src.test_ratio[0] < 1.f && aid_src.test_ratio[1] < 1.f) { fused = fuseVelocity(aid_src); } @@ -221,7 +221,7 @@ void Ekf::controlGnssPosFusion(estimator_aid_source2d_s &aid_src, const bool for // Start fusing the data without reset if possible to avoid disturbing the filter if (_local_origin_lat_lon.isInitialized() && !do_reset - && ((aid_src.test_ratio[0] + aid_src.test_ratio[1]) < sq(0.5f))) { + && aid_src.test_ratio[0] < 1.f && aid_src.test_ratio[1] < 1.f) { fused = fuseHorizontalPosition(aid_src); }