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

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

% Load Data
% The measurement data are loaded and the offset are removed using the =detrend= command.


load('int_enc_huddle_test.mat', 'interferometer', 'encoder', 't');

interferometer = detrend(interferometer, 0);
encoder = detrend(encoder, 0);

% Time Domain Results
% The measurement of both the encoder and interferometer are shown in Figure [[fig:huddle_test_time_domain]].


figure;
hold on;
plot(t, encoder, 'DisplayName', 'Encoder')
plot(t, interferometer, 'DisplayName', 'Interferometer')
hold off;
xlabel('Time [s]'); ylabel('Displacement [m]');
legend('location', 'northeast');



% #+name: fig:huddle_test_time_domain
% #+caption: Huddle test - Time domain signals
% #+RESULTS:
% [[file:figs/huddle_test_time_domain.png]]

% The raw signals are filtered with a Low Pass filter (defined below) such that we can see the low frequency motion (Figure [[fig:huddle_test_time_domain_filtered]]).

G_lpf = 1/(1 + s/2/pi/10);

figure;
hold on;
plot(t, lsim(G_lpf, encoder, t), 'DisplayName', 'Encoder')
plot(t, lsim(G_lpf, interferometer, t), 'DisplayName', 'Interferometer')
hold off;
xlabel('Time [s]'); ylabel('Displacement [m]');
legend('location', 'northeast');

% Frequency Domain Noise
% The noise of the measurement (supposing there is no motion) is now translated in the frequency domain by computed the Amplitude Spectral Density.


Ts = 1e-4;
win = hann(ceil(10/Ts));

[p_i, f] = pwelch(interferometer, win, [], [], 1/Ts);
[p_e, ~] = pwelch(encoder,        win, [], [], 1/Ts);



% The comparison of the ASD of the encoder and interferometer are shown in Figure [[fig:huddle_test_asd]].

% It is clear that although the encoder exhibit higher frequency noise, is it more stable at low frequency as the length of the beam path in the air is much smaller and thus changed of temperature/pressure/humity of the air has much smaller effect on the measured displacement.


figure;
hold on;
plot(f, sqrt(p_i), 'DisplayName', 'Interferometer');
plot(f, sqrt(p_e), 'DisplayName', 'Encoder');
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('ASD [$m/\sqrt{Hz}$]'); xlabel('Frequency [Hz]');
legend();
xlim([1e-1, 5e3]);
