From a8824b4e606a825d7d9a5e0d2091169c5afac3b2 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 21 Mar 2022 15:51:32 -0400 Subject: [PATCH] ekf2: resetMagHeading() split out simple init case --- src/modules/ekf2/EKF/ekf.cpp | 14 +++++- src/modules/ekf2/EKF/ekf.h | 2 +- src/modules/ekf2/EKF/ekf_helper.cpp | 73 ++++++++++++++--------------- 3 files changed, 50 insertions(+), 39 deletions(-) diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index a623f1622f..17d67084ae 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -196,7 +196,19 @@ bool Ekf::initialiseFilter() // calculate the initial magnetic field and yaw alignment // but do not mark the yaw alignement complete as it needs to be // reset once the leveling phase is done - resetMagHeading(false, false); + if ((_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) && (_mag_counter != 0)) { + // rotate the magnetometer measurements into earth frame using a zero yaw angle + // the angle of the projection onto the horizontal gives the yaw angle + const Vector3f mag_earth_pred = updateYawInRotMat(0.f, _R_to_earth) * _mag_lpf.getState(); + float yaw_new = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination(); + + // update quaternion states and corresponding covarainces + resetQuatStateYaw(yaw_new, 0.f, false); + + // set the earth magnetic field states using the updated rotation + _state.mag_I = _R_to_earth * _mag_lpf.getState(); + _state.mag_B.zero(); + } // initialise the state covariance matrix now we have starting values for all the states initialiseCovariance(); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 578e867ac1..a8ba2a4ce3 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -713,7 +713,7 @@ private: // reset the heading and magnetic field states using the declination and magnetometer measurements // return true if successful - bool resetMagHeading(bool increase_yaw_var = true, bool update_buffer = true); + bool resetMagHeading(); // reset the heading using the external vision measurements // return true if successful diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 8830804d5f..f2a5596ef8 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -447,60 +447,52 @@ bool Ekf::realignYawGPS(const Vector3f &mag) } // Reset heading and magnetic field states -bool Ekf::resetMagHeading(bool increase_yaw_var, bool update_buffer) +bool Ekf::resetMagHeading() { // prevent a reset being performed more than once on the same frame if (_imu_sample_delayed.time_us == _flt_mag_align_start_time) { return true; } + const bool mag_available = (_mag_counter != 0) && isRecent(_time_last_mag, 500000) + && !magFieldStrengthDisturbed(_mag_lpf.getState()); + // low pass filtered mag required - if (_mag_counter == 0) { + if (!mag_available) { return false; } - const Vector3f mag_init = _mag_lpf.getState(); - - // calculate the observed yaw angle and yaw variance - float yaw_new; - float yaw_new_variance = 0.0f; - - const bool heading_required_for_navigation = _control_status.flags.gps || _control_status.flags.ev_pos; + const bool heading_required_for_navigation = _control_status.flags.gps; if ((_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) || ((_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR) && heading_required_for_navigation)) { + const Vector3f mag_init = _mag_lpf.getState(); + // rotate the magnetometer measurements into earth frame using a zero yaw angle const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth); // the angle of the projection onto the horizontal gives the yaw angle const Vector3f mag_earth_pred = R_to_earth * mag_init; - yaw_new = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination(); - if (increase_yaw_var) { - yaw_new_variance = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)); - } + // calculate the observed yaw angle and yaw variance + float yaw_new = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination(); + float yaw_new_variance = sq(fmaxf(_params.mag_heading_noise, 1.e-2f)); + + // update quaternion states and corresponding covarainces + resetQuatStateYaw(yaw_new, yaw_new_variance); + + // set the earth magnetic field states using the updated rotation + _state.mag_I = _R_to_earth * mag_init; + + resetMagCov(); + + // record the time for the magnetic field alignment event + _flt_mag_align_start_time = _imu_sample_delayed.time_us; - } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR) { - // we are operating temporarily without knowing the earth frame yaw angle return true; - - } else { - // there is no magnetic yaw observation - return false; } - // update quaternion states and corresponding covarainces - resetQuatStateYaw(yaw_new, yaw_new_variance, update_buffer); - - // set the earth magnetic field states using the updated rotation - _state.mag_I = _R_to_earth * mag_init; - - resetMagCov(); - - // record the time for the magnetic field alignment event - _flt_mag_align_start_time = _imu_sample_delayed.time_us; - - return true; + return false; } bool Ekf::resetYawToEv() @@ -1229,8 +1221,11 @@ void Ekf::stopMagHdgFusion() void Ekf::startMagHdgFusion() { - stopMag3DFusion(); - _control_status.flags.mag_hdg = true; + if (!_control_status.flags.mag_hdg) { + stopMag3DFusion(); + ECL_INFO("starting mag heading fusion"); + _control_status.flags.mag_hdg = true; + } } void Ekf::startMag3DFusion() @@ -1590,12 +1585,14 @@ void Ekf::startGpsYawFusion() stopMag3DFusion(); _control_status.flags.gps_yaw = true; } - } void Ekf::stopGpsYawFusion() { - _control_status.flags.gps_yaw = false; + if (_control_status.flags.gps_yaw) { + ECL_INFO("stopping GPS yaw fusion"); + _control_status.flags.gps_yaw = false; + } } void Ekf::startEvPosFusion() @@ -1652,7 +1649,10 @@ void Ekf::stopEvVelFusion() void Ekf::stopEvYawFusion() { - _control_status.flags.ev_yaw = false; + if (_control_status.flags.ev_yaw) { + ECL_INFO("stopping EV yaw fusion"); + _control_status.flags.ev_yaw = false; + } } void Ekf::stopAuxVelFusion() @@ -1707,7 +1707,6 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer) // apply the change in attitude quaternion to our newest quaternion estimate // which was already taken out from the output buffer _output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal; - } // capture the reset event