fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 01:00:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

move ekf2 Matrix helper utilities to mathlib

Browse Source
This commit is contained in:
Daniel Agar
2022-08-04 11:17:59 -04:00
parent 6ebc88fed7
commit bce4237963
11 changed files with 173 additions and 108 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/test/test_EKF_utils.cpp
-41
View File
@@ -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));
}