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

ekf2: use estimator_aid_src for all yaw sources (mag, gnss, ev)

Browse Source
This commit is contained in:
Daniel Agar
2022-07-29 11:20:48 -04:00
parent d5d88cba5b
commit a397c09e59
16 changed files with 334 additions and 155 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/zero_innovation_heading_update.cpp
+6 -3
View File
@@ -47,7 +47,8 @@ void Ekf::controlZeroInnovationHeadingUpdate()
// fuse zero heading innovation during the leveling fine alignment step to keep the yaw variance low
float innovation = 0.f;
float obs_var = _control_status.flags.vehicle_at_rest ? 0.001f : 0.1f;
fuseYaw(innovation, obs_var);
estimator_aid_source_1d_s unused;
fuseYaw(innovation, obs_var, unused);
_last_static_yaw = NAN;
@@ -61,7 +62,8 @@ void Ekf::controlZeroInnovationHeadingUpdate()
if (!yaw_aiding && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) {
float innovation = wrap_pi(euler_yaw - _last_static_yaw);
float obs_var = 0.01f;
fuseYaw(innovation, obs_var);
estimator_aid_source_1d_s unused;
fuseYaw(innovation, obs_var, unused);
}
} else {
@@ -76,7 +78,8 @@ void Ekf::controlZeroInnovationHeadingUpdate()
if (!yaw_aiding && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) {
float innovation = 0.f;
float obs_var = 0.01f;
fuseYaw(innovation, obs_var);
estimator_aid_source_1d_s unused;
fuseYaw(innovation, obs_var, unused);
}
_last_static_yaw = NAN;