You should have received a copy of the GNU Affero General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
"""
import matplotlib.pyplot as plt
import numpy as np
from src import rotate_accelerations
from plots_scripts.plot_utils import plot_vectors
from tests.test_integration import CircularTrajectoryGenerator, \
cumulative_integrate
if __name__ == "__main__":
# plots circular trajectory and integrated one for debugging and comparison purpose
trajectory_gen = CircularTrajectoryGenerator()
times = trajectory_gen.times
accelerations = trajectory_gen.get_analytical_local_accelerations()
angular_velocities = trajectory_gen.get_angular_velocities()
# convert to laboratory frame of reference
accelerations, angular_positions = rotate_accelerations(times, accelerations, angular_velocities,
initial_angular_position=[0, 0, np.pi / 2])
initial_speed = np.array([[0], [trajectory_gen.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)
plot_vectors([positions])
plot_vectors([trajectory_gen.trajectory])
# blocking call to show all plots
plt.show()
def plot_trajectory(self):
plot_vectors([self.trajectory], ["analytical trajectory"])
# 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
plot_vectors([accelerations], ['$m/s^2$'], tri_dim=False, horiz=times)
plot_vectors([correct_velocities], ['m/s'], tri_dim=False, horiz=times)
figure = plot_vectors([correct_position], ['$m$'])
lim3d = figure.axes[1].get_xbound()
figure.axes[1].set_zbound(lim3d)
figure.axes[1].set_ybound(lim3d)
plt.show()
from src.clean_data_utils import converts_measurement_units
from src.gnss_utils import get_positions
from plots_scripts.plot_utils import plot_vectors
if __name__ == "__main__":
# load dataset
crash_01_dataset = 'tests/test_fixtures/split_log_1321_1322_USB0_unmodified-fullinertial.txt'
times, coordinates, altitudes, gps_speed, heading, accelerations, angular_velocities = parse_input(
crash_01_dataset, [InputType.UNMOD_FULLINERTIAL])
converts_measurement_units(accelerations, angular_velocities, gps_speed,
coordinates)
#get cartesian position from world one
gnss_positions, headings = get_positions(coordinates, altitudes)
# creates empty array to fill
approx_points = np.zeros((2, len(times)))
# size of step using iterating array
step = round(len(times) / 10)
for i in range(0, len(times), step):
# creates local slice of times and positions
local_times = times[i:i + step]
local_positions = gnss_positions[0:2, i:i + step]
# calculate coefficients of polynomial approximation for x-coordinates
poly_x = np.polyfit(local_times, local_positions[0], deg=1)
# calculate coefficients of polynomial approximation for y-coordinates
poly_y = np.polyfit(local_times, local_positions[1], deg=1)
# plot interpolation and approximation
approx_points[0, i:i + step] = np.polyval(poly_x, local_times)
approx_points[1, i:i + step] = np.polyval(poly_y, local_times)
plot_vectors([gnss_positions[0:2], approx_points], ["interp", "approx"])
plt.show()
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU Affero General Public License as published
by the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU Affero General Public License for more details.
You should have received a copy of the GNU Affero General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
"""
import matplotlib.pyplot as plt
from plots_scripts.plot_utils import plot_vectors
from tests.test_integration import get_integrated_trajectory, SpringTrajectoryGenerator, \
cumulative_integrate
if __name__ == "__main__":
trajectory = SpringTrajectoryGenerator()
trajectory.plot_trajectory()
accelerations = trajectory.get_analytical_accelerations()
plot_vectors([accelerations], ["accelerations"])
integrated_trajectory = get_integrated_trajectory(cumulative_integrate)
plot_vectors([integrated_trajectory], ["integrated trajectory"])
# blocking call to show all plots
plt.show()
real_speeds = np.linalg.norm(real_velocities, axis=0)
stationary_times = get_stationary_times(gps_speed)
angular_velocities = clear_gyro_drift(angular_velocities, stationary_times)
normalize_timestamp(times)
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()
plot_vectors([accelerations[0:2], angular_velocities[2]],
['inertial_ax', 'omega_z'],
title="inertial accelerations before rotations",
tri_dim=False)
accelerations = correct_xy_orientation(accelerations, angular_velocities)
plot_vectors([accelerations[0:2], angular_velocities[2]],
['inertial_ax', 'omega_z'],
title="inertial accelerations after rotations",
tri_dim=False)
# 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
def plot_trajectory(self):
""" Plots analytical trajectory"""
plot_vectors([self.trajectory], ["analytical trajectory"])
