TEST(MEKFGyroAccMagTestGroup, vect_measurement_transform) { Eigen::Quaternionf att(Eigen::AngleAxisf(0.1, Eigen::Vector3f::UnitZ())); Eigen::Vector3f v_i(1, 2, 3); // expectation of v in inertial frame k.reset(att); Eigen::Matrix3f b_to_m = k.vect_measurement_basis_b_frame(v_i); Eigen::Vector3f v_b = att.conjugate()._transformVector(v_i); Eigen::Vector2f z = MEKFGyroAccMag::vect_measurement_transform(b_to_m, v_b); // the measurement z of the expectation is zero CHECK_TRUE(z.isZero()); }