def test_quaternion_rot_matrix_det_one(self):
''' Test that the determinant of the rotation matrix is 1.'''
for _ in range(10):
theta = np.random.normal(0., 1., 4)
theta = Quaternion(theta/np.linalg.norm(theta))
R = theta.rotation_matrix()
self.assertAlmostEqual(np.linalg.det(R), 1.0)
def __init__(self, src=None, timestamp=None):
if src is not None:
Quaternion.__init__(self, src.w, src.x, src.y, src.z)
else:
Quaternion.__init__(self)
self.timestamp = timestamp
self.valid = True
self.interpolated = False
def generate_quaternion(): q1 = Quaternion.random() q2 = Quaternion.random() while True: for q in Quaternion.intermediates(q1, q2, 20, include_endpoints=True): yield q #q1, q2 = q2, q1 q1 = q2 q2 = Quaternion.random()
def test_q_bar_matrix(self):
a, b, c, d = randomElements()
q = Quaternion(a, b, c, d)
M = np.array([
[a, -b, -c, -d],
[b, a, d, -c],
[c, -d, a, b],
[d, c, -b, a]])
self.assertTrue(np.array_equal(q._q_bar_matrix(), M))
def test_inverse(self):
q1 = Quaternion(randomElements())
q2 = Quaternion.random()
if q1:
self.assertEqual(q1 * q1.inverse(), Quaternion(1.0, 0.0, 0.0, 0.0))
else:
with self.assertRaises(ZeroDivisionError):
q1 * q1.inverse()
self.assertEqual(q2 * q2.inverse(), Quaternion(1.0, 0.0, 0.0, 0.0))
def fqa(msr):
ax = msr['accel.x']
ay = msr['accel.y']
az = msr['accel.z']
norm = math.sqrt(ax*ax + ay*ay + az*az)
# estimation of elevation quaternion (around y axes)
sin_theta_a = (ax / norm)
cos_theta_a = math.sqrt(1 - sin_theta_a ** 2)
sin_half_theta = math.copysign(1, sin_theta_a) * math.sqrt((1 - cos_theta_a) / 2)
cos_half_theta = math.sqrt((1 + cos_theta_a) / 2)
q_e = Quaternion(cos_half_theta, 0, sin_half_theta, 0)
# estimation of roll quaternion (around x axis)
sin_phi = (-ay / norm) / cos_theta_a
cos_phi = (-az / norm) / cos_theta_a
sin_half_phi = half_sin(sin_phi, cos_phi)
cos_half_phi = half_cos(sin_phi, cos_phi)
# TODO singularity avoidance!!!
q_r = Quaternion(cos_half_phi, sin_half_phi, 0, 0)
# estimation of azimuth quaternion (around z axis)
mx = msr['mag.x']
my = msr['mag.y']
mz = msr['mag.z']
norm = math.sqrt(mx*mx + my*my + mz*mz)
qm = Quaternion(0, mx / norm, my / norm, mz / norm)
qm_a = q_e.multiply(q_r).multiply(qm).multiply(q_r.inverse()).multiply(q_e.inverse())
Quaternion.normalize(qm_a)
cos_zeta = qm_a.c
sin_zeta = qm_a.b
sin_half_zeta = half_sin(sin_zeta, cos_zeta)
cos_half_zeta = half_cos(sin_zeta, cos_zeta)
q_a = Quaternion(cos_half_zeta, 0, 0, sin_half_zeta)
q_fqa = q_a.multiply(q_e).multiply(q_r)
return q_fqa
def update(self, msr):
t = msr['t']
dt = t - self.t
self.t = t
w = Quaternion(0, msr['gyro.x'], msr['gyro.y'], msr['gyro.z'])
q_fqa = fqa(msr)
dq_static = q_fqa.add((self.q.scalar_multiply(-1))).scalar_multiply(1/dt)
dq_dynamic = w.multiply(self.q).scalar_multiply(0.5)
dq = dq_static.scalar_multiply(self.k).add(dq_dynamic.scalar_multiply(1 - self.k))
self.q = self.q.add(dq.scalar_multiply(dt))
self.q = Quaternion.normalize(self.q)
def test_matrix_io(self):
v = np.random.uniform(-100, 100, 3)
for i in range(10):
q0 = Quaternion.random()
R = q0.rotation_matrix()
q1 = Quaternion(matrix=R)
np.testing.assert_almost_equal(q0.rotate(v), np.dot(R, v), decimal=ALMOST_EQUAL_TOLERANCE)
np.testing.assert_almost_equal(q0.rotate(v), q1.rotate(v), decimal=ALMOST_EQUAL_TOLERANCE)
np.testing.assert_almost_equal(q1.rotate(v), np.dot(R, v), decimal=ALMOST_EQUAL_TOLERANCE)
self.assertTrue((q0 == q1) or (q0 == -q1)) # q1 and -q1 are equivalent rotations
def __init__(self, cx, cy, radius):
"""
Initialize instance of ArcBall with no constraint on axis of rotation
"""
self.center_x = cx
self.center_y = cy
self.radius = radius
self.v_down = PVector()
self.v_drag = PVector()
self.q_now = Quaternion()
self.q_down = Quaternion()
self.q_drag = Quaternion()
self.axis_set = [PVector(1.0, 0.0, 0.0), PVector(0.0, 1.0, 0.0), PVector(0.0, 0.0, 1.0)]
self.axis = -1
def get(self, requested_timestamp):
if len(self.history) == 0:
return None
if requested_timestamp in self.history:
match = self.history[requested_timestamp]
return TimestampedQuaternion(match.w, match.x, match.y, match.z, requested_timestamp)
else:
keys = list(self.history.keys())
keys.sort()
if keys[0] > requested_timestamp or keys[-1] < requested_timestamp:
return None
for i, key in enumerate(keys):
if key > requested_timestamp:
previous_timestamp = keys[i - 1]
next_timestamp = key
timestamp_delta = next_timestamp - previous_timestamp
requested_timestamp_offset = requested_timestamp - previous_timestamp
requested_timestamp_ratio = requested_timestamp_offset / timestamp_delta
interpolated_quaternion = Quaternion.slerp(self.history[previous_timestamp],
self.history[next_timestamp],
requested_timestamp_ratio)
match = TimestampedQuaternion(interpolated_quaternion.w,
interpolated_quaternion.x,
interpolated_quaternion.y,
interpolated_quaternion.z, requested_timestamp)
match.setInterpolated(True)
return match
return None
def test_differentiation(self):
q = Quaternion.random()
omega = np.random.uniform(-1, 1, 3) # Random angular velocity
q_dash = 0.5 * q * Quaternion(vector=omega)
self.assertEqual(q_dash, q.derivative(omega))
def test_quaternion_rotation_angle(self):
''' Test generating rotation angle from quaternion. '''
# First construct any random unit quaternion. Not uniform.
s = 2*random.random() - 1.
p1 = (2. - 2*np.abs(s))*random.random() - (1. - np.abs(s))
p2 = ((2. - 2.*np.abs(s) - 2.*np.abs(p1))*random.random() -
(1. - np.abs(s) - np.abs(p1)))
p3 = np.sqrt(1. - s**2 - p1**2 - p2**2)
theta = Quaternion(np.array([s, p1, p2, p3]))
rotation_angle = theta.rotation_angle()
phi = Quaternion.from_rotation(rotation_angle)
self.assertAlmostEqual(phi.s, theta.s)
self.assertAlmostEqual(phi.p[0], theta.p[0])
self.assertAlmostEqual(phi.p[1], theta.p[1])
self.assertAlmostEqual(phi.p[2], theta.p[2])
def test_quaternion_inverse(self):
'''Test that the quaternion inverse works.'''
# First construct any random unit quaternion. Not uniform.
s = 2*random.random() - 1.
p1 = (2. - 2*np.abs(s))*random.random() - (1. - np.abs(s))
p2 = ((2. - 2.*np.abs(s) - 2.*np.abs(p1))*random.random() -
(1. - np.abs(s) - np.abs(p1)))
p3 = np.sqrt(1. - s**2 - p1**2 - p2**2)
theta = Quaternion(np.array([s, p1, p2, p3]))
theta_inv = theta.inverse()
identity = theta*theta_inv
self.assertAlmostEqual(identity.s, 1.0)
self.assertAlmostEqual(identity.p[0], 0.0)
self.assertAlmostEqual(identity.p[1], 0.0)
self.assertAlmostEqual(identity.p[2], 0.0)
class ComplimentaryFilter:
def __init__(self, k=0.5):
self.q = Quaternion(1, 0, 0, 0)
self.t = 0
self.k = k
def update(self, msr):
t = msr['t']
dt = t - self.t
self.t = t
w = Quaternion(0, msr['gyro.x'], msr['gyro.y'], msr['gyro.z'])
q_fqa = fqa(msr)
dq_static = q_fqa.add((self.q.scalar_multiply(-1))).scalar_multiply(1/dt)
dq_dynamic = w.multiply(self.q).scalar_multiply(0.5)
dq = dq_static.scalar_multiply(self.k).add(dq_dynamic.scalar_multiply(1 - self.k))
self.q = self.q.add(dq.scalar_multiply(dt))
self.q = Quaternion.normalize(self.q)
def test_init_copy(self):
q1 = Quaternion.random()
q2 = Quaternion(q1)
self.assertIsInstance(q2, Quaternion)
self.assertEqual(q2, q1)
with self.assertRaises(TypeError):
q3 = Quaternion(None)
with self.assertRaises(ValueError):
q4 = Quaternion("String")
def test_interpolate(self):
q1 = Quaternion(axis=[1, 0, 0], angle=0.0)
q2 = Quaternion(axis=[1, 0, 0], angle=2*pi/3)
num_intermediates = 3
base = pi/6
list1 = list(Quaternion.intermediates(q1, q2, num_intermediates, include_endpoints=False))
list2 = list(Quaternion.intermediates(q1, q2, num_intermediates, include_endpoints=True))
self.assertEqual(len(list1), num_intermediates)
self.assertEqual(len(list2), num_intermediates+2)
self.assertEqual(list1[0], list2[1])
self.assertEqual(list1[1], list2[2])
self.assertEqual(list1[2], list2[3])
self.assertEqual(list2[0], q1)
self.assertEqual(list2[1], Quaternion(axis=[1, 0, 0], angle=base))
self.assertEqual(list2[2], Quaternion(axis=[1, 0, 0], angle=2*base))
self.assertEqual(list2[3], Quaternion(axis=[1, 0, 0], angle=3*base))
self.assertEqual(list2[4], q2)
def estimate_divergence(self):
'''
Estimate the divergence term with a deterministic
finite difference approach. This is for a single quaternion.
'''
delta = 1e-6
div_term = np.zeros(3)
for k in range(3):
omega = np.zeros(3)
omega[k] = 1.
quaternion_dt = Quaternion.from_rotation(omega*delta/2.)
quaternion_tilde_1 = quaternion_dt*self.orientation[0]
quaternion_dt = Quaternion.from_rotation(-1.*omega*delta/2.)
quaternion_tilde_2 = quaternion_dt*self.orientation[0]
div_term += np.inner((self.mobility([quaternion_tilde_1]) -
self.mobility([quaternion_tilde_2])),
omega/delta)
return div_term
def test_conjugate(self):
a, b, c, d = randomElements()
q1 = Quaternion(a, b, c, d)
q2 = Quaternion.random()
self.assertEqual(q1.conjugate(), Quaternion(a, -b, -c, -d))
self.assertEqual((q1 * q2).conjugate(), q2.conjugate() * q1.conjugate())
self.assertEqual((q1 + q1.conjugate()) / 2, Quaternion(scalar=q1.scalar()))
self.assertEqual((q1 - q1.conjugate()) / 2, Quaternion(vector=q1.vector()))
def test_quaternion_from_rotation(self): ''' Test that we correctly create a quaternion from an angle ''' # Generate a random rotation vector phi = np.random.rand(3) phi_norm = np.linalg.norm(phi) theta = Quaternion.from_rotation(phi) self.assertAlmostEqual(theta.entries[0], np.cos(phi_norm/2.)) self.assertAlmostEqual(theta.entries[1], np.sin(phi_norm/2.)*phi[0]/phi_norm) self.assertAlmostEqual(theta.entries[2], np.sin(phi_norm/2.)*phi[1]/phi_norm) self.assertAlmostEqual(theta.entries[3], np.sin(phi_norm/2.)*phi[2]/phi_norm)
def test_quaternion_rotation_matrix(self):
''' Test that we create the correct rotation matrix for a quaternion. '''
# First construct any random unit quaternion. Not uniform.
s = 2*random.random() - 1.
p1 = (2. - 2*np.abs(s))*random.random() - (1. - np.abs(s))
p2 = ((2. - 2.*np.abs(s) - 2.*np.abs(p1))*random.random() -
(1. - np.abs(s) - np.abs(p1)))
p3 = np.sqrt(1. - s**2 - p1**2 - p2**2)
theta = Quaternion(np.array([s, p1, p2, p3]))
R = theta.rotation_matrix()
self.assertAlmostEqual(R[0][0], 2.*(theta.s**2 + theta.p[0]**2 - 0.5))
self.assertAlmostEqual(R[0][1], 2.*(theta.p[0]*theta.p[1] -
theta.s*theta.p[2]))
self.assertAlmostEqual(R[1][0], 2.*(theta.p[0]*theta.p[1] +
theta.s*theta.p[2]))
self.assertAlmostEqual(R[1][1], 2.*(theta.s**2 + theta.p[1]**2 - 0.5))
self.assertAlmostEqual(R[2][2], 2.*(theta.s**2 + theta.p[2]**2 - 0.5))
self.assertAlmostEqual(R[2][0], 2.*(theta.p[0]*theta.p[2] - theta.s*theta.p[1]))
def additive_em_time_step(self, dt):
'''
Take a simple Euler Maruyama step assuming that the mobility is
constant. This for testing and debugging. We also use it to make sure
that we need the drift for the correct distribution, etc.
'''
if self.has_location:
mobility = self.mobility(self.location, self.orientation)
mobility_half = np.linalg.cholesky(mobility)
noise = np.random.normal(0.0, 1.0, self.dim*6)
force = self.force_calculator(self.location, self.orientation)
torque = self.torque_calculator(self.location, self.orientation)
velocity_and_omega = (np.dot(mobility, np.concatenate([force, torque])) +
np.sqrt(2.0*self.kT/dt)*
np.dot(mobility_half, noise))
velocity = velocity_and_omega[0:(3*self.dim)]
omega = velocity_and_omega[(3*self.dim):(6*self.dim)]
new_location = self.location + dt*velocity
new_orientation = []
for i in range(self.dim):
quaternion_dt = Quaternion.from_rotation((omega[(i*3):(i*3+3)])*dt)
new_orientation.append(quaternion_dt*self.orientation[i])
if self.check_new_state(new_location, new_orientation):
self.successes += 1
self.orientation = new_orientation
self.location = new_location
else:
mobility = self.mobility(self.orientation)
mobility_half = np.linalg.cholesky(mobility)
torque = self.torque_calculator(self.orientation)
noise = np.random.normal(0.0, 1.0, self.dim*3)
omega = (np.dot(mobility, torque) +
np.sqrt(2.0*self.kT/dt)*np.dot(mobility_half, noise))
new_orientation = []
for i in range(self.dim):
quaternion_dt = Quaternion.from_rotation((omega[(i*3):(i*3+3)])*dt)
new_orientation.append(quaternion_dt*self.orientation[i])
if self.check_new_state(None, new_orientation):
self.successes += 1
self.orientation = new_orientation
def test_divide_by_scalar(self):
a, b, c, d = randomElements()
q1 = Quaternion(a, b, c, d)
for s in [30.0, 0.3, -2, -4.7]:
q2 = Quaternion(a/s, b/s, c/s, d/s)
q3 = q1
self.assertEqual(q1 / s, q2)
if q1:
self.assertEqual(s / q1, q2.inverse())
else:
with self.assertRaises(ZeroDivisionError):
s / q1
q3 /= repr(s)
self.assertEqual(q3, q2)
with self.assertRaises(ZeroDivisionError):
q4 = q1 / 0.0
with self.assertRaises(TypeError):
q4 = q1 / None
with self.assertRaises(ValueError):
q4 = q1 / 's'
def test_output_of_components(self):
a, b, c, d = randomElements()
q = Quaternion(a, b, c, d)
# Test scalar
self.assertEqual(q.scalar(), a)
self.assertEqual(q.real(), a)
# Test vector
self.assertTrue(np.array_equal(q.vector(), [b, c, d]))
self.assertTrue(np.array_equal(q.imaginary(), [b, c, d]))
self.assertEqual(tuple(q.vector()), (b, c, d))
self.assertEqual(list(q.imaginary()), [b, c, d])
def test_power(self):
q1 = Quaternion.random()
q2 = Quaternion(q1)
self.assertEqual(q1 ** 0, Quaternion())
self.assertEqual(q1 ** 1, q1)
q2 **= 4
self.assertEqual(q2, q1 * q1 * q1 * q1)
self.assertEqual((q1 ** 0.5) * (q1 ** 0.5), q1)
self.assertEqual(q1 ** -1, q1.inverse())
self.assertEqual(4 ** Quaternion(2), Quaternion(16))
with self.assertRaises(TypeError):
q1 ** None
with self.assertRaises(ValueError):
q1 ** 's'
def validate_axis_angle(self, axis, angle):
def wrap_angle(theta):
""" Wrap any angle to lie between -pi and pi
Odd multiples of pi are wrapped to +pi (as opposed to -pi)
"""
result = ((theta + pi) % (2*pi)) - pi
if result == -pi: result = pi
return result
theta = wrap_angle(angle)
v = axis
q = Quaternion(angle=theta, axis=v)
v_ = q.axis()
theta_ = q.angle()
if theta == 0.0: # axis is irrelevant (check defaults to x=y=z)
np.testing.assert_almost_equal(theta_, 0.0, decimal=ALMOST_EQUAL_TOLERANCE)
np.testing.assert_almost_equal(v_, np.zeros(3), decimal=ALMOST_EQUAL_TOLERANCE)
return
elif abs(theta) == pi: # rotation in either direction is equivalent
self.assertTrue(
np.isclose(theta, pi) or np.isclose(theta, -pi)
and
np.isclose(v, v_).all() or np.isclose(v, -v_).all()
)
else:
self.assertTrue(
np.isclose(theta, theta_) and np.isclose(v, v_).all()
or
np.isclose(theta, -theta_) and np.isclose(v, -v_).all()
)
# Ensure the returned axis is a unit vector
np.testing.assert_almost_equal(np.linalg.norm(v_), 1.0, decimal=ALMOST_EQUAL_TOLERANCE)
def test_init_from_explicit_rotation_params(self):
vx = random()
vy = random()
vz = random()
theta = random() * 2.0 * pi
v1 = (vx, vy, vz) # tuple format
v2 = [vx, vy, vz] # list format
v3 = np.array(v2) # array format
q1 = Quaternion(axis=v1, angle=theta)
q2 = Quaternion(axis=v2, angle=theta)
q3 = Quaternion(axis=v3, angle=theta)
# normalise v to a unit vector
v3 = v3 / np.linalg.norm(v3)
q4 = Quaternion(angle=theta, axis=v3)
# Construct the true quaternion
t = theta / 2.0
a = cos(t)
b = v3[0] * sin(t)
c = v3[1] * sin(t)
d = v3[2] * sin(t)
truth = Quaternion(a, b, c, d)
self.assertEqual(q1, truth)
self.assertEqual(q2, truth)
self.assertEqual(q3, truth)
self.assertEqual(q4, truth)
# Result should be a versor (Unit Quaternion)
self.assertAlmostEqual(q1.norm(), 1.0, ALMOST_EQUAL_TOLERANCE)
self.assertAlmostEqual(q2.norm(), 1.0, ALMOST_EQUAL_TOLERANCE)
self.assertAlmostEqual(q3.norm(), 1.0, ALMOST_EQUAL_TOLERANCE)
self.assertAlmostEqual(q4.norm(), 1.0, ALMOST_EQUAL_TOLERANCE)
with self.assertRaises(ValueError):
q = Quaternion(angle=theta)
q = Quaternion(axis=v1)
q = Quaternion(axis=[b, c], angle=theta)
q = Quaternion(axis=(b, c, d, d), angle=theta)
with self.assertRaises(ZeroDivisionError):
q = Quaternion(axis=[0., 0., 0.], angle=theta)
def test_normalisation(self): # normalise to unit quaternion
r = randomElements()
q1 = Quaternion(*r)
v = q1.unit()
n = q1.normalised()
if q1 == Quaternion(0): # small chance with random generation
return # a 0 quaternion does not normalise
# Test normalised objects are unit quaternions
np.testing.assert_almost_equal(v.q, q1.elements() / q1.norm(), decimal=ALMOST_EQUAL_TOLERANCE)
np.testing.assert_almost_equal(n.q, q1.elements() / q1.norm(), decimal=ALMOST_EQUAL_TOLERANCE)
self.assertAlmostEqual(v.norm(), 1.0, ALMOST_EQUAL_TOLERANCE)
self.assertAlmostEqual(n.norm(), 1.0, ALMOST_EQUAL_TOLERANCE)
# Test axis and angle remain the same
np.testing.assert_almost_equal(q1.axis(), v.axis(), decimal=ALMOST_EQUAL_TOLERANCE)
np.testing.assert_almost_equal(q1.axis(), n.axis(), decimal=ALMOST_EQUAL_TOLERANCE)
self.assertAlmostEqual(q1.angle(), v.angle(), ALMOST_EQUAL_TOLERANCE)
self.assertAlmostEqual(q1.angle(), n.angle(), ALMOST_EQUAL_TOLERANCE)
# Test special case where q is zero
q2 = Quaternion(0)
self.assertEqual(q2, q2.normalised())
def test_norm(self):
r = randomElements()
q1 = Quaternion(*r)
q2 = Quaternion.random()
self.assertEqual(q1.norm(), np.linalg.norm(np.array(r)))
self.assertEqual(q1.magnitude(), np.linalg.norm(np.array(r)))
# Multiplicative norm
self.assertAlmostEqual((q1 * q2).norm(), q1.norm() * q2.norm(), ALMOST_EQUAL_TOLERANCE)
# Scaled norm
for s in [30.0, 0.3, -2, -4.7]:
self.assertAlmostEqual((q1 * s).norm(), q1.norm() * abs(s), ALMOST_EQUAL_TOLERANCE)
class RigidTransform(object):
def __init__(self, rotation_quat, translation_vec):
self.quat = Quaternion(rotation_quat)
self.tvec = numpy.array(translation_vec)
def inverse(self):
""" returns a new RigidTransform that corresponds to the inverse of this one """
qinv = self.quat.inverse()
return RigidTransform(qinv, qinv.rotate(- self.tvec))
def interpolate(self, other_transform, this_weight):
assert this_weight >= 0 and this_weight <= 1
t = self.tvec * this_weight + other_transform.tvec * (1 - this_weight)
r = self.quat.interpolate(other_transform.quat, this_weight)
return RigidTransform(r, t)
def __mul__(self, other):
if isinstance(other, RigidTransform):
t = self.quat.rotate(other.tvec) + self.tvec
r = self.quat * other.quat
return RigidTransform(r, t)
else:
olen = len(other)
if olen == 3:
r = numpy.array(self.quat.rotate(other))
return r + self.tvec
elif olen == 4:
return np.dot(self.to_homogeneous_matrix(), other)
else:
raise ValueError()
def to_homogeneous_matrix(self):
result = self.quat.to_matrix_homogeneous()
result.A[:3, 3] = self.tvec
return result
def to_roll_pitch_yaw_x_y_z(self):
r, p, y = self.quat.to_roll_pitch_yaw()
return numpy.array((r, p, y, self.tvec[0], self.tvec[1], self.tvec[2]))
@staticmethod
def from_roll_pitch_yaw_x_y_z(r, p, yaw, x, y, z):
q = Quaternion.from_roll_pitch_yaw(r, p, yaw)
return RigidTransform(q, (x, y, z))
def quaternion(self):
return self.quat
def translation(self):
return self.tvec
@staticmethod
def identity():
return RigidTransform((1, 0, 0, 0), (0, 0, 0))
def processTelemetryMsg(self, msg):
# this version only creates a normalized message
if(msg.state & 1): # only process if coaster is in play
if(False):
# code here is non-normalized (real) translation and rotation messages
quat = Quaternion(msg.quatX,msg.quatY,msg.quatZ,msg.quatW)
pitch = degrees(quat.toPitchFromYUp())
yaw = degrees(quat.toYawFromYUp())
roll = degrees(quat.toRollFromYUp())
else: #normalize
quat = Quaternion(msg.quatX,msg.quatY,msg.quatZ,msg.quatW)
pitch = quat.toPitchFromYUp() / pi
yaw = quat.toYawFromYUp() / pi
roll = quat.toRollFromYUp() / pi
x = max(min(1.0, msg.gForceX),-1) # clamp the value
y = max(min(1.0, msg.gForceY),-1)
z = max(min(1.0, msg.gForceZ),-1)
data = [x,y,z,roll, pitch, yaw]
formattedData = [ '%.3f' % elem for elem in data ]
self.coaster_msg_q.put(formattedData)
def newUpdateStateFunction(self, data):
if self.up == True:
rotationData = data['rot_vector']['data']
linAccelerationData = data['lin_accel']['data']
accelerometerData = data['accel']['data']
magData = data['mag']['data']
gyroData = data['gyro']['data']
#print(linAccelerationData)
for index, i in enumerate(rotationData, start=0):
self.currentTime = i[0]
if self.up == False:
break
timeDifference = (self.currentTime - self.previousTime) / 1000
self.currentQuaternion = pyrr.quaternion.create(
i[1][0], i[1][1], i[1][2], i[1][3])
quat = self.currentQuaternion
if self.currentTime != self.previousTime and self.previousTime != 0:
if index < len(gyroData) and index < len(
accelerometerData) and index < len(magData):
self.madgwickahrs.update(gyroData[index][1],
accelerometerData[index][1],
magData[index][1],
timeDifference)
#elif index < len(gyroData) and index < len(accelerometerData):
#self.madgwickahrs.update_imu(gyroData[index][1],accelerometerData[index][1] ,timeDifference)
#self.currentQuaternion=pyrr.Quaternion.from_eulers(self.madgwickahrs.quaternion.to_euler123())
#c= self.currentQuaternion-self.initialQuaternion
a = pyrr.matrix33.create_from_inverse_of_quaternion(
self.currentQuaternion)
b = pyrr.matrix33.create_from_quaternion(
self.initialQuaternion)
adjustedAccelerations1 = pyrr.matrix33.apply_to_vector(
a, linAccelerationData[index][1])
adjustedAccelerations = pyrr.matrix33.apply_to_vector(
b, adjustedAccelerations1)
#adjustedAccelerations=adjustedAccelerations- numpy.array([ 1.14548891e-05 , 3.24514926e-05, -3.21555242e-04])
#filtered=self.highpass(adjustedAccelerations)
#for i in range(0,len(adjustedAccelerations)):
# if abs(adjustedAccelerations[i]) < 0.01:
# adjustedAccelerations[i]=0
kalmanx = self.kalmanX.update(adjustedAccelerations[0])
kalmany = self.kalmanY.update(adjustedAccelerations[1])
kalmanz = self.kalmanZ.update(adjustedAccelerations[2])
print("x" + str(kalmanx))
print("y" + str(kalmany))
print("z" + str(kalmanz))
#adjustedAccelerations= [kalmanx[2],kalmany[2],kalmanz[2]]
#adjustedAccelerations= self.lowPass(adjustedAccelerations)
#for i in range(0,len(adjustedAccelerations)):
# if abs(adjustedAccelerations[i]) < 0.01:
# adjustedAccelerations[i]=0
#adjustedVelocities= numpy.add(self.currentVelocities,numpy.add(adjustedAccelerations * timeDifference, numpy.array(numpy.add(adjustedAccelerations,self.currentAccelerations*-1)*timeDifference *timeDifference * 0.5)))
#adjustedVelocities=[kalmanx[1],kalmany[1],kalmanz[1]]
#adjustedAccelerations=numpy.array([self.kalmanX.update(adjustedAccelerations[0]),self.kalmanY.update(adjustedAccelerations[1]),self.kalmanZ.update(adjustedAccelerations[2])])
#print("quaternion from madgwick"+str(self.madgwickahrs.quaternion._get_q()))
#print("quaternion measured"+str(self.currentQuaternion))
for i in range(0, len(adjustedAccelerations)):
if abs(adjustedAccelerations[i]) < 0.02:
adjustedAccelerations[i] = 0
#self.average= (self.average * self.num + adjustedAccelerations)/( self.num + 1)
#self.num+=1
interm1 = numpy.add(
adjustedAccelerations,
numpy.negative(
self.currentAccelerations)) / timeDifference
interm2 = numpy.array(
interm1) * timeDifference * timeDifference * 0.5
#interm2=0
interm3 = numpy.array(
self.currentAccelerations) * timeDifference
interm4 = numpy.add(interm2, interm3)
adjustedVelocities = numpy.add(self.currentVelocities,
interm4)
for k in range(0, len(adjustedVelocities)):
if abs(adjustedVelocities[k]) < 0.1:
adjustedVelocities[k] = 0.0
interm5 = 0.5 * interm3 * timeDifference
interm6 = (interm2 * timeDifference) / 3
#self.positionVectors=numpy.add(self.positionVectors,interm5)
#self.positionVectors=numpy.add(self.positionVectors,interm6)
#adjustedVelocities=numpy.array([kalmanx[1],kalmany[1],kalmanz[1]])
self.positionVectors = numpy.add(
self.positionVectors,
numpy.array(adjustedVelocities * timeDifference))
self.previousTime = self.currentTime
self.currentAccelerations = adjustedAccelerations
self.currentVelocities = adjustedVelocities
self.previousMeasurement = linAccelerationData[index][1]
print(str(timeDifference) + " s")
print(str(self.num))
#print("Average"+str(self.average))
print("Adjusted Acceleration" + str(adjustedAccelerations))
#print("Filtered Acceleration"+ str(filtered))
print("unadjusted acceleration" +
str(data['lin_accel']['data'][index][1]))
print("rotation vectors" + str(quat))
print("madgwick vectors" +
str(self.madgwickahrs.quaternion.q))
print("Adjusted Velocities" + str(adjustedVelocities))
print("Distance " + str(self.positionVectors))
# for j in range(0,len(self.positionVectors)):
# if abs(self.positionVectors[j])>1:
# sleep(10)
#print("euler", Quaternion(self.currentQuaternion).to_euler123()[0]*180/3.14)
print("__________________________________________________")
elif self.previousTime == 0:
self.previousTime = self.currentTime
#self.currentAccelerations=linAccelerationData[index][1]
#self.madgwickahrs=MadgwickAHRS(quaternion=Quaternion(i[1][0],i[1][1],i[1][2],i[1][3]))
self.madgwickahrs = MadgwickAHRS(
quaternion=Quaternion(self.currentQuaternion))
self.initialQuaternion = self.currentQuaternion
self.previousMeasurement = linAccelerationData[index][1]
self.kalmanX = Kalman_Accel(0)
self.kalmanY = Kalman_Accel(0)
self.kalmanZ = Kalman_Accel(0)
else:
self.currentAccelerations = numpy.zeros(3)
self.currentVelocities = numpy.zeros(3)
self.previousAccelerations = numpy.zeros(3)
print("Not Moving")
print("Distance " + str(self.positionVectors))
if self.previousTime:
print("rotation vectors" + str(self.currentQuaternion))
#!/usr/bin/env python
#
import quaternion
from quaternion import Quaternion
import math
import cmath
qlist = (Quaternion(+0.16, +0.32, +1.48,
+0.80), Quaternion(+1.16, +1.32, +1.48, -0.80),
Quaternion(+2.16, +0.00, -0.01,
+0.00), Quaternion(+3.16, +0.32, -1.48, -2.80),
Quaternion(+4.16, -2.32, +1.48,
+0.80), Quaternion(+0.16, -0.32, +1.48, -0.80),
Quaternion(+6.16, -3.32, -1.48,
+3.80), Quaternion(+7.16, -0.32, -3.48, -0.80),
Quaternion(+0.16, +0.32, +1.48, +4.80))
def qfoo(q):
""" Rotate axis by +tau/3 about axis (1,1,1) """
return Quaternion(q.w, q.y, q.z, q.x)
def qbar(q):
""" Rotate axis by -tau/3 about axis (1,1,1) """
return Quaternion(q.w, q.z, q.x, q.y)
def check(rfn, cfn, qfn, q):
""" compares the Quaternion function (qfn) with the equivilent
def qfoo(q):
""" Rotate axis by +tau/3 about axis (1,1,1) """
return Quaternion(q.w, q.y, q.z, q.x)
def qbar(q):
""" Rotate axis by -tau/3 about axis (1,1,1) """
return Quaternion(q.w, q.z, q.x, q.y)
def compute_mean(Y, prev_estimate):
mean = np.zeros(7)
err_vecs = np.zeros((3, Y.shape[1]))
q = Quaternion()
q.q = prev_estimate[:4]
qi = Quaternion()
Quat_part = Y[:4, :]
Omg_part = Y[4:, :]
e = Quaternion()
for _ in range(5):
for j in range(Y.shape[1]):
qi.q = Quat_part[:, j]
err_q = qi * q.inv()
err_vecs[:, j] = err_q.axis_angle()
err_mean = np.mean(err_vecs, axis=1)
e.from_axis_angle(err_mean)
q.q = (e * q).q
if np.linalg.norm(err_mean) < 0.001:
break
omg_mean = np.mean(Omg_part, axis=1)
mean[:4] = q.q
mean[4:] = omg_mean
return mean, err_vecs
def load_vicon(data_path):
# names = ['Time (s)', 'Gyroscope X (deg/s)', 'Gyroscope Y (deg/s)', 'Gyroscope Z (deg/s)', 'Accelerometer X (g)', 'Accelerometer Y (g)','Accelerometer Z (g)','Magnetometer X (uT)','Magnetometer Y (uT)','Magnetometer Z (uT)']
df = pd.read_csv(data_path, index_col=False)
# df=df[35:-35,:]
i_end = np.zeros([len(df.values[:, 0]), 3])
i_start = np.copy(i_end)
j_end = np.copy(i_end)
j_start = np.copy(i_end)
k_start = np.copy(i_end)
k_end = np.copy(i_end)
temp_mts = np.zeros(len(df.values[:, 11]))
temp_ts = np.zeros(len(df.values[:, 10]))
ref_delta_t = np.zeros(len(df.values[:, 11]))
ref_ts = np.copy(temp_ts)
temp_mts[:] = df.values[:, 11]
temp_ts[:] = df.values[:, 10]
ref_ts[:] = temp_ts[:] + temp_mts[:] * 0.001
for i in range(len(temp_ts) - 1):
tempp = temp_ts[i + 1] - temp_ts[i]
if tempp >= 0:
ref_delta_t[i] = tempp
else:
ref_delta_t[i] = (61 - temp_ts[i]) + temp_ts[i + 1]
for i in range(2, len(ref_delta_t)):
if ref_delta_t[i] > 0.5:
ref_delta_t[i] = (ref_delta_t[i - 1] + ref_delta_t[i - 2]) / 2
ref_ts = np.cumsum(ref_delta_t)
j_end[:, :] = df.values[:, 14:17] #1 +x方向8-1
j_start[:, :] = df.values[:, 49:52] #8
i_end[:, :] = df.values[:, 24:27] #3#
i_start[:, :] = df.values[:, 29:32] #4#
k_start[:, :] = df.values[:, 19:22] #2 #y 2-5
k_end[:, :] = df.values[:, 34:37] #5#
j_end = j_end[:, [0, 2, 1]]
j_start = j_start[:, [0, 2, 1]]
i_end = i_end[:, [0, 2, 1]]
i_start = i_start[:, [0, 2, 1]]
k_end = k_end[:, [0, 2, 1]]
k_start = k_start[:, [0, 2, 1]]
c_xm = mag_v(i_end - i_start)
c_ym = mag_v(j_end - j_start)
c_zm = mag_v(k_end - k_start)
Q = np.zeros([len(c_xm), 4])
tempa = np.zeros([3, 3])
for i in range(len(Q) - 2):
tempa = np.vstack((c_xm[i, :], c_ym[i, :], c_zm[i, :]))
tempa = -tempa.transpose()
Q[i, :] = quaternions.mat2quat(tempa)
# Q[:,:]=df.values[:,6:10]
# Q=Q[:,[3,0,1,2]]
Q = Q[:, [0, 1, 3, 2]]
Q[:, 0] = -Q[:, 0]
# Q=Q[:,[0,2,1,3]]
# Q[:,2]=-Q[:,2]
ref_eu = np.zeros([len(Q), 3])
# Qb=Quaternion([1,0,0,0])
# Qc=np.array([[0,0,-0.707,0.707],
# [0,0,0.707,0.707],
# [0.707,-0.707,0,0],
# [-0.707,-0.707,0,0]])
# Q=np.dot(Q,Qc)
# Qb=Quaternion(np.mean(Q[100:400,:],axis=0))
# Qb=Quaternion(0.246265,-0.0168084,0.0457804,0.967974)
# Qb=Qb.conj() #有加這樣才對 用static測試
for i in range(1, len(Q)):
tempq = Quaternion(Q[i, :])
# tempq=tempq*Qb.conj()
# Q[i,:]=tempq.__array__()
ref_eu[i, 0], ref_eu[i,
1], ref_eu[i,
2] = tempq.to_euler_angles_by_wiki()
# ref_eu[i,0],ref_eu[i,1],ref_eu[i,2]=tempq.to_euler_angles_no_Gimbal()
return ref_ts, c_xm, c_ym, c_zm, Q, ref_eu / np.pi * 180, ref_delta_t
def _mat4_rotation(w=0, x=1, y=0, z=0):
n = np.array([x, y, z])
n *= 1. / np.sqrt(1. * np.sum(n**2))
q = Quaternion(np.cos(.5 * w), *(np.sin(.5 * w) * n))
return q.toRotation4()
def local_to_global(self, vector):
vector = q * Quaternion() # todo
return vector
while True:
gyro_x = read_gyro_x(address)
gyro_y = read_gyro_y(address)
gyro_z = read_gyro_z(address)
bes_x = read_bes_x(address)
bes_y = read_bes_y(address)
bes_z = read_bes_z(address)
mag_x = read_mag_x(mag_address)
mag_y = read_mag_y(mag_address)
mag_z = read_mag_z(mag_address)
bes_arr = [bes_x, bes_y, bes_z]
gyro_arr = [gyro_x, gyro_y, gyro_z]
mag_arr = [mag_x, mag_y, mag_z]
q = Quaternion(1, 0, 0, 0)
m = MadgwickAHRS(1 / 1000, q, 1)
#print("before: ",m.quaternion.q)
MadgwickAHRS.update(m, bes_arr, gyro_arr, mag_arr)
#print("after: ",m.quaternion.q)
roll = math.atan2(
2.0 * (m.quaternion.q[2] * m.quaternion.q[3] +
m.quaternion.q[0] * m.quaternion.q[1]),
m.quaternion.q[0] * m.quaternion.q[0] - m.quaternion.q[1] *
m.quaternion.q[1] - m.quaternion.q[2] * m.quaternion.q[2] +
m.quaternion.q[3] * m.quaternion.q[3]) * (180.0 / math.pi)
pitch = math.asin(
-2.0 * (m.quaternion.q[1] * m.quaternion.q[3] -
m.quaternion.q[0] * m.quaternion.q[2])) * (180.0 / math.pi)
yaw = math.atan2(
def estimate_rot(data_num=1):
# loading the data from the IMU and Vicon
imu = io.loadmat(
os.path.join(os.path.dirname(__file__),
"imu/imuRaw" + str(data_num) + ".mat"))
imu = io.loadmat('imu/imuRaw' + str(data_num) + '.mat')
vicon = io.loadmat('vicon/viconRot' + str(data_num) + '.mat')
accel = imu['vals'][0:3, :].astype('int')
gyro = imu['vals'][3:6, :].astype('int')
T = np.shape(imu['ts'])[1]
imu_time = imu['ts']
time = vicon['ts']
rots = vicon['rots']
m = Quaternion()
quats_x = []
rot_arr = []
for i in range(len(time[0])):
r = Rotation.from_matrix(rots[:, :, i]).as_euler('xyz', degrees=False)
rot_arr.append(r)
m.from_rotm(rots[:, :, i])
quats_x.append(m.q[3])
time = np.array(time)
rotationss = np.array(rot_arr)
# Processing the raw data of the IMU
true_accel = (accel - 506) * 3300 * 9.81 / (1023 * 320)
test = (gyro[1, :] - 373.8) / (3.3 / 2.78) * np.pi / 180
test2 = (gyro[2, :] - 375.8) / (3.3 / 2.78) * np.pi / 180
test3 = (gyro[0, :] - 369.8) * 1.3 / (3.3 / 2.78) * np.pi / 180
x_angle = 0
y_angle = 0
z_angle = 0
xs = []
ys = []
zs = []
for i in range(len(imu_time[0, :]) - 1):
x_angle += test[i] * (imu_time[0, i + 1] - imu_time[0, i])
xs.append(x_angle)
y_angle += test2[i] * (imu_time[0, i + 1] - imu_time[0, i])
ys.append(y_angle)
z_angle += test3[i] * (imu_time[0, i + 1] - imu_time[0, i])
zs.append(z_angle)
x_accel = -1 * true_accel[0, :]
y_accel = -1 * true_accel[1, :]
z_accel = true_accel[2, :]
measurements_cal = np.stack(
(x_accel, y_accel, z_accel, test, test2, test3))
pitch_angs = []
roll_angs = []
for i in range(len(imu_time[0])):
pitch_ang = math.atan2(-1 * x_accel[i],
np.sqrt(y_accel[i]**2 + z_accel[i]**2))
roll_ang = math.atan2(y_accel[i], z_accel[i])
pitch_angs.append(pitch_ang)
roll_angs.append(roll_ang)
init_state = np.zeros((7))
P = 1 * np.identity(6)
Q = 500 * np.identity(6)
R = 100 * np.identity(6)
init_omegas = np.array([0, 0, 0])
init_quat = Quaternion()
init_quat.q = np.array([1, 0, 0, 0])
init_state[:4] = init_quat.q
init_state[4:] = init_omegas
state_estimate = init_state
phi = []
theta = []
psi = []
quat_for_plot = Quaternion()
for i in range(len(imu_time[0, :])):
if i == len(imu_time[0, :]) - 1:
delta_t = imu_time[0, -1] - imu_time[0, -2]
else:
delta_t = imu_time[0, i + 1] - imu_time[0, i]
W = get_W(P, Q)
X = get_X(state_estimate, W)
Y = process_model(X, delta_t)
mean_minus, err_vecs = compute_mean(Y, state_estimate)
W_dash = get_W_dash(Y, err_vecs, mean_minus)
Pk_minus = get_covariance(W_dash)
Z = measurement_model(Y)
zk_minus = compute_z_mean(Z)
Z_dash = get_Z_dash(Z, zk_minus)
nu_k = measurements_cal[:, i] - zk_minus
P_zz = get_covariance(Z_dash)
P_vv = P_zz + R
P_xz = get_cross_covariance(W_dash, Z_dash)
K = np.dot(P_xz, np.linalg.inv(P_vv))
state_estimate = get_new_estimate(mean_minus, K, nu_k)
P = Pk_minus - np.dot(K, np.dot(P_vv, K.T))
quat_for_plot.q = state_estimate[:4]
euls = quat_for_plot.euler_angles()
phi.append(euls[0])
theta.append(euls[1])
psi.append(euls[2])
phi = np.asarray(phi)
theta = np.asarray(theta)
psi = np.asarray(psi)
# Comparing the ground truth orientation and orientation produced by the Unscented Kalman Filter
(fig, axes) = plt.subplots(nrows=3,
ncols=1,
sharex=True,
num='Orientation given by UKF')
ax = axes[0]
ax.plot(time[0, :], rotationss[:, 0], 'r', imu_time[0, :], phi, 'b')
ax.legend(("vicon", "imu"))
ax.set_ylabel('orientation x')
ax.grid('major')
ax = axes[1]
ax.plot(time[0, :], rotationss[:, 1], 'r', imu_time[0, :], theta, 'b')
ax.legend(("vicon", "imu"))
ax.set_ylabel('orientation y')
ax.grid('major')
ax = axes[2]
ax.plot(time[0, :], rotationss[:, 2], 'r', imu_time[0, :], psi, 'b')
ax.legend(("vicon", "imu"))
ax.set_ylabel('orientation z')
ax.grid('major')
plt.show()
# True orientation of the drone given by the Vicon
(fig, axes) = plt.subplots(nrows=3,
ncols=1,
sharex=True,
num='Ground Truth Orientation vs Time')
ax = axes[0]
ax.plot(time[0, :], rotationss[:, 0], 'r')
ax.set_ylabel('rotation x')
ax.grid('major')
ax = axes[1]
ax.plot(time[0, :], rotationss[:, 1], 'r')
ax.set_ylabel('rotation y')
ax.grid('major')
ax = axes[2]
ax.plot(time[0, :], rotationss[:, 2], 'r')
ax.set_ylabel('rotation z')
ax.grid('major')
plt.show()
# Orientation as tracked only by the gyroscope
(fig, axes) = plt.subplots(nrows=3,
ncols=1,
sharex=True,
num='Orientation given by Gyroscope')
ax = axes[0]
ax.plot(time[0, :], rotationss[:, 0], 'r', imu_time[0, :-1], xs, 'b')
ax.legend(("vicon", "imu"))
ax.set_ylabel('orientation x')
ax.grid('major')
ax = axes[1]
ax.plot(time[0, :], rotationss[:, 1], 'r', imu_time[0, :-1], ys, 'b')
ax.legend(("vicon", "imu"))
ax.set_ylabel('orientation y')
ax.grid('major')
ax = axes[2]
ax.plot(time[0, :], rotationss[:, 2], 'r', imu_time[0, :-1], zs, 'b')
ax.legend(("vicon", "imu"))
ax.set_ylabel('orientation z')
ax.grid('major')
plt.show()
return imu_time, phi, theta, psi
