def test_local_to_lab(self):
""" Tests conversion from local acceleration reference frame to laboratory reference frame
by rotation using integrated angular velocity. Tests both integrator and rotator.
"""
# generated Trajectory Generator object
circular_tra = CircularTrajectoryGenerator()
# get trajectory times
times = circular_tra.times
# get local accelerations
accelerations = circular_tra.get_analytical_local_accelerations()
# get angular velocities
angular_velocities = circular_tra.get_angular_velocities()
heading = None
# convert to laboratory frame of reference
accelerations, angular_positions = rotate_accelerations(
times,
accelerations,
angular_velocities,
heading,
initial_angular_position=[0, 0, np.pi / 2])
initial_speed = np.array([[0], [circular_tra.initial_tangential_speed],
[0]])
velocities = cumulative_integrate(times, accelerations, initial_speed)
initial_position = np.array([[1], [0], [0]])
positions = cumulative_integrate(times, velocities, initial_position)
# if the integrated trajectory and the analytical one are equal thant both the integrator and the rotator works
np.testing.assert_array_almost_equal(positions,
circular_tra.trajectory,
decimal=3)
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)
# 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)
initial_speed = np.array([[gps_speed[0]], [0], [0]])
# integrate acceleration with gss velocities correction
import time
start_time = time.time()
correct_velocities_simps = cumulative_integrate(times, accelerations, initial_speed, simps_integrate_delta)
print("time integrating velocities simps " + str(time.time()-start_time))
start_time = time.time()
correct_velocities_trapz = cumulative_integrate(times, accelerations, initial_speed, trapz_integrate_delta)
print("time integrating velocities trapz " + str(time.time()-start_time))
correct_position_simps = cumulative_integrate(times, correct_velocities_simps)
correct_position_trapz = cumulative_integrate(times,correct_velocities_trapz)
plt.plot(correct_position_simps[0],label='PaIS')
plt.plot(correct_position_trapz[0],label='Trapezoid')
plt.ylabel('meters')
plt.xlabel('seconds')
plt.legend()
plt.show()
# 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()
accelerations_module = np.linalg.norm(accelerations, axis=0)
accelerations_module = np.reshape(accelerations_module, (1, len(accelerations_module)))
real_acc_module = np.linalg.norm(real_acc, axis=0)
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)
correct_distance = cumulative_integrate(times, correct_velocities_module, adjust_data=gnss_distance,
adjust_frequency=1)
print("Execution time: %s seconds" % (time.time() - start_time))
# plotting
fig1, ax1 = plt.subplots()
ax1.plot(real_acc_module, label='acc gnss')
ax1.plot(accelerations_module.T, label='acc sensor')
ax1.legend()
fig, ax2 = plt.subplots()
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
# 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)
print("Execution time: %s seconds" % (time.time() - start_time))
# plotting