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)