waypointsToMeasurementsExample.m

This example shows how to use the Trajectory and JumpSensor classes to create a trajectory and generate realistic sensor data from it.

This example shows how to use the Trajectory and JumpSensor classes to create a trajectory and generate realistic sensor data from it.

% =========================================================================== %
%> @example waypointsToMeasurementsExample.m
%>
%> This example shows how to use the @ref Trajectory and @ref JumpSensor 
%> classes to create a trajectory and generate realistic sensor data from it.
% =========================================================================== %
 
% Create a set start times and stay times for each waypoint
t_start = seconds(0:10:120);
t_stay  = repmat(seconds(0), size(t_start));
 
% Create a set of waypoint positions
pos     = [...
    0 0 0; ...
    1 0 0; ...
    1 1 0; ...
    0 1 0; ...
    0 0 1; ...
    1 0 1; ...
    1 1 1; ...
    0 1 1; ...
    0 0 2; ...
    1 0 2; ...
    1 1 2; ...
    0 1 2; ...
    0 0 0; ...
    ];
 
% Create a set of waypoint orientations
% this is only a yaw-rotation of 90 ° per row and after 360° the pitch also
% changes by 90°.
ori = quaternion([...
     0     1     0     0;    ...
     0    -0.71 -0.71  0;    ...
     0     0     1     0;    ...
     0     0.71 -0.71  0;    ...
     0    -0.71  0     0.71; ...
    -0.5  -0.5  -0.5   0.5;  ...
    -0.71  0    -0.71  0;    ...
    -0.5   0.5  -0.5  -0.5;  ...
     0     0     0     1;    ...
    -0.71  0     0     0.71; ...
     1     0     0     0;    ...
    -0.71  0     0    -0.71; ...
     0     1     0     0;    ...
    ]);
 
% Create a trajectory object
traj = Trajectory();
 
% Add the waypoints to the trajectory
traj.addWaypoints(t_start, t_stay, pos, ori);
 
% generate the Trajectory
traj.generateTrajectory(pos="minSnap", ori="makima");
 
% visualize the trajectory
trajectoryVisualizationApp(traj);
 
% Create a JumpSensor object
sensor = JumpSensor();
 
data = sensor.generateFromTrajectory(traj);
 
% visualize the sensor data
figure;
subplot(211); % acceleration
plot(data.tt.t, data.tt.acc);
title('Acceleration');
xlabel('Time [s]');
ylabel('Acceleration [m/s^2]');
grid on;
 
subplot(212); % angular velocity
plot(data.tt.t, data.tt.gyr);
title('Angular Velocity');
xlabel('Time [s]');
ylabel('Angular Velocity [°/s]');
grid on;