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: separate mag and mag heading control logic (#21212)

Browse Source 
- split mag_3d into new standalone mag fusion and mag fusion allowed to update all states (full mag_3d)
 - new dedicated control logic for mag/mag_3d fusion and standalone mag heading fusion
 - if WMM available use for mag_I and mag_B init
 - mag states reset if external yaw reset (yaw estimator, GPS yaw, etc)
 - mag reset if declination changed (eliminate _mag_yaw_reset_req)
 - mag fusion (but not mag_hdg or mag_3d) can be active during gps_yaw or ev_yaw (if yaw aligned north)

Co-authored-by: bresch <brescianimathieu@gmail.com>
This commit is contained in:
Daniel Agar
2023-08-04 10:39:16 -04:00
committed by GitHub
co-authored by bresch
parent 7db667558e
commit d75bb62a65
26 changed files with 1650 additions and 1397 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf_helper.cpp
+11 -22
View File
@@ -1086,33 +1086,24 @@ void Ekf::increaseQuatYawErrVariance(float yaw_variance)
P(3,2) -= yaw_variance*SQ[0]*SQ[3];
}
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
void Ekf::saveMagCovData()
{
// save variances for XYZ body axis field
_saved_mag_bf_variance(0) = P(19, 19);
_saved_mag_bf_variance(1) = P(20, 20);
_saved_mag_bf_variance(2) = P(21, 21);
// save the NED axis covariance sub-matrix
_saved_mag_ef_covmat = P.slice<3, 3>(16, 16);
// save the NE axis covariance sub-matrix
_saved_mag_ef_ne_covmat = P.slice<2, 2>(16, 16);
// save variance for the D earth axis
_saved_mag_ef_d_variance = P(18, 18);
// save the XYZ body covariance sub-matrix
_saved_mag_bf_covmat = P.slice<3, 3>(19, 19);
}
void Ekf::loadMagCovData()
{
// re-instate variances for the XYZ body axis field
P(19, 19) = _saved_mag_bf_variance(0);
P(20, 20) = _saved_mag_bf_variance(1);
P(21, 21) = _saved_mag_bf_variance(2);
// re-instate the NED axis covariance sub-matrix
P.uncorrelateCovarianceSetVariance<3>(16, 0.f);
P.slice<3, 3>(16, 16) = _saved_mag_ef_covmat;
// re-instate the NE axis covariance sub-matrix
P.slice<2, 2>(16, 16) = _saved_mag_ef_ne_covmat;
// re-instate the D earth axis variance
P(18, 18) = _saved_mag_ef_d_variance;
// re-instate the XYZ body axis covariance sub-matrix
P.uncorrelateCovarianceSetVariance<3>(19, 0.f);
P.slice<3, 3>(19, 19) = _saved_mag_bf_covmat;
}
void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
@@ -1133,7 +1124,7 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
uncorrelateQuatFromOtherStates();
// update the yaw angle variance
if (yaw_variance > FLT_EPSILON) {
if (PX4_ISFINITE(yaw_variance) && (yaw_variance > FLT_EPSILON)) {
increaseQuatYawErrVariance(yaw_variance);
}
@@ -1182,8 +1173,6 @@ bool Ekf::resetYawToEKFGSF()
resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());
// record a magnetic field alignment event to prevent possibility of the EKF trying to reset the yaw to the mag later in flight
_flt_mag_align_start_time = _time_delayed_us;
_control_status.flags.yaw_align = true;
_information_events.flags.yaw_aligned_to_imu_gps = true;