def test_is_equal(self, arr):
assume(GeneralQuaternion(*arr).norm() > DEFAULT_TOLERANCE)
q = Quaternion(*arr)
assert q == q
assert q == -q
assert q != q + Quaternion(1, 2, 3, 4)
assert q == GeneralQuaternion(*q.coordinates)
def test_from_ra_dec_roll(self, arr):
xyz = np.deg2rad(arr)
c3, c2, c1 = np.cos(xyz)
s3, s2, s1 = np.sin(xyz)
expected = (np.array([
[c2 * c3, -c2 * s3, s2], # noqa
[c1 * s3 + c3 * s1 * s2, c1 * c3 - s1 * s2 * s3, -c2 * s1], # noqa
[s1 * s3 - c1 * c3 * s2, c3 * s1 + c1 * s2 * s3, c1 * c2] # noqa
]))
assert Quaternion.from_ra_dec_roll(*arr).is_equal(Quaternion.from_matrix(expected), tolerance=5e-8)
def lie_integration(self,w,Ts):
# Store trajectory
trajectory = [Quaternion()]
# Perform Euler integration for every gyro sample
for i in range(w.shape[1]):
q_dt = Quaternion.from_algebra(w[:,i].reshape(3,1)*Ts)
self.equal(q_dt*self)
trajectory.append(self.copy())
return trajectory
def test_from_qmethod_with_noise(self, r):
assume(np.linalg.norm(r) > Quaternion.tolerance)
q = Quaternion.from_rotation_vector(r)
vectors = np.random.normal(scale=1.0, size=(3, 6))
norms = np.linalg.norm(vectors, axis=0)
vectors /= norms
noise_sigma = 1e-6
errors = np.random.normal(scale=noise_sigma, size=(3, 6))
rotated_vectors = q.matrix.dot(vectors) + errors
q_calculated = Quaternion.from_qmethod(vectors, rotated_vectors, np.ones(6))
assert q.is_equal(q_calculated, tolerance=10*noise_sigma)
def test_qmethod(self):
v1, v2 = [2 / 3, 2 / 3, 1 / 3], [2 / 3, -1 / 3, -2 / 3]
w1, w2 = [0.8, 0.6, 0], [-0.6, 0.8, 0]
q = Quaternion.from_qmethod(np.array([v1, v2]).T, np.array([w1, w2]).T)
np.testing.assert_allclose(q(v1), w1, atol=1e-10)
np.testing.assert_allclose(q(v2), w2, atol=1e-10)
def test_average_std_sigma_lerner(self):
for star_vectors_noise_arcsec in self.results.keys():
quat_diff_matlab_quats = self.quat_diff_matlab_quats[star_vectors_noise_arcsec]
_, sigma_lerner_deg = Quaternion.average_and_std_lerner(*quat_diff_matlab_quats)
assert abs(sigma_lerner_deg - self.results[star_vectors_noise_arcsec]['sigma_lerner']) \
< self.tolerance_deg
def test_apply(self):
q = Quaternion(1, 2, 3, 4)
v = [1, 2, 3]
assert (q(v) == q * v).all()
assert isinstance(q(v), np.ndarray)
assert len(q(v)) == 3
def test_rotation_vector(self, arr):
assume(np.linalg.norm(np.asarray(arr[1:])) > DEFAULT_TOLERANCE)
q = Quaternion(*arr)
complex_part = np.array([q.qi, q.qj, q.qk])
complex_norm = np.linalg.norm(complex_part)
# test alternative, direct way to calculate rotation axis:
np.testing.assert_allclose(complex_part / complex_norm, q.rotation_axis())
# test alternative, direct way to calculate rotation angle:
angle = 2 * np.math.atan2(complex_norm, q.qr)
assert angle == pytest.approx(q.rotation_angle())
# test rotation of q^2 is 2*rotation:
assert q*q == Quaternion.from_rotation_vector(2 * np.asarray(q.rotation_vector))
def test_rotate_vector_schaub(self):
q1 = exp(Quaternion(0, .1, .02, -.3))
vector = QuaternionTest.schaub_example_dcm[:, 1]
rotated_vector = q1 * vector
np.testing.assert_allclose(rotated_vector,
q1.matrix.dot(vector),
atol=1e-5,
rtol=0)
def test_average_std_naive(self):
for star_vectors_noise_arcsec in self.results.keys():
quat_diff_matlab_quats = self.quat_diff_matlab_quats[star_vectors_noise_arcsec]
_, mean_total_rotation_angle_deg = Quaternion.average_and_std_naive(*quat_diff_matlab_quats)
assert abs(mean_total_rotation_angle_deg - self.results[star_vectors_noise_arcsec]['mean_total_angle'])\
< self.tolerance_deg
def test_average(self, arr):
q = GeneralQuaternion(*arr[:4])
assume(q.norm() > DEFAULT_TOLERANCE) # ignore quaternions of norm==0, whose inverse is numerically unstable
q = q.normalized()
randoms = [GeneralQuaternion(*arr[4*i: 4*i+4]) for i in range(1, self.NUM_ELEMENTS+1)]
q_with_noise = [q + n * (.1 * DEFAULT_TOLERANCE) for n in randoms]
# test without weights:
average = Quaternion.average(*q_with_noise)
assert average == q or average == -q
# test with weights:
weights = [1] + (self.NUM_ELEMENTS-1) * [0] # only uses q_with_noise[0]
average = Quaternion.average(*q_with_noise, weights=weights)
assert average == q_with_noise[0] or average == -q_with_noise[0]
def test_distance(self, arr, angle_rad):
assume(GeneralQuaternion(*arr).norm() > DEFAULT_TOLERANCE)
q = Quaternion(*arr)
assert q.distance(-q) == pytest.approx(0) or q.distance(-q) == pytest.approx(2 * np.math.pi)
for diff in [Quaternion.from_ra_dec_roll(np.degrees(angle_rad), 0, 0),
Quaternion.from_ra_dec_roll(0, np.degrees(angle_rad), 0),
Quaternion.from_ra_dec_roll(0, 0, np.degrees(angle_rad))]:
assert q.distance(q * diff) == pytest.approx(angle_rad)
assert q.distance(diff * q) == pytest.approx(angle_rad)
def draw(self,trajectory=[Quaternion()]):
for i in range(len(trajectory)):
if trajectory[i].magnitude() < 2:
v = trajectory[i]*self.v
self.ax.scatter(v[0,0],v[1,0],v[2,0],s=50,c='r', edgecolors='none')
plt.show()
def setUpClass(cls):
with open(os.path.join(cls.matlab_basedir, 'matlab_results.json'), 'r') as fid:
cls.results = json.load(fid)
cls.quat_diff_matlab_quats = {}
for star_vectors_noise_arcsec in cls.results.keys():
quat_diff_matlab = np.loadtxt(
os.path.join(cls.matlab_basedir, 'quat_diff_ST1_ST2_{}_arcsec.txt'.format(star_vectors_noise_arcsec)))
cls.quat_diff_matlab_quats[star_vectors_noise_arcsec] = [Quaternion(*q) for q in quat_diff_matlab]
def test_mul(self, arr):
assume(GeneralQuaternion(*arr).norm() > DEFAULT_TOLERANCE)
q = Quaternion(*arr)
# multiply by scalar:
assert isinstance(2 * q, Quaternion)
assert isinstance(q * 2, Quaternion)
assert q == q * 2 == 2 * q # Note: up to scale; differs from GeneralQuaternion() * 2
# multiply by Quaternion:
other = Quaternion(1, 2, 3, 4)
assert isinstance(q * other, Quaternion)
assert (q * other).is_unitary()
# multiply by GeneralQuaternion:
other = GeneralQuaternion(1, 2, 3, 4)
for mul in [other * q, q * other]:
assert isinstance(mul, GeneralQuaternion) and not isinstance(mul, Quaternion)
assert mul.norm() == pytest.approx(other.norm())
def quat_2_angle_axis(q):
quat = Quaternion(q[0], q[1], q[2], q[3])
angle = 2 * math.acos(quat.w) / math.pi * 180.0
s = math.sqrt(1 - quat.w * quat.w)
x = quat.x
y = quat.y
z = quat.z
if s > 0.000001:
x = quat.x / s
y = quat.y / s
z = quat.z / s
return (angle, x, y, z)
def setUp(self):
random = Random(100)
N = 50 * 4
int_vals = [random.randint(-1000, 1000) for _ in range(N // 2)]
float_vals = [random.uniform(-1000, 1000) for _ in range(N // 2)]
val_list = int_vals + float_vals
random.shuffle(val_list)
self.p = [
Quaternion(
val_list[4 * n],
val_list[4 * n + 1],
val_list[4 * n + 2],
val_list[4 * n + 3],
) for n in range(N // 8)
]
self.p = [p.unit() for p in self.p]
self.q = [
Quaternion(val_list[4 * n], val_list[4 * n + 1],
val_list[4 * n + 2], val_list[4 * n + 3])
for n in range(N // 8, N // 4)
]
self.q = [q.unit() for q in self.q]
self.all = self.p + self.q
def test_average_and_covariance(self):
# This tests that the trace of the resultant covariance matrix of the
# averaged test is around the same value than the input covariance matrix
# if an individual quaternion divided np.sqrt(N-1), where N is the number of
# quaternions
for star_vectors_noise_arcsec in self.results.keys():
quat_diff_matlab_quats = self.quat_diff_matlab_quats[star_vectors_noise_arcsec]
sigma_lerner_in_deg = self.results[star_vectors_noise_arcsec]['sigma_lerner']
_, covariance_rad = Quaternion.average_and_covariance(
*quat_diff_matlab_quats, R=np.deg2rad(sigma_lerner_in_deg)**2*np.eye(3))
sigma_lerner_out_deg = np.rad2deg(np.sqrt(np.trace(covariance_rad)/3))\
* np.sqrt(len(quat_diff_matlab_quats)-1)
assert abs(sigma_lerner_in_deg - sigma_lerner_out_deg ) \
< self.tolerance_deg
def euler_integration(self, w, Ts=0.01, normalize=False):
# Store trajectory
trajectory = [Quaternion()]
# Perform Euler integration for every gyro sample
for i in range(w.shape[1]):
self.equal(self+self.get_derivative(w[:,i].reshape(3,1),Ts))
if normalize:
self.normalize()
# self.p()
trajectory.append(self.copy())
return trajectory
def setUp(self):
random = Random(100)
N = 50*(4+3)
int_vals = [random.randint(-1000, 1000) for _ in range(N//2)]
float_vals = [random.uniform(-1000, 1000) for _ in range(N // 2)]
val_list = int_vals + float_vals
random.shuffle(val_list)
self.all = [
DualQuaternion(Quaternion(val_list[7*n], val_list[7*n+1],
val_list[7*n+2], val_list[7*n+3]),
Quaternion.from_translation(
[val_list[7*n+4], val_list[7*n+5],
val_list[7*n+6]])) for n in range(N//7)]
self.p = self.all[:N//len(self.all)]
self.q = self.all[N//len(self.all):]
def test_weights_change_in_qmethod(self):
v1, v2 = [1, 0, 0], [0, 1, 0]
w1, w2 = [1, 0, 0], [-1/2, np.sqrt(3) / 2, 0]
previous_cos_1 = np.inf
previous_cos_2 = -np.inf
for second_weight in np.linspace(0.1, 2, 10):
q = Quaternion.from_qmethod(np.array([v1, v2]).T, np.array([w1, w2]).T, [1, second_weight])
current_cos_1 = q(v1).dot(w1)
current_cos_2 = q(v2).dot(w2)
# since first weight is constant and second weight increases, correlation with
# first vector should go down and correlation with second vector should go up
assert current_cos_1 < previous_cos_1
assert current_cos_2 > previous_cos_2
previous_cos_1, previous_cos_2 = current_cos_1, current_cos_2
def from_euler(r, p, y, rads=True, axes=321):
"""
Creates a quaternion given a set of euler angles.
Args:
r: roll
p: pitch
y: yaw
rads: are the angles in radians or degrees? default is rads
axes: order of axis rotations, default is 3-2-1 (aerospace body-to-inertial)
Returns:
A quaternion
"""
if not rads:
r = r2d(r)
p = r2d(p)
y = r2d(y)
return Quaternion.from_euler([r, p, y])
def test_type(self):
# Unit quaternion can be unitary or general:
assert isinstance(GeneralQuaternion.unit(), GeneralQuaternion)
assert isinstance(Quaternion.unit(), Quaternion)
# Unit quaternion can not be unitary:
assert isinstance(GeneralQuaternion.zero(), GeneralQuaternion)
assert not isinstance(Quaternion.zero(), Quaternion)
assert isinstance(Quaternion.zero(), GeneralQuaternion)
assert isinstance(exp(GeneralQuaternion(1, 2, 3, 4)), GeneralQuaternion)
assert isinstance(exp(Quaternion(1, 2, 3, 4)), Quaternion)
assert isinstance(log(Quaternion(1, 2, 3, 4)), GeneralQuaternion)
assert not isinstance(log(Quaternion(1, 2, 3, 4)), Quaternion)
def rk4_integration(self,w,Ts=0.01, normalize = False):
# Store trajectory
trajectory = [Quaternion()]
f = lambda i,t,q: q.get_derivative(w[:,i].reshape(3,1),1)
# Perform Euler integration for every gyro sample
for i in range(w.shape[1]):
t = i*Ts
k1 = f(i,t,self)
# k1.p()
k2 = f(i, t+Ts/2, self + k1*(Ts/2))
k3 = f(i, t+Ts/2, self + k2*(Ts/2))
k4 = f(i, t+Ts, self + k3*Ts)
self += (k1+k2*2+k3*2+k4)*(Ts/6)
if normalize:
self.normalize()
# self.p()
trajectory.append(self.copy())
return trajectory
def test_from_Quaternion(self):
for quat in self.all:
self.assertEqual(Quaternion.from_quaternion(quat), quat)
def test_inverse_a(self):
"""inverse(q) q = Quaternion(1,0,0,0)"""
for q in self.all:
self.assertTrue(
(q.inverse() * q).almost_equal(Quaternion(1, 0, 0, 0)))
def test_inverse_b(self):
"""q inverse(q) = Quaternion(1,0,0,0)"""
for q in self.all:
self.assertTrue(
(q * q.inverse()).almost_equal(Quaternion(1, 0, 0, 0)))
def test_from_Quaternion(self):
for quat in self.all:
self.assertEqual(Quaternion.from_quaternion(quat), quat)
def test_exp(self):
self.assertEqual(qmath.exp(1), qmath.e)
self.assertEqual(qmath.exp(Quaternion(1)), qmath.e)
self.assertEqual(qmath.exp(q), exp_q)
def test_power(self):
self.assertEqual(q1 * q1, q1**2)
self.assertEqual(q1.inverse(), q1**-1)
self.assertEqual(q1, q1**1)
self.assertEqual(q1**0, Quaternion(1))
self.assertEqual(q1**-2, q1.inverse() * q1.inverse())
import unittest
from quaternions import Quaternion
q1 = Quaternion(1, -2, -3, 4)
q2 = Quaternion(1, 4, -3, -2)
q1_plus_q2 = Quaternion(2, 2, -6, 2)
q1_minus_q2 = Quaternion(0, -6, 0, 6)
q2_minus_q1 = Quaternion(0, 6, 0, -6)
q1_times_q2 = Quaternion(8.0, 20.0, 6.0, 20.0)
q2_times_q1 = Quaternion(8.0, -16.0, -18.0, -16.0)
q1_dividedby_q2 = Quaternion(-0.19999999999999996, -0.8, -0.4, -0.4)
q2_inverse_times_q1 = Quaternion(-0.19999999999999996, 0.4, 0.4, 0.8)
q2_dividedby_q1 = Quaternion(-0.19999999999999996, 0.8, 0.4, 0.4)
q1_inverse_times_q2 = Quaternion(-0.19999999999999996, -0.4, -0.4, -0.8)
class TestQuaternions(unittest.TestCase):
def test_addition(self):
self.assertEqual(q1 + q2, q1_plus_q2)
self.assertEqual(q2 + q1, q1_plus_q2)
def test_subtraction(self):
self.assertEqual(q1 - q2, q1_minus_q2)
self.assertEqual(q2 - q1, q2_minus_q1)
def test_multiplication(self):
self.assertEqual(q1 * q2, q1_times_q2)
self.assertEqual(q2 * q1, q2_times_q1)
self.assertEqual(1 / q2 * q1, q2_inverse_times_q1)
def test_log(self):
self.assertEqual(qmath.log(1), 0)
self.assertEqual(qmath.log(Quaternion(1)), 0)
self.assertEqual(qmath.log(q), log_q)
self.assertEqual(qmath.log10(q), log10_q)