From 88ffc177f72ba0dc9e4374b47ca9fc7deb1914b9 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 1 Feb 2022 20:59:33 -0500 Subject: [PATCH] ekf2: utils add getEulerYaw() that uses the best rotation sequence --- src/modules/ekf2/EKF/EKFGSF_yaw.cpp | 5 +---- src/modules/ekf2/EKF/airspeed_fusion.cpp | 4 +--- src/modules/ekf2/EKF/covariance.cpp | 2 +- src/modules/ekf2/EKF/ekf.cpp | 1 - src/modules/ekf2/EKF/ekf_helper.cpp | 9 +++------ src/modules/ekf2/EKF/gps_control.cpp | 4 +--- src/modules/ekf2/EKF/mag_fusion.cpp | 8 ++------ src/modules/ekf2/EKF/utils.cpp | 17 +---------------- src/modules/ekf2/EKF/utils.hpp | 18 ++++++++++++++---- 9 files changed, 24 insertions(+), 44 deletions(-) diff --git a/src/modules/ekf2/EKF/EKFGSF_yaw.cpp b/src/modules/ekf2/EKF/EKFGSF_yaw.cpp index c02fa64763..bbb0473c0b 100644 --- a/src/modules/ekf2/EKF/EKFGSF_yaw.cpp +++ b/src/modules/ekf2/EKF/EKFGSF_yaw.cpp @@ -245,10 +245,7 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index) } // Calculate the yaw state using a projection onto the horizontal that avoids gimbal lock - const Dcmf &R = _ahrs_ekf_gsf[model_index].R; - _ekf_gsf[model_index].X(2) = shouldUse321RotationSequence(R) ? - getEuler321Yaw(R) : - getEuler312Yaw(R); + _ekf_gsf[model_index].X(2) = getEulerYaw(_ahrs_ekf_gsf[model_index].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/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/airspeed_fusion.cpp index e6df5135f1..5d0db4d245 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/airspeed_fusion.cpp @@ -187,9 +187,7 @@ void Ekf::resetWind() */ void Ekf::resetWindUsingAirspeed() { - const float euler_yaw = shouldUse321RotationSequence(_R_to_earth) - ? getEuler321Yaw(_state.quat_nominal) - : getEuler312Yaw(_state.quat_nominal); + const float euler_yaw = getEulerYaw(_R_to_earth); // estimate wind using zero sideslip assumption and airspeed measurement if airspeed available _state.wind_vel(0) = _state.vel(0) - _airspeed_sample_delayed.true_airspeed * cosf(euler_yaw); diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index e1dbec9c4b..f1b7478bd9 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -1137,7 +1137,7 @@ void Ekf::resetWindCovarianceUsingAirspeed() { // Derived using EKF/matlab/scripts/Inertial Nav EKF/wind_cov.py // TODO: explicitly include the sideslip angle in the derivation - const float euler_yaw = getEuler321Yaw(_state.quat_nominal); + const float euler_yaw = getEulerYaw(_R_to_earth); const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f)); constexpr float initial_sideslip_uncertainty = math::radians(15.0f); const float initial_wind_var_body_y = sq(_airspeed_sample_delayed.true_airspeed * sinf(initial_sideslip_uncertainty)); diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index d2a2ff4663..7bc524f4c0 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -261,7 +261,6 @@ void Ekf::predictState() // rotate the previous quaternion by the delta quaternion using a quaternion multiplication _state.quat_nominal = (_state.quat_nominal * dq).normalized(); - _R_to_earth = Dcmf(_state.quat_nominal); // Calculate an earth frame delta velocity diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 397ea3a44e..c6f33cb64d 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -428,7 +428,7 @@ bool Ekf::realignYawGPS(const Vector3f &mag) if (!_control_status.flags.mag_aligned_in_flight) { // This is our first flight alignment so we can assume that the recent change in velocity has occurred due to a // forward direction takeoff or launch and therefore the inertial and GPS ground course discrepancy is due to yaw error - const float current_yaw = getEuler321Yaw(_state.quat_nominal); + const float current_yaw = getEulerYaw(_R_to_earth); yaw_new = current_yaw + courseYawError; _control_status.flags.mag_aligned_in_flight = true; @@ -454,7 +454,6 @@ bool Ekf::realignYawGPS(const Vector3f &mag) // Use the last magnetometer measurements to reset the field states _state.mag_B.zero(); - _R_to_earth = Dcmf(_state.quat_nominal); _state.mag_I = _R_to_earth * mag; resetMagCov(); @@ -541,7 +540,7 @@ bool Ekf::resetMagHeading(bool increase_yaw_var, bool update_buffer) bool Ekf::resetYawToEv() { - const float yaw_new = getEuler312Yaw(_ev_sample_delayed.quat); + 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); @@ -1695,10 +1694,8 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer) const Quatf quat_before_reset = _state.quat_nominal; // update transformation matrix from body to world frame using the current estimate - _R_to_earth = Dcmf(_state.quat_nominal); - // update the rotation matrix using the new yaw value - _R_to_earth = updateYawInRotMat(yaw, _R_to_earth); + _R_to_earth = updateYawInRotMat(yaw, Dcmf(_state.quat_nominal)); // calculate the amount that the quaternion has changed by const Quatf quat_after_reset(_R_to_earth); diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index eb5184021b..47e807ff8d 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -192,9 +192,7 @@ bool Ekf::isYawFailure() const return false; } - const float euler_yaw = shouldUse321RotationSequence(_R_to_earth) - ? getEuler321Yaw(_R_to_earth) - : getEuler312Yaw(_R_to_earth); + const float euler_yaw = getEulerYaw(_R_to_earth); const float yaw_error = wrap_pi(euler_yaw - _yawEstimator.getYaw()); return fabsf(yaw_error) > math::radians(25.f); diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index f2ffd2b0d5..f967952858 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -720,11 +720,7 @@ void Ekf::fuseHeading(float measured_hdg, float obs_var) float R_YAW = PX4_ISFINITE(obs_var) ? obs_var : 0.01f; // update transformation matrix from body to world frame using the current state estimate - _R_to_earth = Dcmf(_state.quat_nominal); - - const bool use_321_rotation_seq = shouldUse321RotationSequence(_R_to_earth); - - const float predicted_hdg = use_321_rotation_seq ? getEuler321Yaw(_R_to_earth) : getEuler312Yaw(_R_to_earth); + const float predicted_hdg = getEulerYaw(_R_to_earth); if (!PX4_ISFINITE(measured_hdg)) { measured_hdg = predicted_hdg; @@ -765,7 +761,7 @@ void Ekf::fuseHeading(float measured_hdg, float obs_var) _last_static_yaw = predicted_hdg; } - if (use_321_rotation_seq) { + if (shouldUse321RotationSequence(_R_to_earth)) { fuseYaw321(measured_hdg, R_YAW, fuse_zero_innov); } else { diff --git a/src/modules/ekf2/EKF/utils.cpp b/src/modules/ekf2/EKF/utils.cpp index c09f3b000d..a5abecbfa9 100644 --- a/src/modules/ekf2/EKF/utils.cpp +++ b/src/modules/ekf2/EKF/utils.cpp @@ -32,7 +32,7 @@ float kahanSummation(float sum_previous, float input, float &accumulator) return t; } -matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat) +matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat) { const float q00 = quat(0) * quat(0); const float q11 = quat(1) * quat(1); @@ -59,11 +59,6 @@ matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat) return dcm; } -bool shouldUse321RotationSequence(const matrix::Dcmf &R) -{ - return fabsf(R(2, 0)) < fabsf(R(2, 1)); -} - float getEuler321Yaw(const matrix::Quatf &q) { // Values from yaw_input_321.c file produced by @@ -73,11 +68,6 @@ float getEuler321Yaw(const matrix::Quatf &q) return atan2f(a, b); } -float getEuler321Yaw(const matrix::Dcmf &R) -{ - return atan2f(R(1, 0), R(0, 0)); -} - float getEuler312Yaw(const matrix::Quatf &q) { // Values from yaw_input_312.c file produced by @@ -87,11 +77,6 @@ float getEuler312Yaw(const matrix::Quatf &q) return atan2f(a, b); } -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); diff --git a/src/modules/ekf2/EKF/utils.hpp b/src/modules/ekf2/EKF/utils.hpp index b2e86484c1..3cc34365dd 100644 --- a/src/modules/ekf2/EKF/utils.hpp +++ b/src/modules/ekf2/EKF/utils.hpp @@ -27,13 +27,23 @@ 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); +inline bool shouldUse321RotationSequence(const matrix::Dcmf &R) { return fabsf(R(2, 0)) < fabsf(R(2, 1)); } + +inline float getEuler321Yaw(const matrix::Dcmf &R) { return atan2f(R(1, 0), R(0, 0)); } +inline float getEuler312Yaw(const matrix::Dcmf &R) { return atan2f(-R(0, 1), R(1, 1)); } float getEuler321Yaw(const matrix::Quatf &q); -float getEuler321Yaw(const matrix::Dcmf &R); - float getEuler312Yaw(const matrix::Quatf &q); -float getEuler312Yaw(const matrix::Dcmf &R); + +inline float getEulerYaw(const matrix::Dcmf &R) +{ + if (shouldUse321RotationSequence(R)) { + return getEuler321Yaw(R); + + } else { + return getEuler312Yaw(R); + } +} matrix::Dcmf updateEuler321YawInRotMat(float yaw, const matrix::Dcmf &rot_in); matrix::Dcmf updateEuler312YawInRotMat(float yaw, const matrix::Dcmf &rot_in);