wind_estimator: added simple check for validity of synthetic airspeed

- synthetic airspeed will only be declared valid as soon as the wind variance
has dropped below a parameterized threshold. This is useful for vehicles without
an airspeed sensor which rely on synthetic airspeed but only once the vehicle
has turned sufficiently for the wind estimates to be reliable.

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/lib/wind_estimator/WindEstimator.cpp

@@ -78,8 +78,8 @@ WindEstimator::initialise(const matrix::Vector3f &velI, const matrix::Vector2f &
 	// get an estimate of the state covariance matrix given the estimated variance of ground velocity
 	// and measured airspeed
 	_P.setZero();
-	_P(INDEX_W_N, INDEX_W_N) = velIvar(0);
-	_P(INDEX_W_E, INDEX_W_E) = velIvar(1);
+	_P(INDEX_W_N, INDEX_W_N) = INITIAL_WIND_VAR;
+	_P(INDEX_W_E, INDEX_W_E) = INITIAL_WIND_VAR;
 	_P(INDEX_TAS_SCALE, INDEX_TAS_SCALE) = 0.0001f;
 
 	_P = L * _P * L.transpose();

src/lib/wind_estimator/WindEstimator.hpp

@@ -97,6 +97,8 @@ private:
 		INDEX_TAS_SCALE
 	};	///< enum which can be used to access state.
 
+	static constexpr float INITIAL_WIND_VAR = 5.f; // initial variance for each wind state
+
 	matrix::Vector3f _state{0.f, 0.f, 1.f};
 	matrix::Matrix3f _P;		///< state covariance matrix