def _quaternion_from_acc_mag(self, v_acc, v_mag):
v_down = v_acc * -1.0 #(sign change depends on sensor design?)
v_east = np.cross(v_down, v_mag)
v_north = np.cross(v_east, v_down)
#Normalise the vectors...
v_down /= sqrt((v_down ** 2).sum())
v_east /= sqrt((v_east ** 2).sum())
v_north /= sqrt((v_north ** 2).sum())
return quaternion_from_rotation_matrix_rows(v_north, v_east, v_down)
def _quaternion_from_acc_mag(self, v_acc, v_mag):
v_down = v_acc * -1.0 #(sign change depends on sensor design?)
v_east = np.cross(v_down, v_mag)
v_north = np.cross(v_east, v_down)
#Normalise the vectors...
v_down /= sqrt((v_down**2).sum())
v_east /= sqrt((v_east**2).sum())
v_north /= sqrt((v_north**2).sum())
return quaternion_from_rotation_matrix_rows(v_north, v_east, v_down)
def update(self):
"""Read the current sensor values & store them for smoothing. No return value."""
t = time()
delta_t = t - self._last_gyro_time
if delta_t < 0.020:
#Want at least 20ms of data
return
v_gyro = np.array(self.read_gyro(), np.float)
v_acc = np.array(self.read_accel(), np.float)
v_mag = np.array(self.read_compass(), np.float)
self._last_gyro_time = t
#Gyro only quaternion calculation (expected to drift)
rot_mag = sqrt(sum(v_gyro**2))
v_rotation = v_gyro / rot_mag
q_rotation = quaternion_from_axis_angle(v_rotation, rot_mag * delta_t)
self._current_gyro_only_q = quaternion_multiply(
self._current_gyro_only_q, q_rotation)
self._current_hybrid_orientation_q = quaternion_multiply(
self._current_hybrid_orientation_q, q_rotation)
if abs(sqrt(sum(v_acc**2)) - 1) < 0.3:
#Approx 1g, should be stationary, and can use this for down axis...
v_down = v_acc * -1.0
v_east = np.cross(v_down, v_mag)
v_north = np.cross(v_east, v_down)
v_down /= sqrt((v_down**2).sum())
v_east /= sqrt((v_east**2).sum())
v_north /= sqrt((v_north**2).sum())
#Complementary Filter
#Combine (noisy) orientation from acc/mag, 2%
#with (drifting) orientation from gyro, 98%
q_mag_acc = quaternion_from_rotation_matrix_rows(
v_north, v_east, v_down)
self._current_hybrid_orientation_q = tuple(
0.02 * a + 0.98 * b
for a, b in zip(q_mag_acc, self._current_hybrid_orientation_q))
#1st order approximation of quaternion for this rotation (v_rotation, delta_t)
#using small angle approximation, cos(theta) = 1, sin(theta) = theta
#w, x, y, z = (1, v_rotation[0] * delta_t/2, v_rotation[1] *delta_t/2, v_rotation[2] * delta_t/2)
#q_rotation = (1, v_rotation[0] * delta_t/2, v_rotation[1] *delta_t/2, v_rotation[2] * delta_t/2)
return
def update(self):
"""Read the current sensor values & store them for smoothing. No return value."""
t = time()
delta_t = t - self._last_gyro_time
if delta_t < 0.020:
#Want at least 20ms of data
return
v_gyro = np.array(self.read_gyro(), np.float)
v_acc = np.array(self.read_accel(), np.float)
v_mag = np.array(self.read_compass(), np.float)
self._last_gyro_time = t
#Gyro only quaternion calculation (expected to drift)
rot_mag = sqrt(sum(v_gyro**2))
v_rotation = v_gyro / rot_mag
q_rotation = quaternion_from_axis_angle(v_rotation, rot_mag * delta_t)
self._current_gyro_only_q = quaternion_multiply(self._current_gyro_only_q, q_rotation)
self._current_hybrid_orientation_q = quaternion_multiply(self._current_hybrid_orientation_q, q_rotation)
if abs(sqrt(sum(v_acc**2)) - 1) < 0.3:
#Approx 1g, should be stationary, and can use this for down axis...
v_down = v_acc * -1.0
v_east = np.cross(v_down, v_mag)
v_north = np.cross(v_east, v_down)
v_down /= sqrt((v_down**2).sum())
v_east /= sqrt((v_east**2).sum())
v_north /= sqrt((v_north**2).sum())
#Complementary Filter
#Combine (noisy) orientation from acc/mag, 2%
#with (drifting) orientation from gyro, 98%
q_mag_acc = quaternion_from_rotation_matrix_rows(v_north, v_east, v_down)
self._current_hybrid_orientation_q = tuple(0.02*a + 0.98*b for a, b in
zip(q_mag_acc, self._current_hybrid_orientation_q))
#1st order approximation of quaternion for this rotation (v_rotation, delta_t)
#using small angle approximation, cos(theta) = 1, sin(theta) = theta
#w, x, y, z = (1, v_rotation[0] * delta_t/2, v_rotation[1] *delta_t/2, v_rotation[2] * delta_t/2)
#q_rotation = (1, v_rotation[0] * delta_t/2, v_rotation[1] *delta_t/2, v_rotation[2] * delta_t/2)
return
