def integrate_all(self):
"""go through each sample and integrate to find orientation. Assumes sample N contains change between N and N-1
Returns:
(np.ndarray, np.ndarray): tuple (time_list, quaternion orientation array)
"""
if self.already_integrated:
return (self.time_list, self.orientation_list)
# temp lists to save data
temp_orientation_list = []
temp_time_list = []
temp_orientation_list.append(np.copy(self.orientation))
temp_time_list.append(self.data[0][0] - 1)
for i in range(self.num_data_points):
# angular velocity vector
omega = self.data[i][1:]
# get current time
this_time = self.data[i][0]
# symmetrical dt calculation. Should give slightly better results when missing data
delta_time = 1 # frame
# Only calculate if angular velocity is present
if np.any(omega):
# calculate rotation quaternion
delta_q = self.rate_to_quat(omega, delta_time)
# rotate orientation by this quaternion
self.orientation = quat.quaternion_multiply(self.orientation, delta_q) # Maybe change order
self.orientation = quat.normalize(self.orientation)
temp_orientation_list.append(np.copy(self.orientation))
temp_time_list.append(this_time)
self.orientation_list = np.array(temp_orientation_list)
self.time_list = np.array(temp_time_list)
self.already_integrated = True
return (self.time_list, self.orientation_list)
def integrate_all(self):
"""go through each gyro sample and integrate to find orientation
Returns:
(np.ndarray, np.ndarray): tuple (time_list, quaternion orientation array)
"""
if self.already_integrated:
return (self.time_list, self.orientation_list)
# temp lists to save data
temp_orientation_list = []
temp_time_list = []
for i in range(self.num_data_points):
# angular velocity vector
omega = self.data[i][1:]
# get current and adjecent times
last_time = self.data[i - 1][0] if i > 0 else self.data[i][0]
this_time = self.data[i][0]
next_time = self.data[
i + 1][0] if i < self.num_data_points - 1 else self.data[i][0]
# symmetrical dt calculation. Should give slightly better results when missing data
delta_time = (next_time - last_time) / 2
# Only calculate if angular velocity is present
if np.any(omega):
# calculate rotation quaternion
delta_q = self.rate_to_quart(omega, delta_time)
# rotate orientation by this quaternion
self.orientation = quart.quaternion_multiply(
self.orientation, delta_q) # Maybe change order
self.orientation = quart.normalize(self.orientation)
temp_orientation_list.append(np.copy(self.orientation))
temp_time_list.append(this_time)
self.orientation_list = np.array(temp_orientation_list)
self.time_list = np.array(temp_time_list)
self.already_integrated = True
return (self.time_list, self.orientation_list)
def integrate_all(self, use_acc=False):
"""go through each gyro sample and integrate to find orientation
Returns:
(np.ndarray, np.ndarray): tuple (time_list, quaternion orientation array)
"""
if self.already_integrated and (use_acc == self.last_used_acc
or not self.acc_available):
return (self.time_list, self.orientation_list)
apply_complementary = self.acc_available and use_acc
self.last_used_acc = use_acc
if apply_complementary:
# find valid accelation data points
#print(self.acc)
#print(self.acc.shape)
asquared = np.sum(self.acc[:, 1:]**2, 1)
# between 0.9 and 1.1 g
complementary_mask = np.logical_and(0.81 < asquared,
asquared < 1.21)
self.orientation = np.copy(self.initial_orientation)
# temp lists to save data
temp_orientation_list = []
temp_time_list = []
start_time = self.gyro[0][0] # seconds
for i in range(self.num_data_points):
# angular velocity vector
omega = self.gyro[i][1:]
# get current and adjecent times
last_time = self.gyro[i - 1][0] if i > 0 else self.gyro[i][0]
this_time = self.gyro[i][0]
next_time = self.gyro[
i + 1][0] if i < self.num_data_points - 1 else self.gyro[i][0]
# symmetrical dt calculation. Should give slightly better results when missing data
delta_time = (next_time - last_time) / 2
# Only calculate if angular velocity is present
if np.any(omega) or apply_complementary:
# complementary filter
if apply_complementary:
if complementary_mask[i]:
avec = self.acc[i][1:]
avec /= np.linalg.norm(avec)
accWorldVec = quat.rotate_vector_fast(
self.orientation, avec)
correctionWorld = np.cross(accWorldVec, self.grav_vec)
# high weight for first two seconds to "lock" it, then
weight = 10 if this_time - start_time < 1.5 else 0.6
correctionBody = weight * quat.rotate_vector_fast(
quat.conjugate(self.orientation), correctionWorld)
omega = omega + correctionBody
# calculate rotation quaternion
delta_q = self.rate_to_quat(omega, delta_time)
# rotate orientation by this quaternion
self.orientation = quat.quaternion_multiply(
self.orientation, delta_q) # Maybe change order
self.orientation = quat.normalize(self.orientation)
temp_orientation_list.append(np.copy(self.orientation))
temp_time_list.append(this_time)
self.orientation_list = np.array(temp_orientation_list)
self.time_list = np.array(temp_time_list)
self.already_integrated = True
return (self.time_list, self.orientation_list)
