WindEstimator: use vehicle heading instead of ground course for initi…

…alisation Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
PX4 · Jan 14, 2022 · 0ce44ce · 0ce44ce
1 parent 1a4b2b3
commit 0ce44ce
Show file tree

Hide file tree

Showing 3 changed files with 12 additions and 11 deletions.
diff --git a/src/lib/wind_estimator/WindEstimator.cpp b/src/lib/wind_estimator/WindEstimator.cpp
@@ -39,7 +39,8 @@
 #include "WindEstimator.hpp"
 
 bool
-WindEstimator::initialise(const matrix::Vector3f &velI, const matrix::Vector2f &velIvar, const float tas_meas)
+WindEstimator::initialise(const matrix::Vector3f &velI, const matrix::Vector2f &velIvar, const float tas_meas,
+			  const matrix::Quatf q_att)
 {
 	// do no initialise if ground velocity is low
 	// this should prevent the filter from initialising on the ground
@@ -50,8 +51,7 @@ WindEstimator::initialise(const matrix::Vector3f &velI, const matrix::Vector2f &
 	const float v_n = velI(0);
 	const float v_e = velI(1);
 
-	// estimate heading from ground velocity
-	const float heading_est = atan2f(v_e, v_n);
+	const float heading_est = matrix::Eulerf(q_att).psi();
 
 	// initilaise wind states assuming zero side slip and horizontal flight
 	_state(INDEX_W_N) = velI(INDEX_W_N) - tas_meas * cosf(heading_est);
@@ -138,13 +138,13 @@ WindEstimator::update(uint64_t time_now)
 
 void
 WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const matrix::Vector3f &velI,
-			     const matrix::Vector2f &velIvar)
+			     const matrix::Vector2f &velIvar, const matrix::Quatf &q_att)
 {
 	matrix::Vector2f velIvar_constrained = { math::max(0.01f, velIvar(0)), math::max(0.01f, velIvar(1)) };
 
 	if (!_initialised) {
 		// try to initialise
-		_initialised =	initialise(velI, velIvar_constrained, true_airspeed);
+		_initialised = initialise(velI, velIvar_constrained, true_airspeed, q_att);
 		return;
 	}
 
@@ -197,7 +197,7 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const
 	if (meas_is_rejected || _tas_innov_var < 0.f) {
 		// only reset filter if _tas_innov_var gets unfeasible
 		if (_tas_innov_var < 0.0f) {
-			_initialised =	initialise(velI, matrix::Vector2f(0.1f, 0.1f), true_airspeed);
+			_initialised = initialise(velI, matrix::Vector2f(0.1f, 0.1f), true_airspeed, q_att);
 		}
 
 		// we either did a filter reset or the current measurement was rejected so do not fuse
@@ -219,7 +219,7 @@ void
 WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const matrix::Quatf &q_att)
 {
 	if (!_initialised) {
-		_initialised =	initialise(velI, matrix::Vector2f(0.1f, 0.1f), velI.length());
+		_initialised = initialise(velI, matrix::Vector2f(0.1f, 0.1f), velI.length(), q_att);
 		return;
 	}
 
@@ -291,7 +291,7 @@ WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const
 
 	if (meas_is_rejected || reinit_filter) {
 		if (reinit_filter) {
-			_initialised =	initialise(velI, matrix::Vector2f(0.1f, 0.1f), velI.length());
+			_initialised = initialise(velI, matrix::Vector2f(0.1f, 0.1f), velI.length(), q_att);
 		}
 
 		// we either did a filter reset or the current measurement was rejected so do not fuse

diff --git a/src/lib/wind_estimator/WindEstimator.hpp b/src/lib/wind_estimator/WindEstimator.hpp
@@ -58,7 +58,7 @@ class WindEstimator
 	void update(uint64_t time_now);
 
 	void fuse_airspeed(uint64_t time_now, float true_airspeed, const matrix::Vector3f &velI,
-			   const matrix::Vector2f &velIvar);
+			   const matrix::Vector2f &velIvar, const matrix::Quatf &q_att);
 	void fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const matrix::Quatf &q_att);
 
 	bool is_estimate_valid() { return _initialised; }
@@ -123,7 +123,8 @@ class WindEstimator
 	bool _wind_estimator_reset = false; ///< wind estimator was reset in this cycle
 
 	// initialise state and state covariance matrix
-	bool initialise(const matrix::Vector3f &velI, const matrix::Vector2f &velIvar, const float tas_meas);
+	bool initialise(const matrix::Vector3f &velI, const matrix::Vector2f &velIvar, const float tas_meas,
+			const matrix::Quatf q_att);
 
 	void run_sanity_checks();
 };
diff --git a/src/modules/airspeed_selector/AirspeedValidator.cpp b/src/modules/airspeed_selector/AirspeedValidator.cpp
@@ -82,7 +82,7 @@ AirspeedValidator::update_wind_estimator(const uint64_t time_now_usec, float air
 
 		// 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)});
+		_wind_estimator.fuse_airspeed(time_now_usec, airspeed_true_raw, vI, Vector2f{vel_var(0), vel_var(1)}, q);
 
 		// sideslip fusion
 		_wind_estimator.fuse_beta(time_now_usec, vI, q);