diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index d630fe6546..3b5d90c1fd 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -73,7 +73,7 @@ void Ekf::fuseAirspeed() if (v_tas_pred > 3.0f) { // Calculate the observation jacobian // intermediate variable from algebraic optimisation - SH_TAS[0] = 1 / (sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); + SH_TAS[0] = 1 / v_tas_pred; SH_TAS[1] = (SH_TAS[0] * (2 * ve - 2 * vwe)) / 2.0f; SH_TAS[2] = (SH_TAS[0] * (2 * vn - 2 * vwn)) / 2.0f;