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

ekf2: cleanup zero innovation heading fusion

Browse Source 
- refactor updateQuaternion() to compute the yaw jacobian directly (respecting  the rotation sequence determination)
 - fuseHeading()/fuseYaw321()/fuseYaw312() helpers are eliminated and now mag heading fusion and EV yaw fusion compute the innovation in place
 - clear up logic for performing zero innovation heading fusion when quaternion variance exceeds threshold (no more _is_yaw_fusion_inhibited flag manipulation)
 - when at rest continue fusing last static heading with very low variance even if other heading sources are active
This commit is contained in:
Daniel Agar
2022-07-19 11:58:27 -04:00
parent ed14151734
commit 39453405a0
11 changed files with 948 additions and 990 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/mag_control.cpp
+5 -16
View File
@@ -78,25 +78,11 @@ void Ekf::controlMagFusion()
_num_bad_flight_yaw_events = 0;
}
// When operating without a magnetometer and no other source of yaw aiding is active,
// yaw fusion is run selectively to enable yaw gyro bias learning when stationary on
// ground and to prevent uncontrolled yaw variance growth
// Also fuse zero heading innovation during the leveling fine alignment step to keep the yaw variance low
if (_params.mag_fusion_type >= MagFuseType::NONE
|| _control_status.flags.mag_fault
|| !_control_status.flags.tilt_align) {
stopMagFusion();
if (noOtherYawAidingThanMag()) {
// TODO: setting _is_yaw_fusion_inhibited to true is required to tell
// fuseHeading to perform a "zero innovation heading fusion"
// We should refactor it to avoid using this flag here
_is_yaw_fusion_inhibited = true;
fuseHeading();
_is_yaw_fusion_inhibited = false;
}
return;
}
@@ -347,16 +333,19 @@ void Ekf::runMagAndMagDeclFusions(const Vector3f &mag)
if (_control_status.flags.mag_3D) {
run3DMagAndDeclFusions(mag);
} else if (_control_status.flags.mag_hdg) {
} else if (_control_status.flags.mag_hdg && !_is_yaw_fusion_inhibited) {
// Rotate the measurements into earth frame using the zero yaw angle
Dcmf R_to_earth = shouldUse321RotationSequence(_R_to_earth) ? updateEuler321YawInRotMat(0.f, _R_to_earth) : updateEuler312YawInRotMat(0.f, _R_to_earth);
Vector3f mag_earth_pred = R_to_earth * (mag - _state.mag_B);
// the angle of the projection onto the horizontal gives the yaw angle
// calculate the yaw innovation and wrap to the interval between +-pi
float measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
fuseHeading(measured_hdg, sq(_params.mag_heading_noise));
float innovation = wrap_pi(getEulerYaw(_R_to_earth) - measured_hdg);
float obs_var = fmaxf(sq(_params.mag_heading_noise), 1.e-4f);
fuseYaw(innovation, obs_var);
}
}