diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 983db8aa86..ac410b4d8d 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -308,10 +308,10 @@ bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, fl pos_corrected += _state.vel.xy() * dt_s; } - const float obs_var = math::max(accuracy, sq(0.01f)); + const float obs_var = math::max(sq(accuracy), sq(0.01f)); const Vector2f innov = Vector2f(_state.pos.xy()) - pos_corrected; - const Vector2f innov_var = Vector2f(getStateVariance()) + obs_var; + const Vector2f innov_var = Vector2f(getStateVariance()) + obs_var; const float sq_gate = sq(5.f); // magic hardcoded gate const Vector2f test_ratio{sq(innov(0)) / (sq_gate * innov_var(0)), diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 362c68c372..c42a499e09 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -529,8 +529,9 @@ void EKF2::Run() } else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE) { - if ((_ekf.control_status_flags().wind_dead_reckoning || _ekf.control_status_flags().inertial_dead_reckoning) && - PX4_ISFINITE(vehicle_command.param2) && PX4_ISFINITE(vehicle_command.param5) && PX4_ISFINITE(vehicle_command.param6) + if ((_ekf.control_status_flags().wind_dead_reckoning || _ekf.control_status_flags().inertial_dead_reckoning + || (!_ekf.control_status_flags().in_air && !_ekf.control_status_flags().gps)) && PX4_ISFINITE(vehicle_command.param2) + && PX4_ISFINITE(vehicle_command.param5) && PX4_ISFINITE(vehicle_command.param6) ) { const float measurement_delay_seconds = math::constrain(vehicle_command.param2, 0.0f,