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

ekf2: consolidate airspeed fusion logic and helpers

Browse Source 
- pass new airspeed sample around when available
 - can't completely eliminate _airspeed_sample_delayed until resetWind()
called from sideslip fusion is updated
This commit is contained in:
Daniel Agar
2023-02-22 09:08:33 -05:00
parent 241cee2bb7
commit 08f111f694
6 changed files with 91 additions and 98 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/covariance.cpp
-31
View File
@@ -605,34 +605,3 @@ void Ekf::resetZDeltaAngBiasCov()
P.uncorrelateCovarianceSetVariance<1>(12, init_delta_ang_bias_var);
}
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 = 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));
constexpr float R_yaw = sq(math::radians(10.0f));
const float cos_yaw = cosf(euler_yaw);
const float sin_yaw = sinf(euler_yaw);
// rotate wind velocity into earth frame aligned with vehicle yaw
const float Wx = _state.wind_vel(0) * cos_yaw + _state.wind_vel(1) * sin_yaw;
const float Wy = -_state.wind_vel(0) * sin_yaw + _state.wind_vel(1) * cos_yaw;
// it is safer to remove all existing correlations to other states at this time
P.uncorrelateCovarianceSetVariance<2>(22, 0.0f);
P(22, 22) = R_TAS * sq(cos_yaw) + R_yaw * sq(-Wx * sin_yaw - Wy * cos_yaw) + initial_wind_var_body_y * sq(sin_yaw);
P(22, 23) = R_TAS * sin_yaw * cos_yaw + R_yaw * (-Wx * sin_yaw - Wy * cos_yaw) * (Wx * cos_yaw - Wy * sin_yaw) -
initial_wind_var_body_y * sin_yaw * cos_yaw;
P(23, 22) = P(22, 23);
P(23, 23) = R_TAS * sq(sin_yaw) + R_yaw * sq(Wx * cos_yaw - Wy * sin_yaw) + initial_wind_var_body_y * sq(cos_yaw);
// Now add the variance due to uncertainty in vehicle velocity that was used to calculate the initial wind speed
P(22, 22) += P(4, 4);
P(23, 23) += P(5, 5);
}