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