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)