void DCM_ut::test_Euler_to_DCM()
{
const units::Radians roll = units::Radians(2);
const units::Radians pitch = units::Radians(1.5);
const units::Radians yaw = units::Radians(1);
const Euler e(roll, pitch, yaw);
const DCM d1 = e.toDCM();
const DCM d2 = DCM(e);
CPPUNIT_ASSERT(Matrix3(d1) == Matrix3(d2));
}
DCM Euler::toDCM() const
{
const double sr = sin(m_roll);
const double sp = sin(m_pitch);
const double sy = sin(m_yaw);
const double cr = cos(m_roll);
const double cp = cos(m_pitch);
const double cy = cos(m_yaw);
const double spsy = sp * sy;
const double spcy = sp * cy;
return DCM(cp * cy, cp * sy, -sp, sr * spcy - cr * sy,
sr * spsy + cr * cy, sr * cp, cr * spcy + sr * sy,
cr * spsy - sr * cy, cr * cp);
}
void Quaternion_ut::test_Quaternion_from_DCM()
{
/* Example from Matlab documentation.
*/
double epsilon = 1e-12;
DCM dcm(0, 1, 0, 1, 0, 0, 0, 0, 1);
Quaternion q(dcm);
Vec3<> v = q.vector();
CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, q.scalar(), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v(1), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v(2), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v(3), epsilon);
/* Start with DCM generated from random numbers.
*/
epsilon = 0.0001;
dcm = DCM(0.9649, 0.9572, 0.1419, 0.1576, 0.4854, 0.4218, 0.9706,
0.8003, 0.9157);
q = Quaternion(dcm);
v = q.vector();
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.9409, q.scalar(), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-0.1058, v(1), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.2316, v(2), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.2235, v(3), epsilon);
dcm = DCM(q);
/* The conversion process changes the matrix, so we don't get the same
* result back...
*/
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.7928, dcm(1, 1), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.3715, dcm(1, 2), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-0.4832, dcm(1, 3), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-0.4696, dcm(2, 1), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.8777, dcm(2, 2), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-0.0956, dcm(2, 3), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.3886, dcm(3, 1), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.3026, dcm(3, 2), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.8703, dcm(3, 3), epsilon);
/* Convert the same DCM back to a quaternion
*/
q = Quaternion(dcm);
v = q.vector();
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.9409, q.scalar(), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-0.1058, v(1), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.2316, v(2), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.2235, v(3), epsilon);
dcm = DCM(q);
/* And now back to a DCM again. We should get the scaling this time.
*/
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.7928, dcm(1, 1), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.3715, dcm(1, 2), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-0.4832, dcm(1, 3), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-0.4696, dcm(2, 1), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.8777, dcm(2, 2), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-0.0956, dcm(2, 3), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.3886, dcm(3, 1), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.3026, dcm(3, 2), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.8703, dcm(3, 3), epsilon);
/* 180 degree roll.
*/
epsilon = 1e-12;
dcm = DCM(1, 0, 0, 0, -1, 0, 0, 0, -1);
q = Quaternion(dcm);
v = q.vector();
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, q.scalar(), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, v(1), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v(2), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v(3), epsilon);
dcm = DCM(q);
CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, dcm(1, 1), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, dcm(1, 2), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, dcm(1, 3), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, dcm(2, 1), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-1.0, dcm(2, 2), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, dcm(2, 3), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, dcm(3, 1), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, dcm(3, 2), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-1.0, dcm(3, 3), epsilon);
/* 180 degree pitch.
*/
dcm = DCM(-1, 0, 0, 0, 1, 0, 0, 0, -1);
q = Quaternion(dcm);
v = q.vector();
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, q.scalar(), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v(1), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, v(2), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v(3), epsilon);
dcm = DCM(q);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-1.0, dcm(1, 1), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, dcm(1, 2), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, dcm(1, 3), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, dcm(2, 1), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, dcm(2, 2), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, dcm(2, 3), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, dcm(3, 1), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, dcm(3, 2), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-1.0, dcm(3, 3), epsilon);
/* 180 degree yaw.
*/
dcm = DCM(-1, 0, 0, 0, -1, 0, 0, 0, 1);
q = Quaternion(dcm);
v = q.vector();
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, q.scalar(), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v(1), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v(2), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, v(3), epsilon);
dcm = DCM(q);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-1.0, dcm(1, 1), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, dcm(1, 2), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, dcm(1, 3), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, dcm(2, 1), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-1.0, dcm(2, 2), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, dcm(2, 3), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, dcm(3, 1), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, dcm(3, 2), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, dcm(3, 3), epsilon);
/* 90 degree roll.
*/
epsilon = .0001;
dcm = DCM(1, 0, 0, 0, 0, 1, 0, -1, 0);
q = Quaternion(dcm);
v = q.vector();
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.7071, q.scalar(), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.7071, v(1), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v(2), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v(3), epsilon);
/* roll = pi; pitch = -pi/3; yaw = pi/6;
*/
dcm = DCM(0.4330, 0.2500, 0.8660, -0.7500, -0.4330, 0.5000, 0.5000,
-0.8660, 0);
q = Quaternion(dcm);
v = q.vector();
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.5, q.scalar(), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.6830, v(1), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(-0.1830, v(2), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.5, v(3), epsilon);
}
