fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 21:23:35 +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/sideslip_fusion.cpp
+3 -3
View File
@@ -141,9 +141,9 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
sym::ComputeSideslipHAndK(getStateAtFusionHorizonAsVector(), P, sideslip.innovation_variance, 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, sideslip.innovation_variance, sideslip.innovation);