Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-27 02:06:27 +08:00
Code Issues Actions 4 Packages Projects Releases Wiki Activity

wind-estimator: learn airspeed scale faster at beginning of flight

Browse Source 
Multiply TAS scale process noise by 100 during the first 5 minutes
when ASPD_SCALE_n = 1.0 (default), enabling faster convergence on first flights.
This commit is contained in:
mahima-yoga
2025-12-05 14:58:43 +01:00
committed by Mahima Yoga
parent bcd67b7bad
commit 18477554e0
2 changed files with 10 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/wind_estimator/WindEstimator.cpp
+6 -1
View File
@@ -68,6 +68,7 @@ WindEstimator::initialise(const matrix::Vector3f &velI, const float hor_vel_vari
} }
_wind_estimator_reset = true; _wind_estimator_reset = true;
_time_initialised = hrt_absolute_time();
return true; return true;
} }
@@ -94,10 +95,14 @@ WindEstimator::update(uint64_t time_now)
const float dt = (float)(time_now - _time_last_update) * 1e-6f; const float dt = (float)(time_now - _time_last_update) * 1e-6f;
_time_last_update = time_now; _time_last_update = time_now;
// if airspeed scale is at default (1.0) and we are in the first 5 minutes of flight time, multiply _tas_scale_psd by 100 for faster learning
const float tas_psd_multiplier = (fabsf(_scale_init - 1.0f) < FLT_EPSILON && (time_now - _time_initialised < kTASScaleFastLearnTime)) ?
kTASScalePSDMultiplier : 1.f;
matrix::Matrix3f Qk; matrix::Matrix3f Qk;
Qk(INDEX_W_N, INDEX_W_N) = _wind_psd * dt; Qk(INDEX_W_N, INDEX_W_N) = _wind_psd * dt;
Qk(INDEX_W_E, INDEX_W_E) = Qk(INDEX_W_N, INDEX_W_N); Qk(INDEX_W_E, INDEX_W_E) = Qk(INDEX_W_N, INDEX_W_N);
Qk(INDEX_TAS_SCALE, INDEX_TAS_SCALE) = _tas_scale_psd * dt; Qk(INDEX_TAS_SCALE, INDEX_TAS_SCALE) = _tas_scale_psd * tas_psd_multiplier * dt;
_P += Qk; _P += Qk;
} }
src/lib/wind_estimator/WindEstimator.hpp
+4
View File
@@ -139,6 +139,10 @@ private:
uint64_t _time_last_airspeed_fuse = 0; ///< timestamp of last airspeed fusion 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_beta_fuse = 0; ///< timestamp of last sideslip fusion
uint64_t _time_last_update = 0; ///< timestamp of last covariance prediction uint64_t _time_last_update = 0; ///< timestamp of last covariance prediction
uint64_t _time_initialised = 0; ///< timestamp when estimator is initialised
static constexpr float kTASScalePSDMultiplier = 100;
static constexpr hrt_abstime kTASScaleFastLearnTime = 300_s;
bool _wind_estimator_reset = false; ///< wind estimator was reset in this cycle bool _wind_estimator_reset = false; ///< wind estimator was reset in this cycle