Updated reconstruction plot scripts

physycom · Feb 18, 2019 · 1d9a592 · 1d9a592
1 parent dd493b1
commit 1d9a592
Show file tree

Hide file tree

Showing 2 changed files with 42 additions and 21 deletions.
diff --git a/plots_scripts/inertial_data.py → plots_scripts/reconstruction_with_gps.py b/plots_scripts/inertial_data.py → plots_scripts/reconstruction_with_gps.py
@@ -30,7 +30,7 @@
 from plots_scripts.plot_utils import plot_vectors
 from src.clean_data_utils import converts_measurement_units, reduce_disturbance, \
     clear_gyro_drift, correct_z_orientation, normalize_timestamp, \
-    sign_inversion_is_necessary, get_stationary_times
+    sign_inversion_is_necessary, get_stationary_times, truncate_if_crash
 from src.gnss_utils import get_positions, get_velocities, get_accelerations, get_first_motion_time, \
     get_initial_angular_position
 from src.input_manager import parse_input, InputType
@@ -46,35 +46,44 @@
     start_time = time.time()
 
     parking_fullinertial_unmod = 'tests/test_fixtures/parking.tsv'
+    crash = False
     times, coordinates, altitudes, gps_speed, heading, accelerations, angular_velocities = parse_input(
         parking_fullinertial_unmod, [InputType.UNMOD_FULLINERTIAL])
-    converts_measurement_units(accelerations, angular_velocities, gps_speed, coordinates)
+    converts_measurement_units(accelerations, angular_velocities, gps_speed, coordinates, heading)
+    period = times[1] - times[0]
 
-    # GNSS data handling
-    gnss_positions, headings = get_positions(coordinates, altitudes)
+    times, gps_speed, accelerations, angular_velocities, coordinates, heading = \
+        truncate_if_crash(crash, times, gps_speed, accelerations, angular_velocities, 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 others array to match length of times array
+    # 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)
-    real_acc = get_accelerations(times, real_velocities)
+    # scalar speed from GNSS position (better than from dataset because avoids Kalmar filter)
     real_speeds = np.linalg.norm(real_velocities, axis=0)
 
-    stationary_times = get_stationary_times(gps_speed)
+    # 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)
+    # 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()
 
-    # convert to laboratory frame of reference
     motion_time = get_first_motion_time(stationary_times, gnss_positions)
     initial_angular_position = get_initial_angular_position(gnss_positions, motion_time)
 
@@ -84,16 +93,18 @@
 
     # rotate to align y to north, x to east
     accelerations = align_to_world(gnss_positions, accelerations, motion_time)
-
-    figure = plot_vectors([accelerations[0:2], real_acc[0:2], angular_velocities[2]],
-                          ['inertial_ax', 'gnss_acc', 'omega_z'],
-                          title="comparison inertial and gnss accelerations in word reference frame", tri_dim=False)
-    plt.show()
+    # 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)
 
     print("Execution time: %s seconds" % (time.time() - start_time))

diff --git a/plots_scripts/inertial_data_module.py → ...scripts/reconstruction_with_gps_module.py b/plots_scripts/inertial_data_module.py → ...scripts/reconstruction_with_gps_module.py
@@ -30,7 +30,7 @@
 
 from src.clean_data_utils import converts_measurement_units, reduce_disturbance, \
     clear_gyro_drift, correct_z_orientation, normalize_timestamp, \
-    get_stationary_times
+    get_stationary_times, truncate_if_crash
 from src.gnss_utils import get_positions, get_velocities, get_accelerations
 from src.input_manager import parse_input, InputType
 from src.integrate import cumulative_integrate
@@ -43,16 +43,20 @@
 
     start_time = time.time()
 
-    # input pats
+    #input path
     parking_fullinertial_unmod = 'tests/test_fixtures/parking.tsv'
-    # parse input
+    crash = False
     times, coordinates, altitudes, gps_speed, heading, accelerations, angular_velocities = parse_input(
         parking_fullinertial_unmod, [InputType.UNMOD_FULLINERTIAL])
+    converts_measurement_units(accelerations, angular_velocities, gps_speed, coordinates, heading)
+    period = times[1] - times[0]
 
-    converts_measurement_units(accelerations, angular_velocities, gps_speed, coordinates)
+    times, gps_speed, accelerations, angular_velocities, coordinates, heading = \
+        truncate_if_crash(crash, times, gps_speed, accelerations, angular_velocities, coordinates, heading)
+
+    # get positions from GNSS data
+    gnss_positions, headings_2 = get_positions(coordinates, altitudes)
 
-    # GNSS data handling
-    gnss_positions, headings = get_positions(coordinates, altitudes)
     gnss_distance = np.linalg.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
@@ -62,21 +66,26 @@
     real_velocities = get_velocities(times, gnss_positions)
     real_acc = get_accelerations(times, real_velocities)
 
+    # scalar speed from GNSS position (better than from dataset because avoids Kalmar filter)
     real_velocities_module = np.linalg.norm(real_velocities, axis=0)
 
-    stationary_times = get_stationary_times(gps_speed)
+    stationary_times = get_stationary_times(gps_speed,period)
     # 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
+    # truncate other array to match length of acc, thetas, times array
     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[:, round(window_size / 2):-round(window_size / 2)]
 
+    # 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()
@@ -88,6 +97,7 @@
 
     real_velocities_module = np.reshape(real_velocities_module, (1, len(real_velocities_module)))
 
+    # integrate acceleration with gss velocities correction
     correct_velocities_module = cumulative_integrate(times, accelerations_module, real_velocities_module[0, 0],
                                                      adjust_data=real_velocities_module,
                                                      adjust_frequency=1)