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)
def test_rotation(self):
# 90°/s around z
angular_velocity = np.array([[0, 0, 0], [0, 0, 0],
[pi / 2, pi / 2, pi / 2]])
times = np.array([0, 1, 2])
# unitary x vector
vectors_to_rotate = np.array([[1, 1, 1], [0, 0, 0], [0, 0, 0]])
headings = None
# test also initial angular position with different angle for testing quaternion product order correctness
vectors_to_rotate, angular_velocity = \
rotate_accelerations(times, vectors_to_rotate, angular_velocity, headings, np.array([0, 0, pi]))
expected_result = np.array([[-1, 0, 1], [0, -1, 0], [0, 0, 0]])
np.testing.assert_array_almost_equal(vectors_to_rotate,
expected_result)
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)
# 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)
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()