fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/lib/wind_estimator/WindEstimator.cpp
View File

@ -68,6 +68,7 @@ WindEstimator::initialise(const matrix::Vector3f &velI, const float hor_vel_vari
}
_wind_estimator_reset = true;
_time_initialised = hrt_absolute_time();
return true;
}
@ -94,10 +95,14 @@ WindEstimator::update(uint64_t time_now)
const float dt = (float)(time_now - _time_last_update) * 1e-6f;
_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;
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_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;
}

4
src/lib/wind_estimator/WindEstimator.hpp
View File

@ -139,6 +139,10 @@ 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_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