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