From a863fde9f184de636a1010200b7aa54841c5b45b Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 11 Jan 2024 14:26:44 -0500 Subject: [PATCH] ekf2: GPS checks apply pos deriv limits to low pass, not raw value --- src/modules/ekf2/EKF/gps_checks.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/ekf2/EKF/gps_checks.cpp b/src/modules/ekf2/EKF/gps_checks.cpp index d4414f8a51..f06c08be53 100644 --- a/src/modules/ekf2/EKF/gps_checks.cpp +++ b/src/modules/ekf2/EKF/gps_checks.cpp @@ -183,13 +183,13 @@ bool Ekf::runGnssChecks(const gnssSample &gps) _gps_alt_prev = gps.alt; } - // Calculate the horizontal and vertical drift velocity components and limit to 10x the threshold + // Calculate the horizontal and vertical drift velocity components const Vector3f vel_limit(_params.req_hdrift, _params.req_hdrift, _params.req_vdrift); - Vector3f pos_derived(delta_pos_n, delta_pos_e, (_gps_alt_prev - gps.alt)); - pos_derived = matrix::constrain(pos_derived / dt, -10.0f * vel_limit, 10.0f * vel_limit); + const Vector3f pos_deriv(delta_pos_n / dt, delta_pos_e / dt, (_gps_alt_prev - gps.alt) / dt); - // Apply a low pass filter - _gps_pos_deriv_filt = pos_derived * filter_coef + _gps_pos_deriv_filt * (1.0f - filter_coef); + // Apply a low pass filter and limit to 10x the threshold + _gps_pos_deriv_filt = matrix::constrain(pos_deriv * filter_coef + _gps_pos_deriv_filt * (1.f - filter_coef), + -10.f * vel_limit, 10.f * vel_limit); // Calculate the horizontal drift speed and fail if too high _gps_horizontal_position_drift_rate_m_s = Vector2f(_gps_pos_deriv_filt.xy()).norm();