diff --git a/src/lib/wind_estimator/WindEstimator.cpp b/src/lib/wind_estimator/WindEstimator.cpp index 5f6e04a676..f1ec03947d 100644 --- a/src/lib/wind_estimator/WindEstimator.cpp +++ b/src/lib/wind_estimator/WindEstimator.cpp @@ -42,25 +42,20 @@ bool WindEstimator::initialise(const matrix::Vector3f &velI, const float hor_vel_variance, const float heading_rad, const float tas_meas, const float tas_variance) { - // do no initialise if ground velocity is low - // this should prevent the filter from initialising on the ground - if (sqrtf(velI(0) * velI(0) + velI(1) * velI(1)) < 3.0f) { - return false; - } - if (PX4_ISFINITE(tas_meas) && PX4_ISFINITE(tas_variance)) { - // initialise wind states assuming zero side slip and horizontal flight - _state(INDEX_W_N) = velI(INDEX_W_N) - tas_meas * cosf(heading_rad); - _state(INDEX_W_E) = velI(INDEX_W_E) - tas_meas * sinf(heading_rad); - _state(INDEX_TAS_SCALE) = _scale_init; - - constexpr float initial_sideslip_uncertainty = math::radians(15.0f); - const float initial_wind_var_body_y = sq(tas_meas * sinf(initial_sideslip_uncertainty)); - constexpr float heading_variance = sq(math::radians(10.0f)); const float cos_heading = cosf(heading_rad); const float sin_heading = sinf(heading_rad); + // initialise wind states assuming zero side slip and horizontal flight + _state(INDEX_W_N) = velI(0) - tas_meas * cos_heading; + _state(INDEX_W_E) = velI(1) - tas_meas * sin_heading; + _state(INDEX_TAS_SCALE) = _scale_init; + + constexpr float initial_sideslip_uncertainty = math::radians(INITIAL_BETA_ERROR_DEG); + const float initial_wind_var_body_y = sq(tas_meas * sinf(initial_sideslip_uncertainty)); + constexpr float heading_variance = sq(math::radians(INITIAL_HEADING_ERROR_DEG)); + // rotate wind velocity into earth frame aligned with vehicle yaw const float Wx = _state(INDEX_W_N) * cos_heading + _state(INDEX_W_E) * sin_heading; const float Wy = -_state(INDEX_W_N) * sin_heading + _state(INDEX_W_E) * cos_heading; @@ -83,7 +78,7 @@ WindEstimator::initialise(const matrix::Vector3f &velI, const float hor_vel_vari _state.setZero(); _state(INDEX_TAS_SCALE) = 1.0f; _P.setZero(); - _P(INDEX_W_N, INDEX_W_N) = _P(INDEX_W_E, INDEX_W_E) = INITIAL_WIND_VAR; + _P(INDEX_W_N, INDEX_W_N) = _P(INDEX_W_E, INDEX_W_E) = sq(INITIAL_WIND_ERROR); } // reset the timestamp for measurement rejection diff --git a/src/lib/wind_estimator/WindEstimator.hpp b/src/lib/wind_estimator/WindEstimator.hpp index a73fdf728a..8dbb2251f7 100644 --- a/src/lib/wind_estimator/WindEstimator.hpp +++ b/src/lib/wind_estimator/WindEstimator.hpp @@ -98,7 +98,12 @@ 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 + static constexpr float INITIAL_WIND_ERROR = + 2.5f; // initial uncertainty of each wind state when filter is initialised without airspeed [m/s] + static constexpr float INITIAL_BETA_ERROR_DEG = + 15.0f; // sidelip uncertainty used to initialise the filter with a true airspeed measurement [deg] + static constexpr float INITIAL_HEADING_ERROR_DEG = + 10.0f; // heading uncertainty used to initialise the filter with a true airspeed measurement [deg] matrix::Vector3f _state{0.f, 0.f, 1.f}; matrix::Matrix3f _P; ///< state covariance matrix diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index 47e690778e..60d587c0a6 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -185,7 +185,7 @@ private: (ParamInt) _checks_clear_delay, /**< delay to declare airspeed valid again */ (ParamFloat) _param_fw_airspd_stall, - (ParamFloat) _param_wind_var_threshold + (ParamFloat) _param_wind_sigma_max_synth_tas ) void init(); /**< initialization of the airspeed validator instances */ @@ -517,7 +517,8 @@ void AirspeedModule::update_wind_estimator_sideslip() _wind_estimator_sideslip.update(_time_now_usec); if (_vehicle_local_position_valid - && _vtol_vehicle_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW) { + && _vtol_vehicle_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW && + _vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { Vector3f vI(_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz); Quatf q(_vehicle_attitude.q); @@ -543,10 +544,9 @@ void AirspeedModule::update_wind_estimator_sideslip() void AirspeedModule::update_ground_minus_wind_airspeed() { - const float wind_variance = sqrtf(_wind_estimate_sideslip.variance_north * _wind_estimate_sideslip.variance_north + - _wind_estimate_sideslip.variance_east * _wind_estimate_sideslip.variance_east); + const float wind_uncertainty = sqrtf(_wind_estimate_sideslip.variance_north + _wind_estimate_sideslip.variance_east); - if (wind_variance < _param_wind_var_threshold.get()) { + if (wind_uncertainty < _param_wind_sigma_max_synth_tas.get()) { // calculate airspeed estimate based on groundspeed-windspeed const float TAS_north = _vehicle_local_position.vx - _wind_estimate_sideslip.windspeed_north; const float TAS_east = _vehicle_local_position.vy - _wind_estimate_sideslip.windspeed_east; diff --git a/src/modules/airspeed_selector/airspeed_selector_params.c b/src/modules/airspeed_selector/airspeed_selector_params.c index 2a3394aa6f..e2f13b9722 100644 --- a/src/modules/airspeed_selector/airspeed_selector_params.c +++ b/src/modules/airspeed_selector/airspeed_selector_params.c @@ -229,15 +229,15 @@ PARAM_DEFINE_INT32(ASPD_FS_T_STOP, 2); PARAM_DEFINE_INT32(ASPD_FS_T_START, -1); /** - * Horizontal wind variance threshold for synthetic airspeed. + * Horizontal wind uncertainty threshold for synthetic airspeed. * * The synthetic airspeed estimate (from groundspeed and heading) will be declared valid - * as soon as the wind variance drops below this value. + * as soon and as long the horizontal wind uncertainty drops below this value. * - * @unit (m/s)^2 + * @unit m/s * @min 0.001 * @max 5 * @decimal 3 * @group Airspeed Validator */ -PARAM_DEFINE_FLOAT(ASPD_WVAR_THR, 0.3f); +PARAM_DEFINE_FLOAT(ASPD_WERR_THR, 0.55f);