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

ekf2: utils add getEulerYaw() that uses the best rotation sequence

Browse Source
This commit is contained in:
Daniel Agar
2022-02-02 08:54:03 -05:00
parent c3e0b93fc8
commit 88ffc177f7
9 changed files with 24 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/covariance.cpp
+1 -1
View File
@@ -1137,7 +1137,7 @@ void Ekf::resetWindCovarianceUsingAirspeed()
{
// Derived using EKF/matlab/scripts/Inertial Nav EKF/wind_cov.py
// TODO: explicitly include the sideslip angle in the derivation
const float euler_yaw = getEuler321Yaw(_state.quat_nominal);
const float euler_yaw = getEulerYaw(_R_to_earth);
const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f));
constexpr float initial_sideslip_uncertainty = math::radians(15.0f);
const float initial_wind_var_body_y = sq(_airspeed_sample_delayed.true_airspeed * sinf(initial_sideslip_uncertainty));