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

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

addpath('./mat/');

% Load Data
% As before, the identification data is loaded and any offset if removed.

id = load('identification_noise_opt_iff.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');

id.d = detrend(id.d, 0);
id.acc_1 = detrend(id.acc_1, 0);
id.acc_2 = detrend(id.acc_2, 0);
id.geo_1 = detrend(id.geo_1, 0);
id.geo_2 = detrend(id.geo_2, 0);
id.f_meas = detrend(id.f_meas, 0);

% ASD of the Measured displacement
% The Power Spectral Density of the displacement as measured by the interferometer and the inertial sensors is computed.

Ts = id.t(2) - id.t(1);
win = hann(ceil(10/Ts));

[p_id_d,     f] = pwelch(id.d,      win, [], [], 1/Ts);
[p_id_acc1,  ~] = pwelch(id.acc_1,  win, [], [], 1/Ts);
[p_id_acc2,  ~] = pwelch(id.acc_2,  win, [], [], 1/Ts);
[p_id_geo1,  ~] = pwelch(id.geo_1,  win, [], [], 1/Ts);
[p_id_geo2,  ~] = pwelch(id.geo_2,  win, [], [], 1/Ts);



% Let's use a model of the accelerometer and geophone to compute the motion from the measured voltage.

G_acc = 1/(1 + s/2/pi/2500); % [V/(m/s2)]
G_geo = -1200*s^2/(s^2 + 2*0.7*2*pi*2*s + (2*pi*2)^2); % [[V/(m/s)]



% The obtained ASD in $m/\sqrt{Hz}$ is shown in Figure [[fig:measure_displacement_all_sensors]].


figure;
hold on;
set(gca, 'ColorOrderIndex', 1);
plot(f, sqrt(p_id_acc1)./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))), ...
     'DisplayName', 'Accelerometer');
set(gca, 'ColorOrderIndex', 1);
plot(f, sqrt(p_id_acc2)./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))), ...
     'HandleVisibility', 'off');
set(gca, 'ColorOrderIndex', 2);
plot(f, sqrt(p_id_geo1)./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))), ...
     'DisplayName', 'Geophone');
set(gca, 'ColorOrderIndex', 2);
plot(f, sqrt(p_id_geo2)./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))), ...
     'HandleVisibility', 'off');
set(gca, 'ColorOrderIndex', 3);
plot(f, sqrt(p_id_d), 'DisplayName', 'Interferometer');
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('ASD [$m/\sqrt{Hz}$]'); xlabel('Frequency [Hz]');
title('Huddle Test')
legend();
xlim([1e-1, 5e3]); ylim([1e-12, 1e-4]);

% ASD of the Sensor Noise
% The noise of a sensor can be estimated using two identical sensors by computing:
% - the Power Spectral Density of the measured motion by the two sensors
% - the Cross Spectral Density between the two sensors (coherence)

% This technique to estimate the sensor noise is described in cite:barzilai98_techn_measur_noise_sensor_presen.

% The Power Spectral Density of the sensor noise can be estimated using the following equation:
% \begin{equation}
%   |S_n(\omega)| = |S_{x_1}(\omega)| \Big( 1 - \gamma_{x_1 x_2}(\omega) \Big)
% \end{equation}
% with $S_{x_1}$ the PSD of one of the sensor and $\gamma_{x_1 x_2}$ the coherence between the two sensors.

% The coherence between the two accelerometers and between the two geophones is computed.

[coh_acc, ~] = mscohere(id.acc_1, id.acc_2, win, [], [], 1/Ts);
[coh_geo, ~] = mscohere(id.geo_1, id.geo_2, win, [], [], 1/Ts);



% Finally, the Power Spectral Density of the sensors is computed and converted in $[m^2/Hz]$.

pN_acc = p_id_acc1.*(1 - coh_acc) .* ... % [V^2/Hz]
         1./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))).^2; % [(m/V)^2]
pN_geo = p_id_geo1.*(1 - coh_geo) .* ... % [V^2/Hz]
         1./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))).^2; % [(m/V)^2]



% The ASD of obtained noises are compared with the ASD of the measured signals in Figure [[fig:noise_inertial_sensors_comparison]].

figure;
hold on;
plot(f, sqrt(p_id_acc1)./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))), ...
     'DisplayName', 'Accelerometer');
plot(f, sqrt(p_id_geo1)./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))), ...
     'DisplayName', 'Geophone');
plot(f, sqrt(pN_acc), '-', 'DisplayName', 'Accelerometers - Noise');
plot(f, sqrt(pN_geo), '-', 'DisplayName', 'Geophones - Noise');
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{m}{\sqrt{Hz}}\right]$');
xlim([1, 5000]); ylim([1e-12, 1e-5]);
legend('location', 'northeast');

% Noise Model
% Transfer functions are adjusted in order to fit the ASD of the sensor noises (expressed in $[m/s/\sqrt{Hz}]$ for more easy fitting).

% These transfer functions are defined below and compared with the measured ASD in Figure [[fig:noise_models_velocity]].

N_acc = 1*(s/(2*pi*2000) + 1)^2/(s + 0.1*2*pi)/(s + 1e3*2*pi); % [m/sqrt(Hz)]
N_geo = 4e-4*(s/(2*pi*200) + 1)/(s + 1e3*2*pi); % [m/sqrt(Hz)]

freqs = logspace(0, 4, 1000);

figure;
hold on;
plot(f, sqrt(pN_acc).*(2*pi*f), '-', 'DisplayName', 'Accelerometers - Noise');
plot(f, sqrt(pN_geo).*(2*pi*f), '-', 'DisplayName', 'Geophones - Noise');
set(gca, 'ColorOrderIndex', 1);
plot(freqs, abs(squeeze(freqresp(N_acc, freqs, 'Hz'))), '--', 'DisplayName', 'Accelerometer - Noise Model');
plot(freqs, abs(squeeze(freqresp(N_geo, freqs, 'Hz'))), '--', 'DisplayName', 'Geophones - Noise Model');
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{m/s}{\sqrt{Hz}}\right]$');
xlim([1, 5000]);
legend('location', 'northeast');

% $\mathcal{H}_2$ Synthesis of the Complementary Filters
% We now wish to synthesize two complementary filters to merge the geophone and the accelerometer signal in such a way that the fused signal has the lowest possible RMS noise.

% To do so, we use the $\mathcal{H}_2$ synthesis where the transfer functions representing the noise density of both sensors are used as weights.

% The generalized plant used for the synthesis is defined below.

P = [0      N_acc  1;
     N_geo -N_acc  0];



% And the $\mathcal{H}_2$ synthesis is done using the =h2syn= command.

[H_geo, ~, gamma] = h2syn(P, 1, 1);
H_acc = 1 - H_geo;



% The obtained complementary filters are shown in Figure [[fig:complementary_filters_velocity_H2]].


figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(H_acc, freqs, 'Hz'))), '-', 'DisplayName', '$H_{acc}$');
plot(freqs, abs(squeeze(freqresp(H_geo, freqs, 'Hz'))), '-', 'DisplayName', '$H_{geo}$');
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Magnitude');  set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'northeast');

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(H_acc, freqs, 'Hz'))), '-');
plot(freqs, 180/pi*angle(squeeze(freqresp(H_geo, freqs, 'Hz'))), '-');
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Phase'); xlabel('Frequency [Hz]');
hold off;
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);

linkaxes([ax1,ax2], 'x');
xlim([freqs(1), freqs(end)]);

% Results
% Finally, the signals of both sensors are merged using the complementary filters and the super sensor noise is estimated and compared with the individual sensor noises in Figure [[fig:super_sensor_noise_asd_velocity]].


freqs = logspace(0, 4, 1000);

figure;
hold on;
plot(f, pN_acc.*(2*pi*f), '-', 'DisplayName', 'Accelerometers - Noise');
plot(f, pN_geo.*(2*pi*f), '-', 'DisplayName', 'Geophones - Noise');
plot(f, sqrt((pN_acc.*(2*pi*f)).^2.*abs(squeeze(freqresp(H_acc, f, 'Hz'))).^2 + (pN_geo.*(2*pi*f)).^2.*abs(squeeze(freqresp(H_geo, f, 'Hz'))).^2), 'k-', 'DisplayName', 'Super Sensor - Noise');
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{m/s}{\sqrt{Hz}}\right]$');
xlim([1, 5000]);
legend('location', 'northeast');



% #+name: fig:super_sensor_noise_asd_velocity
% #+caption: ASD of the super sensor noise (velocity)
% #+RESULTS:
% [[file:figs/super_sensor_noise_asd_velocity.png]]

% Finally, the Cumulative Power Spectrum is computed and compared in Figure [[fig:super_sensor_noise_cas_velocity]].

[~, i_1Hz] = min(abs(f - 1));

CPS_acc = 1/pi*flip(-cumtrapz(2*pi*flip(f), flip((pN_acc.*(2*pi*f)).^2)));
CPS_geo = 1/pi*flip(-cumtrapz(2*pi*flip(f), flip((pN_geo.*(2*pi*f)).^2)));
CPS_SS  = 1/pi*flip(-cumtrapz(2*pi*flip(f), flip((pN_acc.*(2*pi*f)).^2.*abs(squeeze(freqresp(H_acc, f, 'Hz'))).^2 + (pN_geo.*(2*pi*f)).^2.*abs(squeeze(freqresp(H_geo, f, 'Hz'))).^2)));

figure;
hold on;
plot(f, CPS_acc, '-',  'DisplayName', sprintf('$\\sigma_{\\hat{x}_{acc}} = %.0f\\,\\mu m/s (rms)$', 1e6*sqrt(CPS_acc(i_1Hz))));
plot(f, CPS_geo, '-',  'DisplayName', sprintf('$\\sigma_{\\hat{x}_{geo}} = %.0f\\,\\mu m/s (rms)$', 1e6*sqrt(CPS_geo(i_1Hz))));
plot(f, CPS_SS,  'k-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}} = %.0f\\,\\mu m/s (rms)$',       1e6*sqrt(CPS_SS(i_1Hz))));
set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum');
hold off;
xlim([1, 4e3]);
legend('location', 'northeast');