%% Clear Workspace and Close figures
clear; close all; clc;

%% Intialize Laplace variable
s = zpk('s');

% Importation of the data
% First, load all the measurement files:

meas = {};
meas{1} = load('./mat/Measurement1.mat');
meas{2} = load('./mat/Measurement2.mat');
meas{3} = load('./mat/Measurement3.mat');
meas{4} = load('./mat/Measurement4.mat');
meas{5} = load('./mat/Measurement5.mat');



% Change the track name for measurements 3 and 4.

meas{3}.Track1_Name = 'Input 1: Hexa Z';
meas{4}.Track1_Name = 'Input 1: Hexa Z';

% Variables for analysis
% We define the sampling frequency and the time vectors for the plots.


Fs = 256; % [Hz]
dt = 1/(Fs);
t1  = dt*(0:length(meas{1}.Track1)-1);
t2  = dt*(0:length(meas{2}.Track1)-1);
t3  = dt*(0:length(meas{3}.Track1)-1);
t4  = dt*(0:length(meas{4}.Track1)-1);
t5  = dt*(0:length(meas{5}.Track1)-1);



% For the frequency analysis, we define the frequency limits for the plot.

fmin = 1; % [Hz]
fmax = 100; % [Hz]



% Then we define the windows that will be used to average the results.

psd_window = hanning(2*fmin/dt);

% Coherence between the two vertical geophones on the Tilt Stage
% We first compute the coherence between the two geophones located on the tilt stage. The result is shown on figure [[fig:coherence_vertical_tilt_sensors]].

[coh, f] = mscohere(meas{1}.Track1(:), meas{1}.Track2(:), psd_window, [], [], Fs);

figure;
plot(f, coh);
set(gca, 'xscale', 'log');
ylim([0, 1]);
xlabel('Frequency [Hz]'); ylabel('Coherence');



% #+NAME: fig:coherence_vertical_tilt_sensors
% #+CAPTION: Coherence between the two vertical sensors positionned on the Tilt Stage
% #+RESULTS: fig:coherence_vertical_tilt_sensors
% [[file:figs/coherence_vertical_tilt_sensors.png]]

% We then compute the transfer function from one sensor to the other (figure [[fig:tf_vertical_tilt_sensors]]).

[tf23, f] = tfestimate(meas{1}.Track1(:), meas{1}.Track2(:), psd_window, [], [], Fs);

figure;
ax1 = subplot(2,1,1);
plot(f, abs(tf23));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude [V/(m/s)]');

ax2 = subplot(2,1,2);
plot(f, 180/pi*angle(tf23));
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
linkaxes([ax1,ax2],'x');

% Data Post Processing
% When using two geophone sensors on the same tilt stage (measurements 1 and 2), we post-process the data to obtain the z displacement and the rotation of the tilt stage:


meas1_z    = (meas{1}.Track1+meas{1}.Track2)/2;
meas1_tilt = (meas{1}.Track1-meas{1}.Track2)/2;

meas{1}.Track1 = meas1_z;
meas{1}.Track1_Y_Magnitude = 'Meter / second';
meas{1}.Track1_Name = 'Ry Z';
meas{1}.Track2 = meas1_tilt;
meas{1}.Track2_Y_Magnitude = 'Rad / second';
meas{1}.Track2_Name = 'Ry Tilt';

meas2_z    = (meas{2}.Track1+meas{2}.Track2)/2;
meas2_tilt = (meas{2}.Track1-meas{2}.Track2)/2;
meas{2}.Track1 = meas2_z;
meas{2}.Track1_Y_Magnitude = 'Meter / second';
meas{2}.Track1_Name = 'Ry Z';
meas{2}.Track2 = meas2_tilt;
meas{2}.Track2_Y_Magnitude = 'Rad / second';
meas{2}.Track2_Name = 'Ry Tilt';

% Normalization
% Parameters of the geophone are defined below.
% The transfer function from geophone velocity to measured voltage is shown on figure [[fig:L4C_bode_plot]].

% Measurements will be normalized by the inverse of this transfer function in order to go from voltage measurement to velocity measurement.


L4C_w0 = 2*pi; % [rad/s]
L4C_ksi = 0.28;
L4C_G0 = 276.8; % [V/(m/s)]
L4C_G = L4C_G0*(s/L4C_w0)^2/((s/L4C_w0)^2 + 2*L4C_ksi*(s/L4C_w0) + 1);

freqs = logspace(-2, 2, 1000);

figure;
ax1 = subplot(2,1,1);
plot(freqs, abs(squeeze(freqresp(L4C_G, freqs, 'Hz'))));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude [V/(m/s)]');

ax2 = subplot(2,1,2);
plot(freqs, 180/pi*angle(squeeze(freqresp(L4C_G, freqs, 'Hz'))));
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
linkaxes([ax1,ax2],'x');



% #+NAME: fig:L4C_bode_plot
% #+CAPTION: Bode plot of the L4C Geophone
% #+RESULTS: fig:L4C_bode_plot
% [[file:figs/L4C_bode_plot.png]]


meas{1}.Track1 = (meas{1}.Track1)./276.8;
meas{1}.Track2 = (meas{1}.Track2)./276.8;
meas{1}.Track3 = (meas{1}.Track3)./276.8;

meas{2}.Track1 = (meas{2}.Track1)./276.8;
meas{2}.Track2 = (meas{2}.Track2)./276.8;
meas{2}.Track3 = (meas{2}.Track3)./276.8;

meas{3}.Track1 = (meas{3}.Track1)./276.8;
meas{3}.Track2 = (meas{3}.Track2)./276.8;
meas{3}.Track3 = (meas{3}.Track3)./276.8;

meas{4}.Track1 = (meas{4}.Track1)./276.8;
meas{4}.Track2 = (meas{4}.Track2)./276.8;
meas{4}.Track3 = (meas{4}.Track3)./276.8;

meas{5}.Track1 = (meas{5}.Track1)./276.8;
meas{5}.Track2 = (meas{5}.Track2)./276.8;
meas{5}.Track3 = (meas{5}.Track3)./276.8;
meas{5}.Track4 = (meas{5}.Track4)./276.8;

meas{1}.Track1_norm = lsim(inv(L4C_G), meas{1}.Track1, t1);
meas{1}.Track2_norm = lsim(inv(L4C_G), meas{1}.Track2, t1);
meas{1}.Track3_norm = lsim(inv(L4C_G), meas{1}.Track3, t1);

meas{2}.Track1_norm = lsim(inv(L4C_G), meas{2}.Track1, t2);
meas{2}.Track2_norm = lsim(inv(L4C_G), meas{2}.Track2, t2);
meas{2}.Track3_norm = lsim(inv(L4C_G), meas{2}.Track3, t2);

meas{3}.Track1_norm = lsim(inv(L4C_G), meas{3}.Track1, t3);
meas{3}.Track2_norm = lsim(inv(L4C_G), meas{3}.Track2, t3);
meas{3}.Track3_norm = lsim(inv(L4C_G), meas{3}.Track3, t3);

meas{4}.Track1_norm = lsim(inv(L4C_G), meas{4}.Track1, t4);
meas{4}.Track2_norm = lsim(inv(L4C_G), meas{4}.Track2, t4);
meas{4}.Track3_norm = lsim(inv(L4C_G), meas{4}.Track3, t4);

meas{5}.Track1_norm = lsim(inv(L4C_G), meas{5}.Track1, t5);
meas{5}.Track2_norm = lsim(inv(L4C_G), meas{5}.Track2, t5);
meas{5}.Track3_norm = lsim(inv(L4C_G), meas{5}.Track3, t5);
meas{5}.Track4_norm = lsim(inv(L4C_G), meas{5}.Track4, t5);

% Measurement 1 - Effect of Ty stage
% The configuration for this measurement is shown table [[tab:conf_meas1]].

% #+CAPTION: Stages configuration - Measurement 1
% #+NAME: tab:conf_meas1
% | Time     | 0-309 | 309-end |
% |----------+-------+---------|
% | Ty       | OFF   | *ON*    |
% | Ry       | OFF   | OFF     |
% | SlipRing | OFF   | OFF     |
% | Spindle  | OFF   | OFF     |
% | Hexa     | OFF   | OFF     |

% We then plot the measurements in time domain (figure [[fig:meas1]]).

% #+begin_important
%   We observe strange behavior when the Ty stage is turned on.
%   How can we explain that?
% #+end_important


figure;
hold on;
plot(t1(ceil(300/dt):ceil(340/dt)), meas{1}.Track1(ceil(300/dt):ceil(340/dt)));
plot(t1(ceil(300/dt):ceil(340/dt)), meas{1}.Track2(ceil(300/dt):ceil(340/dt)));
plot(t1(ceil(300/dt):ceil(340/dt)), meas{1}.Track3(ceil(300/dt):ceil(340/dt)));
hold off;
xlabel('Time [s]'); ylabel('Velocity [m/s]');
legend({meas{1}.Track1_Name, meas{1}.Track2_Name, meas{1}.Track3_Name}, 'Location', 'northeast')



% #+LABEL: fig:meas1
% #+CAPTION: Time domain - measurement 1
% #+RESULTS: fig:meas1
% [[file:figs/meas1.png]]

% To understand what is going on, instead of looking at the velocity, we can look at the displacement by integrating the data. The displacement is computed by integrating the velocity using =cumtrapz= function.

% Then we plot the position with respect to time (figure [[fig:meas1_disp]]).

figure;
hold on;
plot(t1, cumtrapz(t1, meas{1}.Track3));
hold off;
xlim([300, 340]);
xlabel('Time [s]'); ylabel('Displacement [m]');



% #+LABEL: fig:meas1_disp
% #+CAPTION: Y displacement of the Ty stage
% #+RESULTS: fig:meas1_disp
% [[file:figs/meas1_disp.png]]

% We when compute the power spectral density of each measurement before and after turning on the stage.

[pxx111, f11] = pwelch(meas{1}.Track1(1:ceil(300/dt)),   psd_window, [], [], Fs);
[pxx112, f12] = pwelch(meas{1}.Track1(ceil(350/dt):end), psd_window, [], [], Fs);

[pxx121, ~] = pwelch(meas{1}.Track2(1:ceil(300/dt)),   psd_window, [], [], Fs);
[pxx122, ~] = pwelch(meas{1}.Track2(ceil(350/dt):end), psd_window, [], [], Fs);

[pxx131, ~] = pwelch(meas{1}.Track3(1:ceil(300/dt)),   psd_window, [], [], Fs);
[pxx132, ~] = pwelch(meas{1}.Track3(ceil(350/dt):end), psd_window, [], [], Fs);



% We finally plot the power spectral density of each track (figures [[fig:meas1_ry_z_psd]], [[fig:meas1_ry_tilt_psd]], [[fig:meas1_ty_y_psd]]).


figure;
hold on;
plot(f11, sqrt(pxx111)./abs(squeeze(freqresp(L4C_G, f11, 'Hz'))));
plot(f12, sqrt(pxx112)./abs(squeeze(freqresp(L4C_G, f12, 'Hz'))));
xlim([fmin, fmax]);
xticks([1, 10, 100]);
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('PSD [$m/s/\sqrt{Hz}$]');
title(sprintf('%s', meas{1}.Track1_Name));
legend({'0-300', '350-end'}, 'Location', 'southwest');
hold off;



% #+LABEL: fig:meas1_ry_z_psd
% #+CAPTION: PSD of the Z velocity of Ry stage - measurement 1
% #+RESULTS: fig:meas1_ry_z_psd
% [[file:figs/meas1_ry_z_psd.png]]


figure;
hold on;
plot(f11, sqrt(pxx121)./abs(squeeze(freqresp(L4C_G, f11, 'Hz'))));
plot(f12, sqrt(pxx122)./abs(squeeze(freqresp(L4C_G, f12, 'Hz'))));
xlim([fmin, fmax]);
xticks([1, 10, 100]);
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('PSD [$rad/s/\sqrt{Hz}$]');
title(sprintf('%s', meas{1}.Track2_Name));
legend({'0-300', '350-end'}, 'Location', 'southwest');
hold off;



% #+LABEL: fig:meas1_ry_tilt_psd
% #+CAPTION: PSD of the Rotation of Ry Stage - measurement 1
% #+RESULTS: fig:meas1_ry_tilt_psd
% [[file:figs/meas1_ry_tilt_psd.png]]


figure;
hold on;
plot(f11, sqrt(pxx131)./abs(squeeze(freqresp(L4C_G, f11, 'Hz'))));
plot(f12, sqrt(pxx132)./abs(squeeze(freqresp(L4C_G, f12, 'Hz'))));
xlim([fmin, fmax]);
xticks([1, 10, 100]);
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('PSD [$m/s/\sqrt{Hz}$]');
title(sprintf('%s', meas{1}.Track3_Name));
legend({'0-300', '350-end'}, 'Location', 'southwest');
hold off;

% Measurement 2 - Effect of Ry stage
% The tilt stage is turned ON at around 326 seconds (table [[tab:conf_meas2]]).

% #+CAPTION: Stages configuration - Measurement 2
% #+NAME: tab:conf_meas2
% | Time     | 0-326 | 326-end |
% |----------+-------+---------|
% | Ty       | OFF   | OFF     |
% | Ry       | OFF   | *ON*    |
% | SlipRing | OFF   | OFF     |
% | Spindle  | OFF   | OFF     |
% | Hexa     | OFF   | OFF     |

% We plot the time domain (figure [[fig:meas2]]) and we don't observe anything special in the time domain.


figure;
hold on;
plot(t2(ceil(300/dt):ceil(350/dt)), meas{2}.Track1(ceil(300/dt):ceil(350/dt)));
plot(t2(ceil(300/dt):ceil(350/dt)), meas{2}.Track3(ceil(300/dt):ceil(350/dt)));
plot(t2(ceil(300/dt):ceil(350/dt)), meas{2}.Track2(ceil(300/dt):ceil(350/dt)));
hold off;
xlabel('Time [s]'); ylabel('Velocity [m/s]');
legend({meas{2}.Track1_Name, meas{2}.Track2_Name, meas{2}.Track3_Name}, 'Location', 'northeast')
xlim([300, 350]);



% #+LABEL: fig:meas2
% #+CAPTION: Time domain - measurement 2
% #+RESULTS: fig:meas2
% [[file:figs/meas2.png]]


figure;
hold on;
plot(t2, cumtrapz(t2, meas{2}.Track1));
plot(t2, cumtrapz(t2, meas{2}.Track2));
plot(t2, cumtrapz(t2, meas{2}.Track3));
hold off;
xlim([300, 350]);
xlabel('Time [s]'); ylabel('Displacement [m]');
legend({meas{2}.Track1_Name, meas{2}.Track2_Name, meas{2}.Track3_Name}, 'Location', 'northeast')



% #+LABEL: fig:meas2_disp
% #+CAPTION: Time domain - measurement 2
% #+RESULTS: fig:meas2_disp
% [[file:figs/meas2_disp.png]]

% We compute the PSD of each track and we plot them (figures [[fig:meas2_ry_z_psd]], [[fig:meas2_ry_tilt_psd]] and [[fig:meas2_ty_y_psd]] ).

[pxx211, f21] = pwelch(meas{2}.Track1(1:ceil(326/dt)),   psd_window, [], [], Fs);
[pxx212, f22] = pwelch(meas{2}.Track1(ceil(326/dt):end), psd_window, [], [], Fs);

[pxx221, ~] = pwelch(meas{2}.Track2(1:ceil(326/dt)),   psd_window, [], [], Fs);
[pxx222, ~] = pwelch(meas{2}.Track2(ceil(326/dt):end), psd_window, [], [], Fs);

[pxx231, ~] = pwelch(meas{2}.Track3(1:ceil(326/dt)),   psd_window, [], [], Fs);
[pxx232, ~] = pwelch(meas{2}.Track3(ceil(326/dt):end), psd_window, [], [], Fs);

figure;
hold on;
plot(f21, sqrt(pxx211)./abs(squeeze(freqresp(L4C_G, f21, 'Hz'))));
plot(f22, sqrt(pxx212)./abs(squeeze(freqresp(L4C_G, f22, 'Hz'))));
xlim([fmin, fmax]);
xticks([1, 10, 100]);
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('PSD [$m/s/\sqrt{Hz}$]');
title(sprintf('%s', meas{2}.Track1_Name));
legend({'0-326', '326-end'}, 'Location', 'southwest');
hold off;



% #+LABEL: fig:meas2_ry_z_psd
% #+CAPTION: PSD of the Z velocity of Ry Stage - measurement 2
% #+RESULTS: fig:meas2_ry_z_psd
% [[file:figs/meas2_ry_z_psd.png]]


figure;
hold on;
plot(f21, sqrt(pxx221)./abs(squeeze(freqresp(L4C_G, f21, 'Hz'))));
plot(f22, sqrt(pxx222)./abs(squeeze(freqresp(L4C_G, f22, 'Hz'))));
xlim([fmin, fmax]);
xticks([1, 10, 100]);
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('PSD [$rad/s/\sqrt(Hz)$]');
title(sprintf('%s', meas{2}.Track2_Name));
legend({'0-326', '326-end'}, 'Location', 'southwest');
hold off;



% #+LABEL: fig:meas2_ry_tilt_psd
% #+CAPTION: PSD of the Rotation motion of Ry Stage - measurement 2
% #+RESULTS: fig:meas2_ry_tilt_psd
% [[file:figs/meas2_ry_tilt_psd.png]]


figure;
hold on;
plot(f21, sqrt(pxx231)./abs(squeeze(freqresp(L4C_G, f21, 'Hz'))));
plot(f22, sqrt(pxx232)./abs(squeeze(freqresp(L4C_G, f22, 'Hz'))));
xlim([fmin, fmax]);
xticks([1, 10, 100]);
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('PSD [$m/s/\sqrt{Hz}$]');
title(sprintf('%s', meas{2}.Track3_Name));
legend({'0-326', '326-end'}, 'Location', 'southwest');
hold off;

% Measurement 3 - Effect of the Hexapod
% The hexapod is turned off after 406 seconds (table [[tab:conf_meas3]]).

% #+CAPTION: Stages configuration - Measurement 3
% #+NAME: tab:conf_meas3
% | Time     | 0-406 | 406-end |
% |----------+-------+---------|
% | Ty       | OFF   | OFF     |
% | Ry       | *ON*  | *ON*    |
% | SlipRing | OFF   | OFF     |
% | Spindle  | OFF   | OFF     |
% | Hexa     | *ON*  | OFF     |

% The time domain result is shown figure [[fig:meas3]].


figure;
hold on;
plot(t3(ceil(380/dt):ceil(420/dt)), meas{3}.Track1(ceil(380/dt):ceil(420/dt)));
plot(t3(ceil(380/dt):ceil(420/dt)), meas{3}.Track2(ceil(380/dt):ceil(420/dt)));
plot(t3(ceil(380/dt):ceil(420/dt)), meas{3}.Track3(ceil(380/dt):ceil(420/dt)));
hold off;
xlabel('Time [s]'); ylabel('Velocity [m/s]');
legend({meas{3}.Track1_Name, meas{3}.Track2_Name, meas{3}.Track3_Name}, 'Location', 'northeast')



% #+LABEL: fig:meas3
% #+CAPTION: Time domain - measurement 3
% #+RESULTS: fig:meas3
% [[file:figs/meas3.png]]


figure;
hold on;
plot(t3, cumtrapz(t3, meas{3}.Track1));
plot(t3, cumtrapz(t3, meas{3}.Track2));
plot(t3, cumtrapz(t3, meas{3}.Track3));
hold off;
xlim([350, 450]);
xlabel('Time [s]'); ylabel('Displacement [m]');
legend({meas{3}.Track1_Name, meas{3}.Track2_Name, meas{3}.Track3_Name}, 'Location', 'northeast')



% #+LABEL: fig:meas3_disp
% #+CAPTION: Time domain - measurement 3
% #+RESULTS: fig:meas3_disp
% [[file:figs/meas3_disp.png]]

% We then compute the PSD of each track before and after turning off the hexapod and plot the results in the figures [[fig:meas3_hexa_z_psd]], [[fig:meas3_ry_z_psd]] and [[fig:meas3_ty_y_psd]].

[pxx311, f31] = pwelch(meas{3}.Track1(1:ceil(400/dt)),   psd_window, [], [], Fs);
[pxx312, f32] = pwelch(meas{3}.Track1(ceil(420/dt):end), psd_window, [], [], Fs);

[pxx321, ~] = pwelch(meas{3}.Track2(1:ceil(400/dt)),   psd_window, [], [], Fs);
[pxx322, ~] = pwelch(meas{3}.Track2(ceil(420/dt):end), psd_window, [], [], Fs);

[pxx331, ~] = pwelch(meas{3}.Track3(1:ceil(400/dt)),   psd_window, [], [], Fs);
[pxx332, ~] = pwelch(meas{3}.Track3(ceil(420/dt):end), psd_window, [], [], Fs);

figure;
hold on;
plot(f31, sqrt(pxx311)./abs(squeeze(freqresp(L4C_G, f31, 'Hz'))));
plot(f32, sqrt(pxx312)./abs(squeeze(freqresp(L4C_G, f32, 'Hz'))));
xlim([fmin, fmax]);
xticks([1, 10, 100]);
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('PSD [$m/s/\sqrt{Hz}$]');
title(sprintf('%s', meas{3}.Track1_Name));
legend({'0-400', '420-end'}, 'Location', 'southwest');
hold off;



% #+LABEL: fig:meas3_hexa_z_psd
% #+CAPTION: PSD of the Z velocity of the Hexapod - measurement 3
% #+RESULTS: fig:meas3_hexa_z_psd
% [[file:figs/meas3_hexa_z_psd.png]]


figure;
hold on;
plot(f31, sqrt(pxx321)./abs(squeeze(freqresp(L4C_G, f31, 'Hz'))));
plot(f32, sqrt(pxx322)./abs(squeeze(freqresp(L4C_G, f32, 'Hz'))));
xlim([fmin, fmax]);
xticks([1, 10, 100]);
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('PSD [$m/s/\sqrt{Hz}$]');
title(sprintf('%s', meas{3}.Track2_Name));
legend({'0-400', '420-end'}, 'Location', 'southwest');
hold off;



% #+LABEL: fig:meas3_ry_z_psd
% #+CAPTION: PSD of the Z velocity of the Ry stage - measurement 3
% #+RESULTS: fig:meas3_ry_z_psd
% [[file:figs/meas3_ry_z_psd.png]]


figure;
hold on;
plot(f31, sqrt(pxx331)./abs(squeeze(freqresp(L4C_G, f31, 'Hz'))));
plot(f32, sqrt(pxx332)./abs(squeeze(freqresp(L4C_G, f32, 'Hz'))));
xlim([fmin, fmax]);
xticks([1, 10, 100]);
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('PSD [$m/s/\sqrt{Hz}$]');
title(sprintf('%s', meas{3}.Track3_Name));
legend({'0-400', '420-end'}, 'Location', 'southwest');
hold off;

% Measurement 4 - Effect of the Splip-Ring and Spindle
% The slip ring is turned on at 300s, then the spindle is turned on at 620s (table [[tab:conf_meas4]]). The time domain signals are shown figure [[fig:meas4]].

% #+CAPTION: Stages configuration - Measurement 4
% #+NAME: tab:conf_meas4
% | Time     | 0-300 | 300-620 | 620-end |
% |----------+-------+---------+---------|
% | Ty       | OFF   | OFF     | OFF     |
% | Ry       | OFF   | OFF     | OFF     |
% | SlipRing | OFF   | *ON*    | *ON*    |
% | Spindle  | OFF   | OFF     | *ON*    |
% | Hexa     | OFF   | OFF     | OFF     |


figure;
hold on;
plot(t4, meas{4}.Track1);
plot(t4, meas{4}.Track2);
plot(t4, meas{4}.Track3);
hold off;
xlim([t4(1), t4(end)]);
xlabel('Time [s]'); ylabel('Velocity [m/s]');
legend({meas{4}.Track1_Name, meas{4}.Track2_Name, meas{4}.Track3_Name}, 'Location', 'southwest')



% #+LABEL: fig:meas4
% #+CAPTION: Time domain - measurement 4
% #+RESULTS: fig:meas4
% [[file:figs/meas4.png]]


figure;
subplot(1, 2, 1);
hold on;
plot(t4, cumtrapz(t4, meas{4}.Track1));
plot(t4, cumtrapz(t4, meas{4}.Track2));
plot(t4, cumtrapz(t4, meas{4}.Track3));
hold off;
xlim([250, 350]);
xlabel('Time [s]'); ylabel('Displacement [m]');
legend({meas{4}.Track1_Name, meas{4}.Track2_Name, meas{4}.Track3_Name}, 'Location', 'northwest')

subplot(1, 2, 2);
hold on;
plot(t4, cumtrapz(t4, meas{4}.Track1));
plot(t4, cumtrapz(t4, meas{4}.Track2));
plot(t4, cumtrapz(t4, meas{4}.Track3));
hold off;
xlim([600, 650]);
xlabel('Time [s]'); ylabel('Displacement [m]');



% #+LABEL: fig:meas4_int
% #+CAPTION: Time domain - measurement 4
% #+RESULTS: fig:meas4_int
% [[file:figs/meas4_int.png]]

% The PSD of each track are computed using the code below.

[pxx411, f41] = pwelch(meas{4}.Track1(1:ceil(280/dt)),            psd_window, [], [], Fs);
[pxx412, f42] = pwelch(meas{4}.Track1(ceil(280/dt):ceil(600/dt)), psd_window, [], [], Fs);
[pxx413, f43] = pwelch(meas{4}.Track1(ceil(640/dt):end),          psd_window, [], [], Fs);

[pxx421, ~] = pwelch(meas{4}.Track2(1:ceil(280/dt)),            psd_window, [], [], Fs);
[pxx422, ~] = pwelch(meas{4}.Track2(ceil(280/dt):ceil(600/dt)), psd_window, [], [], Fs);
[pxx423, ~] = pwelch(meas{4}.Track2(ceil(640/dt):end),          psd_window, [], [], Fs);

[pxx431, ~] = pwelch(meas{4}.Track3(1:ceil(280/dt)),            psd_window, [], [], Fs);
[pxx432, ~] = pwelch(meas{4}.Track3(ceil(280/dt):ceil(600/dt)), psd_window, [], [], Fs);
[pxx433, ~] = pwelch(meas{4}.Track3(ceil(640/dt):end),          psd_window, [], [], Fs);

f41 = f41(2:end);
f42 = f42(2:end);
f43 = f43(2:end);

pxx411 = pxx411(2:end);
pxx412 = pxx412(2:end);
pxx413 = pxx413(2:end);

pxx421 = pxx421(2:end);
pxx422 = pxx422(2:end);
pxx423 = pxx423(2:end);

pxx431 = pxx431(2:end);
pxx432 = pxx432(2:end);
pxx433 = pxx433(2:end);

figure;
hold on;
plot(f41, sqrt(pxx411)./abs(squeeze(freqresp(L4C_G, f41, 'Hz'))));
plot(f42, sqrt(pxx412)./abs(squeeze(freqresp(L4C_G, f42, 'Hz'))));
plot(f43, sqrt(pxx413)./abs(squeeze(freqresp(L4C_G, f43, 'Hz'))));
xlim([fmin, fmax]);
xticks([1, 10, 100]);
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('PSD [$m/s/\sqrt{Hz}$]');
title(sprintf('%s', meas{4}.Track1_Name));
legend({'0-280', '320-600', '640-end'}, 'Location', 'southwest');
hold off;



% #+LABEL: fig:meas4_hexa_z_psd
% #+CAPTION: PSD of the Z velocity of the Hexapod - measurement 4
% #+RESULTS: fig:meas4_hexa_z_psd
% [[file:figs/meas4_hexa_z_psd.png]]

% We plot the PSD of the displacement.

figure;
hold on;
plot(f41, sqrt(pxx411)./abs(squeeze(freqresp(L4C_G, f41, 'Hz')))./(2*pi*f41));
plot(f42, sqrt(pxx412)./abs(squeeze(freqresp(L4C_G, f42, 'Hz')))./(2*pi*f42));
plot(f43, sqrt(pxx413)./abs(squeeze(freqresp(L4C_G, f43, 'Hz')))./(2*pi*f43));
xlim([fmin, fmax]);
xticks([1, 10, 100]);
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('PSD [$m/\sqrt{Hz}$]');
title(sprintf('%s', meas{4}.Track1_Name));
legend({'0-280', '320-600', '640-end'}, 'Location', 'southwest');
hold off;



% #+LABEL: fig:meas4_hexa_z_psd_int
% #+CAPTION: PSD_INT of the Z velocity of the Hexapod - measurement 4
% #+RESULTS: fig:meas4_hexa_z_psd_int
% [[file:figs/meas4_hexa_z_psd_int.png]]

% And we compute the Cumulative amplitude spectrum.

figure;
hold on;
plot(f41, sqrt(cumsum(pxx431./abs(squeeze(freqresp(L4C_G, f41, 'Hz'))).^2./(2*pi*f41).*(f41 - [0; f41(1:end-1)]))));
plot(f42, sqrt(cumsum(pxx432./abs(squeeze(freqresp(L4C_G, f42, 'Hz'))).^2./(2*pi*f42).*(f42 - [0; f42(1:end-1)]))));
plot(f43, sqrt(cumsum(pxx433./abs(squeeze(freqresp(L4C_G, f43, 'Hz'))).^2./(2*pi*f43).*(f43 - [0; f43(1:end-1)]))));
xlim([fmin, fmax]);
xticks([1, 10, 100]);
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('CAS [$m$ rms]');
title(sprintf('%s', meas{4}.Track1_Name));
legend({'0-280', '320-600', '640-end'}, 'Location', 'southwest');
hold off;

figure;
hold on;
plot(f41, sqrt(pxx421)./abs(squeeze(freqresp(L4C_G, f41, 'Hz'))));
plot(f42, sqrt(pxx422)./abs(squeeze(freqresp(L4C_G, f42, 'Hz'))));
plot(f43, sqrt(pxx423)./abs(squeeze(freqresp(L4C_G, f43, 'Hz'))));
xlim([fmin, fmax]);
xticks([1, 10, 100]);
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('PSD [$m/s/\sqrt{Hz}$]');
title(sprintf('%s', meas{4}.Track2_Name));
legend({'0-280', '320-600', '640-end'}, 'Location', 'southwest');
hold off;



% #+LABEL: fig:meas4_ry_z_psd
% #+CAPTION: PSD of the Ry rotation in the Y direction - measurement 4
% #+RESULTS: fig:meas4_ry_z_psd
% [[file:figs/meas4_ry_z_psd.png]]



figure;
hold on;
plot(f41, sqrt(pxx431)./abs(squeeze(freqresp(L4C_G, f41, 'Hz'))));
plot(f42, sqrt(pxx432)./abs(squeeze(freqresp(L4C_G, f42, 'Hz'))));
plot(f43, sqrt(pxx433)./abs(squeeze(freqresp(L4C_G, f43, 'Hz'))));
xlim([fmin, fmax]);
xticks([1, 10, 100]);
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('PSD [$m/s/\sqrt{Hz}$]');
title(sprintf('%s', meas{4}.Track3_Name));
legend({'0-280', '320-600', '640-end'}, 'Location', 'southwest');
hold off;

% Measurement 5 - Transmission from ground to marble

% This measurement just consists of measurement of Y-Z motion of the ground and the marble.

% The time domain signals are shown on figure [[fig:meas5]].


figure;
hold on;
plot(t5, meas{5}.Track1);
plot(t5, meas{5}.Track2);
plot(t5, meas{5}.Track3);
plot(t5, meas{5}.Track4);
hold off;
xlabel('Time [s]'); ylabel('Velocity [m/s]');
legend({meas{5}.Track1_Name, meas{5}.Track2_Name, meas{5}.Track3_Name, meas{5}.Track4_Name}, 'Location', 'northeast')



% #+LABEL: fig:meas5
% #+CAPTION: Time domain - measurement 5
% #+RESULTS: fig:meas5
% [[file:figs/meas5.png]]

% We compute the PSD of each track and we plot the PSD of the Z motion for the ground and marble on figure [[fig:meas5_z_psd]] and for the Y motion on figure [[fig:meas5_y_psd]].


[pxx51, f51] = pwelch(meas{5}.Track1(:), psd_window, [], [], Fs);
[pxx52, f52] = pwelch(meas{5}.Track2(:), psd_window, [], [], Fs);
[pxx53, f53] = pwelch(meas{5}.Track3(:), psd_window, [], [], Fs);
[pxx54, f54] = pwelch(meas{5}.Track4(:), psd_window, [], [], Fs);

figure;
hold on;
plot(f51, sqrt(pxx51)./abs(squeeze(freqresp(L4C_G, f51, 'Hz'))));
plot(f52, sqrt(pxx52)./abs(squeeze(freqresp(L4C_G, f52, 'Hz'))));
xlim([fmin, fmax]);
xticks([1, 10, 100]);
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('PSD [$m/s/\sqrt{Hz}$]');
legend({meas{5}.Track1_Name, meas{5}.Track2_Name}, 'Location', 'northwest');
hold off;



% #+LABEL: fig:meas5_z_psd
% #+CAPTION: PSD of the ground and marble in the Z direction
% #+RESULTS: fig:meas5_z_psd
% [[file:figs/meas5_z_psd.png]]


figure;
hold on;
plot(f53, sqrt(pxx53)./abs(squeeze(freqresp(L4C_G, f53, 'Hz'))));
plot(f54, sqrt(pxx54)./abs(squeeze(freqresp(L4C_G, f54, 'Hz'))));
xlim([fmin, fmax]);
xticks([1, 10, 100]);
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('PSD [$m/s/\sqrt{Hz}$]');
legend({meas{5}.Track3_Name, meas{5}.Track4_Name}, 'Location', 'northwest');
hold off;



% #+LABEL: fig:meas5_y_psd
% #+CAPTION: PSD of the ground and marble in the Y direction
% #+RESULTS: fig:meas5_y_psd
% [[file:figs/meas5_y_psd.png]]

% Then, instead of looking at the Power Spectral Density, we can try to estimate the transfer function from a ground motion to the motion of the marble.
% The transfer functions are shown on figure [[fig:meas5_tf]] and the coherence on figure [[fig:meas5_coh]].


[tfz, fz] = tfestimate(meas{5}.Track1(:), meas{5}.Track2(:), psd_window, [], [], Fs);
[tfy, fy] = tfestimate(meas{5}.Track3(:), meas{5}.Track4(:), psd_window, [], [], Fs);

figure;
ax1 = subplot(2,1,1);
hold on;
plot(fz, abs(tfz));
plot(fy, abs(tfy));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude');
hold off;
ax2 = subplot(2,1,2);
hold on;
plot(fz, 180/pi*angle(tfz));
plot(fy, 180/pi*angle(tfy));
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
linkaxes([ax1,ax2],'x');
xlim([fmin, fmax]);
legend({'Z direction', 'Y direction'}, 'Location', 'southwest')



% #+LABEL: fig:meas5_tf
% #+CAPTION: Transfer function estimation - measurement 5
% #+RESULTS: fig:meas5_tf
% [[file:figs/meas5_tf.png]]


[cohz, fz] = mscohere(meas{5}.Track1(:), meas{5}.Track2(:), psd_window, [], [], Fs);
[cohy, fy] = mscohere(meas{5}.Track3(:), meas{5}.Track4(:), psd_window, [], [], Fs);

figure;
hold on;
plot(fz, cohz);
plot(fy, cohy);
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Coherence');
xlabel('Frequency [Hz]');
xlim([fmin, fmax]);
legend({'Z direction', 'Y direction'}, 'Location', 'southwest')