AirspeedValidator: fuse true airspeed before sideslip (#742)

- the states can be initialised more accurately using an airspeed measurement and the wind estimator uses the first measurement to initialise Signed-off-by: RomanBapst <[email protected]>
PX4 · Dec 10, 2020 · de21999 · de21999
1 parent 72420df
commit de21999
Showing 1 changed file with 5 additions and 3 deletions.
diff --git a/src/lib/airspeed_validator/AirspeedValidator.cpp b/src/lib/airspeed_validator/AirspeedValidator.cpp
@@ -77,12 +77,14 @@ AirspeedValidator::update_wind_estimator(const uint64_t time_now_usec, float air
 		Vector3f vI(lpos_vx, lpos_vy, lpos_vz);
 		Quatf q(att_q);
 
-		// sideslip fusion
-		_wind_estimator.fuse_beta(time_now_usec, vI, q);
-
 		// airspeed fusion (with raw TAS)
 		const Vector3f vel_var{Dcmf(q) *Vector3f{lpos_evh, lpos_evh, lpos_evv}};
 		_wind_estimator.fuse_airspeed(time_now_usec, airspeed_true_raw, vI, Vector2f{vel_var(0), vel_var(1)});
+
+		// sideslip fusion
+		_wind_estimator.fuse_beta(time_now_usec, vI, q);
+
+
 	}
 }