diff --git a/mat/identification_chirp_01_1000_iff.mat b/mat/identification_chirp_01_1000_iff.mat
new file mode 100644
index 0000000..3a55c2f
Binary files /dev/null and b/mat/identification_chirp_01_1000_iff.mat differ
diff --git a/mat/identification_chirp_02_2000_iff.mat b/mat/identification_chirp_02_2000_iff.mat
new file mode 100644
index 0000000..8930463
Binary files /dev/null and b/mat/identification_chirp_02_2000_iff.mat differ
diff --git a/mat/identification_chirp_40_400_bis.mat b/mat/identification_chirp_40_400_bis.mat
new file mode 100644
index 0000000..c635886
Binary files /dev/null and b/mat/identification_chirp_40_400_bis.mat differ
diff --git a/mat/identification_noise.mat b/mat/identification_noise.mat
new file mode 100644
index 0000000..875530f
Binary files /dev/null and b/mat/identification_noise.mat differ
diff --git a/mat/identification_noise_bis.mat b/mat/identification_noise_bis.mat
new file mode 100644
index 0000000..036593d
Binary files /dev/null and b/mat/identification_noise_bis.mat differ
diff --git a/runtest.m b/runtest.m
index eec41cd..76871b6 100644
--- a/runtest.m
+++ b/runtest.m
@@ -17,7 +17,7 @@ u = data(:, 6);
 f_meas = data(:, 7);
 t = data(:, 8);
 
-save('./mat/identification_noise_iff.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
+save('./mat/identification_noise_bis.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
 
 %%
 d = detrend(d, 0);
diff --git a/sensor_fusion_analysis.m b/sensor_fusion_analysis.m
index 8aed01d..71801c0 100644
--- a/sensor_fusion_analysis.m
+++ b/sensor_fusion_analysis.m
@@ -88,7 +88,7 @@ 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');
+id_ol = load('./mat/identification_noise_bis.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
 
 % Detrend Data
 id_ol.d = detrend(id_ol.d, 0);
@@ -117,7 +117,7 @@ hold off;
 
 ax2 = subplot(2, 1, 2);
 hold on;
-plot(f, 180/pi*unwrap(angle(tf_fmeas_est)), '-')
+plot(f, 180/pi*angle(tf_fmeas_est), '-')
 set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
 ylabel('Phase'); xlabel('Frequency [Hz]');
 hold off;
@@ -198,10 +198,10 @@ xlim([40, 400]);
 % Model
 wz = 2*pi*103;
 xi_z = 0.01;
-wp = 2*pi*237;
+wp = 2*pi*238;
 xi_p = 0.015;
 
-Giff = -20*(s^2 + 2*xi_z*s*wz + wz^2)/(s^2 + 2*xi_p*s*wp + wp^2);
+Giff = 2*(s^2 + 2*xi_z*s*wz + wz^2)/(s^2 + 2*xi_p*s*wp + wp^2);
 
 % Comparison model and identification
 figure;
@@ -232,7 +232,7 @@ 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)));
+    cl_poles = pole(feedback(Giff, gains(i)/(s + 2*pi*2)));
     plot(real(cl_poles), imag(cl_poles), 'k.');
 end
 ylim([0, 1800]);
@@ -240,9 +240,9 @@ xlim([-1600,200]);
 xlabel('Real Part')
 ylabel('Imaginary Part')
 axis square
-
+    
 % Optimal Controller
-Kiff_opt = -110/(s + 2*pi*2);
+Kiff_opt = 1100/(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');
@@ -287,7 +287,7 @@ 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');
+id = load('./mat/identification_noise.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
diff --git a/sensor_fusion_test_bench.slx b/sensor_fusion_test_bench.slx
index bdc03d4..1259cec 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 9680842..7610e57 100644
--- a/setup.m
+++ b/setup.m
@@ -4,15 +4,20 @@ Ts = 1e-4; % [s]
 
 %% 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');
+G_pf = 1/(1 + s/2/pi/50); % Used to excite with constant velocity
+G_pf = c2d(G_pf, Ts, 'tustin');
+
+%% Force Sensor Filter (HPF)
+Gf_hpf = s/(s + 2*pi*2);
+Gf_hpf = c2d(Gf_hpf, Ts, 'tustin');
 
 %% IFF Controller
-Kiff = -1/(s + 2*pi*2);
+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);
+% u_exc = timeseries(chirp(t, 0.1, Tsim, 1e3, 'logarithmic'), t);
+u_exc = timeseries(chirp(t, 40, Tsim, 400, 'logarithmic'), t);