From 2be738e8067f064932259fdbbe6800b18e772f26 Mon Sep 17 00:00:00 2001 From: Kamil Ritz Date: Sat, 15 Aug 2020 13:54:21 +0200 Subject: [PATCH] Add helper function to alter heading in rotation matrices --- EKF/EKFGSF_yaw.cpp | 22 ++++++---------------- EKF/ekf_helper.cpp | 20 ++++---------------- EKF/mag_fusion.cpp | 19 +++++++------------ EKF/utils.cpp | 13 +++++++++++++ EKF/utils.hpp | 3 +++ 5 files changed, 33 insertions(+), 44 deletions(-) diff --git a/EKF/EKFGSF_yaw.cpp b/EKF/EKFGSF_yaw.cpp index a253b35c89..19e87a131a 100644 --- a/EKF/EKFGSF_yaw.cpp +++ b/EKF/EKFGSF_yaw.cpp @@ -225,24 +225,14 @@ 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)) { - // get the roll, pitch, yaw estimates from the rotation matrix using a 321 Tait-Bryan rotation sequence - Eulerf euler_init(_ahrs_ekf_gsf[model_index].R); - - // set the yaw angle - euler_init(2) = wrap_pi(_ekf_gsf[model_index].X(2)); - - // update the rotation matrix - _ahrs_ekf_gsf[model_index].R = Dcmf(euler_init); + // 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); } else { - // Calculate the 312 Tait-Bryan rotation sequence that rotates from earth to body frame - const Vector3f rot312(wrap_pi(_ekf_gsf[model_index].X(2)), // yaw - asinf(_ahrs_ekf_gsf[model_index].R(2, 1)), // roll - atan2f(-_ahrs_ekf_gsf[model_index].R(2, 0), - _ahrs_ekf_gsf[model_index].R(2, 2))); // pitch - - // Calculate the body to earth frame rotation matrix - _ahrs_ekf_gsf[model_index].R = taitBryan312ToRotMat(rot312); + // 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; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index c64fe450c5..4c18cec9e0 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -484,16 +484,11 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool Dcmf R_to_earth; if (shouldUse321RotationSequence(_R_to_earth)) { // rolled more than pitched so use 321 rotation order - Eulerf euler321(_state.quat_nominal); - euler321(2) = 0.0f; - R_to_earth = Dcmf(euler321); + R_to_earth = updateEuler321YawInRotMat(0.f, _R_to_earth); } else { // pitched more than rolled so use 312 rotation order - const Vector3f rotVec312(0.0f, // yaw - asinf(_R_to_earth(2, 1)), // roll - atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2))); // pitch - R_to_earth = taitBryan312ToRotMat(rotVec312); + R_to_earth = updateEuler312YawInRotMat(0.f, _R_to_earth); } @@ -1654,19 +1649,12 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer) // update the rotation matrix using the new yaw value // determine if a 321 or 312 Euler sequence is best if (shouldUse321RotationSequence(_R_to_earth)) { - // use a 321 sequence - Eulerf euler321(_R_to_earth); - euler321(2) = yaw; - _R_to_earth = Dcmf(euler321); + _R_to_earth = updateEuler321YawInRotMat(yaw, _R_to_earth); } else { - // Calculate the 312 Tait-Bryan rotation sequence that rotates from earth to body frame // We use a 312 sequence as an alternate when there is more pitch tilt than roll tilt // to avoid gimbal lock - const Vector3f rot312(yaw, - asinf(_R_to_earth(2, 1)), - atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2))); - _R_to_earth = taitBryan312ToRotMat(rot312); + _R_to_earth = updateEuler312YawInRotMat(yaw, _R_to_earth); } diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index f6a09c63d9..ae2a446a4a 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -702,7 +702,6 @@ void Ekf::fuseHeading() { Vector3f mag_earth_pred; float measured_hdg; - float predicted_hdg; // Calculate the observation variance float R_YAW; @@ -725,12 +724,11 @@ void Ekf::fuseHeading() // 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 - Eulerf euler321(_state.quat_nominal); - predicted_hdg = euler321(2); + const float predicted_hdg = getEuler321Yaw(_R_to_earth); + if (_control_status.flags.mag_hdg) { - // Set the yaw angle to zero and rotate the measurements into earth frame using the zero yaw angle - euler321(2) = 0.0f; - const Dcmf R_to_earth(euler321); + // Rotate the measurements into earth frame using the zero yaw angle + const Dcmf R_to_earth = updateEuler321YawInRotMat(0.f, _R_to_earth); 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; @@ -788,12 +786,9 @@ void Ekf::fuseHeading() 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 - const Vector3f rotVec312(0.0f, // yaw - asinf(_R_to_earth(2, 1)), // roll - atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2))); // pitch - const Dcmf R_to_earth = taitBryan312ToRotMat(rotVec312); + // Calculate the body to earth frame rotation matrix from the euler angles + // using a 312 rotation sequence with yaw angle set to to zero + 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) { diff --git a/EKF/utils.cpp b/EKF/utils.cpp index cf7d63a0de..fd7c2425f8 100644 --- a/EKF/utils.cpp +++ b/EKF/utils.cpp @@ -86,3 +86,16 @@ float getEuler312Yaw(const matrix::Quatf& q) { float getEuler312Yaw(const matrix::Dcmf& R) { return atan2f(-R(0, 1), R(1, 1)); } + +matrix::Dcmf updateEuler321YawInRotMat(float yaw, const matrix::Dcmf& rot_in) { + matrix::Eulerf euler321(rot_in); + euler321(2) = yaw; + return matrix::Dcmf(euler321); +} + +matrix::Dcmf updateEuler312YawInRotMat(float yaw, const matrix::Dcmf& rot_in) { + const matrix::Vector3f rotVec312(yaw, // yaw + asinf(rot_in(2, 1)), // roll + atan2f(-rot_in(2, 0), rot_in(2, 2))); // pitch + return taitBryan312ToRotMat(rotVec312); +} diff --git a/EKF/utils.hpp b/EKF/utils.hpp index 9908b53077..71281e092e 100644 --- a/EKF/utils.hpp +++ b/EKF/utils.hpp @@ -31,6 +31,9 @@ float getEuler321Yaw(const matrix::Dcmf& R); float getEuler312Yaw(const matrix::Quatf& q); float getEuler312Yaw(const matrix::Dcmf& R); +matrix::Dcmf updateEuler321YawInRotMat(float yaw, const matrix::Dcmf& rot_in); +matrix::Dcmf updateEuler312YawInRotMat(float yaw, const matrix::Dcmf& rot_in); + namespace ecl{ inline float powf(float x, int exp) {