diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 11b98f4173..fd73d0c52c 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -762,7 +762,7 @@ private: #if defined(CONFIG_EKF2_MAGNETOMETER) // ekf sequential fusion of magnetometer measurements - bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true); + bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true, bool mag_heading = false); // fuse magnetometer declination measurement // argument passed in is the declination uncertainty in radians diff --git a/src/modules/ekf2/EKF/mag_3d_control.cpp b/src/modules/ekf2/EKF/mag_3d_control.cpp index 89f502ac93..18c6685890 100644 --- a/src/modules/ekf2/EKF/mag_3d_control.cpp +++ b/src/modules/ekf2/EKF/mag_3d_control.cpp @@ -62,7 +62,11 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star _control_status.flags.mag_3D = (_params.mag_fusion_type == MagFuseType::AUTO) && _control_status.flags.mag && _control_status.flags.mag_aligned_in_flight - && (_control_status.flags.mag_heading_consistent || !_control_status.flags.gps) + && !_control_status.flags.mag_fault + && !_control_status.flags.ev_yaw + && !_control_status.flags.gps_yaw; + _control_status.flags.mag_hdg = ((_params.mag_fusion_type == MagFuseType::HEADING) || (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D)) + && _control_status.flags.mag && !_control_status.flags.mag_fault && !_control_status.flags.ev_yaw && !_control_status.flags.gps_yaw; @@ -102,14 +106,14 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star // states for the first few observations. fuseDeclination(0.02f); _mag_decl_cov_reset = true; - fuseMag(mag_sample.mag, aid_src, false); + fuseMag(mag_sample.mag, aid_src, false, false); } else { // The normal sequence is to fuse the magnetometer data first before fusing // declination angle at a higher uncertainty to allow some learning of // declination angle over time. - const bool update_all_states = _control_status.flags.mag_3D; - fuseMag(mag_sample.mag, aid_src, update_all_states); + const bool update_all_states = _control_status.flags.mag_3D || _control_status.flags.mag_hdg; + fuseMag(mag_sample.mag, aid_src, update_all_states, _control_status.flags.mag_hdg); if (_control_status.flags.mag_dec) { fuseDeclination(0.5f); @@ -176,7 +180,7 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star } else { ECL_INFO("starting %s fusion", AID_SRC_NAME); - fuseMag(mag_sample.mag, aid_src, false); + fuseMag(mag_sample.mag, aid_src, false, false); } aid_src.time_last_fuse = _time_delayed_us; diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index b491228c39..9c937771d8 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -47,9 +47,6 @@ void Ekf::controlMagFusion() _control_status.flags.mag_aligned_in_flight = false; } - checkYawAngleObservability(); - checkMagHeadingConsistency(); - if (_params.mag_fusion_type == MagFuseType::NONE) { stopMagFusion(); stopMagHdgFusion(); @@ -106,7 +103,7 @@ void Ekf::controlMagFusion() } controlMag3DFusion(mag_sample, starting_conditions_passing, _aid_src_mag); - controlMagHeadingFusion(mag_sample, starting_conditions_passing, _aid_src_mag_heading); + // controlMagHeadingFusion(mag_sample, starting_conditions_passing, _aid_src_mag_heading); } else if (!isNewestSampleRecent(_time_last_mag_buffer_push, 2 * MAG_MAX_INTERVAL)) { // No data anymore. Stop until it comes back. @@ -222,36 +219,6 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading) } } -void Ekf::checkYawAngleObservability() -{ - if (_control_status.flags.gps) { - // Check if there has been enough change in horizontal velocity to make yaw observable - // Apply hysteresis to check to avoid rapid toggling - if (_yaw_angle_observable) { - _yaw_angle_observable = _accel_lpf_NE.norm() > _params.mag_acc_gate; - - } else { - _yaw_angle_observable = _accel_lpf_NE.norm() > _params.mag_acc_gate * 2.f; - } - - } else { - _yaw_angle_observable = false; - } -} - -void Ekf::checkMagHeadingConsistency() -{ - if (fabsf(_mag_heading_innov_lpf.getState()) < _params.mag_heading_noise) { - if (_yaw_angle_observable) { - // yaw angle must be observable to consider consistency - _control_status.flags.mag_heading_consistent = true; - } - - } else { - _control_status.flags.mag_heading_consistent = false; - } -} - bool Ekf::checkMagField(const Vector3f &mag_sample) { _control_status.flags.mag_field_disturbed = false; diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index b1a8cc68fe..d269a68f95 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -51,7 +51,7 @@ #include -bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states) +bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states, bool mag_heading) { // XYZ Measurement uncertainty. Need to consider timing errors for fast rotations const float R_MAG = math::max(sq(_params.mag_noise), sq(0.01f)); @@ -178,6 +178,17 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo VectorState Kfusion = P * H / aid_src_mag.innovation_variance[index]; + if (mag_heading) { + // Modify the Kalman gain to zero the delta quaternion changes perpendicular to the down NED axis + // To do this, transform K from body to NED, zero the xy components and transform back to body frame. + // Transformation from body to NED of a delta quaternion in the tangent space is done using the adjoint + // at the current attitude (i.e.: the current rotation matrix) + Vector3f K_rot = Kfusion.slice<3, 1>(State::quat_nominal.idx, State::quat_nominal.idx); + Vector3f row = _R_to_earth.row(2); + K_rot = row * row.dot(K_rot); + Kfusion.slice<3, 1>(State::quat_nominal.idx, State::quat_nominal.idx) = K_rot; + } + if (!update_all_states) { // zero non-mag Kalman gains if not updating all states