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

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

addpath('./mat/');

% Load Data
% The data is loaded in the Matlab workspace.

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



% Then, any offset is removed.

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

% Excitation Signal
% The generated voltage used to excite the system is a white noise and can be seen in Figure [[fig:excitation_signal_first_identification]].


figure;
plot(id_ol.t, id_ol.u)
xlabel('Time [s]'); ylabel('Voltage [V]');

% Identified Plant
% The transfer function from the excitation voltage to the mass displacement and to the force sensor stack voltage are identified using the =tfestimate= command.


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

[tf_fmeas_est, f] = tfestimate(id_ol.u, id_ol.f_meas, win, [], [], 1/Ts); % [V/V]
[tf_G_ol_est,  ~] = tfestimate(id_ol.u, id_ol.d, win, [], [], 1/Ts); % [m/V]



% The bode plots of the obtained dynamics are shown in Figures [[fig:force_sensor_bode_plot]] and [[fig:displacement_sensor_bode_plot]].


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

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(tf_fmeas_est), '-')
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude'); set(gca, 'XTickLabel',[]);

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(tf_fmeas_est), '-')
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([1, 1e3]);



% #+name: fig:force_sensor_bode_plot
% #+caption: Bode plot of the dynamics from excitation voltage to measured force sensor stack voltage
% #+RESULTS:
% [[file:figs/force_sensor_bode_plot.png]]


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

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(tf_G_ol_est), '-')
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(tf_G_ol_est), '-')
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([1, 1e3]);

% Simscape Model - Comparison
% A simscape model representing the test-bench has been developed.
% The same transfer functions as the one identified using the test-bench can be obtained thanks to the simscape model.

% They are compared in Figure [[fig:simscape_comp_iff_plant]] and [[fig:simscape_comp_disp_plant]].
% It is shown that there is a good agreement between the model and the experiment.


load('piezo_amplified_3d.mat', 'int_xyz', 'int_i', 'n_xyz', 'n_i', 'nodes', 'M', 'K');

m = 10;
Kiff = tf(0);

%% Name of the Simulink File
mdl = 'sensor_fusion_test_bench_simscape';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Fd'], 1, 'openinput');  io_i = io_i + 1; % External Vertical Force [N]
io(io_i) = linio([mdl, '/w'],  1, 'openinput');  io_i = io_i + 1; % Base Motion [m]
io(io_i) = linio([mdl, '/Va'], 1, 'openinput');  io_i = io_i + 1; % Actuator Voltage [V]
io(io_i) = linio([mdl, '/Interferometer'],  1, 'openoutput'); io_i = io_i + 1; % Vertical Displacement [m]
io(io_i) = linio([mdl, '/Voltage_Conditioner'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensor [V]

options = linearizeOptions('SampleTime', 1e-4);
G = linearize(mdl, io, options);

G.InputName = {'Fd', 'w', 'Va'};
G.OutputName = {'y', 'Vs'};

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

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(tf_fmeas_est), 'DisplayName', 'Identification')
plot(f, abs(squeeze(freqresp(G('Vs', 'Va'), f, 'Hz'))), 'DisplayName', 'Simscape Model')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-1, 1e3]);
legend('location', 'northwest');

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(tf_fmeas_est))
plot(f, 180/pi*angle(squeeze(freqresp(G('Vs', 'Va'), f, '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([1, 5e3]);



% #+name: fig:simscape_comp_iff_plant
% #+caption: Comparison of the dynamics from excitation voltage to measured force sensor stack voltage - Identified dynamics and Simscape Model
% #+RESULTS:
% [[file:figs/simscape_comp_iff_plant.png]]


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

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(tf_G_ol_est), 'DisplayName', 'Identification')
plot(f, abs(squeeze(freqresp(G('y', 'Va'), f, 'Hz'))), 'DisplayName', 'Simscape Model')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(tf_G_ol_est))
plot(f, 180/pi*angle(squeeze(freqresp(G('y', 'Va'), f, '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([1, 5e3]);

% Integral Force Feedback
% The force sensor stack can be used to damp the system.
% This makes the system easier to excite properly without too much amplification near resonances.

% This is done thanks to the integral force feedback control architecture.

% The force sensor stack signal is integrated (or rather low pass filtered) and fed back to the force sensor stacks.

% The low pass filter used as the controller is defined below:

Kiff = 102/(s + 2*pi*2);



% The integral force feedback control strategy is applied to the simscape model as well as to the real test bench.


%% Name of the Simulink File
mdl = 'sensor_fusion_test_bench_simscape';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Fd'], 1, 'openinput');  io_i = io_i + 1; % External Vertical Force [N]
io(io_i) = linio([mdl, '/w'],  1, 'openinput');  io_i = io_i + 1; % Base Motion [m]
io(io_i) = linio([mdl, '/Va'], 1, 'openinput');  io_i = io_i + 1; % Actuator Voltage [V]
io(io_i) = linio([mdl, '/Interferometer'],  1, 'openoutput'); io_i = io_i + 1; % Vertical Displacement [m]
io(io_i) = linio([mdl, '/Voltage_Conditioner'], 1, 'output'); io_i = io_i + 1; % Force Sensor [V]

options = linearizeOptions('SampleTime', 1e-4);
G_cl = linearize(mdl, io, options);

G_cl.InputName = {'Fd', 'w', 'Va'};
G_cl.OutputName = {'y', 'Vs'};



% The damped system is then identified again using a noise excitation.

% The data is loaded into Matlab and any offset is removed.

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

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



% The transfer functions are estimated using =tfestimate=.

[tf_G_cl_est, ~] = tfestimate(id_cl.u, id_cl.d, win, [], [], 1/Ts);
[co_G_cl_est, ~] = mscohere(  id_cl.u, id_cl.d, win, [], [], 1/Ts);



% The dynamics from driving voltage to the measured displacement are compared both in the open-loop and IFF case, and for the test-bench experimental identification and for the Simscape model in Figure [[fig:iff_ol_cl_identified_simscape_comp]].
% This shows that the Integral Force Feedback architecture effectively damps the first resonance of the system.


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

ax1 = nexttile([2,1]);
hold on;
set(gca, 'ColorOrderIndex', 1);
plot(f, abs(tf_G_ol_est), '-', 'DisplayName', 'OL - Ident.')
set(gca, 'ColorOrderIndex', 1);
plot(f, abs(squeeze(freqresp(G('y', 'Va'), f, 'Hz'))), '--', 'DisplayName', 'OL - Simscape')
set(gca, 'ColorOrderIndex', 2);
plot(f, abs(tf_G_cl_est), '-', 'DisplayName', 'CL - Ident.')
set(gca, 'ColorOrderIndex', 2);
plot(f, abs(squeeze(freqresp(G_cl('y', 'Va'), f, 'Hz'))), '--', 'DisplayName', 'CL - Simscape')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
legend('location', 'northeast');
hold off;
ylim([1e-7, 1e-3]);

ax2 = nexttile;
hold on;
set(gca, 'ColorOrderIndex', 1);
plot(f, 180/pi*angle(tf_G_ol_est), '-')
set(gca, 'ColorOrderIndex', 1);
plot(f, 180/pi*angle(squeeze(freqresp(G('y', 'Va'), f, 'Hz'))), '--')
set(gca, 'ColorOrderIndex', 2);
plot(f, 180/pi*angle(tf_G_cl_est), '-')
set(gca, 'ColorOrderIndex', 2);
plot(f, 180/pi*angle(squeeze(freqresp(G_cl('y', 'Va'), f, '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([1, 5e3]);

% Inertial Sensors
% In order to estimate the dynamics of the inertial sensor (the transfer function from the "absolute" displacement to the measured voltage), the following experiment can be performed:
% - The mass is excited such that is relative displacement as measured by the interferometer is much larger that the ground "absolute" motion.
% - The transfer function from the measured displacement by the interferometer to the measured voltage generated by the inertial sensors can be estimated.

% The first point is quite important in order to have a good coherence between the interferometer measurement and the inertial sensor measurement.

% Here, a first identification is performed were the excitation signal is a white noise.


% As usual, the data is loaded and any offset is 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);



% Then the transfer functions from the measured displacement by the interferometer to the generated voltage of the inertial sensors are computed..

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

[tf_acc1_est, f] = tfestimate(id.d, id.acc_1, win, [], [], 1/Ts);
[co_acc1_est, ~] = mscohere(  id.d, id.acc_1, win, [], [], 1/Ts);
[tf_acc2_est, ~] = tfestimate(id.d, id.acc_2, win, [], [], 1/Ts);
[co_acc2_est, ~] = mscohere(  id.d, id.acc_2, win, [], [], 1/Ts);

[tf_geo1_est, ~] = tfestimate(id.d, id.geo_1, win, [], [], 1/Ts);
[co_geo1_est, ~] = mscohere(  id.d, id.geo_1, win, [], [], 1/Ts);
[tf_geo2_est, ~] = tfestimate(id.d, id.geo_2, win, [], [], 1/Ts);
[co_geo2_est, ~] = mscohere(  id.d, id.geo_2, win, [], [], 1/Ts);



% The same transfer functions are estimated using the Simscape model.


m = 10;
Kiff = tf(0);

%% Name of the Simulink File
mdl = 'sensor_fusion_test_bench_simscape';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Va'], 1, 'openinput');  io_i = io_i + 1; % Actuator Voltage [V]
io(io_i) = linio([mdl, '/Interferometer'],  1, 'openoutput'); io_i = io_i + 1; % Vertical Displacement [m]
io(io_i) = linio([mdl, '/Vertical_Accelerometer_1'], 1, 'openoutput'); io_i = io_i + 1; % Accelerometer [V]
io(io_i) = linio([mdl, '/Voltage_Ampl_geo_1'], 1, 'openoutput'); io_i = io_i + 1; % Geophone [V]

options = linearizeOptions('SampleTime', 1e-4);
G = linearize(mdl, io, options);

G.InputName = {'Va'};
G.OutputName = {'y', 'a', 'v'};

G_acc = G('a', 'Va')*inv(G('y', 'Va')); % [V/m]
G_geo = G('v', 'Va')*inv(G('y', 'Va')); % [V/m]



% The obtained dynamics of the accelerometer are compared in Figure [[fig:comp_dynamics_accelerometer]] while the one of the geophones are compared in Figure [[fig:comp_dynamics_geophone]].


freqs = logspace(-1, 4, 1000)';

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

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(tf_acc1_est./(1i*2*pi*f).^2), '.')
plot(f, abs(tf_acc2_est./(1i*2*pi*f).^2), '.')
plot(freqs, abs(squeeze(freqresp(G_acc, freqs, 'Hz'))./(1i*2*pi*freqs).^2), 'k-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude $\left[\frac{V}{m/s^2}\right]$');  set(gca, 'XTickLabel',[]);
hold off;

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(tf_acc1_est./(1i*2*pi*f).^2), '.')
plot(f, 180/pi*angle(tf_acc2_est./(1i*2*pi*f).^2), '.')
plot(freqs, 180/pi*angle(squeeze(freqresp(G_acc, freqs, 'Hz'))./(1i*2*pi*freqs).^2), 'k-')
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([2, 2e3]);



% #+name: fig:comp_dynamics_accelerometer
% #+caption: Comparison of the measured accelerometer dynamics
% #+RESULTS:
% [[file:figs/comp_dynamics_accelerometer.png]]


freqs = logspace(-1, 4, 1000)';

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

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(tf_geo1_est./(1i*2*pi*f)), '.')
plot(f, abs(tf_geo2_est./(1i*2*pi*f)), '.')
plot(freqs, abs(squeeze(freqresp(G_geo, freqs, 'Hz'))./(1i*2*pi*freqs)), 'k-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude $\left[\frac{V}{m/s}\right]$');  set(gca, 'XTickLabel',[]);
hold off;

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(tf_geo1_est./(1i*2*pi*f)), '.')
plot(f, 180/pi*angle(tf_geo2_est./(1i*2*pi*f)), '.')
plot(freqs, 180/pi*angle(squeeze(freqresp(G_geo, freqs, 'Hz'))./(1i*2*pi*freqs)), 'k-')
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([0.5, 2e3]);