diff --git a/src/lib/mathlib/CMakeLists.txt b/src/lib/mathlib/CMakeLists.txt index 3afabfc860..3b4377932a 100644 --- a/src/lib/mathlib/CMakeLists.txt +++ b/src/lib/mathlib/CMakeLists.txt @@ -45,4 +45,5 @@ px4_add_unit_gtest(SRC math/test/MedianFilterTest.cpp) px4_add_unit_gtest(SRC math/test/NotchFilterTest.cpp) px4_add_unit_gtest(SRC math/test/second_order_reference_model_test.cpp) px4_add_unit_gtest(SRC math/FunctionsTest.cpp) +px4_add_unit_gtest(SRC math/test/UtilitiesTest.cpp) px4_add_unit_gtest(SRC math/WelfordMeanTest.cpp) diff --git a/src/modules/ekf2/EKF/utils.cpp b/src/lib/mathlib/math/Utilities.hpp similarity index 64% rename from src/modules/ekf2/EKF/utils.cpp rename to src/lib/mathlib/math/Utilities.hpp index 76327487d2..9b8f7047c4 100644 --- a/src/modules/ekf2/EKF/utils.cpp +++ b/src/lib/mathlib/math/Utilities.hpp @@ -31,9 +31,27 @@ * ****************************************************************************/ -#include "utils.hpp" +#ifndef MATH_UTILITIES_H +#define MATH_UTILITIES_H -matrix::Dcmf taitBryan312ToRotMat(const matrix::Vector3f &rot312) +#include + +namespace math +{ + +namespace Utilities +{ + +// 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) +// rot312(1) - Second rotation is a RH rotation about the X axis (rad) +// rot312(2) - Third rotation is a RH rotation about the Y axis (rad) +// See http://www.atacolorado.com/eulersequences.doc +inline matrix::Dcmf taitBryan312ToRotMat(const matrix::Vector3f &rot312) { // Calculate the frame2 to frame 1 rotation matrix from a 312 Tait-Bryan rotation sequence const float c2 = cosf(rot312(2)); // third rotation is pitch @@ -57,15 +75,7 @@ matrix::Dcmf taitBryan312ToRotMat(const matrix::Vector3f &rot312) return R; } -float kahanSummation(float sum_previous, float input, float &accumulator) -{ - const float y = input - accumulator; - const float t = sum_previous + y; - accumulator = (t - sum_previous) - y; - return t; -} - -matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat) +inline matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat) { const float q00 = quat(0) * quat(0); const float q11 = quat(1) * quat(1); @@ -92,7 +102,25 @@ matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat) return dcm; } -float getEuler321Yaw(const matrix::Quatf &q) +// 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. +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)); +} + +inline 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 @@ -101,7 +129,7 @@ float getEuler321Yaw(const matrix::Quatf &q) return atan2f(a, b); } -float getEuler312Yaw(const matrix::Quatf &q) +inline 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 @@ -110,14 +138,24 @@ float getEuler312Yaw(const matrix::Quatf &q) return atan2f(a, b); } -matrix::Dcmf updateEuler321YawInRotMat(float yaw, const matrix::Dcmf &rot_in) +inline float getEulerYaw(const matrix::Dcmf &R) +{ + if (shouldUse321RotationSequence(R)) { + return getEuler321Yaw(R); + + } else { + return getEuler312Yaw(R); + } +} + +inline 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) +inline matrix::Dcmf updateEuler312YawInRotMat(float yaw, const matrix::Dcmf &rot_in) { const matrix::Vector3f rotVec312(yaw, // yaw asinf(rot_in(2, 1)), // roll @@ -125,9 +163,18 @@ matrix::Dcmf updateEuler312YawInRotMat(float yaw, const matrix::Dcmf &rot_in) return taitBryan312ToRotMat(rotVec312); } -matrix::Dcmf updateYawInRotMat(float yaw, const matrix::Dcmf &rot_in) +// Checks which euler rotation sequence to use and update yaw in rotation matrix +inline matrix::Dcmf updateYawInRotMat(float yaw, const matrix::Dcmf &rot_in) { - return shouldUse321RotationSequence(rot_in) ? - updateEuler321YawInRotMat(yaw, rot_in) : - updateEuler312YawInRotMat(yaw, rot_in); + if (shouldUse321RotationSequence(rot_in)) { + return updateEuler321YawInRotMat(yaw, rot_in); + + } else { + return updateEuler312YawInRotMat(yaw, rot_in); + } } + +} // namespace Utilities +} // namespace math + +#endif // MATH_UTILITIES_H diff --git a/src/lib/mathlib/math/test/UtilitiesTest.cpp b/src/lib/mathlib/math/test/UtilitiesTest.cpp new file mode 100644 index 0000000000..59901ef3b3 --- /dev/null +++ b/src/lib/mathlib/math/test/UtilitiesTest.cpp @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (c) 2019 ECL Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file test_EKF_utils.cpp + * + * @brief Unit tests for the miscellaneous EKF utilities + */ + +#include +#include +#include +#include + +using namespace math::Utilities; + +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)); +} + +TEST(shouldUse321RotationSequenceTest, pitch90) +{ + matrix::Eulerf euler(0.f, math::radians(90), 0.f); + matrix::Dcmf R(euler); + EXPECT_FALSE(shouldUse321RotationSequence(R)); +} + +TEST(shouldUse321RotationSequenceTest, roll90) +{ + matrix::Eulerf euler(math::radians(90.f), 0.f, 0.f); + matrix::Dcmf R(euler); + EXPECT_TRUE(shouldUse321RotationSequence(R)); +} + +TEST(shouldUse321RotationSequenceTest, moreRollThanPitch) +{ + matrix::Eulerf euler(math::radians(45.f), math::radians(30.f), 0.f); + matrix::Dcmf R(euler); + EXPECT_TRUE(shouldUse321RotationSequence(R)); +} + +TEST(shouldUse321RotationSequenceTest, morePitchThanRoll) +{ + matrix::Eulerf euler(math::radians(30.f), math::radians(45.f), 0.f); + matrix::Dcmf R(euler); + EXPECT_FALSE(shouldUse321RotationSequence(R)); +} diff --git a/src/lib/mathlib/mathlib.h b/src/lib/mathlib/mathlib.h index 488f5b2930..e8059eacb9 100644 --- a/src/lib/mathlib/mathlib.h +++ b/src/lib/mathlib/mathlib.h @@ -45,5 +45,6 @@ #include "math/Functions.hpp" #include "math/SearchMin.hpp" #include "math/TrajMath.hpp" +#include "math/Utilities.hpp" #endif diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 178e6f21d7..bfa188ee82 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -70,7 +70,6 @@ px4_add_module( EKF/sensor_range_finder.cpp EKF/sideslip_fusion.cpp EKF/terrain_estimator.cpp - EKF/utils.cpp EKF/vel_pos_fusion.cpp EKF/zero_innovation_heading_update.cpp EKF/zero_velocity_update.cpp diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index bd04a01dd3..091f2a89e1 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -55,7 +55,6 @@ add_library(ecl_EKF sensor_range_finder.cpp sideslip_fusion.cpp terrain_estimator.cpp - utils.cpp vel_pos_fusion.cpp zero_innovation_heading_update.cpp zero_velocity_update.cpp diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 0a0ecbf57b..2387dc5a15 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -45,6 +45,7 @@ #include #include +#include namespace estimator { @@ -58,6 +59,12 @@ using matrix::Vector2f; using matrix::Vector3f; using matrix::wrap_pi; +using math::Utilities::getEulerYaw; +using math::Utilities::quatToInverseRotMat; +using math::Utilities::shouldUse321RotationSequence; +using math::Utilities::sq; +using math::Utilities::updateYawInRotMat; + // maximum sensor intervals in usec #define BARO_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between pressure altitude measurements (uSec) #define EV_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between external vision system measurements (uSec) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index c26b8e686d..68e4f61267 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -362,7 +362,7 @@ void Ekf::controlExternalVisionFusion() // determine if we should use the yaw observation resetEstimatorAidStatus(_aid_src_ev_yaw); - const float measured_hdg = shouldUse321RotationSequence(_R_to_earth) ? getEuler321Yaw(_ev_sample_delayed.quat) : getEuler312Yaw(_ev_sample_delayed.quat); + const float measured_hdg = getEulerYaw(_ev_sample_delayed.quat); const float ev_yaw_obs_var = fmaxf(_ev_sample_delayed.angVar, 1.e-4f); if (PX4_ISFINITE(measured_hdg)) { @@ -383,7 +383,7 @@ void Ekf::controlExternalVisionFusion() } else { // populate estimator_aid_src_ev_yaw with delta heading innovations for logging // use the change in yaw since the last measurement - const float measured_hdg_prev = shouldUse321RotationSequence(_R_to_earth) ? getEuler321Yaw(_ev_sample_delayed_prev.quat) : getEuler312Yaw(_ev_sample_delayed_prev.quat); + const float measured_hdg_prev = getEulerYaw(_ev_sample_delayed_prev.quat); // calculate the change in yaw since the last measurement const float ev_delta_yaw = wrap_pi(measured_hdg - measured_hdg_prev); diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index 1119f12c03..03ccf9d208 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -70,7 +70,7 @@ void Ekf::controlMagFusion() // compute mag heading innovation (for estimator_aid_src_mag_heading logging) const Vector3f mag_observation = mag_sample.mag - _state.mag_B; - const Dcmf R_to_earth = shouldUse321RotationSequence(_R_to_earth) ? updateEuler321YawInRotMat(0.f, _R_to_earth) : updateEuler312YawInRotMat(0.f, _R_to_earth); + const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth); const Vector3f mag_earth_pred = R_to_earth * mag_observation; resetEstimatorAidStatus(_aid_src_mag_heading); @@ -355,7 +355,7 @@ void Ekf::runMagAndMagDeclFusions(const Vector3f &mag) } else if (_control_status.flags.mag_hdg && !_is_yaw_fusion_inhibited) { // Rotate the measurements into earth frame using the zero yaw angle - Dcmf R_to_earth = shouldUse321RotationSequence(_R_to_earth) ? updateEuler321YawInRotMat(0.f, _R_to_earth) : updateEuler312YawInRotMat(0.f, _R_to_earth); + Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth); Vector3f mag_earth_pred = R_to_earth * (mag - _state.mag_B); diff --git a/src/modules/ekf2/EKF/utils.hpp b/src/modules/ekf2/EKF/utils.hpp index 0234574f22..4d911f6e20 100644 --- a/src/modules/ekf2/EKF/utils.hpp +++ b/src/modules/ekf2/EKF/utils.hpp @@ -36,54 +36,18 @@ #ifndef EKF_UTILS_HPP #define EKF_UTILS_HPP -// 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) -// rot312(1) - Second rotation is a RH rotation about the X axis (rad) -// rot312(2) - Third rotation is a RH rotation about the Y axis (rad) -// See http://www.atacolorado.com/eulersequences.doc -matrix::Dcmf taitBryan312ToRotMat(const matrix::Vector3f &rot312); - // Use Kahan summation algorithm to get the sum of "sum_previous" and "input". // This function relies on the caller to be responsible for keeping a copy of // "accumulator" and passing this value at the next iteration. // Ref: https://en.wikipedia.org/wiki/Kahan_summation_algorithm -float kahanSummation(float sum_previous, float input, float &accumulator); - -// calculate the inverse rotation matrix from a quaternion rotation -// 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. -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 getEuler312Yaw(const matrix::Quatf &q); - -inline float getEulerYaw(const matrix::Dcmf &R) +inline float kahanSummation(float sum_previous, float input, float &accumulator) { - if (shouldUse321RotationSequence(R)) { - return getEuler321Yaw(R); - - } else { - return getEuler312Yaw(R); - } + const float y = input - accumulator; + const float t = sum_previous + y; + accumulator = (t - sum_previous) - y; + return t; } -matrix::Dcmf updateEuler321YawInRotMat(float yaw, const matrix::Dcmf &rot_in); -matrix::Dcmf updateEuler312YawInRotMat(float yaw, const matrix::Dcmf &rot_in); - -// Checks which euler rotation sequence to use and update yaw in rotation matrix -matrix::Dcmf updateYawInRotMat(float yaw, const matrix::Dcmf &rot_in); - namespace ecl { inline float powf(float x, int exp) @@ -105,5 +69,7 @@ inline float powf(float x, int exp) return 1.0f; } -} + +} // namespace ecl + #endif // EKF_UTILS_HPP diff --git a/src/modules/ekf2/test/test_EKF_utils.cpp b/src/modules/ekf2/test/test_EKF_utils.cpp index 99cf67d860..77a99a35d4 100644 --- a/src/modules/ekf2/test/test_EKF_utils.cpp +++ b/src/modules/ekf2/test/test_EKF_utils.cpp @@ -56,44 +56,3 @@ 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)); -} - -TEST(shouldUse321RotationSequenceTest, pitch90) -{ - matrix::Eulerf euler(0.f, math::radians(90), 0.f); - matrix::Dcmf R(euler); - EXPECT_FALSE(shouldUse321RotationSequence(R)); -} - -TEST(shouldUse321RotationSequenceTest, roll90) -{ - matrix::Eulerf euler(math::radians(90.f), 0.f, 0.f); - matrix::Dcmf R(euler); - EXPECT_TRUE(shouldUse321RotationSequence(R)); -} - -TEST(shouldUse321RotationSequenceTest, moreRollThanPitch) -{ - matrix::Eulerf euler(math::radians(45.f), math::radians(30.f), 0.f); - matrix::Dcmf R(euler); - EXPECT_TRUE(shouldUse321RotationSequence(R)); -} - -TEST(shouldUse321RotationSequenceTest, morePitchThanRoll) -{ - matrix::Eulerf euler(math::radians(30.f), math::radians(45.f), 0.f); - matrix::Dcmf R(euler); - EXPECT_FALSE(shouldUse321RotationSequence(R)); -}