%%
Dmeas.Data = Dmeas.Data - Dmeas.Data(1, :);

%% Time domain X-Y-Z
figure;
hold on;
plot(Dmeas.Time, Dmeas.Data(:, 1));
plot(Dmeas.Time, Dmeas.Data(:, 2));
plot(Dmeas.Time, Dmeas.Data(:, 3));
legend({'x', 'y', 'z'})
hold off;
xlabel('Time [s]'); ylabel('Displacement [m]');

exportFig('tomo_time_translations', 'normal-normal')

%% Time domain angles
figure;
hold on;
plot(Dmeas.Time, Dmeas.Data(:, 4));
plot(Dmeas.Time, Dmeas.Data(:, 5));
legend({'$\theta_x$', '$\theta_y$'})
hold off;
xlabel('Time [s]'); ylabel('Displacement [m]');

exportFig('tomo_time_rotations', 'normal-normal')

%% PSD X-Y-Z
han_windows = hanning(ceil(length(Dmeas.Time)/10));

[psd_x, freqs_x] = pwelch(Dmeas.Data(:, 1), han_windows, 0, [], 1/Ts);
[psd_y, freqs_y] = pwelch(Dmeas.Data(:, 2), han_windows, 0, [], 1/Ts);
[psd_z, freqs_z] = pwelch(Dmeas.Data(:, 3), han_windows, 0, [], 1/Ts);

figure;
hold on;
plot(freqs_x, sqrt(psd_x));
plot(freqs_y, sqrt(psd_y));
plot(freqs_z, sqrt(psd_z));
set(gca,'xscale','log'); set(gca,'yscale','log');
xlabel('Frequency [Hz]'); ylabel('PSD [$m/\sqrt{Hz}$]');
legend({'x', 'y', 'z'})
hold off;

exportFig('tomo_psd_translations', 'normal-normal')

%% PSD X-Y-Z
han_windows = hanning(ceil(length(Dmeas.Time)/10));

[psd_x, freqs_x] = pwelch(Dmeas.Data(:, 4), han_windows, 0, [], 1/Ts);
[psd_y, freqs_y] = pwelch(Dmeas.Data(:, 5), han_windows, 0, [], 1/Ts);

figure;
hold on;
plot(freqs_x, sqrt(psd_x));
plot(freqs_y, sqrt(psd_y));
set(gca,'xscale','log'); set(gca,'yscale','log');
xlabel('Frequency [Hz]'); ylabel('PSD [$rad/s/\sqrt{Hz}$]');
legend({'$\theta_x$', '$\theta_y$'})
hold off;

exportFig('tomo_psd_rotations', 'normal-normal')


%%
save('./data/tomography_exp_ol.mat', 'Dmeas')