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

ekf2: update more hardcoded indexes

Browse Source
This commit is contained in:
bresch
2023-09-21 08:27:52 -04:00
committed by Daniel Agar
parent ebf962bf68
commit 9e962f3efa
4 changed files with 32 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/airspeed_fusion.cpp
+4 -4
View File
@@ -200,9 +200,9 @@ void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_sour
sym::ComputeAirspeedHAndK(getStateAtFusionHorizonAsVector(), P, innov_var, FLT_EPSILON, &H, &K);
if (update_wind_only) {
for (unsigned row = 0; row <= 21; row++) {
K(row) = 0.f;
}
const Vector2f K_wind = K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0);
K.setZero();
K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) = K_wind;
}
const bool is_fused = measurementUpdate(K, aid_src.innovation_variance, aid_src.innovation);
@@ -258,7 +258,7 @@ void Ekf::resetWindCovarianceUsingAirspeed(const airspeedSample &airspeed_sample
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.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, 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) -