Page History

%% Analysis

% Analyze and plot data
quadrupole_position = simOut.yout.getElement('Quadrupole position').Values;
quadrupole_orientation = simOut.yout.getElement('Quadrupole orientation').Values;

% Plot quadrupole positions
figure();
tiledlayout(3, 1);
nexttile;
plot(quadrupole_position.Quad_Y.Time, quadrupole_position.Quad_Y.Data - quadrupole_offset(2));
title('Quadrupole x position');
xlabel('Time [s]');
ylabel('Displacement [mm]');
grid on;

nexttile;
plot(quadrupole_position.Quad_Z.Time, quadrupole_position.Quad_Z.Data - quadrupole_offset(3));
title('Quadrupole y position');
xlabel('Time [s]');
ylabel('Displacement [mm]');
grid on;

nexttile;
plot(quadrupole_position.Quad_X.Time, quadrupole_position.Quad_X.Data - quadrupole_offset(1));
title('Quadrupole z position');
xlabel('Time [s]');
ylabel('Displacement [mm]');
grid on;

% Plot quadrupole orientations
figure();
tiledlayout(3, 1);
nexttile;
plot(quadrupole_orientation.Time, (quadrupole_orientation.Data(:, 1) - 90) * pi / 180);
title('Quadrupole yaw');
xlabel('Time [s]');
ylabel('Displacement [rad]');
grid on;

nexttile;
plot(quadrupole_orientation.Time, quadrupole_orientation.Data(:, 2) * pi / 180);
title('Quadrupole roll');
xlabel('Time [s]');
ylabel('Displacement [rad]');
grid on;

nexttile;
plot(quadrupole_orientation.Time, (quadrupole_orientation.Data(:, 3) - 90) * pi / 180);
title('Quadrupole pitch');
xlabel('Time [s]');
ylabel('Pitch [rad]');
grid on;

disp("Sim done");

% Error analysis
interpolated_x = interp1(quadrupole_position.Quad_X.Time - quadrupole_position.Quad_X.Time(1), quadrupole_position.Quad_Y.Data - quadrupole_offset(2), time);
interpolated_y = interp1(quadrupole_position.Quad_Z.Time - quadrupole_position.Quad_Z.Time(1), quadrupole_position.Quad_Z.Data - quadrupole_offset(3), time);
interpolated_z = interp1(quadrupole_position.Quad_Y.Time, quadrupole_position.Quad_Y.Data - quadrupole_offset(1), time);

interpolated_pitch = interp1(quadrupole_orientation.Time, (quadrupole_orientation.Data(:, 3) - 90) * pi / 180, time);
interpolated_roll = interp1(quadrupole_orientation.Time, quadrupole_orientation.Data(:, 2) * pi / 180, time);
interpolated_yaw = interp1(quadrupole_orientation.Time, (quadrupole_orientation.Data(:, 1) - 90) * pi / 180, time);

% Plot inverse kinematic errors
figure();
tiledlayout(3, 1);
nexttile;
plot(time, x - interpolated_x);
title('Inverse kinematic x error');
xlabel('Time [s]');
ylabel('Error [mm]');
grid on;

nexttile;
plot(time, y - interpolated_y);
title('Inverse kinematic y error');
xlabel('Time [s]');
ylabel('Error [mm]');
grid on;

nexttile;
plot(time, 0 - interpolated_z);
title('Inverse kinematic z error');
xlabel('Time [s]');
ylabel('Error [mm]');
grid on;

% Plot orientation errors
figure();
tiledlayout(3, 1);
nexttile;
plot(time, yaw - interpolated_yaw);
title('Inverse kinematic yaw error');
xlabel('Time [s]');
ylabel('Displacement [rad]');
grid on;

nexttile;
plot(time, roll - interpolated_roll);
title('Inverse kinematic roll error');
xlabel('Time [s]');
ylabel('Displacement [rad]');
grid on;

nexttile;
plot(time, yaw - interpolated_yaw);
title('Inverse kinematic yaw error');
xlabel('Time [s]');
ylabel('Displacement [rad]');
grid on;
%%
%XY
figure()
tiledlayout(2,1)
nexttile
plot(interpolated_y, interpolated_x)
title('Cross correlation X-Y')
xlabel('Y position [mm]')
ylabel('X position [mm]')
grid on
nexttile
plot(interpolated_x, interpolated_y)
title('Cross correlation Y-X')
xlabel('X position [mm]')
ylabel('Y position [mm]')
grid on

%YAW
figure()
tiledlayout(2,1)
nexttile
plot(interpolated_yaw, interpolated_x)
title('Cross correlation X-Yaw')
xlabel('Yaw [rad]')
ylabel('X position [mm]')
grid on
nexttile
plot(interpolated_yaw, interpolated_y)
title('Cross correlation Y-Yaw ')
xlabel('Yaw [rad]')
ylabel('Y position [mm]')
grid on

%PITCH
figure()
tiledlayout(2,1)
nexttile
plot(interpolated_pitch, interpolated_x)
title('Cross correlation X-Pitch')
xlabel('Pitch [rad]')
ylabel('X position [mm]')
grid on
nexttile
plot(interpolated_pitch, interpolated_y)
title('Cross correlation Y-Pitch')
xlabel('Pitch [rad]')
ylabel('Y position [mm]')
grid on

%ROLL
figure()
tiledlayout(2,1)
nexttile
plot(interpolated_roll, interpolated_x)
title('Cross correlation X-Roll')
xlabel('Roll [rad]')
ylabel('X position [mm]')
grid on
nexttile
plot(interpolated_roll, interpolated_y)
title('Cross correlation Y-Roll')
xlabel('Pitch [rad]')
ylabel('Y position [mm]')
grid on