move ekf2 Matrix helper utilities to mathlib

2026-07-01 23:40:34 +08:00 · 2022-08-04 11:17:59 -04:00
parent 6ebc88fed7
commit bce4237963
11 changed files with 173 additions and 108 deletions
@@ -0,0 +1,180 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020-2022 PX4 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.
+ *
+ ****************************************************************************/
+
+#ifndef MATH_UTILITIES_H
+#define MATH_UTILITIES_H
+
+#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
+	const float s2 = sinf(rot312(2));
+	const float s1 = sinf(rot312(1)); // second rotation is roll
+	const float c1 = cosf(rot312(1));
+	const float s0 = sinf(rot312(0)); // first rotation is yaw
+	const float c0 = cosf(rot312(0));
+
+	matrix::Dcmf R;
+	R(0, 0) = c0 * c2 - s0 * s1 * s2;
+	R(1, 1) = c0 * c1;
+	R(2, 2) = c2 * c1;
+	R(0, 1) = -c1 * s0;
+	R(0, 2) = s2 * c0 + c2 * s1 * s0;
+	R(1, 0) = c2 * s0 + s2 * s1 * c0;
+	R(1, 2) = s0 * s2 - s1 * c0 * c2;
+	R(2, 0) = -s2 * c1;
+	R(2, 1) = s1;
+
+	return R;
+}
+
+inline matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat)
+{
+	const float q00 = quat(0) * quat(0);
+	const float q11 = quat(1) * quat(1);
+	const float q22 = quat(2) * quat(2);
+	const float q33 = quat(3) * quat(3);
+	const float q01 = quat(0) * quat(1);
+	const float q02 = quat(0) * quat(2);
+	const float q03 = quat(0) * quat(3);
+	const float q12 = quat(1) * quat(2);
+	const float q13 = quat(1) * quat(3);
+	const float q23 = quat(2) * quat(3);
+
+	matrix::Dcmf dcm;
+	dcm(0, 0) = q00 + q11 - q22 - q33;
+	dcm(1, 1) = q00 - q11 + q22 - q33;
+	dcm(2, 2) = q00 - q11 - q22 + q33;
+	dcm(1, 0) = 2.0f * (q12 - q03);
+	dcm(2, 0) = 2.0f * (q13 + q02);
+	dcm(0, 1) = 2.0f * (q12 + q03);
+	dcm(2, 1) = 2.0f * (q23 - q01);
+	dcm(0, 2) = 2.0f * (q13 - q02);
+	dcm(1, 2) = 2.0f * (q23 + q01);
+
+	return dcm;
+}
+
+// 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
+	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);
+}
+
+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
+	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);
+}
+
+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);
+}
+
+inline 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);
+}
+
+// Checks which euler rotation sequence to use and update yaw in rotation matrix
+inline matrix::Dcmf updateYawInRotMat(float yaw, const matrix::Dcmf &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
@@ -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));
+}