move ekf2 Matrix helper utilities to mathlib

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)

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 <matrix/math.hpp>
+
+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)
+inline matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat)
-{
-	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)
 {
 	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) ?
+	if (shouldUse321RotationSequence(rot_in)) {
-	       updateEuler321YawInRotMat(yaw, rot_in) :
+		return updateEuler321YawInRotMat(yaw, rot_in);
-	       updateEuler312YawInRotMat(yaw, rot_in);
+
+	} else {
+		return updateEuler312YawInRotMat(yaw, rot_in);
+	}
 }
+
+} // namespace Utilities
+} // namespace math
+
+#endif // MATH_UTILITIES_H

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 <gtest/gtest.h>
+#include <cmath>
+#include <vector>
+#include <mathlib/mathlib.h>
+
+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));
+}

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

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

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

src/modules/ekf2/EKF/common.h

@@ -45,6 +45,7 @@
 #include <cstdint>
 
 #include <matrix/math.hpp>
+#include <mathlib/math/Utilities.hpp>
 
 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)

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);

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);
 

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);
+inline 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)
 {
-	if (shouldUse321RotationSequence(R)) {
+	const float y = input - accumulator;
-		return getEuler321Yaw(R);
+	const float t = sum_previous + y;
-
+	accumulator = (t - sum_previous) - y;
-	} else {
+	return t;
-		return getEuler312Yaw(R);
-	}
 }
 
-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

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));
-}