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

EKF2: only fuse zero innovation if yaw var is large

Browse Source
This commit is contained in:
bresch
2022-12-29 12:56:36 -05:00
committed by Daniel Agar
parent 40e3503c39
commit c75a9058a5
4 changed files with 31 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/zero_innovation_heading_update.cpp
+12 -5
View File
@@ -74,12 +74,19 @@ void Ekf::controlZeroInnovationHeadingUpdate()
} else {
// vehicle moving and tilt alignment completed
// fuse zero innovation at a limited rate (every 200 milliseconds)
// fuse zero innovation at a limited rate if the yaw variance is too large
if (!yaw_aiding && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) {
float innovation = 0.f;
float obs_var = 0.01f;
estimator_aid_source1d_s unused;
fuseYaw(innovation, obs_var, unused);
float obs_var = 0.25f;
estimator_aid_source1d_s aid_src_status;
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);
}
}
_last_static_yaw = NAN;