diff --git a/index.org b/index.org
index 859c9fd..7b86462 100644
--- a/index.org
+++ b/index.org
@@ -86,7 +86,7 @@ This takes into account the sensibility of the sensor and possible integration t
 #+begin_src matlab
   acc_1 = lsim(inv(G_acc), acc_1, t);
   acc_2 = lsim(inv(G_acc), acc_2, t);
-  geo_1 = 1e2*lsim(inv(G_geo), geo_1, t);
+  geo_1 = lsim(inv(G_geo), geo_1, t);
   geo_2 = lsim(inv(G_geo), geo_2, t);
 #+end_src
 
diff --git a/mat/huddle_test.mat b/mat/huddle_test.mat
index f947643..5c80fad 100644
Binary files a/mat/huddle_test.mat and b/mat/huddle_test.mat differ
diff --git a/mat/identification_chirp_40_400.mat b/mat/identification_chirp_40_400.mat
new file mode 100644
index 0000000..95ec0e9
Binary files /dev/null and b/mat/identification_chirp_40_400.mat differ
diff --git a/mat/identification_chirp_40_400_iff.mat b/mat/identification_chirp_40_400_iff.mat
new file mode 100644
index 0000000..e522666
Binary files /dev/null and b/mat/identification_chirp_40_400_iff.mat differ
diff --git a/mat/identification_noise_iff.mat b/mat/identification_noise_iff.mat
new file mode 100644
index 0000000..d65fa91
Binary files /dev/null and b/mat/identification_noise_iff.mat differ
diff --git a/runtest.m b/runtest.m
index 6044317..eec41cd 100644
--- a/runtest.m
+++ b/runtest.m
@@ -8,53 +8,85 @@ close(f);
 %% Convert the Data
 data = SimulinkRealTime.utils.getFileScopeData('data/apa95ml.dat').data;
 
-acc_1 = data(:, 1);
-acc_2 = data(:, 2);
-geo_1 = data(:, 3);
-geo_2 = data(:, 4);
-t = data(:, 5);
+d = data(:, 1);
+acc_1 = data(:, 2);
+acc_2 = data(:, 3);
+geo_1 = data(:, 4);
+geo_2 = data(:, 5);
+u = data(:, 6);
+f_meas = data(:, 7);
+t = data(:, 8);
 
-save('./mat/huddle_test.mat', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 't');
+save('./mat/identification_noise_iff.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
 
 %%
-figure;
-subplot(1,2,1);
-plot(t, u)
-subplot(1,2,2);
-plot(t, y)
-
-%%
-load('../mat/apa95ml_5kg_10V.mat', 'u', 't', 'y');
-ht = load('../mat/huddle_test.mat', 'u', 't', 'y');
-u = u - mean(u);
-y = y - mean(y);
-
-%%
-[pxx, f] = pwelch(y, win, [], [], 1/Ts);
-[pht, ~] = pwelch(ht.y, win, [], [], 1/Ts);
-
-figure;
-
-hold on;
-plot(f, pxx);
-plot(f, pht);
-hold off;
-set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
-ylabel('PSD'); xlabel('Frequency [Hz]');
+d = detrend(d, 0);
+acc_1 = detrend(acc_1, 0);
+acc_2 = detrend(acc_2, 0);
+geo_1 = detrend(geo_1, 0);
+geo_2 = detrend(geo_2, 0);
+u = detrend(u, 0);
 
 %%
 run setup;
 
-win = hann(ceil(0.1/Ts));
+win = hann(ceil(10/Ts));
 
-[tf_est, f] = tfestimate(u, y, win, [], [], 1/Ts);
-[co_est, ~] = mscohere(u, y, win, [], [], 1/Ts);
+[p_d, f] = pwelch(d, win, [], [], 1/Ts);
+[p_acc1, ~] = pwelch(acc_1, win, [], [], 1/Ts);
+[p_acc2, ~] = pwelch(acc_2, win, [], [], 1/Ts);
+[p_geo1, ~] = pwelch(geo_1, win, [], [], 1/Ts);
+[p_geo2, ~] = pwelch(geo_2, win, [], [], 1/Ts);
+
+%%
+figure;
+hold on;
+plot(f, p_acc1);
+plot(f, p_acc2);
+hold off;
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('PSD [$(m/s^2)^2/Hz$]'); xlabel('Frequency [Hz]');
+
+figure;
+hold on;
+plot(f, p_geo1);
+plot(f, p_geo2);
+hold off;
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('PSD [$(m/s)^2/Hz$]'); xlabel('Frequency [Hz]');
+
+figure;
+hold on;
+plot(f, p_d);
+hold off;
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('PSD [$m^2/Hz$]'); xlabel('Frequency [Hz]');
+
+%%
+run setup;
+
+win = hann(ceil(10/Ts));
+
+[tf_geo1_est, f] = tfestimate(d, geo_1, win, [], [], 1/Ts);
+[co_geo1_est, ~] = mscohere(d, geo_1, win, [], [], 1/Ts);
+
+[tf_geo2_est, ~] = tfestimate(d, geo_2, win, [], [], 1/Ts);
+[co_geo2_est, ~] = mscohere(d, geo_2, win, [], [], 1/Ts);
+
+[tf_acc1_est, ~] = tfestimate(d, acc_1, win, [], [], 1/Ts);
+[co_acc1_est, ~] = mscohere(d, acc_1, win, [], [], 1/Ts);
+
+[tf_acc2_est, ~] = tfestimate(d, acc_2, win, [], [], 1/Ts);
+[co_acc2_est, ~] = mscohere(d, acc_2, win, [], [], 1/Ts);
 
 %%
 figure;
 
 hold on;
-plot(f, co_est, 'k-')
+plot(f, co_geo1_est, '-')
+plot(f, co_geo2_est, '-')
+plot(f, co_acc1_est, '-')
+plot(f, co_acc2_est, '-')
 set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
 ylabel('Coherence'); xlabel('Frequency [Hz]');
 hold off;
@@ -63,51 +95,61 @@ hold off;
 figure;
 ax1 = subplot(2, 1, 1);
 hold on;
-plot(f, abs(tf_est), 'k-', 'DisplayName', 'Identified')
+plot(f, abs(tf_geo1_est), '-', 'DisplayName', 'Geo1')
 set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
 ylabel('Amplitude'); xlabel('Frequency [Hz]');
 hold off;
 
 ax2 = subplot(2, 1, 2);
 hold on;
-plot(f, 180/pi*unwrap(angle(-tf_est)), 'k-')
+plot(f, 180/pi*unwrap(angle(tf_geo1_est)), '-')
 set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
 ylabel('Phase'); xlabel('Frequency [Hz]');
 hold off;
 
 linkaxes([ax1,ax2], 'x');
-xlim([10, 5000]);
-
-%%
-win = hann(ceil(0.2/Ts));
-
-[tf_est, f] = tfestimate(u, v, win, [], [], 1/Ts);
-[co_est, ~] = mscohere(u, v, win, [], [], 1/Ts);
-
-%%
-figure;
-
-hold on;
-plot(f, co_est, 'k-')
-set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
-ylabel('Coherence'); xlabel('Frequency [Hz]');
-hold off;
 
 %%
 figure;
 ax1 = subplot(2, 1, 1);
 hold on;
-plot(f, abs(tf_est), 'k-', 'DisplayName', 'Identified')
+plot(f, abs(tf_acc1_est), '-', 'DisplayName', 'Acc1')
+plot(f, abs(tf_acc2_est), '-', 'DisplayName', 'Acc2')
 set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
 ylabel('Amplitude'); xlabel('Frequency [Hz]');
 hold off;
 
 ax2 = subplot(2, 1, 2);
 hold on;
-plot(f, 180/pi*unwrap(angle(-tf_est)), 'k-')
+plot(f, 180/pi*unwrap(angle(tf_acc1_est)), '-')
+plot(f, 180/pi*unwrap(angle(tf_acc2_est)), '-')
 set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
 ylabel('Phase'); xlabel('Frequency [Hz]');
 hold off;
 
 linkaxes([ax1,ax2], 'x');
-xlim([10, 5000]);
+
+%%
+win = hann(ceil(10/Ts));
+
+[p_acc_1, f] = pwelch(acc_1, win, [], [], 1/Ts);
+[co_acc12, ~] = mscohere(acc_1, acc_2, win, [], [], 1/Ts);
+
+[p_geo_1, ~] = pwelch(geo_1, win, [], [], 1/Ts);
+[co_geo12, ~] = mscohere(geo_1, geo_2, win, [], [], 1/Ts);
+
+p_acc_N = p_acc_1.*(1 - co_acc12);
+p_geo_N = p_geo_1.*(1 - co_geo12);
+
+
+figure;
+hold on;
+plot(f, sqrt(p_acc_N)./abs(tf_acc1_est));
+plot(f, sqrt(p_geo_N)./abs(tf_geo1_est));
+hold off;
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('PSD'); xlabel('Frequency [Hz]');
+
+
+
+
diff --git a/sensor_fusion_analysis.m b/sensor_fusion_analysis.m
new file mode 100644
index 0000000..8aed01d
--- /dev/null
+++ b/sensor_fusion_analysis.m
@@ -0,0 +1,421 @@
+
+%% Huddle Test
+ht = load('./mat/huddle_test.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
+
+% Detrend Data
+ht.d = detrend(ht.d, 0);
+ht.acc_1 = detrend(ht.acc_1, 0);
+ht.acc_2 = detrend(ht.acc_2, 0);
+ht.geo_1 = detrend(ht.geo_1, 0);
+ht.geo_2 = detrend(ht.geo_2, 0);
+ht.f_meas = detrend(ht.f_meas, 0);
+
+% Compute PSD
+run setup;
+
+win = hann(ceil(10/Ts));
+
+[p_d, f] = pwelch(ht.d, win, [], [], 1/Ts);
+[p_acc1, ~] = pwelch(ht.acc_1, win, [], [], 1/Ts);
+[p_acc2, ~] = pwelch(ht.acc_2, win, [], [], 1/Ts);
+[p_geo1, ~] = pwelch(ht.geo_1, win, [], [], 1/Ts);
+[p_geo2, ~] = pwelch(ht.geo_2, win, [], [], 1/Ts);
+[p_fmeas, ~] = pwelch(ht.f_meas, win, [], [], 1/Ts);
+
+% Plot PSD
+figure;
+hold on;
+plot(f, p_acc1);
+plot(f, p_acc2);
+hold off;
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('PSD [$V^2/Hz$]'); xlabel('Frequency [Hz]');
+title('Huddle Test - Accelerometers')
+
+figure;
+hold on;
+plot(f, p_geo1);
+plot(f, p_geo2);
+hold off;
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('PSD [$V^2/Hz$]'); xlabel('Frequency [Hz]');
+title('Huddle Test - Geophones')
+
+figure;
+hold on;
+plot(f, p_d);
+hold off;
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('PSD [$m^2/Hz$]'); xlabel('Frequency [Hz]');
+title('Huddle Test - Interferometers')
+
+figure;
+hold on;
+plot(f, p_fmeas);
+hold off;
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('PSD [$V^2/Hz$]'); xlabel('Frequency [Hz]');
+title('Huddle Test - Force Sensor')
+
+%% Accelerometer and Geophone Models
+% Accelerometer used: https://www.pcb.com/products?model=393B05
+% Geophone used: L22 https://www.sercel.com/products/Lists/ProductSpecification/Geophones_brochure_Sercel_EN.pdf
+
+G_acc = 1/(1 + s/2/pi/2500); % [V/(m/s2)]
+G_geo = 120*s^2/(s^2 + 2*0.7*2*pi*2*s + (2*pi*2)^2); % [[V/(m/s)]
+
+% PSD of intertial sensors in [m^2/Hz]
+figure;
+hold on;
+set(gca, 'ColorOrderIndex', 1);
+plot(f, p_acc1./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))), ...
+    'DisplayName', 'Accelerometer');
+set(gca, 'ColorOrderIndex', 1);
+plot(f, p_acc2./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))), ...
+    'HandleVisibility', 'off');
+set(gca, 'ColorOrderIndex', 2);
+plot(f, p_geo1./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))), ...
+    'DisplayName', 'Geophone');
+set(gca, 'ColorOrderIndex', 2);
+plot(f, p_geo2./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))), ...
+    'HandleVisibility', 'off');
+set(gca, 'ColorOrderIndex', 3);
+plot(f, p_d, 'DisplayName', 'Interferometer');
+hold off;
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('PSD [$m^2/Hz$]'); xlabel('Frequency [Hz]');
+title('Huddle Test')
+legend();
+
+%% Compare Theoretical model with identified one
+id_ol = load('./mat/identification_chirp_40_400.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
+
+% Detrend Data
+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);
+
+% Identification Parameters
+run setup;
+win = hann(ceil(10/Ts));
+
+% IFF Plant
+[tf_fmeas_est, f] = tfestimate(id_ol.u, id_ol.f_meas, win, [], [], 1/Ts); % [V/m]
+[co_fmeas_est, ~] = mscohere(id_ol.u, id_ol.f_meas, win, [], [], 1/Ts);
+
+figure;
+ax1 = subplot(2, 1, 1);
+hold on;
+plot(f, abs(tf_fmeas_est), '-')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('Amplitude'); xlabel('Frequency [Hz]');
+hold off;
+
+ax2 = subplot(2, 1, 2);
+hold on;
+plot(f, 180/pi*unwrap(angle(tf_fmeas_est)), '-')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
+ylabel('Phase'); xlabel('Frequency [Hz]');
+hold off;
+
+linkaxes([ax1,ax2], 'x');
+xlim([40, 400]);
+
+% Geophones
+[tf_geo1_est, ~] = tfestimate(id_ol.d, id_ol.geo_1, win, [], [], 1/Ts); % [V/m]
+[co_geo1_est, ~] = mscohere(id_ol.d, id_ol.geo_1, win, [], [], 1/Ts);
+
+[tf_geo2_est, ~] = tfestimate(id_ol.d, id_ol.geo_2, win, [], [], 1/Ts); % [V/m]
+[co_geo2_est, ~] = mscohere(id_ol.d, id_ol.geo_2, win, [], [], 1/Ts);
+
+figure;
+ax1 = subplot(2, 1, 1);
+hold on;
+set(gca, 'ColorOrderIndex', 1);
+plot(f, abs(tf_geo1_est), '.')
+set(gca, 'ColorOrderIndex', 1);
+plot(f, abs(tf_geo2_est), '.')
+plot(f, abs(squeeze(freqresp(G_geo, f, 'Hz')).*(1i*2*pi*f)), 'k-')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('Amplitude'); xlabel('Frequency [Hz]');
+hold off;
+
+ax2 = subplot(2, 1, 2);
+hold on;
+set(gca, 'ColorOrderIndex', 1);
+plot(f, 180/pi*angle(tf_geo1_est), '.')
+set(gca, 'ColorOrderIndex', 1);
+plot(f, 180/pi*angle(tf_geo2_est), '.')
+plot(f, 180/pi*angle(-squeeze(freqresp(G_geo, f, 'Hz')).*(1i*2*pi*f)), 'k-')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
+ylabel('Phase'); xlabel('Frequency [Hz]');
+hold off;
+
+linkaxes([ax1,ax2], 'x');
+xlim([40, 400]);
+
+% Accelerometers
+[tf_acc1_est, ~] = tfestimate(id_ol.d, id_ol.acc_1, win, [], [], 1/Ts); % [V/m]
+[co_acc1_est, ~] = mscohere(id_ol.d, id_ol.acc_1, win, [], [], 1/Ts);
+
+[tf_acc2_est, ~] = tfestimate(id_ol.d, id_ol.acc_2, win, [], [], 1/Ts); % [V/m]
+[co_acc2_est, ~] = mscohere(id_ol.d, id_ol.acc_2, win, [], [], 1/Ts);
+
+figure;
+ax1 = subplot(2, 1, 1);
+hold on;
+set(gca, 'ColorOrderIndex', 1);
+plot(f, abs(tf_acc1_est), '.')
+set(gca, 'ColorOrderIndex', 1);
+plot(f, abs(tf_acc2_est), '.')
+plot(f, abs(squeeze(freqresp(G_acc, f, 'Hz')).*(1i*2*pi*f).^2), 'k-')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('Amplitude'); xlabel('Frequency [Hz]');
+hold off;
+
+ax2 = subplot(2, 1, 2);
+hold on;
+set(gca, 'ColorOrderIndex', 1);
+plot(f, 180/pi*angle(tf_acc1_est), '.')
+set(gca, 'ColorOrderIndex', 1);
+plot(f, 180/pi*angle(tf_acc2_est), '.')
+plot(f, 180/pi*angle(squeeze(freqresp(G_acc, f, 'Hz')).*(1i*2*pi*f).^2), 'k-')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
+ylabel('Phase'); xlabel('Frequency [Hz]');
+hold off;
+
+linkaxes([ax1,ax2], 'x');
+xlim([40, 400]);
+
+%% IFF development
+[tf_fmeas_est, f] = tfestimate(id_ol.u, id_ol.f_meas, win, [], [], 1/Ts); % [V/m]
+[co_fmeas_est, ~] = mscohere(id_ol.u, id_ol.f_meas, win, [], [], 1/Ts);
+
+% Model
+wz = 2*pi*103;
+xi_z = 0.01;
+wp = 2*pi*237;
+xi_p = 0.015;
+
+Giff = -20*(s^2 + 2*xi_z*s*wz + wz^2)/(s^2 + 2*xi_p*s*wp + wp^2);
+
+% Comparison model and identification
+figure;
+ax1 = subplot(2, 1, 1);
+hold on;
+plot(f, abs(tf_fmeas_est), '.')
+plot(f, abs(squeeze(freqresp(Giff, f, 'Hz'))), 'k-')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('Amplitude'); xlabel('Frequency [Hz]');
+hold off;
+
+ax2 = subplot(2, 1, 2);
+hold on;
+plot(f, 180/pi*angle(tf_fmeas_est), '.')
+plot(f, 180/pi*angle(squeeze(freqresp(Giff, f, 'Hz'))), 'k-')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
+ylabel('Phase'); xlabel('Frequency [Hz]');
+hold off;
+
+linkaxes([ax1,ax2], 'x');
+xlim([40, 400]);
+
+% Root Locus
+gains = logspace(0, 5, 1000);
+
+figure;
+hold on;
+plot(real(pole(Giff)),  imag(pole(Giff)),  'kx');
+plot(real(tzero(Giff)),  imag(tzero(Giff)),  'ko');
+for i = 1:length(gains)
+    cl_poles = pole(feedback(Giff, -gains(i)/(s + 2*pi*2)));
+    plot(real(cl_poles), imag(cl_poles), 'k.');
+end
+ylim([0, 1800]);
+xlim([-1600,200]); 
+xlabel('Real Part')
+ylabel('Imaginary Part')
+axis square
+
+% Optimal Controller
+Kiff_opt = -110/(s + 2*pi*2);
+
+%% New identification 
+id_ol = load('./mat/identification_chirp_40_400.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
+id_cl = load('./mat/identification_chirp_40_400_iff.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
+
+% Used controller
+Kiff = -110/(s + 2*pi*2);
+
+[tf_G_ol_est, f] = tfestimate(id_ol.u, id_ol.d, win, [], [], 1/Ts);
+[co_G_ol_est, ~] = mscohere(id_ol.u, id_ol.d, win, [], [], 1/Ts);
+[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);
+
+figure;
+hold on;
+plot(f, co_G_ol_est, '-')
+plot(f, co_G_cl_est, '-')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
+ylabel('Coherence'); xlabel('Frequency [Hz]');
+hold off;
+xlim([40, 400]); ylim([0, 1])
+
+% Comparison model and identification
+figure;
+ax1 = subplot(2, 1, 1);
+hold on;
+plot(f, abs(tf_G_ol_est), '-')
+plot(f, abs(tf_G_cl_est), '-')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('Amplitude'); xlabel('Frequency [Hz]');
+hold off;
+
+ax2 = subplot(2, 1, 2);
+hold on;
+plot(f, 180/pi*angle(tf_G_ol_est), '-')
+plot(f, 180/pi*angle(tf_G_cl_est), '-')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
+ylabel('Phase'); xlabel('Frequency [Hz]');
+hold off;
+
+linkaxes([ax1,ax2], 'x');
+xlim([40, 400]);
+
+%% Estimation of the inertial sensor transfer functions
+id = load('./mat/identification_noise_iff.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
+ht = load('./mat/huddle_test.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
+
+% Compare PSD
+run setup;
+win = hann(ceil(1/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);
+[p_id_fmeas, ~] = pwelch(id.f_meas, win, [], [], 1/Ts);
+
+[p_ht_d, ~] = pwelch(ht.d, win, [], [], 1/Ts);
+[p_ht_acc1, ~] = pwelch(ht.acc_1, win, [], [], 1/Ts);
+[p_ht_acc2, ~] = pwelch(ht.acc_2, win, [], [], 1/Ts);
+[p_ht_geo1, ~] = pwelch(ht.geo_1, win, [], [], 1/Ts);
+[p_ht_geo2, ~] = pwelch(ht.geo_2, win, [], [], 1/Ts);
+[p_ht_fmeas, ~] = pwelch(ht.f_meas, win, [], [], 1/Ts);
+
+figure;
+hold on;
+set(gca, 'ColorOrderIndex', 1);
+plot(f, p_ht_acc1, 'DisplayName', 'Huddle Test');
+set(gca, 'ColorOrderIndex', 1);
+plot(f, p_ht_acc2, 'HandleVisibility',  'off');
+set(gca, 'ColorOrderIndex', 2);
+plot(f, p_id_acc1, 'DisplayName', 'Identification Test');
+set(gca, 'ColorOrderIndex', 2);
+plot(f, p_id_acc2, 'HandleVisibility',  'off');
+hold off;
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('PSD [$V^2/Hz$]'); xlabel('Frequency [Hz]');
+title('Huddle Test - Accelerometers')
+legend();
+
+figure;
+hold on;
+set(gca, 'ColorOrderIndex', 1);
+plot(f, p_ht_geo1, 'DisplayName', 'Huddle Test');
+set(gca, 'ColorOrderIndex', 1);
+plot(f, p_ht_geo2, 'HandleVisibility',  'off');
+set(gca, 'ColorOrderIndex', 2);
+plot(f, p_id_geo1, 'DisplayName', 'Identification Test');
+set(gca, 'ColorOrderIndex', 2);
+plot(f, p_id_geo2, 'HandleVisibility',  'off');
+hold off;
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('PSD [$V^2/Hz$]'); xlabel('Frequency [Hz]');
+title('Huddle Test - Geophones')
+legend();
+
+figure;
+hold on;
+plot(f, p_ht_d, 'DisplayName', 'Huddle Test');
+plot(f, p_id_d, 'DisplayName', 'Identification Test');
+hold off;
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('PSD [$m^2/Hz$]'); xlabel('Frequency [Hz]');
+title('Huddle Test - Interferometers')
+legend();
+
+% tf and coh computation
+[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);
+
+% Coherence
+figure;
+hold on;
+set(gca, 'ColorOrderIndex', 1);
+plot(f, co_acc1_est, '-', 'DisplayName', 'Accelerometer')
+set(gca, 'ColorOrderIndex', 1);
+plot(f, co_acc2_est, '-',  'HandleVisibility', 'off')
+set(gca, 'ColorOrderIndex', 2);
+plot(f, co_geo1_est, '-', 'DisplayName', 'Geophone')
+set(gca, 'ColorOrderIndex', 2);
+plot(f, co_geo2_est, '-',  'HandleVisibility', 'off')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
+ylabel('Coherence'); xlabel('Frequency [Hz]');
+hold off;
+xlim([2, 2e3]); ylim([0, 1])
+legend();
+
+% Transfer Functions
+figure;
+ax1 = subplot(2, 1, 1);
+hold on;
+plot(f, abs(tf_acc1_est), '-')
+plot(f, abs(tf_acc2_est), '-')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('Amplitude'); xlabel('Frequency [Hz]');
+hold off;
+
+ax2 = subplot(2, 1, 2);
+hold on;
+plot(f, 180/pi*angle(tf_acc1_est), '-')
+plot(f, 180/pi*angle(tf_acc2_est), '-')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
+ylabel('Phase'); xlabel('Frequency [Hz]');
+hold off;
+
+linkaxes([ax1,ax2], 'x');
+xlim([2, 2e3]);
+
+
+figure;
+ax1 = subplot(2, 1, 1);
+hold on;
+plot(f, abs(tf_geo1_est), '-')
+plot(f, abs(tf_geo2_est), '-')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('Amplitude'); xlabel('Frequency [Hz]');
+hold off;
+
+ax2 = subplot(2, 1, 2);
+hold on;
+plot(f, 180/pi*angle(tf_geo1_est), '-')
+plot(f, 180/pi*angle(tf_geo2_est), '-')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
+ylabel('Phase'); xlabel('Frequency [Hz]');
+hold off;
+
+linkaxes([ax1,ax2], 'x');
+xlim([2, 2e3]);
diff --git a/sensor_fusion_test_bench.slx b/sensor_fusion_test_bench.slx
index 47c3066..bdc03d4 100644
Binary files a/sensor_fusion_test_bench.slx and b/sensor_fusion_test_bench.slx differ
diff --git a/setup.m b/setup.m
index a292fe7..9680842 100644
--- a/setup.m
+++ b/setup.m
@@ -1,6 +1,18 @@
+%%
 s = tf('s');
-Ts = 1e-4;
+Ts = 1e-4; % [s]
 
-Glpf = 1/(1 + s/2/pi/500);
+%% Pre-Filter
+% Glpf = 1/(1 + s/2/pi/2e3);
+Glpf = 1/(1 + s/2/pi/50); % Used to excite with constant velocity
+Gz = c2d(Glpf, Ts, 'tustin');
 
-Gz = c2d(Glpf, Ts, 'tustin');
\ No newline at end of file
+%% IFF Controller
+Kiff = -1/(s + 2*pi*2);
+Kiff = c2d(Kiff, Ts, 'tustin');
+
+%% Excitation Signal
+Tsim = 180; % Excitation time + Measurement time [s]
+
+t = 0:Ts:Tsim;
+u_exc = timeseries(chirp(t, 0.1, Tsim, 1e3, 'logarithmic'), t);