TEST_F(QuaternionTest, default_quaternion_is_identity_quaternion) { const Math::Quaternion<double> quat; EXPECT_EQ(1, quat.w()); EXPECT_EQ(0, quat.x()); EXPECT_EQ(0, quat.y()); EXPECT_EQ(0, quat.z()); }
void OnlineRotHec::addMeasurement( const Math::Quaternion& q, const Math::Quaternion& r ) { // make sure the signs of both w's are equal const double nq = q.w() < 0 ? -1 : 1; const double nr = r.w() < 0 ? -1 : 1; Math::ErrorVector< double, 3 > kalmanMeasurement; kalmanMeasurement.value( 0 ) = r.x() * nr - q.x() * nq; kalmanMeasurement.value( 1 ) = r.y() * nr - q.y() * nq; kalmanMeasurement.value( 2 ) = r.z() * nr - q.z() * nq; kalmanMeasurement.covariance = Math::Matrix< double, 3, 3 >::identity(); // do the filter update Math::Matrix< double, 3, 3 > h; skewMatrix( h, Math::Vector< double, 3 >( q.x() * nq + r.x() * nr, q.y() * nq + r.y() * nr, q.z() * nq + r.z() * nr ) ); Tracking::kalmanMeasurementUpdate< 3, 3 >( m_state, Math::Function::LinearFunction< 3, 3, double >( h ), kalmanMeasurement, 0, m_state.value.size() ); }
Math::Quaternion<double> create_quaternion_from_small_real_component(const Math::Matrix4d& matrix) { Math::Quaternion<double> result; result.w() = 0.5 * std::sqrt(matrix_trace(matrix)); result.x() = 0.5 * std::sqrt(matrix(0,0) - matrix(1,1) - matrix(2,2) + matrix(3,3)); result.y() = 0.5 * std::sqrt(-matrix(0,0) + matrix(1,1) - matrix(2,2) + matrix(3,3)); result.z() = 0.5 * std::sqrt(-matrix(0,0) - matrix(1,1) + matrix(2,2) + matrix(3,3)); return result; }
Math::Quaternion<double> create_quaternion_from_large_real_component(const Math::Matrix4d& matrix) { Math::Quaternion<double> result; result.w() = 0.5 * std::sqrt(matrix_trace(matrix)); result.x() = 0.25 *(matrix(2,1) - matrix(1,2))/result.w(); result.y() = 0.25 *(matrix(0,2) - matrix(2,0))/result.w(); result.z() = 0.25 *(matrix(1,0) - matrix(0,1))/result.w(); return result; }
Math::Matrix4d make_matrix_from_quaternion(const Math::Quaternion<double>& quat) { Math::Matrix4d matrix; const auto s = 2.0 / quaternion_norm(quat); matrix(0,0) -= s *(quat.y() * quat.y() + quat.z() * quat.z()); matrix(0,1) += s *(quat.x() * quat.y() - quat.w() * quat.z()); matrix(0,2) += s *(quat.x() * quat.z() + quat.w() * quat.y()); matrix(1,0) += s *(quat.x() * quat.y() + quat.w() * quat.z()); matrix(1,1) -= s *(quat.x() * quat.x() + quat.z() * quat.z()); matrix(1,2) += s *(quat.y() * quat.z() - quat.w() * quat.x()); matrix(2,0) += s *(quat.x() * quat.z() - quat.w() * quat.y()); matrix(2,1) += s *(quat.y() * quat.z() + quat.w() * quat.x()); matrix(2,2) -= s *(quat.x() * quat.x() + quat.y() * quat.y()); return matrix; }