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

WindEstimator: reworked filter initialisation

Browse Source 
- separate initialisation with and without airspeed

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2022-08-02 15:29:25 +02:00
committed by Silvan Fuhrer
parent a63f1b71fe
commit fbd4534edc
4 changed files with 54 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/airspeed_selector/AirspeedValidator.cpp
+3 -3
View File
@@ -81,11 +81,11 @@ AirspeedValidator::update_wind_estimator(const uint64_t time_now_usec, float air
Quatf q(att_q);
// airspeed fusion (with raw TAS)
const Vector3f vel_var{Dcmf(q) *Vector3f{lpos_evh, lpos_evh, lpos_evv}};
_wind_estimator.fuse_airspeed(time_now_usec, airspeed_true_raw, vI, Vector2f{vel_var(0), vel_var(1)}, q);
const float hor_vel_variance = lpos_evh * lpos_evh;
_wind_estimator.fuse_airspeed(time_now_usec, airspeed_true_raw, vI, hor_vel_variance, q);
// sideslip fusion
_wind_estimator.fuse_beta(time_now_usec, vI, q);
_wind_estimator.fuse_beta(time_now_usec, vI, hor_vel_variance, q);
}
}
src/modules/airspeed_selector/airspeed_selector_main.cpp
+3 -1
View File
@@ -521,7 +521,9 @@ void AirspeedModule::update_wind_estimator_sideslip()
Vector3f vI(_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz);
Quatf q(_vehicle_attitude.q);
_wind_estimator_sideslip.fuse_beta(_time_now_usec, vI, q);
const float hor_vel_variance = _vehicle_local_position.evh * _vehicle_local_position.evh;
_wind_estimator_sideslip.fuse_beta(_time_now_usec, vI, hor_vel_variance, q);
}
_wind_estimate_sideslip.timestamp = _time_now_usec;