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

ekf2: simplify fuseYaw() signature and use consistently

Browse Source 
- make it safe to call for other aid sources, no EKF state is changed unless fusion_enabled
This commit is contained in:
Daniel Agar
2023-07-24 10:34:00 -04:00
committed by GitHub
parent 01bcc47fb1
commit 1ef9ee7622
5 changed files with 83 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/zero_innovation_heading_update.cpp
+9 -4
View File
@@ -50,16 +50,21 @@ void Ekf::controlZeroInnovationHeadingUpdate()
// Use an observation variance larger than usual but small enough
// to constrain the yaw variance just below the threshold
float obs_var = 0.25f;
estimator_aid_source1d_s aid_src_status;
const float obs_var = 0.25f;
estimator_aid_source1d_s aid_src_status{};
aid_src_status.observation = getEulerYaw(_state.quat_nominal);
aid_src_status.observation_variance = obs_var;
aid_src_status.innovation = 0.f;
Vector24f H_YAW;
computeYawInnovVarAndH(obs_var, aid_src_status.innovation_variance, H_YAW);
if ((aid_src_status.innovation_variance - obs_var) > sq(_params.mag_heading_noise)) {
// The yaw variance is too large, fuse fake measurement
float innovation = 0.f;
fuseYaw(innovation, obs_var, aid_src_status, H_YAW);
aid_src_status.fusion_enabled = true;
fuseYaw(aid_src_status, H_YAW);
}
}
}