fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-11 05:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Wind Estimator: remove filter reset due to beta fusion timeout

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2022-10-24 09:56:18 +02:00
parent ed558e199f
commit 128e49358e
2 changed files with 15 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/wind_estimator/WindEstimator.cpp
+14 -40
View File
@@ -67,10 +67,6 @@ WindEstimator::initialise(const matrix::Vector3f &velI, const float hor_vel_vari
_P(INDEX_W_N, INDEX_W_N) = _P(INDEX_W_E, INDEX_W_E) = sq(INITIAL_WIND_ERROR);
}
// reset the timestamp for measurement rejection
_time_rejected_tas = 0;
_time_rejected_beta = 0;
_wind_estimator_reset = true;
return true;
@@ -129,20 +125,14 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const
sym::FuseAirspeed(velI, _state, _P, true_airspeed, _tas_var, FLT_EPSILON,
&H_tas, &K, &_tas_innov_var, &_tas_innov);
bool reinit_filter = false;
bool meas_is_rejected = false;
const bool meas_is_rejected = check_if_meas_is_rejected(_tas_innov, _tas_innov_var, _tas_gate);
// note: _time_rejected_tas and reinit_filter are not used anymore as filter can't get reset due to tas rejection
meas_is_rejected = check_if_meas_is_rejected(time_now, _tas_innov, _tas_innov_var, _tas_gate, _time_rejected_tas,
reinit_filter);
if (_tas_innov_var < 0.0f) {
// re init filter in case of a negative variance, and trigger early return to not fuse measurement
_initialised = initialise(velI, hor_vel_variance, matrix::Eulerf(q_att).psi(), true_airspeed, _tas_var);
return;
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, hor_vel_variance, matrix::Eulerf(q_att).psi(), true_airspeed, _tas_var);
}
// we either did a filter reset or the current measurement was rejected so do not fuse
} else if (meas_is_rejected) {
return;
}
@@ -180,20 +170,14 @@ WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const
sym::FuseBeta(velI, _state, _P, q_att, _beta_var, FLT_EPSILON,
&H_beta, &K, &_beta_innov_var, &_beta_innov);
bool reinit_filter = false;
bool meas_is_rejected = false;
const bool meas_is_rejected = check_if_meas_is_rejected(_beta_innov, _beta_innov_var, _beta_gate);
meas_is_rejected = check_if_meas_is_rejected(time_now, _beta_innov, _beta_innov_var, _beta_gate, _time_rejected_beta,
reinit_filter);
if (_beta_innov_var < 0.0f) {
// re init filter in case of a negative variance, and trigger early return to not fuse measurement
_initialised = initialise(velI, hor_vel_variance, matrix::Eulerf(q_att).psi());
return;
reinit_filter |= _beta_innov_var < 0.0f;
if (meas_is_rejected || reinit_filter) {
if (reinit_filter) {
_initialised = initialise(velI, hor_vel_variance, matrix::Eulerf(q_att).psi());
}
// we either did a filter reset or the current measurement was rejected so do not fuse
} else if (meas_is_rejected) {
return;
}
@@ -243,17 +227,7 @@ WindEstimator::run_sanity_checks()
}
bool
WindEstimator::check_if_meas_is_rejected(uint64_t time_now, float innov, float innov_var, uint8_t gate_size,
uint64_t &time_meas_rejected, bool &reinit_filter)
WindEstimator::check_if_meas_is_rejected(float innov, float innov_var, uint8_t gate_size)
{
if (innov * innov > gate_size * gate_size * innov_var) {
time_meas_rejected = time_meas_rejected == 0 ? time_now : time_meas_rejected;
} else {
time_meas_rejected = 0;
}
reinit_filter = time_now - time_meas_rejected > 5_s && time_meas_rejected != 0;
return time_meas_rejected != 0;
return (innov * innov > gate_size * gate_size * innov_var);
}
src/lib/wind_estimator/WindEstimator.hpp
+1 -5
View File
@@ -70,8 +70,7 @@ public:
bool is_estimate_valid() { return _initialised; }
bool check_if_meas_is_rejected(uint64_t time_now, float innov, float innov_var, uint8_t gate_size,
uint64_t &time_meas_rejected, bool &reinit_filter);
bool check_if_meas_is_rejected(float innov, float innov_var, uint8_t gate_size);
matrix::Vector2f get_wind() { return matrix::Vector2f{_state(INDEX_W_N), _state(INDEX_W_E)}; }
@@ -132,9 +131,6 @@ private:
uint64_t _time_last_airspeed_fuse = 0; ///< timestamp of last airspeed fusion
uint64_t _time_last_beta_fuse = 0; ///< timestamp of last sideslip fusion
uint64_t _time_last_update = 0; ///< timestamp of last covariance prediction
uint64_t _time_rejected_beta = 0; ///< timestamp of when sideslip measurements have consistently started to be rejected
uint64_t _time_rejected_tas =
0; ///< timestamp of when true airspeed measurements have consistently started to be rejected
bool _wind_estimator_reset = false; ///< wind estimator was reset in this cycle