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_converts_measurement_units(self): # create temporary measures to convert accelerations = np.array([[1.0], [1.0], [1.0]]) angular_velocities = np.array([[180.0], [180.0], [180.0]]) gps_speed = np.array([[1.0]]) converts_measurement_units(accelerations, angular_velocities, gps_speed) # check measurement unit conversion np.testing.assert_array_almost_equal(accelerations, g) np.testing.assert_array_almost_equal(angular_velocities, pi) np.testing.assert_array_almost_equal(gps_speed, kmh)
def test_converts_measurement_units(self): # create temporary measures to convert accelerations = np.array([[1.0], [1.0], [1.0]]) angular_velocities = np.array([[180.0], [180.0], [180.0]]) gps_speed = np.array([[1.0]]) converts_measurement_units(accelerations, angular_velocities, gps_speed) # import scipy constants from scipy.constants import g, pi, kmh # check measurement unit conversion self.assertTrue(all(accelerations == g)) self.assertTrue(all(angular_velocities == pi)) self.assertTrue(all(gps_speed == kmh))
fig1_ax2.set_ylabel("angular velocity (deg/s)", color='r') fig1_ax2.tick_params("y", colors='r') fig1_ax2.plot(times, -angular_velocities[2], color='r') fig1_ax2.set_ylim(ylim) fig1_ax2.set_xlim(xlim) plt.legend() if __name__ == "__main__": 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( 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
from src.integrate import cumulative_integrate, trapz_integrate_delta, simps_integrate_delta from src import rotate_accelerations, align_to_world from plots_scripts.plot_utils import plot_vectors import matplotlib.pyplot as plt if __name__ == '__main__': window_size = 20 path = '../tests/test_fixtures/parking.tsv' # 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) # get positions from GNSS data gnss_positions, headings = 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)
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
def setUp(self): filepath = 'tests/test_fixtures/crash_01.txt' self.times, self.gps_speed, self.accelerations, self.angular_velocities = parse_input( filepath, [InputType.INERTIAL]) converts_measurement_units(self.accelerations, self.angular_velocities, self.gps_speed)