diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 00740cf1d5..9dae933cee 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -176,13 +176,6 @@ bool Ekf::initialiseFilter() } } - if (_params.mag_fusion_type <= MagFuseType::MAG_3D) { - if (_mag_counter < _obs_buffer_length) { - // not enough mag samples accumulated - return false; - } - } - if (_baro_counter < _obs_buffer_length) { // not enough baro samples accumulated return false; @@ -198,18 +191,25 @@ 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 - if ((_params.mag_fusion_type <= MagFuseType::MAG_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(); + if (_params.mag_fusion_type <= MagFuseType::MAG_3D) { + if (_mag_counter > 1) { + // 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); + // update the rotation matrix using the new yaw value + _R_to_earth = updateYawInRotMat(yaw_new, Dcmf(_state.quat_nominal)); + _state.quat_nominal = _R_to_earth; - // set the earth magnetic field states using the updated rotation - _state.mag_I = _R_to_earth * _mag_lpf.getState(); - _state.mag_B.zero(); + // set the earth magnetic field states using the updated rotation + _state.mag_I = _R_to_earth * _mag_lpf.getState(); + _state.mag_B.zero(); + + } else { + // not enough mag samples accumulated + return false; + } } // initialise the state covariance matrix now we have starting values for all the states diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 05bf64a153..2b613233fa 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -1071,8 +1071,7 @@ private: // reset the quaternion states and covariances to the new yaw value, preserving the roll and pitch // yaw : Euler yaw angle (rad) // yaw_variance : yaw error variance (rad^2) - // update_buffer : true if the state change should be also applied to the output observer buffer - void resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer = true); + void resetQuatStateYaw(float yaw, float yaw_variance); // Declarations used to control use of the EKF-GSF yaw estimator diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 588e25f9f3..986fb23436 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -370,7 +370,7 @@ bool Ekf::realignYawGPS(const Vector3f &mag) const float yaw_variance_new = sq(asinf(sineYawError)); // Apply updated yaw and yaw variance to states and covariances - resetQuatStateYaw(yaw_new, yaw_variance_new, true); + resetQuatStateYaw(yaw_new, yaw_variance_new); // Use the last magnetometer measurements to reset the field states _state.mag_B.zero(); @@ -458,7 +458,7 @@ bool Ekf::resetYawToEv() const float yaw_new = getEulerYaw(_ev_sample_delayed.quat); const float yaw_new_variance = fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f)); - resetQuatStateYaw(yaw_new, yaw_new_variance, true); + resetQuatStateYaw(yaw_new, yaw_new_variance); _R_ev_to_ekf.setIdentity(); return true; @@ -1567,7 +1567,7 @@ void Ekf::stopFlowFusion() } } -void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer) +void Ekf::resetQuatStateYaw(float yaw, float yaw_variance) { // save a copy of the quaternion state for later use in calculating the amount of reset change const Quatf quat_before_reset = _state.quat_nominal; @@ -1593,16 +1593,14 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer) } // add the reset amount to the output observer buffered data - if (update_buffer) { - for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { - _output_buffer[i].quat_nominal = _state_reset_status.quat_change * _output_buffer[i].quat_nominal; - } - - // 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; + for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { + _output_buffer[i].quat_nominal = _state_reset_status.quat_change * _output_buffer[i].quat_nominal; } + // 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; + _last_static_yaw = NAN; // capture the reset event @@ -1618,7 +1616,7 @@ bool Ekf::resetYawToEKFGSF() return false; } - resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar(), true); + 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 = _imu_sample_delayed.time_us; diff --git a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp index d0ff437560..1ae5608b55 100644 --- a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp @@ -216,7 +216,7 @@ bool Ekf::resetYawToGps() const float measured_yaw = _gps_sample_delayed.yaw; const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.e-2f)); - resetQuatStateYaw(measured_yaw, yaw_variance, true); + resetQuatStateYaw(measured_yaw, yaw_variance); _aid_src_gnss_yaw.time_last_fuse = _imu_sample_delayed.time_us; _gnss_yaw_signed_test_ratio_lpf.reset(0.f);