diff --git a/matlab/identif_save.m b/matlab/identif_save.m
index d32d0d5..4edb126 100644
--- a/matlab/identif_save.m
+++ b/matlab/identif_save.m
@@ -1,34 +1,30 @@
-% =frf_save.m= - Save Data
-% :PROPERTIES:
-% :header-args: :tangle matlab/frf_save.m
-% :END:
-
-% First, we get data from the Speedgoat:
-
 tg = slrt;
 
 f = SimulinkRealTime.openFTP(tg);
 mget(f, 'data/data.dat');
 close(f);
 
-
-
 % And we load the data on the Workspace:
 
 data = SimulinkRealTime.utils.getFileScopeData('data/data.dat').data;
 
-de = data(:, 1:6); % Measurment displacement (encoder) [m]
-Vs = data(:, 7:12); % Measured voltage (force sensor) [V]
-u = data(:, 13:18); % DAC Voltage (command) [V]
-Vexc = data(:, 19); % Excitation Voltage [V]
-t  = data(:, end); % Time [s]
+de = data(:, 1:6);   % Measurment displacement (encoder) [m]
+Vs = data(:, 7:12);  % Force Sensor [V]
+u  = data(:, 13:18); % Control Output [V]
+Va = data(:, 19); % Excitation Signal [V]
+t  = data(:, end);   % Time [s]
 
+% strut_num = 6;
+% save(sprintf('mat/frf_exc_iff_strut_%i_enc_plates_noise.mat', strut_num), 't', 'de', 'Vs', 'u', 'Va');
 
-% And we save this to a =mat= file:
-strut_number = 6;
+save('mat/hac_iff_more_lead_nass_scan', 't', 'de', 'Vs', 'u', 'Va');
 
-save(sprintf('mat/iff_strut_%i_noise_g_400.mat', strut_number), 't', 'u', 'Vs', 'Vexc', 'de');
-% save(sprintf('mat/frf_data_exc_strut_%i_noise_lf.mat', strut_number), 't', 'u', 'Vs', 'de');
-% save(sprintf('mat/frf_data_exc_strut_%i_sweep.mat', strut_number), 't', 'u', 'Vs', 'de');
-% save(sprintf('mat/iff_strut_%i_noise_hf.mat', strut_number), 't', 'u', 'Vs', 'de');
-% save(sprintf('mat/frf_data_exc_strut_%i_add_mass_closed_circuit.mat', strut_number), 't', 'u', 'Vs', 'de');
+%%
+load('mat/jacobian.mat', 'J');
+
+X = inv(J)*de';
+X = X';
+
+%%
+figure;
+plot3(X(:,1), X(:,2), X(:,3))
\ No newline at end of file
diff --git a/matlab/init_hac_iff.m b/matlab/init_hac_iff.m
index 19010df..002057b 100644
--- a/matlab/init_hac_iff.m
+++ b/matlab/init_hac_iff.m
@@ -1,7 +1,96 @@
-%% HAC
-load('mat/Khac_iff_struts.mat', 'Khac_iff_struts');
-save('sim_data/Khac_iff_struts.mat', 'Khac_iff_struts');
+%%
+clear;
 
 %% Jacobian
 load('mat/jacobian.mat', 'J');
-save('sim_data/J.mat', 'J');
\ No newline at end of file
+
+Jinv = pinv(J);
+
+save('sim_data/J.mat', 'J', 'Jinv');
+
+%% Saved HAC
+% load('mat/Khac_iff_struts.mat', 'Khac_iff_struts');
+
+%% Developed HAC
+s = zpk('s');
+
+a  = 5;  % Amount of phase lead / width of the phase lead / high frequency gain
+wc = 2*pi*110; % Frequency with the maximum phase lead [rad/s]
+
+H_lead = (1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
+H_lpf = 1/(1 + s/2/pi/300);
+
+gm = 0.02;
+xi = 0.5;
+wn = 2*pi*700;
+
+H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
+
+Khac_iff_struts = -2.2e4 * ... % Gain
+                  H_lead * ...       % Lead
+                  H_lpf * ...       % Lead
+                  H_notch * ...      % Notch
+                  (2*pi*100/s) * ... % Integrator
+                  eye(6);            % 6x6 Diagonal
+
+%% Save Controller
+load('sim_data/data_sim.mat', 'Ts')
+Khac_iff_struts = c2d(Khac_iff_struts, Ts, 'Tustin');
+
+save('sim_data/Khac_iff_struts.mat', 'Khac_iff_struts');
+
+%% Loop Gain
+load('mat/damped_plant_enc_plates.mat', 'f', 'G_enc_iff_opt')
+
+L_frf = zeros(size(G_enc_iff_opt));
+
+for i = 1:size(G_enc_iff_opt, 1)
+    L_frf(i, :, :) = squeeze(G_enc_iff_opt(i,:,:))*freqresp(Khac_iff_struts, f(i), 'Hz');
+end
+
+colors = colororder;
+
+freqs = 2*logspace(1, 3, 1000);
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+% Diagonal Elements FRF
+plot(f, abs(L_frf(:,1,1)), 'color', colors(1,:), ...
+     'DisplayName', 'Diagonal');
+for i = 2:6
+    plot(f, abs(L_frf(:,i,i)), 'color', colors(1,:), ...
+         'HandleVisibility', 'off');
+end
+plot(f, abs(L_frf(:,1,2)), 'color', [colors(2,:), 0.2], ...
+     'DisplayName', 'Off-Diag');
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(L_frf(:,i,j)), 'color', [colors(2,:), 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Loop Gain [-]'); set(gca, 'XTickLabel',[]);
+ylim([1e-3, 1e2]);
+legend('location', 'northeast');
+grid;
+
+ax2 = nexttile;
+hold on;
+for i =1:6
+    plot(f, 180/pi*angle(L_frf(:,i,i)), 'color', colors(1,:));
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-180, 180]);
+grid;
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 2e3]);
diff --git a/matlab/init_iff.m b/matlab/init_iff.m
index fa86a55..5e6bc77 100644
--- a/matlab/init_iff.m
+++ b/matlab/init_iff.m
@@ -1,5 +1,12 @@
+%%
+clear;
+
 %%
 load('mat/Kiff.mat', 'Kiff');
 
+%%
+load('sim_data/data_sim.mat', 'Ts')
+Kiff = c2d(Kiff, Ts, 'Tustin');
+
 %%
 save('sim_data/Kiff.mat', 'Kiff');
\ No newline at end of file
diff --git a/matlab/init_simulation.m b/matlab/init_simulation.m
index c108a53..c9ec911 100644
--- a/matlab/init_simulation.m
+++ b/matlab/init_simulation.m
@@ -7,12 +7,12 @@ s = zpk('s');
 addpath('./src/');
 
 %% Simulation configuration
-Fs = 5e3; % Sampling Frequency [Hz]
+Fs = 10e3; % Sampling Frequency [Hz]
 Ts = 1/Fs; % Sampling Time [s]
 
 %% Data record configuration
-Trec_start = 10;  % Start time for Recording [s]
-Trec_dur   = 600; % Recording Duration [s]
+Trec_start = 5;  % Start time for Recording [s]
+Trec_dur   = 60; % Recording Duration [s]
 
 Tsim = 2*Trec_start + Trec_dur;  % Simulation Time [s]
 
@@ -122,4 +122,7 @@ set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
 xlim([1, Fs/2]); ylim([1e-10, 1e0]);
 
 %% Save data that will be loaded in the Simulink file
-save('sim_data/data_sim.mat', 'Fs', 'Ts', 'Tsim', 'Trec_start', 'Trec_dur', 'V_exc');
+save('sim_data/data_sim.mat', ...
+    'Fs', 'Ts', 'Tsim', 'Trec_start', 'Trec_dur', ...
+    'V_exc', 'V_off', ...
+    'u_min', 'u_max');