From fdc86c247a67fa4e210eda88078660ce18d28147 Mon Sep 17 00:00:00 2001 From: Kamil Ritz Date: Sat, 15 Aug 2020 13:52:56 +0200 Subject: [PATCH] add functions to compute yaw (321 and 312 sequence) from quaternion and rotation matrix --- EKF/EKFGSF_yaw.cpp | 6 ++---- EKF/airspeed_fusion.cpp | 3 +-- EKF/covariance.cpp | 3 +-- EKF/ekf.h | 3 --- EKF/ekf_helper.cpp | 13 ++++--------- EKF/mag_fusion.cpp | 15 ++++----------- EKF/utils.cpp | 26 +++++++++++++++++++++++++- EKF/utils.hpp | 10 ++++++++++ test/test_EKF_utils.cpp | 13 +++++++++++++ 9 files changed, 60 insertions(+), 32 deletions(-) diff --git a/EKF/EKFGSF_yaw.cpp b/EKF/EKFGSF_yaw.cpp index 32020d3000..a253b35c89 100644 --- a/EKF/EKFGSF_yaw.cpp +++ b/EKF/EKFGSF_yaw.cpp @@ -261,11 +261,9 @@ 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)) { - // use 321 Tait-Bryan rotation to define yaw state - _ekf_gsf[model_index].X(2) = atan2f(_ahrs_ekf_gsf[model_index].R(1, 0), _ahrs_ekf_gsf[model_index].R(0, 0)); + _ekf_gsf[model_index].X(2) = getEuler321Yaw(_ahrs_ekf_gsf[model_index].R); } else { - // use 312 Tait-Bryan rotation to define yaw state - _ekf_gsf[model_index].X(2) = atan2f(-_ahrs_ekf_gsf[model_index].R(0, 1), _ahrs_ekf_gsf[model_index].R(1, 1)); // first rotation (yaw) + _ekf_gsf[model_index].X(2) = getEuler312Yaw(_ahrs_ekf_gsf[model_index].R); } // calculate delta velocity in a horizontal front-right frame diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index f1ed00c983..84111988d5 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -182,8 +182,7 @@ void Ekf::get_true_airspeed(float *tas) */ void Ekf::resetWindStates() { - const Eulerf euler321(_state.quat_nominal); - const float euler_yaw = euler321(2); + const float euler_yaw = getEuler321Yaw(_state.quat_nominal); if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < (uint64_t)5e5)) { // estimate wind using zero sideslip assumption and airspeed measurement if airspeed available diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 455696ca7c..c909443dca 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -1096,8 +1096,7 @@ void Ekf::resetWindCovariance() if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < (uint64_t)5e5)) { // Derived using EKF/matlab/scripts/Inertial Nav EKF/wind_cov.py // TODO: explicitly include the sideslip angle in the derivation - Eulerf euler321(_state.quat_nominal); - const float euler_yaw = euler321(2); + const float euler_yaw = getEuler321Yaw(_state.quat_nominal); 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/EKF/ekf.h b/EKF/ekf.h index 2b2106c4df..1aae8fe248 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -816,9 +816,6 @@ private: void checkRangeAidSuitability(); bool isRangeAidSuitable() { return _is_range_aid_suitable; } - // return the square of two floating point numbers - used in auto coded sections - static constexpr float sq(float var) { return var * var; } - // set control flags to use baro height void setControlBaroHeight(); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 76ec1a63a4..c64fe450c5 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -402,8 +402,8 @@ bool Ekf::realignYawGPS() 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 Eulerf euler321(_state.quat_nominal); - yaw_new = euler321(2) + courseYawError; + const float current_yaw = getEuler321Yaw(_state.quat_nominal); + yaw_new = current_yaw + courseYawError; _control_status.flags.mag_aligned_in_flight = true; } else if (_control_status.flags.wind) { @@ -473,11 +473,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool float yaw_new; float yaw_new_variance = 0.0f; if (_control_status.flags.ev_yaw) { - // convert the observed quaternion to a rotation matrix - const Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame - - // calculate the yaw angle for a 312 sequence - yaw_new = atan2f(R_to_earth_ev(1, 0), R_to_earth_ev(0, 0)); + yaw_new = getEuler312Yaw(_ev_sample_delayed.quat); if (increase_yaw_var) { yaw_new_variance = fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f)); @@ -1586,8 +1582,7 @@ void Ekf::startEvVelFusion() { void Ekf::startEvYawFusion() { // reset the yaw angle to the value from the vision quaternion - const Eulerf euler_obs(_ev_sample_delayed.quat); - const float yaw = euler_obs(2); + const float yaw = getEuler321Yaw(_ev_sample_delayed.quat); const float yaw_variance = fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f)); resetQuatStateYaw(yaw, yaw_variance, true); diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 69393c3731..f6a09c63d9 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -744,10 +744,7 @@ void Ekf::fuseHeading() } else if (_control_status.flags.ev_yaw) { // calculate the yaw angle for a 321 sequence - // Expressions obtained from yaw_input_321.c produced by https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw321.m - const float Tbn_1_0 = 2.0f*(_ev_sample_delayed.quat(0)*_ev_sample_delayed.quat(3)+_ev_sample_delayed.quat(1)*_ev_sample_delayed.quat(2)); - const float Tbn_0_0 = sq(_ev_sample_delayed.quat(0))+sq(_ev_sample_delayed.quat(1))-sq(_ev_sample_delayed.quat(2))-sq(_ev_sample_delayed.quat(3)); - measured_hdg = atan2f(Tbn_1_0,Tbn_0_0); + measured_hdg = getEuler321Yaw(_ev_sample_delayed.quat); } else { measured_hdg = predicted_hdg; @@ -786,9 +783,9 @@ 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 - predicted_hdg = atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1)); + 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 @@ -810,11 +807,7 @@ 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 312 sequence - // Values from yaw_input_312.c file produced by https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw312.m - const float Tbn_0_1_neg = 2.0f*(_ev_sample_delayed.quat(0)*_ev_sample_delayed.quat(3)-_ev_sample_delayed.quat(1)*_ev_sample_delayed.quat(2)); - const float Tbn_1_1 = sq(_ev_sample_delayed.quat(0))-sq(_ev_sample_delayed.quat(1))+sq(_ev_sample_delayed.quat(2))-sq(_ev_sample_delayed.quat(3)); - measured_hdg = atan2f(Tbn_0_1_neg,Tbn_1_1); + measured_hdg = getEuler312Yaw(_ev_sample_delayed.quat); } else { measured_hdg = predicted_hdg; diff --git a/EKF/utils.cpp b/EKF/utils.cpp index 903f8f6a11..cf7d63a0de 100644 --- a/EKF/utils.cpp +++ b/EKF/utils.cpp @@ -61,4 +61,28 @@ matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat) bool shouldUse321RotationSequence(const matrix::Dcmf& R) { return fabsf(R(2, 0)) < fabsf(R(2, 1)); -} \ No newline at end of file +} + +float getEuler321Yaw(const matrix::Quatf& q) { + // Values from yaw_input_321.c file produced by + // https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw321.m + const float a = 2.f * (q(0) * q(3) + q(1) * q(2)); + const float b = sq(q(0)) + sq(q(1)) - sq(q(2)) - sq(q(3)); + 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 + // https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw312.m + const float a = 2.f * (q(0) * q(3) - q(1) * q(2)); + const float b = sq(q(0)) - sq(q(1)) + sq(q(2)) - sq(q(3)); + return atan2f(a, b); +} + +float getEuler312Yaw(const matrix::Dcmf& R) { + return atan2f(-R(0, 1), R(1, 1)); +} diff --git a/EKF/utils.hpp b/EKF/utils.hpp index 60debe6468..9908b53077 100644 --- a/EKF/utils.hpp +++ b/EKF/utils.hpp @@ -2,6 +2,9 @@ #pragma once +// return the square of two floating point numbers - used in auto coded sections +static constexpr float sq(float var) { return var * var; } + // converts Tait-Bryan 312 sequence of rotations from frame 1 to frame 2 // to the corresponding rotation matrix that rotates from frame 2 to frame 1 // rot312(0) - First rotation is a RH rotation about the Z axis (rad) @@ -21,6 +24,13 @@ float kahanSummation(float sum_previous, float input, float &accumulator); matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat); bool shouldUse321RotationSequence(const matrix::Dcmf& R); + +float getEuler321Yaw(const matrix::Quatf& q); +float getEuler321Yaw(const matrix::Dcmf& R); + +float getEuler312Yaw(const matrix::Quatf& q); +float getEuler312Yaw(const matrix::Dcmf& R); + namespace ecl{ inline float powf(float x, int exp) { diff --git a/test/test_EKF_utils.cpp b/test/test_EKF_utils.cpp index 467e1937b8..03ed0ba22c 100644 --- a/test/test_EKF_utils.cpp +++ b/test/test_EKF_utils.cpp @@ -55,3 +55,16 @@ TEST(eclPowfTest, compareToStandardImplementation) } } } + +TEST(euler312YawTest, fromQuaternion) +{ + matrix::Quatf q1(3.5f,2.4f,-0.5f,-3.f); + q1.normalize(); + const matrix::Eulerf euler1(q1); + EXPECT_FLOAT_EQ(euler1(2), getEuler321Yaw(q1)); + + matrix::Quatf q2(0.f,0,-1.f,0.f); + q2.normalize(); + const matrix::Eulerf euler2(q2); + EXPECT_FLOAT_EQ(euler2(2), getEuler321Yaw(q2)); +}