def test_quat(): v1 = np.ones(4) q1 = np.ones(4) / 2 np.testing.assert_allclose(q1, conv.quat(v1), atol=tol) with pytest.raises(AssertionError): conv.quat(np.ones(3)) # too short to be a quaternion with pytest.raises(AssertionError): conv.quat(np.zeros(4)) # norm is 0 is invalid
def test_calc_q_dot(): # test against formula from notes q = quat(np.random.rand(4)) w = np.random.rand(3) test1 = .5 * L(q) @ np.append(0, w) test2 = calc_q_dot(q, w) np.testing.assert_allclose(test1, test2, atol=tol) # trivial case q = quat(np.random.rand(4)) w = np.zeros(3) np.testing.assert_allclose(calc_q_dot(q, w), np.zeros(4), atol=tol) # against pyquaternion for i in range(100): q = quat(np.random.rand(4)) w = np.random.rand(3) qd = pyquaternion.Quaternion(q).derivative(w).elements np.testing.assert_allclose(qd, calc_q_dot(q, w), atol=tol)
def helper_test2(r_Earth2Sun, r_sat, q, albedo, atol): '''r_Earth2Sun version''' rtol = atol q = conv.quat(q) sat2sun_input = conv.quatrot(q, sensors.add(r_Earth2Sun, r_sat)) #in body frame meas = sensors.vector2sense(sat2sun_input, r_sat, q, albedo) sat2sun_out = sensors.sense2vector(meas, r_sat, q, albedo) return np.testing.assert_allclose(sat2sun_out, sensors.normalize(sat2sun_input), atol, rtol)
def test_conj(): q = np.array([1, 0, 0, 0]) np.testing.assert_allclose(-q, conv.conj(q), atol=tol) q = conv.quat(np.random.rand(4)) qc = conv.conj(q) # rotate, then unrotate a vector: v = np.random.rand(3) np.testing.assert_allclose(v, conv.quatrot(qc, conv.quatrot(q, v)), atol=tol) # Repeat the test with a 4 indices vector: v = np.random.rand(4) np.testing.assert_allclose(v, conv.quatrot(qc, conv.quatrot(q, v)), atol=tol)
def test_sense2vector(): meas = [1., 2., 0., 0., 0., 0.] r_sat = [8000, 0, 0] q = [1, 0, 0, 0] # identity quaternion vec = sensors.sense2vector(meas, r_sat, q, albedo=False) np.testing.assert_allclose(vec, [-1., 0., 0.], atol=10e-5) meas = [1., 2., 0., 0., 0., 0.] r_sat = [8000, 0, 0] q = [1, 0, 0, 0] # identity quaternion vec = sensors.sense2vector(meas, r_sat, q, albedo=True) np.testing.assert_allclose(vec, [-1., 0., 0.], atol=10e-5) meas = [59., 60., 2., 1., 1., 0.] r_sat = [8000, 0, 0] q = [1, 0, 0, 0] # identity quaternion vec = sensors.sense2vector(meas, r_sat, q, albedo=False) np.testing.assert_allclose(vec, sensors.normalize([-1., 1., 1.]), atol=10e-5) def helper_test(mjd, r_sat, q, albedo, atol): '''mjd version''' q = conv.quat(q) rtol = atol r_Earth2Sun = sun.sun_position_ECI(mjd) sat2sun_input = conv.quatrot(q, sensors.add(r_Earth2Sun, r_sat)) #in body frame meas = sensors.vector2sense(sat2sun_input, r_sat, q, albedo) sat2sun_out = sensors.sense2vector(meas, r_sat, q, albedo) return np.testing.assert_allclose(sat2sun_out, sensors.normalize(sat2sun_input), atol, rtol) def helper_test2(r_Earth2Sun, r_sat, q, albedo, atol): '''r_Earth2Sun version''' rtol = atol q = conv.quat(q) sat2sun_input = conv.quatrot(q, sensors.add(r_Earth2Sun, r_sat)) #in body frame meas = sensors.vector2sense(sat2sun_input, r_sat, q, albedo) sat2sun_out = sensors.sense2vector(meas, r_sat, q, albedo) return np.testing.assert_allclose(sat2sun_out, sensors.normalize(sat2sun_input), atol, rtol) #some exceptable larger deviation is allowed only for cases where albedo is True, otherwise must be exact match #This is per discussion with Zac 1/16/2020 helper_test(54000, [8000, 0, 0], [1, 1, 2, 1], False, 1e-5) helper_test(54104, [7543, 201, 345], [1, 1, 0, 1], False, 1e-5) helper_test(52124, [543, 201, 7345], conv.quat([1, .1, .3, 1]), False, 1e-5) helper_test(54134.1, [1073, 7000, 345], [.2, .8, .4, .1], True, 15e-3) helper_test(54134.9, [523, 6201, 345], [-.6, 1, 3, 1], True, 15e-3) helper_test2([1.5018e08, 0, 0], [0, 7000, 0], [1, .2, .8, .2], True, 50e-4) helper_test2([0, 1.5018e08, 0], [0, 7000, 0], [1, .2, .8, .2], True, 1e-5) helper_test2([1.5018e08, 0, 0], [0, 7000, 0], [1, 1, 1, 1], True, 5e-3) in_meas = [1., 0., 0., 0., 0., 0.] r_sat = [8000, 0, 0] q = [1, 0, 0, 0] # identity quaternion sat2sun = sensors.sense2vector(in_meas, r_sat, q, albedo=False) out_meas = sensors.vector2sense(sat2sun, r_sat, conv.conj(q), albedo=False) np.testing.assert_allclose(out_meas, in_meas, atol=10e-5) in_meas = [2, 1, 100, 100, 0, 0] sat2sun = sensors.sense2vector(in_meas, r_sat, q, albedo=False) out_meas = sensors.vector2sense(sat2sun, r_sat, conv.conj(q), albedo=False) np.testing.assert_allclose(out_meas, [1, 0, 0, 0, 0, 0], atol=10e-5)