diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp index 2db9f1c8d7..87b90e5206 100644 --- a/src/modules/ekf2/EKF/optflow_fusion.cpp +++ b/src/modules/ekf2/EKF/optflow_fusion.cpp @@ -219,13 +219,13 @@ bool Ekf::calcOptFlowBodyRateComp() // calculate the bias estimate using a combined LPF and spike filter _flow_gyro_bias = _flow_gyro_bias * 0.99f + matrix::constrain(measured_body_rate - reference_body_rate, -0.1f, 0.1f) * 0.01f; - - // apply gyro bias - _flow_sample_delayed.gyro_xyz -= (_flow_gyro_bias * _flow_sample_delayed.dt); - - is_body_rate_comp_available = true; } + // apply gyro bias + _flow_sample_delayed.gyro_xyz -= (_flow_gyro_bias * _flow_sample_delayed.dt); + + is_body_rate_comp_available = true; + } else { // Use the EKF gyro data if optical flow sensor gyro data is not available // for clarification of the sign see definition of flowSample and imuSample in common.h