diff --git a/EKF/EKFGSF_yaw.cpp b/EKF/EKFGSF_yaw.cpp index 19e87a131a..cf54820c92 100644 --- a/EKF/EKFGSF_yaw.cpp +++ b/EKF/EKFGSF_yaw.cpp @@ -224,17 +224,12 @@ void EKFGSF_yaw::ahrsAlignYaw() { // Align yaw angle for each model for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) { - if (shouldUse321RotationSequence(_ahrs_ekf_gsf[model_index].R)) { - // update the rotation matrix with 321 rotation sequence - _ahrs_ekf_gsf[model_index].R = updateEuler321YawInRotMat(wrap_pi(_ekf_gsf[model_index].X(2)), - _ahrs_ekf_gsf[model_index].R); + Dcmf& R = _ahrs_ekf_gsf[model_index].R; + const float yaw = wrap_pi(_ekf_gsf[model_index].X(2)); + R = shouldUse321RotationSequence(R) ? + updateEuler321YawInRotMat(yaw, R) : + updateEuler312YawInRotMat(yaw, R); - } else { - // update the rotation matrix with 312 rotation sequence - _ahrs_ekf_gsf[model_index].R = updateEuler312YawInRotMat(wrap_pi(_ekf_gsf[model_index].X(2)), - _ahrs_ekf_gsf[model_index].R); - - } _ahrs_ekf_gsf[model_index].aligned = true; } } @@ -250,11 +245,10 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index) } // Calculate the yaw state using a projection onto the horizontal that avoids gimbal lock - if (shouldUse321RotationSequence(_ahrs_ekf_gsf[model_index].R)) { - _ekf_gsf[model_index].X(2) = getEuler321Yaw(_ahrs_ekf_gsf[model_index].R); - } else { - _ekf_gsf[model_index].X(2) = getEuler312Yaw(_ahrs_ekf_gsf[model_index].R); - } + const Dcmf& R = _ahrs_ekf_gsf[model_index].R; + _ekf_gsf[model_index].X(2) = shouldUse321RotationSequence(R) ? + getEuler321Yaw(R) : + getEuler312Yaw(R); // calculate delta velocity in a horizontal front-right frame const Vector3f del_vel_NED = _ahrs_ekf_gsf[model_index].R * _delta_vel; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 4c18cec9e0..d35b6b658d 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -481,16 +481,9 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) { // rotate the magnetometer measurements into earth frame using a zero yaw angle - Dcmf R_to_earth; - if (shouldUse321RotationSequence(_R_to_earth)) { - // rolled more than pitched so use 321 rotation order - R_to_earth = updateEuler321YawInRotMat(0.f, _R_to_earth); - - } else { - // pitched more than rolled so use 312 rotation order - R_to_earth = updateEuler312YawInRotMat(0.f, _R_to_earth); - - } + const Dcmf R_to_earth = shouldUse321RotationSequence(_R_to_earth) ? + updateEuler321YawInRotMat(0.f, _R_to_earth) : + updateEuler312YawInRotMat(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; @@ -1647,16 +1640,9 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer) _R_to_earth = Dcmf(_state.quat_nominal); // update the rotation matrix using the new yaw value - // determine if a 321 or 312 Euler sequence is best - if (shouldUse321RotationSequence(_R_to_earth)) { - _R_to_earth = updateEuler321YawInRotMat(yaw, _R_to_earth); - - } else { - // We use a 312 sequence as an alternate when there is more pitch tilt than roll tilt - // to avoid gimbal lock - _R_to_earth = updateEuler312YawInRotMat(yaw, _R_to_earth); - - } + _R_to_earth = shouldUse321RotationSequence(_R_to_earth) ? + updateEuler321YawInRotMat(yaw, _R_to_earth) : + updateEuler312YawInRotMat(yaw, _R_to_earth); // calculate the amount that the quaternion has changed by const Quatf quat_after_reset(_R_to_earth); diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index ae2a446a4a..3c93fb8c19 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -721,9 +721,7 @@ void Ekf::fuseHeading() // update transformation matrix from body to world frame using the current state estimate _R_to_earth = Dcmf(_state.quat_nominal); - // determine if a 321 or 312 Euler sequence is best if (shouldUse321RotationSequence(_R_to_earth)) { - // rolled more than pitched so use 321 rotation order to calculate the observed yaw angle const float predicted_hdg = getEuler321Yaw(_R_to_earth); if (_control_status.flags.mag_hdg) { @@ -741,7 +739,6 @@ void Ekf::fuseHeading() measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination(); } else if (_control_status.flags.ev_yaw) { - // calculate the yaw angle for a 321 sequence measured_hdg = getEuler321Yaw(_ev_sample_delayed.quat); } else { @@ -781,16 +778,13 @@ void Ekf::fuseHeading() fuseYaw321(measured_hdg, R_YAW, fuse_zero_innov); } else { - // pitched more than rolled so use 312 rotation order to calculate the observed yaw angle const float predicted_hdg = getEuler312Yaw(_R_to_earth); if (_control_status.flags.mag_hdg) { - // Calculate the body to earth frame rotation matrix from the euler angles - // using a 312 rotation sequence with yaw angle set to to zero + // rotate the magnetometer measurements into earth frame using a zero yaw angle const Dcmf R_to_earth = updateEuler312YawInRotMat(0.f, _R_to_earth); - // rotate the magnetometer measurements into earth frame using a zero yaw angle if (_control_status.flags.mag_3D) { // don't apply bias corrections if we are learning them mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; diff --git a/EKF/utils.hpp b/EKF/utils.hpp index 71281e092e..a718a6df6e 100644 --- a/EKF/utils.hpp +++ b/EKF/utils.hpp @@ -23,6 +23,9 @@ float kahanSummation(float sum_previous, float input, float &accumulator); // this produces the inverse rotation to that produced by the math library quaternion to Dcmf operator matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat); +// We should use a 3-2-1 Tait-Bryan (yaw-pitch-roll) rotation sequence +// when there is more roll than pitch tilt and a 3-1-2 rotation sequence +// when there is more pitch than roll tilt to avoid gimbal lock. bool shouldUse321RotationSequence(const matrix::Dcmf& R); float getEuler321Yaw(const matrix::Quatf& q);