updated calculation

9 years ago · 616725410d
1 changed files with 1 additions and 1 deletions
--- a/EKF/airspeed_fusion.cpp
+++ b/EKF/airspeed_fusion.cpp
@ -73,7 +73,7 @@ void Ekf::fuseAirspeed()
				@@ -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;