def test_correct_xy_orientation(self):
stationary_times = get_stationary_times(self.gps_speed)
# reduce disturbance
_, self.accelerations = reduce_disturbance(
self.times, self.accelerations, reduce_disturbance_window_size)
_, self.angular_velocities = reduce_disturbance(
self.times, self.angular_velocities,
reduce_disturbance_window_size)
# convert measurement units
converts_measurement_units(
self.accelerations,
self.angular_velocities,
self.gps_speed,
)
# align on z-axis
self.accelerations, self.angular_velocities = correct_z_orientation(
self.accelerations, self.angular_velocities, stationary_times)
# clear gyroscope drift
self.angular_velocities = clear_gyro_drift(self.angular_velocities,
stationary_times)
# get number of records that means that there is is a bad xy alignment
bad_align_count_len_before = get_xy_bad_align_count(
self.accelerations, self.angular_velocities)
# check that there is a bad alignment
assert bad_align_count_len_before > 0
# align on xy plane
self.accelerations = correct_xy_orientation(self.accelerations,
self.angular_velocities)
# re-get number of records that means that there is is a bad xy alignment
bad_align_count_len_after = get_xy_bad_align_count(
self.accelerations, self.angular_velocities)
# these records should be now less than before
assert bad_align_count_len_after <= bad_align_count_len_before
def test_clearGyroDrift(self):
drift_tolerance = 0.0002
stationary_times = get_stationary_times(self.gps_speed,self.period)
_, self.angular_velocities = reduce_disturbance(self.times, self.angular_velocities,reduce_disturbance_window_size)
# get initial stationary time angular speed around x-axis
initial_stationary_time_gx_value = self.angular_velocities[0, stationary_times[0][0]:stationary_times[0][1]].mean()
# check there is a gyroscope drift
assert abs(initial_stationary_time_gx_value) > drift_tolerance
# call util to remove drift
self.angular_velocities = clear_gyro_drift(self.angular_velocities,stationary_times)
# re-calculate initial angular speed around x-axis
initial_stationary_time_gx_value = self.angular_velocities[0, stationary_times[0][0]:stationary_times[0][1]].mean()
# check that the drift is lower than a tolerance
assert abs(initial_stationary_time_gx_value) < drift_tolerance
def test_correct_z_orientation(self):
stationary_times = get_stationary_times(self.gps_speed,self.period)
_, self.accelerations = reduce_disturbance(self.times, self.accelerations,reduce_disturbance_window_size)
threshold = 0.1
# get average value in start stationary time
stationary_ax_mean_before = self.accelerations[0, 0:stationary_times[0][1]].mean()
stationary_ay_mean_before = self.accelerations[1, 0:stationary_times[0][1]].mean()
# execute test only if there is a acceleration component on x/y when the car should be stationary
if abs(stationary_ax_mean_before) > threshold or abs(stationary_ay_mean_before) > threshold:
# correct z orientation
self.accelerations, self.angular_velocities = correct_z_orientation(self.accelerations,
self.angular_velocities,stationary_times)
# get average value in start stationary time
stationary_ax_mean_after = self.accelerations[0, 0:stationary_times[0][1]].mean()
stationary_ay_mean_after = self.accelerations[1, 0:stationary_times[0][1]].mean()
# in x and y axis it shouldn't be any acceleration
assert stationary_ax_mean_after < stationary_ax_mean_before
assert stationary_ay_mean_after < stationary_ay_mean_before
# currently default format is unmodified fullinertial but other formats are / will be supported
times, coordinates, altitudes, gps_speed, heading, accelerations, angular_velocities = parse_input(
sys.argv[1], [InputType.UNMOD_FULLINERTIAL])
converts_measurement_units(accelerations, np.array([0.1]), gps_speed,
coordinates, heading)
normalize_timestamp(times)
data_preprocessing_plot("Before")
# reduce accelerations disturbance
times, accelerations = reduce_disturbance(times, accelerations,
window_size)
# reduce angular velocities disturbance
_, angular_velocities = reduce_disturbance(times, angular_velocities,
window_size)
# slice gps_speed to remove null values
gps_speed = gps_speed[round(window_size / 2):-round(window_size / 2)]
# get time windows where vehicle is stationary
stationary_times = get_stationary_times(gps_speed)
# clear gyroscope drift
angular_velocities = clear_gyro_drift(angular_velocities, stationary_times)
# 'after' data preprocessing plot
data_preprocessing_plot("After")
plt.show()
def get_trajectory_from_path(path):
"""
parse input file from path, clean data and integrate positions
:param path: string input file
:return: 3 numpy array: 3xn position, 1xn times, 4xn angular position as quaternions
"""
window_size = 20
# currently default format is unmodified fullinertial but other formats are / will be supported
times, coordinates, altitudes, gps_speed, heading, accelerations, angular_velocities = parse_input(
path, [InputType.UNMOD_FULLINERTIAL])
converts_measurement_units(accelerations, angular_velocities, gps_speed,
coordinates, heading)
# get positions from GNSS data
gnss_positions, headings_2 = get_positions(coordinates, altitudes)
# reduce accelerations disturbance
times, accelerations = reduce_disturbance(times, accelerations,
window_size)
# reduce angular velocities disturbance
_, angular_velocities = reduce_disturbance(times, angular_velocities,
window_size)
# truncate other array to match length of acc, thetas, times array
gnss_positions = gnss_positions[:,
round(window_size /
2):-round(window_size / 2)]
# with "final" times now get velocities and
real_velocities = get_velocities(times, gnss_positions)
# scalar speed from GNSS position (better than from dataset because avoids Kalmar filter)
real_speeds = np.linalg.norm(real_velocities, axis=0)
# get time windows where vehicle is stationary
stationary_times = get_stationary_times(gps_speed)
# clear gyroscope drift
angular_velocities = clear_gyro_drift(angular_velocities, stationary_times)
# set times start to 0
normalize_timestamp(times)
# correct z-axis alignment
accelerations, angular_velocities = correct_z_orientation(
accelerations, angular_velocities, stationary_times)
# remove g
accelerations[2] -= accelerations[
2, stationary_times[0][0]:stationary_times[0][-1]].mean()
# correct alignment in xy plane
#accelerations = correct_xy_orientation(accelerations, angular_velocities)
motion_time = get_first_motion_time(stationary_times, gnss_positions)
initial_angular_position = get_initial_angular_position(
gnss_positions, motion_time)
# convert to laboratory frame of reference
accelerations, angular_positions = rotate_accelerations(
times, accelerations, angular_velocities, heading,
initial_angular_position)
# rotate to align y to north, x to east
accelerations = align_to_world(gnss_positions, accelerations, motion_time)
# angular position doesn't need to be aligned to world if starting angular position is already aligned and following
# angular positions are calculated from that
initial_speed = np.array([[gps_speed[0]], [0], [0]])
# integrate acceleration with gss velocities correction
correct_velocities = cumulative_integrate(times,
accelerations,
initial_speed,
adjust_data=real_velocities,
adjust_frequency=1)
if sign_inversion_is_necessary(correct_velocities):
accelerations *= -1
correct_velocities *= -1
correct_position = cumulative_integrate(times,
correct_velocities,
adjust_data=gnss_positions,
adjust_frequency=1)
return correct_position, times, angular_positions
# currently default format is unmodified fullinertial but other formats are / will be supported
times, coordinates, altitudes, gps_speed, heading, accelerations, angular_velocities = parse_input(sys.argv[1], [
InputType.UNMOD_FULLINERTIAL])
converts_measurement_units(accelerations, np.array([0.1]), gps_speed, coordinates, heading)
normalize_timestamp(times)
data_preprocessing_plot("Before")
# reduce accelerations disturbance
times, accelerations = reduce_disturbance(times, accelerations, window_size)
# reduce angular velocities disturbance
_, angular_velocities = reduce_disturbance(times, angular_velocities, window_size)
# slice gps_speed to remove null values
gps_speed = gps_speed[round(window_size / 2):-round(window_size / 2)]
period = times[1] - times[0]
# get time windows where vehicle is stationary
stationary_times = get_stationary_times(gps_speed,period)
# clear gyroscope drift
angular_velocities = clear_gyro_drift(angular_velocities, stationary_times)
# 'after' data preprocessing plot
data_preprocessing_plot("After")
plt.show()
gnss_positions, headings = get_positions(coordinates, altitudes)
gnss_distance = norm(np.array([
gnss_positions[:, i] - gnss_positions[:, i - 1]
for i, x in enumerate(gnss_positions[:, 1:].T, 1)
]),
axis=1).cumsum()
# insert initial distance
gnss_distance = np.insert(gnss_distance, 0, 0)
# reshape to 1xn shape
gnss_distance = np.reshape(gnss_distance, (1, len(gnss_distance)))
real_velocities = get_velocities(times, gnss_positions)
real_acc = get_accelerations(times, real_velocities)
real_velocities_module = norm(real_velocities, axis=0)
stationary_times = get_stationary_times(real_velocities_module)
# reduce accelerations disturbance
times, accelerations = reduce_disturbance(times, accelerations,
window_size)
# reduce angular velocities disturbance
_, angular_velocities = reduce_disturbance(times, angular_velocities,
window_size)
# resize other arrays after reduce disturbance
real_acc = real_acc[:, round(window_size / 2):-round(window_size / 2)]
real_velocities = real_velocities[:,
round(window_size /
2):-round(window_size / 2)]
gnss_positions = gnss_positions[:,
round(window_size /
2):-round(window_size / 2)]
gnss_distance = gnss_distance[:,
def test_detect_stationary_times(self):
# only check get_stationary_times doesn't raise exceptions
stationary_times = get_stationary_times(self.gps_speed)
self.assertGreater(len(stationary_times), 0)