diff --git a/.gitignore b/.gitignore
deleted file mode 100644
index 41c2be0..0000000
--- a/.gitignore
+++ /dev/null
@@ -1,29 +0,0 @@
-# Windows default autosave extension
-*.asv
-
-# OSX / *nix default autosave extension
-*.m~
-
-# Compiled MEX binaries (all platforms)
-*.mex*
-
-# Packaged app and toolbox files 
-*.mlappinstall 
-*.mltbx 
-  
-# Generated helpsearch folders 
-helpsearch*/ 
-
-# Simulink code generation folders
-slprj/
-sccprj/
-
-# Simulink autosave extension
-*.autosave
-
-# Octave session info
-octave-workspace
-
-# Custom
-stewart_displacement_grt_rtw/
-Figures/
diff --git a/Stewartsimscape.prj b/Stewartsimscape.prj
new file mode 100644
index 0000000..a7df471
--- /dev/null
+++ b/Stewartsimscape.prj
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<SimulinkProject xmlns="http://www.mathworks.com/SimulinkProjectFile" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" version="1.0"/>
diff --git a/active_damping/act_damp_iff_generate.m b/active_damping/act_damp_iff_generate.m
new file mode 100644
index 0000000..de8f7a6
--- /dev/null
+++ b/active_damping/act_damp_iff_generate.m
@@ -0,0 +1,36 @@
+%%
+clear; close all; clc;
+
+%% Load the transfer functions
+load('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');
+
+%% Load Configuration file
+load('./mat/config.mat', 'save_fig', 'freqs');
+
+%%
+s = tf('s');
+
+%% Light Voice Coil
+%sisotool(-G_light_vc.G_iff('Fm1', 'F1')/s);
+
+K_iff_light_vc = 105/s*tf(eye(6));
+
+%% Light Piezo
+%sisotool(-G_light_pz.G_iff('Fm1', 'F1')/s);
+
+K_iff_light_pz = 3300/s*tf(eye(6));
+
+%% Heavy Voice Coil
+%sisotool(-G_heavy_vc.G_iff('Fm1', 'F1')/s);
+
+K_iff_heavy_vc = 22.7/s*tf(eye(6));
+
+%% Heavy Piezo
+%sisotool(-G_heavy_pz.G_iff('Fm1', 'F1')/s);
+
+K_iff_heavy_pz = 720/s*tf(eye(6));
+
+%% Save Controllers
+save('./mat/K_iff_sisotool.mat', ...
+    'K_iff_light_vc', 'K_iff_light_pz', ...
+    'K_iff_heavy_vc', 'K_iff_heavy_pz');
diff --git a/active_damping/act_damp_iff_id.m b/active_damping/act_damp_iff_id.m
new file mode 100644
index 0000000..4e467fd
--- /dev/null
+++ b/active_damping/act_damp_iff_id.m
@@ -0,0 +1,45 @@
+%%
+clear; close all; clc;
+
+%% Load Configuration file
+load('./mat/config.mat', 'save_fig', 'freqs');
+
+%% Load controllers
+load('./mat/K_iff_sisotool.mat', ...
+    'K_iff_light_vc', 'K_iff_light_pz', ...
+    'K_iff_heavy_vc', 'K_iff_heavy_pz');
+
+%%
+initializeSample(struct('mass', 1));
+
+initializeHexapod(struct('actuator', 'lorentz'));
+K_iff = K_iff_light_vc; %#ok
+save('./mat/controllers.mat', 'K_iff', '-append');
+
+G_light_vc_iff = identifyPlant();
+
+initializeHexapod(struct('actuator', 'piezo'));
+K_iff = K_iff_light_pz; %#ok
+save('./mat/controllers.mat', 'K_iff', '-append');
+
+G_light_pz_iff = identifyPlant();
+
+%%
+initializeSample(struct('mass', 50));
+
+initializeHexapod(struct('actuator', 'lorentz'));
+K_iff = K_iff_heavy_vc; %#ok
+save('./mat/controllers.mat', 'K_iff', '-append');
+
+G_heavy_vc_iff = identifyPlant();
+
+initializeHexapod(struct('actuator', 'piezo'));
+K_iff = K_iff_heavy_pz;
+save('./mat/controllers.mat', 'K_iff', '-append');
+
+G_heavy_pz_iff = identifyPlant();
+
+%% Save the obtained transfer functions
+save('./mat/G_iff.mat', ...
+    'G_light_vc_iff', 'G_light_pz_iff', ...
+    'G_heavy_vc_iff', 'G_heavy_pz_iff');
diff --git a/active_damping/act_damp_iff_plots.m b/active_damping/act_damp_iff_plots.m
new file mode 100644
index 0000000..ad18b90
--- /dev/null
+++ b/active_damping/act_damp_iff_plots.m
@@ -0,0 +1,133 @@
+%%
+clear; close all; clc;
+
+%% Load Configuration file
+load('./mat/config.mat', 'save_fig', 'freqs');
+
+%% Load
+load('./mat/G_iff.mat', 'G_light_vc_iff', 'G_light_pz_iff', 'G_heavy_vc_iff', 'G_heavy_pz_iff');
+load('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');
+
+%% New Damped Plant - Horizontal Direction
+figure;
+% Amplitude
+ax1 = subaxis(2,1,1);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_cart('Dx', 'Fx'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G_light_pz.G_cart('Dx', 'Fx'), freqs, 'Hz'))));
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_light_vc_iff.G_cart('Dx', 'Fx'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_light_pz_iff.G_cart('Dx', 'Fx'), freqs, 'Hz'))), '--');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]);
+ylabel('Amplitude [m/N]');
+hold off;
+
+% Phase
+ax2 = subaxis(2,1,2);
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_vc.G_cart('Dx', 'Fx'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_pz.G_cart('Dx', 'Fx'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
+set(gca,'ColorOrderIndex',1)
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_vc_iff.G_cart('Dx', 'Fx'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_pz_iff.G_cart('Dx', 'Fx'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
+set(gca,'xscale','log');
+yticks(-180:90:180);
+ylim([-180 180]);
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+legend('Location', 'southwest');
+hold off;
+
+linkaxes([ax1,ax2],'x');
+
+if save_fig; exportFig('G_hori_iff', 'normal-normal', struct('path', 'active_damping')); end
+
+%% New Damped Plant - Vertical Direction
+figure;
+% Amplitude
+ax1 = subaxis(2,1,1);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_cart('Dz', 'Fz'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G_light_pz.G_cart('Dz', 'Fz'), freqs, 'Hz'))));
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_light_vc_iff.G_cart('Dz', 'Fz'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_light_pz_iff.G_cart('Dz', 'Fz'), freqs, 'Hz'))), '--');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]);
+ylabel('Amplitude [m/N]');
+hold off;
+
+% Phase
+ax2 = subaxis(2,1,2);
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_vc.G_cart('Dz', 'Fz'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_pz.G_cart('Dz', 'Fz'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
+set(gca,'ColorOrderIndex',1)
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_vc_iff.G_cart('Dz', 'Fz'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_pz_iff.G_cart('Dz', 'Fz'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
+set(gca,'xscale','log');
+yticks(-180:90:180);
+ylim([-180 180]);
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+legend('Location', 'southwest');
+hold off;
+
+linkaxes([ax1,ax2],'x');
+
+if save_fig; exportFig('G_vert_iff', 'normal-normal', struct('path', 'active_damping')); end
+
+%% Ground motion Transmissibility - Horizontal Direction
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_db('Dx', 'Dbx'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G_light_pz.G_db('Dx', 'Dbx'), freqs, 'Hz'))));
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_light_vc_iff.G_db('Dx', 'Dbx'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_light_pz_iff.G_db('Dx', 'Dbx'), freqs, 'Hz'))), '--');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
+hold off;
+
+if save_fig; exportFig('G_db_hori_iff', 'normal-normal', struct('path', 'active_damping')); end
+
+%% Ground motion Transmissibility - Vertical Direction
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_db('Dz', 'Dbz'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G_light_pz.G_db('Dz', 'Dbz'), freqs, 'Hz'))));
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_light_vc_iff.G_db('Dz', 'Dbz'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_light_pz_iff.G_db('Dz', 'Dbz'), freqs, 'Hz'))), '--');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
+hold off;
+
+if save_fig; exportFig('G_db_vert_iff', 'normal-normal', struct('path', 'active_damping')); end
+
+%% Direct Forces Compliance - Horizontal Direction
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_fi('Dx', 'Fix'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G_light_pz.G_fi('Dx', 'Fix'), freqs, 'Hz'))));
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_light_vc_iff.G_fi('Dx', 'Fix'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_light_pz_iff.G_fi('Dx', 'Fix'), freqs, 'Hz'))), '--');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
+hold off;
+
+if save_fig; exportFig('G_fi_hori_iff', 'normal-normal', struct('path', 'active_damping')); end
+
+%% Direct Forces Compliance - Vertical Direction
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_fi('Dz', 'Fiz'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G_light_pz.G_fi('Dz', 'Fiz'), freqs, 'Hz'))));
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_light_vc_iff.G_fi('Dz', 'Fiz'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_light_pz_iff.G_fi('Dz', 'Fiz'), freqs, 'Hz'))), '--');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
+hold off;
+
+if save_fig; exportFig('G_fi_vert_iff', 'normal-normal', struct('path', 'active_damping')); end
diff --git a/active_damping/act_damp_main.m b/active_damping/act_damp_main.m
new file mode 100644
index 0000000..8c83ae4
--- /dev/null
+++ b/active_damping/act_damp_main.m
@@ -0,0 +1,8 @@
+% Generate the IFF controls
+run act_damp_iff_generate.m
+
+% Identification of the damped plant
+run act_damp_iff_id.m
+
+% Plot new transfer functions
+run act_damp_iff_plots.m
\ No newline at end of file
diff --git a/analysis/analysis_stiffness.m b/analysis/analysis_stiffness.m
new file mode 100644
index 0000000..9ae1555
--- /dev/null
+++ b/analysis/analysis_stiffness.m
@@ -0,0 +1,14 @@
+%%
+clear; close all; clc;
+
+%%
+sys_0   = initializeHexapod(struct('actuator', 'piezo', 'jacobian', 0));
+sys_100 = initializeHexapod(struct('actuator', 'piezo', 'jacobian', 100));
+sys_200 = initializeHexapod(struct('actuator', 'piezo', 'jacobian', 1000000));
+
+%%
+K_0   = getStiffnessMatrix(sys_0.Leg.k.ax,   sys_0.J  );
+K_100 = getStiffnessMatrix(sys_100.Leg.k.ax, sys_100.J);
+K_200 = getStiffnessMatrix(sys_200.Leg.k.ax, sys_200.J);
+
+%%
diff --git a/analyze_jacobian.m b/analyze_jacobian.m
index a48b1bb..aeb3ae3 100644
--- a/analyze_jacobian.m
+++ b/analyze_jacobian.m
@@ -1,6 +1,7 @@
 %% Script Description
-% 
+%
 
+%%
 figure;
 plot(d_meas.Time, d.Data-d_meas.Data)
 
@@ -46,4 +47,3 @@ plot(jacobian.Time, squeeze(K_change(5, 5, :)));
 plot(jacobian.Time, squeeze(K_change(6, 6, :)));
 legend({'Kxx', 'Kyy', 'Kzz', 'Kmx', 'Kmy', 'Kmz'})
 hold off;
-
diff --git a/config.m b/config.m
new file mode 100644
index 0000000..7ab7f7a
--- /dev/null
+++ b/config.m
@@ -0,0 +1,12 @@
+%%
+addpath('active_damping');
+addpath('analysis');
+addpath('identification');
+addpath('library');
+addpath('studies');
+addpath('src');
+
+%%
+freqs = logspace(-1, 3, 1000);
+save_fig = false;
+save('./mat/config.mat', 'freqs', 'save_fig');
diff --git a/identification/id_G.m b/identification/id_G.m
new file mode 100644
index 0000000..58befb8
--- /dev/null
+++ b/identification/id_G.m
@@ -0,0 +1,20 @@
+%% Script Description
+% Script used to identify various transfer functions
+% of the Stewart platform
+
+%%
+clear; close all; clc;
+
+%%
+K_iff = tf(zeros(6));
+save('./mat/controllers.mat', 'K_iff', '-append');
+
+%% Initialize System
+initializeSample(struct('mass', 50));
+initializeHexapod(struct('actuator', 'piezo'));
+
+%% Identification
+G = identifyPlant();
+
+%% Save
+save('./mat/G.mat', 'G');
diff --git a/identification/id_jacobian.m b/identification/id_jacobian.m
new file mode 100644
index 0000000..2ce5c01
--- /dev/null
+++ b/identification/id_jacobian.m
@@ -0,0 +1,38 @@
+%% Script Description
+%%
+clear; close all; clc;
+
+%%
+K_iff = tf(zeros(6));
+save('./mat/controllers.mat', 'K_iff', '-append');
+
+%% System - perfectly aligned
+initializeHexapod(struct('actuator', 'piezo', 'jacobian', 1, 'density', 0.1));
+initializeSample(struct('mass', 50, 'height', 1, 'measheight', 1, 'offset', [0, 0, -25.5]));
+
+% Identification
+G_center = identifyPlant();
+
+%% System - Jacobian is too high
+initializeHexapod(struct('actuator', 'piezo', 'jacobian', 160));
+initializeSample(struct('mass', 50, 'height', 300, 'measheight', 150));
+
+% Identification
+G_Jac_offset = identifyPlant();
+
+%% System - CoM is too low
+initializeHexapod(struct('actuator', 'piezo', 'jacobian', 150));
+initializeSample(struct('mass', 50, 'height', 280, 'measheight', 150));
+
+% Identification
+G_CoM_offset = identifyPlant();
+
+%% System - Meas point is too high
+initializeHexapod(struct('actuator', 'piezo', 'jacobian', 150));
+initializeSample(struct('mass', 50, 'height', 300, 'measheight', 160));
+
+% Identification
+G_Meas_offset = identifyPlant();
+
+%% Save
+save('./mat/G_jacobian.mat', 'G_center', 'G_Jac_offset', 'G_CoM_offset', 'G_Meas_offset');
diff --git a/identification/id_jacobian_plots.m b/identification/id_jacobian_plots.m
new file mode 100644
index 0000000..f89e546
--- /dev/null
+++ b/identification/id_jacobian_plots.m
@@ -0,0 +1,43 @@
+%% Script Description
+% 
+%%
+clear; close all; clc;
+
+%%
+load('./mat/G_jacobian.mat');
+
+%%
+freqs = logspace(0, 3, 2000);
+
+%%
+bode_opts = bodeoptions;
+bode_opts.FreqUnits = 'Hz';
+bode_opts.MagUnits = 'abs';
+bode_opts.MagScale = 'log';
+bode_opts.PhaseVisible = 'off';
+
+%% Compare when the Jac is above Meas. point and CoM
+% => 
+figure;
+bode(G_center.G_cart, G_Jac_offset.G_cart, 2*pi*freqs, bode_opts);
+
+%% Compare when the CoM is bellow the Meas. point and Jac
+% => This make the tilt resonance frequency a little bit higher.
+figure;
+bode(G_center.G_cart, G_CoM_offset.G_cart, 2*pi*freqs, bode_opts);
+
+%% Compare when the measurement point is higher than CoM and Jac
+% => 
+figure;
+bode(G_center.G_cart, G_Meas_offset.G_cart, 2*pi*freqs, bode_opts);
+
+%% Compare direct forces and forces applied by actuators on the same point
+% => This should be the same is the support is rigid.
+% => Looks like it's close but not equal
+figure;
+bode(G_center.G_cart, G_center.G_comp, 2*pi*freqs, bode_opts);
+
+%% Compare relative sensor and absolute sensor
+% => This should be the same as the support is rigid
+figure;
+bode(G_center.G_iner, G_center.G_comp, 2*pi*freqs, bode_opts);
diff --git a/identification/id_main.m b/identification/id_main.m
new file mode 100644
index 0000000..f62250b
--- /dev/null
+++ b/identification/id_main.m
@@ -0,0 +1,14 @@
+%%
+clear; close all; clc;
+
+%% Jacobian Study
+
+
+%% Identification of the system
+run id_G.m
+
+%% Plots of the identifications
+run id_plot_cart.m
+run id_plot_legs.m
+run id_plot_iff.m
+run id_plot_db.m
\ No newline at end of file
diff --git a/identification/id_plot_cart.m b/identification/id_plot_cart.m
new file mode 100644
index 0000000..767879f
--- /dev/null
+++ b/identification/id_plot_cart.m
@@ -0,0 +1,96 @@
+%%
+clear; close all; clc;
+
+%% Load the transfer functions
+load('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');
+
+%% Load Configuration file
+load('./mat/config.mat', 'save_fig', 'freqs');
+
+%% Plant in the X direction
+figure;
+% Amplitude
+ax1 = subaxis(2,1,1);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_cart('Dx', 'Fx'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G_light_pz.G_cart('Dx', 'Fx'), freqs, 'Hz'))));
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_cart('Dx', 'Fx'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_cart('Dx', 'Fx'), freqs, 'Hz'))), '--');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]);
+ylabel('Amplitude [m/N]');
+hold off;
+
+% Phase
+ax2 = subaxis(2,1,2);
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_vc.G_cart('Dx', 'Fx'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_pz.G_cart('Dx', 'Fx'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
+set(gca,'ColorOrderIndex',1)
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_heavy_vc.G_cart('Dx', 'Fx'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_heavy_pz.G_cart('Dx', 'Fx'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
+set(gca,'xscale','log');
+yticks(-180:90:180);
+ylim([-180 180]);
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+legend('Location', 'southwest');
+hold off;
+
+linkaxes([ax1,ax2],'x');
+
+if save_fig; exportFig('G_hori', 'normal-normal', struct('path', 'identification')); end
+
+%% Plant in the Z direction
+figure;
+% Amplitude
+ax1 = subaxis(2,1,1);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_cart('Dz', 'Fz'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G_light_pz.G_cart('Dz', 'Fz'), freqs, 'Hz'))));
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_cart('Dz', 'Fz'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_cart('Dz', 'Fz'), freqs, 'Hz'))), '--');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]);
+ylabel('Amplitude [m/N]');
+hold off;
+
+% Phase
+ax2 = subaxis(2,1,2);
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_vc.G_cart('Dz', 'Fz'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_pz.G_cart('Dz', 'Fz'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
+set(gca,'ColorOrderIndex',1)
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_heavy_vc.G_cart('Dz', 'Fz'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_heavy_pz.G_cart('Dz', 'Fz'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
+set(gca,'xscale','log');
+yticks(-180:90:180);
+ylim([-180 180]);
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+legend('Location', 'southwest');
+hold off;
+
+linkaxes([ax1,ax2],'x');
+
+if save_fig; exportFig('G_vert', 'normal-normal', struct('path', 'identification')); end
+
+%% Coupling
+figure;
+
+for i_input = 1:3
+    for i_output = 1:3
+        subaxis(3,3,3*(i_input-1)+i_output);
+        hold on;
+        plot(freqs, abs(squeeze(freqresp(G_light_vc.G_cart(i_output, i_input), freqs, 'Hz'))));
+        plot(freqs, abs(squeeze(freqresp(G_light_pz.G_cart(i_output, i_input), freqs, 'Hz'))));
+        set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+        xlim([freqs(1) freqs(end)]); ylim([1e-22, 1e-2]);
+        yticks([1e-20, 1e-15, 1e-10, 1e-5]); xticks([0.1 1 10 100 1000]);
+        if i_output > 1; set(gca,'yticklabel',[]); end
+        if i_input < 3; set(gca,'xticklabel',[]); end
+        hold off;
+    end
+end
+
+if save_fig; exportFig('G_coupling', 'wide-tall', struct('path', 'identification')); end
diff --git a/identification/id_plot_db.m b/identification/id_plot_db.m
new file mode 100644
index 0000000..efd2739
--- /dev/null
+++ b/identification/id_plot_db.m
@@ -0,0 +1,40 @@
+%%
+clear; close all; clc;
+
+%% Load the transfer functions
+load('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');
+
+%% Load Configuration file
+load('./mat/config.mat', 'save_fig', 'freqs');
+
+%%
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_db('Dx', 'Dbx'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
+plot(freqs, abs(squeeze(freqresp(G_light_pz.G_db('Dx', 'Dbx'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_db('Dx', 'Dbx'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
+plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_db('Dx', 'Dbx'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
+hold off;
+legend('Location', 'southeast');
+
+if save_fig; exportFig('G_db_hori', 'normal-normal', struct('path', 'identification')); end
+
+%%
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_db('Dz', 'Dbz'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
+plot(freqs, abs(squeeze(freqresp(G_light_pz.G_db('Dz', 'Dbz'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_db('Dz', 'Dbz'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
+plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_db('Dz', 'Dbz'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
+hold off;
+legend('Location', 'southeast');
+
+if save_fig; exportFig('G_db_vert', 'normal-normal', struct('path', 'identification')); end
+
+%%
diff --git a/identification/id_plot_iff.m b/identification/id_plot_iff.m
new file mode 100644
index 0000000..5e713b8
--- /dev/null
+++ b/identification/id_plot_iff.m
@@ -0,0 +1,57 @@
+%%
+clear; close all; clc;
+
+%% Load the transfer functions
+load('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');
+
+%% Load Configuration file
+load('./mat/config.mat', 'save_fig', 'freqs');
+
+%%
+figure;
+% Amplitude
+ax1 = subaxis(2,1,1);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_iff('Fm1', 'F1'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G_light_pz.G_iff('Fm1', 'F1'), freqs, 'Hz'))));
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_iff('Fm1', 'F1'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_iff('Fm1', 'F1'), freqs, 'Hz'))), '--');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]);
+ylabel('Amplitude [m/N]');
+hold off;
+
+% Phase
+ax2 = subaxis(2,1,2);
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_vc.G_iff('Fm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_pz.G_iff('Fm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
+set(gca,'ColorOrderIndex',1)
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_heavy_vc.G_iff('Fm1', 'F1'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_heavy_pz.G_iff('Fm1', 'F1'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
+set(gca,'xscale','log');
+yticks(-180:90:180);
+ylim([-180 180]);
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+legend('Location', 'southwest');
+hold off;
+
+linkaxes([ax1,ax2],'x');
+
+if save_fig; exportFig('G_iff', 'normal-normal', struct('path', 'identification')); end
+
+%% Coupling
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_iff('Fm1', 'F1'), freqs, 'Hz'))), 'k-');
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_iff('Fm2', 'F1'), freqs, 'Hz'))), 'k--');
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_iff('Fm3', 'F1'), freqs, 'Hz'))), 'k--');
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_iff('Fm4', 'F1'), freqs, 'Hz'))), 'k--');
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_iff('Fm5', 'F1'), freqs, 'Hz'))), 'k--');
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_iff('Fm6', 'F1'), freqs, 'Hz'))), 'k--');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+hold off;
+
+if save_fig; exportFig('G_iff_coupling', 'normal-normal', struct('path', 'identification')); end
diff --git a/identification/id_plot_legs.m b/identification/id_plot_legs.m
new file mode 100644
index 0000000..78380f7
--- /dev/null
+++ b/identification/id_plot_legs.m
@@ -0,0 +1,57 @@
+%%
+clear; close all; clc;
+
+%% Load the transfer functions
+load('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');
+
+%% Load Configuration file
+load('./mat/config.mat', 'save_fig', 'freqs');
+
+%%
+figure;
+% Amplitude
+ax1 = subaxis(2,1,1);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_legs('D1', 'F1'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G_light_pz.G_legs('D1', 'F1'), freqs, 'Hz'))));
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_legs('D1', 'F1'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_legs('D1', 'F1'), freqs, 'Hz'))), '--');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]);
+ylabel('Amplitude [m/N]');
+hold off;
+
+% Phase
+ax2 = subaxis(2,1,2);
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_vc.G_legs('D1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_pz.G_legs('D1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
+set(gca,'ColorOrderIndex',1)
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_heavy_vc.G_legs('D1', 'F1'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_heavy_pz.G_legs('D1', 'F1'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
+set(gca,'xscale','log');
+yticks(-180:90:180);
+ylim([-180 180]);
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+legend('Location', 'southwest');
+hold off;
+
+linkaxes([ax1,ax2],'x');
+
+if save_fig; exportFig('G_legs', 'normal-normal', struct('path', 'identification')); end
+
+%% Coupling
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_legs('D1', 'F1'), freqs, 'Hz'))), 'k-');
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_legs('D2', 'F1'), freqs, 'Hz'))), 'k--');
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_legs('D3', 'F1'), freqs, 'Hz'))), 'k--');
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_legs('D4', 'F1'), freqs, 'Hz'))), 'k--');
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_legs('D5', 'F1'), freqs, 'Hz'))), 'k--');
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_legs('D6', 'F1'), freqs, 'Hz'))), 'k--');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+hold off;
+
+if save_fig; exportFig('G_legs_coupling', 'normal-normal', struct('path', 'identification')); end
diff --git a/identification_cart.m b/identification_cart.m
deleted file mode 100644
index 55ad03a..0000000
--- a/identification_cart.m
+++ /dev/null
@@ -1,56 +0,0 @@
-%% Script Description
-% Script used to identify the transfer functions of the
-% Stewart platform (from actuator to displacement)
-
-%%
-clear; close all; clc;
-
-%%
-initializeNanoHexapod();
-
-%%
-initializeSample(struct('mass', 0));
-
-G_cart_0 = identifyPlantCart();
-
-%%
-initializeSample(struct('mass', 10));
-
-G_cart_10 = identifyPlantCart();
-
-%%
-initializeSample(struct('mass', 50));
-
-G_cart_50 = identifyPlantCart();
-
-%%
-freqs = logspace(1, 4, 1000);
-
-bodeFig({G_cart_0(1, 1), G_cart_10(1, 1), G_cart_50(1, 1)}, freqs, struct('phase', true))
-legend({'$F_x \rightarrow D_x$ - $M = 0Kg$', '$F_x \rightarrow D_x$ - $M = 10Kg$', '$F_x \rightarrow D_x$ - $M = 50Kg$'})
-legend('location', 'southwest')
-exportFig('hexapod_cart_mass_x', 'normal-tall')
-
-bodeFig({G_cart_0(3, 3), G_cart_10(3, 3), G_cart_50(3, 3)}, freqs, struct('phase', true))
-legend({'$F_z \rightarrow D_z$ - $M = 0Kg$', '$F_z \rightarrow D_z$ - $M = 10Kg$', '$F_z \rightarrow D_z$ - $M = 50Kg$'})
-legend('location', 'southwest')
-exportFig('hexapod_cart_mass_z', 'normal-tall')
-
-%%
-% Bode Plot of the linearized function
-freqs = logspace(2, 4, 1000);
-
-bodeFig({G_cart_0(1, 1), G_cart_0(2, 2), G_cart_0(3, 3)}, freqs, struct('phase', true))
-legend({'$F_x \rightarrow D_x$', '$F_y \rightarrow D_y$', '$F_z \rightarrow D_z$'})
-exportFig('hexapod_cart_trans', 'normal-normal')
-
-bodeFig({G_cart_0(4, 4), G_cart_0(5, 5), G_cart_0(6, 6)}, freqs, struct('phase', true))
-legend({'$M_x \rightarrow R_x$', '$M_y \rightarrow R_y$', '$M_z \rightarrow R_z$'})
-exportFig('hexapod_cart_rot', 'normal-normal')
-
-bodeFig({G_cart_0(1, 1), G_cart_0(2, 1), G_cart_0(3, 1)}, freqs, struct('phase', true))
-legend({'$F_x \rightarrow D_x$', '$F_x \rightarrow D_y$', '$F_x \rightarrow D_z$'})
-exportFig('hexapod_cart_coupling', 'normal-normal')
-
-%% Save identify transfer functions
-save('./mat/G_cart.mat', 'G_cart_0', 'G_cart_10', 'G_cart_50');
diff --git a/identification_legs.m b/identification_legs.m
deleted file mode 100644
index ad8f14b..0000000
--- a/identification_legs.m
+++ /dev/null
@@ -1,45 +0,0 @@
-%% Script Description
-% Script used to identify the transfer functions of the
-% Stewart platform (from actuator to displacement)
-
-%%
-clear; close all; clc;
-
-%%
-initializeNanoHexapod();
-
-%%
-initializeSample(struct('mass', 0));
-
-G_legs_0 = identifyPlantLegs();
-
-%%
-initializeSample(struct('mass', 10));
-
-G_legs_10 = identifyPlantLegs();
-
-%%
-initializeSample(struct('mass', 50));
-
-G_legs_50 = identifyPlantLegs();
-
-%%
-freqs = logspace(1, 4, 1000);
-
-bodeFig({G_legs_0(1, 1), G_legs_10(1, 1), G_legs_50(1, 1)}, freqs, struct('phase', true))
-legend({'$F_i \rightarrow D_i$ - $M = 0Kg$', '$F_i \rightarrow D_i$ - $M = 10Kg$', '$F_i \rightarrow D_i$ - $M = 50Kg$'})
-legend('location', 'southwest')
-
-exportFig('hexapod_legs_mass', 'normal-tall')
-
-%%
-freqs = logspace(1, 4, 1000);
-
-bodeFig({G_legs_0(1, 2), G_legs_10(1, 2), G_legs_50(1, 2)}, freqs, struct('phase', true))
-legend({'$F_i \rightarrow D_j$ - $M = 0Kg$', '$F_i \rightarrow D_j$ - $M = 10Kg$', '$F_i \rightarrow D_j$ - $M = 50Kg$'})
-legend('location', 'southwest')
-
-exportFig('hexapod_legs_coupling_mass', 'normal-tall')
-
-%% Save identify transfer functions
-save('./mat/G_legs.mat', 'G_legs_0', 'G_legs_10', 'G_legs_50');
diff --git a/init_simulink.m b/init_simulink.m
deleted file mode 100644
index 8f154eb..0000000
--- a/init_simulink.m
+++ /dev/null
@@ -1,6 +0,0 @@
-%% Script Description
-% 
-
-%% Load the sample and the stewart platform
-load('./mat/sample.mat', 'sample')
-load('./mat/stewart.mat', 'stewart')
diff --git a/library/QuaternionToAngles.slx b/library/QuaternionToAngles.slx
new file mode 100644
index 0000000..04727b9
Binary files /dev/null and b/library/QuaternionToAngles.slx differ
diff --git a/library/compute_jacobian.slx b/library/compute_jacobian.slx
new file mode 100644
index 0000000..bb8fe80
Binary files /dev/null and b/library/compute_jacobian.slx differ
diff --git a/main.m b/main.m
new file mode 100644
index 0000000..4542661
--- /dev/null
+++ b/main.m
@@ -0,0 +1,11 @@
+%%
+clear; close all; clc;
+
+%% Configuration File
+run config.m
+
+%% Identification
+open id_main.m
+
+%% Active Damping
+open act_damp_main.m
diff --git a/readme.org b/readme.org
deleted file mode 100644
index c7d651b..0000000
--- a/readme.org
+++ /dev/null
@@ -1,6 +0,0 @@
-#+TITLE: Stewart Platform using Simscape
-
-* TODOS
-** TODO Add functions to identify transmissibility and sensitivity
-** TODO Rewrite the script to study the effect of various parameters on the stiffness/stroke/...
-** TODO Rewrite the function to study the jacobian, or delete it.
diff --git a/src/getJacobianMatrix.m b/src/getJacobianMatrix.m
deleted file mode 100644
index 3f3bdef..0000000
--- a/src/getJacobianMatrix.m
+++ /dev/null
@@ -1,7 +0,0 @@
-function J  = getJacobianMatrix(RM,M_pos_base)
-    % RM: [3x6] unit vector of each leg in the fixed frame
-    % M_pos_base: [3x6] vector of the leg connection at the top platform location in the fixed frame
-    J = zeros(6);
-    J(:, 1:3) = RM';
-    J(:, 4:6) = cross(M_pos_base, RM)';
-end
diff --git a/src/getStiffnessMatrix.m b/src/getStiffnessMatrix.m
index 5297ade..5a1a4bb 100644
--- a/src/getStiffnessMatrix.m
+++ b/src/getStiffnessMatrix.m
@@ -1,3 +1,5 @@
-function [K] = getStiffnessMatrix(leg, J)
-    K = leg.k.ax*(J'*J);
+function [K] = getStiffnessMatrix(k, J)
+    % k - leg stiffness
+    % J - Jacobian matrix
+    K = k*(J'*J);
 end
diff --git a/src/identifyPlant.m b/src/identifyPlant.m
new file mode 100644
index 0000000..0229fd1
--- /dev/null
+++ b/src/identifyPlant.m
@@ -0,0 +1,51 @@
+function [sys] = identifyPlant(opts_param)
+    %% Default values for opts
+    opts = struct();
+
+    %% Populate opts with input parameters
+    if exist('opts_param','var')
+        for opt = fieldnames(opts_param)'
+            opts.(opt{1}) = opts_param.(opt{1});
+        end
+    end
+
+    %% Options for Linearized
+    options = linearizeOptions;
+    options.SampleTime = 0;
+
+    %% Name of the Simulink File
+    mdl = 'stewart_identification';
+
+    %% Input/Output definition
+    io(1) = linio([mdl, '/F'],  1, 'input'); % Cartesian forces
+    io(2) = linio([mdl, '/Fl'], 1, 'input'); % Leg forces
+    io(3) = linio([mdl, '/Fd'], 1, 'input'); % Direct forces
+    io(4) = linio([mdl, '/Dw'], 1, 'input'); % Base motion
+
+    io(5) = linio([mdl, '/Dm'],  1, 'output'); % Relative Motion
+    io(6) = linio([mdl, '/Dlm'], 1, 'output'); % Displacement of each leg
+    io(7) = linio([mdl, '/Flm'], 1, 'output'); % Force sensor in each leg
+    io(8) = linio([mdl, '/Xm'],  1, 'output'); % Absolute motion of platform
+
+    %% Run the linearization
+    G = linearize(mdl, io, 0);
+
+    %% Input/Output names
+    G.InputName  = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz', ...
+                    'F1', 'F2', 'F3', 'F4', 'F5', 'F6', ...
+                    'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz', ...
+                    'Dwx', 'Dwy', 'Dwz', 'Rwx', 'Rwy', 'Rwz'};
+    G.OutputName = {'Dxm', 'Dym', 'Dzm', 'Rxm', 'Rym', 'Rzm', ...
+                    'D1m', 'D2m', 'D3m', 'D4m', 'D5m', 'D6m', ...
+                    'F1m', 'F2m', 'F3m', 'F4m', 'F5m', 'F6m', ...
+                    'Dxtm', 'Dytm', 'Dztm', 'Rxtm', 'Rytm', 'Rztm'};
+
+    %% Cut into sub transfer functions
+    sys.G_cart = minreal(G({'Dxm', 'Dym', 'Dzm', 'Rxm', 'Rym', 'Rzm'}, {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}));
+    sys.G_forc = minreal(G({'F1m', 'F2m', 'F3m', 'F4m', 'F5m', 'F6m'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}));
+    sys.G_legs = G({'D1m', 'D2m', 'D3m', 'D4m', 'D5m', 'D6m'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'});
+    sys.G_tran = minreal(G({'Dxm', 'Dym', 'Dzm', 'Rxm', 'Rym', 'Rzm'}, {'Dwx', 'Dwy', 'Dwz', 'Rwx', 'Rwy', 'Rwz'}));
+    sys.G_comp = minreal(G({'Dxm', 'Dym', 'Dzm', 'Rxm', 'Rym', 'Rzm'}, {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'}));
+    sys.G_iner = minreal(G({'Dxtm', 'Dytm', 'Dztm', 'Rxtm', 'Rytm', 'Rztm'}, {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'}));
+    sys.G_all  = minreal(G);
+end
diff --git a/src/identifyPlantCart.m b/src/identifyPlantCart.m
deleted file mode 100644
index e287405..0000000
--- a/src/identifyPlantCart.m
+++ /dev/null
@@ -1,35 +0,0 @@
-function [G_cart, G_cart_raw] = identifyPlantCart()
-    %% Default values for opts
-    opts = struct('f_low', 1,...
-                  'f_high', 10000 ...
-    );
-
-    %% Populate opts with input parameters
-    if exist('opts_param','var')
-        for opt = fieldnames(opts_param)'
-            opts.(opt{1}) = opts_param.(opt{1});
-        end
-    end
-
-    %% Options for Linearized
-    options = linearizeOptions;
-    options.SampleTime = 0;
-
-    %% Name of the Simulink File
-    mdl = 'stewart_simscape';
-
-    %% Centralized control (Cartesian coordinates)
-    % Input/Output definition
-    io(1) = linio([mdl, '/F_cart'],1,'input');
-    io(2) = linio([mdl, '/Stewart_Platform'],1,'output');
-
-    % Run the linearization
-    G_cart_raw = linearize(mdl,io, 0);
-
-    G_cart = preprocessIdTf(G_cart_raw, opts.f_low, opts.f_high);
-
-    % Input/Output names
-    G_cart.InputName  = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
-    G_cart.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
-end
-
diff --git a/src/identifyPlantLegs.m b/src/identifyPlantLegs.m
deleted file mode 100644
index 39b064c..0000000
--- a/src/identifyPlantLegs.m
+++ /dev/null
@@ -1,35 +0,0 @@
-function [G_legs, G_legs_raw] = identifyPlantLegs()
-    %% Default values for opts
-    opts = struct('f_low',  1,    ...
-                  'f_high', 10000 ...
-    );
-
-    %% Populate opts with input parameters
-    if exist('opts_param','var')
-        for opt = fieldnames(opts_param)'
-            opts.(opt{1}) = opts_param.(opt{1});
-        end
-    end
-
-    %% Options for Linearized
-    options = linearizeOptions;
-    options.SampleTime = 0;
-
-    %% Name of the Simulink File
-    mdl = 'stewart_simscape';
-
-    %% Centralized control (Cartesian coordinates)
-    % Input/Output definition
-    io(1) = linio([mdl, '/F_legs'],          1,'input');
-    io(2) = linio([mdl, '/Stewart_Platform'],2,'output');
-
-    % Run the linearization
-    G_legs_raw = linearize(mdl,io, 0);
-
-    G_legs = preprocessIdTf(G_legs_raw, opts.f_low, opts.f_high);
-
-    % Input/Output names
-    G_legs.InputName  = {'F1', 'F2', 'F3', 'M4', 'M5', 'M6'};
-    G_legs.OutputName = {'D1', 'D2', 'D3', 'R4', 'R5', 'R6'};
-end
-
diff --git a/src/initializeHexapod.m b/src/initializeHexapod.m
new file mode 100644
index 0000000..afe1bb9
--- /dev/null
+++ b/src/initializeHexapod.m
@@ -0,0 +1,208 @@
+function [stewart] = initializeHexapod(opts_param)
+    %% Default values for opts
+    opts = struct(...
+        'height',   90,       ... % Height of the platform [mm]
+        'jacobian', 150,      ... % Jacobian offset [mm]
+        'density',  8000,     ... % Density of hexapod [mm]
+        'name',     'stewart' ... % Name of the file
+    );
+
+    %% Populate opts with input parameters
+    if exist('opts_param','var')
+        for opt = fieldnames(opts_param)'
+            opts.(opt{1}) = opts_param.(opt{1});
+        end
+    end
+
+    %% Stewart Object
+    stewart = struct();
+    stewart.h        = opts.height;   % Total height of the platform [mm]
+    stewart.jacobian = opts.jacobian; % distance from the center of the top platform
+                                      % where the jacobian is computed [mm]
+
+    %% Bottom Plate
+    BP = struct();
+
+    BP.rad.int   = 0;    % Internal Radius [mm]
+    BP.rad.ext   = 150;  % External Radius [mm]
+    BP.thickness = 10;   % Thickness [mm]
+    BP.leg.rad   = 100;  % Radius where the legs articulations are positionned [mm]
+    BP.leg.ang   = 5;    % Angle Offset [deg]
+    BP.density   = opts.density; % Density of the material [kg/m3]
+    BP.color     = [0.7 0.7 0.7]; % Color [rgb]
+    BP.shape     = [BP.rad.int BP.thickness; BP.rad.int 0; BP.rad.ext 0; BP.rad.ext BP.thickness];
+
+    %% Top Plate
+    TP = struct();
+
+    TP.rad.int   = 0;    % Internal Radius [mm]
+    TP.rad.ext   = 100;  % Internal Radius [mm]
+    TP.thickness = 10;   % Thickness [mm]
+    TP.leg.rad   = 90;   % Radius where the legs articulations are positionned [mm]
+    TP.leg.ang   = 5;    % Angle Offset [deg]
+    TP.density   = opts.density; % Density of the material [kg/m3]
+    TP.color     = [0.7 0.7 0.7]; % Color [rgb]
+    TP.shape     = [TP.rad.int TP.thickness; TP.rad.int 0; TP.rad.ext 0; TP.rad.ext TP.thickness];
+
+    %% Leg
+    Leg = struct();
+
+    Leg.stroke     = 80e-6; % Maximum Stroke of each leg [m]
+    if strcmp(opts.actuator, 'piezo')
+        Leg.k.ax = 1e7; % Stiffness of each leg [N/m]
+        Leg.c.ax = 500; % [N/(m/s)]
+    elseif strcmp(opts.actuator, 'lorentz')
+        Leg.k.ax = 1e4; % Stiffness of each leg [N/m]
+        Leg.c.ax = 200; % [N/(m/s)]
+   elseif isnumeric(opts.actuator)
+        Leg.k.ax = opts.actuator; % Stiffness of each leg [N/m]
+        Leg.c.ax = 100;           % [N/(m/s)]
+   else
+        error('opts.actuator should be piezo or lorentz or numeric value');
+    end
+    Leg.rad.bottom = 12;   % Radius of the cylinder of the bottom part [mm]
+    Leg.rad.top    = 10;   % Radius of the cylinder of the top part [mm]
+    Leg.density    = opts.density; % Density of the material [kg/m3]
+    Leg.color.bottom  = [0.5 0.5 0.5]; % Color [rgb]
+    Leg.color.top     = [0.5 0.5 0.5]; % Color [rgb]
+
+    Leg.sphere.bottom = Leg.rad.bottom; % Size of the sphere at the end of the leg [mm]
+    Leg.sphere.top    = Leg.rad.top; % Size of the sphere at the end of the leg [mm]
+
+    %% Sphere
+    SP = struct();
+
+    SP.height.bottom  = 15; % [mm]
+    SP.height.top     = 15; % [mm]
+    SP.density.bottom = opts.density; % [kg/m^3]
+    SP.density.top    = opts.density; % [kg/m^3]
+    SP.color.bottom   = [0.7 0.7 0.7]; % [rgb]
+    SP.color.top      = [0.7 0.7 0.7]; % [rgb]
+    SP.k.ax           = 0; % [N*m/deg]
+    SP.c.ax           = 0; % [N*m/deg]
+
+    SP.thickness.bottom = SP.height.bottom-Leg.sphere.bottom; % [mm]
+    SP.thickness.top    = SP.height.top-Leg.sphere.top; % [mm]
+    SP.rad.bottom       = Leg.sphere.bottom; % [mm]
+    SP.rad.top          = Leg.sphere.top; % [mm]
+
+
+    %%
+    Leg.support.bottom = [0 SP.thickness.bottom; 0 0; SP.rad.bottom 0; SP.rad.bottom SP.height.bottom];
+    Leg.support.top    = [0 SP.thickness.top; 0 0; SP.rad.top 0; SP.rad.top SP.height.top];
+
+    %%
+    stewart.BP = BP;
+    stewart.TP = TP;
+    stewart.Leg = Leg;
+    stewart.SP = SP;
+
+    %%
+    stewart = initializeParameters(stewart);
+
+    %%
+    save('./mat/stewart.mat', 'stewart')
+
+    %% ==============================================================
+    %  Additional Functions
+    % ===============================================================
+
+    %% Initialize Parameters
+    function [stewart] = initializeParameters(stewart)
+        %% Connection points on base and top plate w.r.t. World frame at the center of the base plate
+        stewart.pos_base = zeros(6, 3);
+        stewart.pos_top = zeros(6, 3);
+
+        alpha_b = stewart.BP.leg.ang*pi/180; % angle de décalage par rapport à 120 deg (pour positionner les supports bases)
+        alpha_t = stewart.TP.leg.ang*pi/180; % +- offset angle from 120 degree spacing on top
+
+        % Height [m] TODO
+        height = (stewart.h-stewart.BP.thickness-stewart.TP.thickness-stewart.Leg.sphere.bottom-stewart.Leg.sphere.top-stewart.SP.thickness.bottom-stewart.SP.thickness.top)*0.001;
+
+        radius_b = stewart.BP.leg.rad*0.001; % rayon emplacement support base
+        radius_t = stewart.TP.leg.rad*0.001; % top radius in meters
+
+        for i = 1:3
+            % base points
+            angle_m_b = (2*pi/3)* (i-1) - alpha_b;
+            angle_p_b = (2*pi/3)* (i-1) + alpha_b;
+            stewart.pos_base(2*i-1,:) =  [radius_b*cos(angle_m_b), radius_b*sin(angle_m_b), 0.0];
+            stewart.pos_base(2*i,:) = [radius_b*cos(angle_p_b), radius_b*sin(angle_p_b), 0.0];
+
+            % top points
+            % Top points are 60 degrees offset
+            angle_m_t = (2*pi/3)* (i-1) - alpha_t + 2*pi/6;
+            angle_p_t = (2*pi/3)* (i-1) + alpha_t + 2*pi/6;
+            stewart.pos_top(2*i-1,:) = [radius_t*cos(angle_m_t), radius_t*sin(angle_m_t), height];
+            stewart.pos_top(2*i,:) = [radius_t*cos(angle_p_t), radius_t*sin(angle_p_t), height];
+        end
+
+        % permute pos_top points so that legs are end points of base and top points
+        stewart.pos_top = [stewart.pos_top(6,:); stewart.pos_top(1:5,:)]; %6th point on top connects to 1st on bottom
+        stewart.pos_top_tranform = stewart.pos_top - height*[zeros(6, 2),ones(6, 1)];
+
+        %% leg vectors
+        legs = stewart.pos_top - stewart.pos_base;
+        leg_length = zeros(6, 1);
+        leg_vectors = zeros(6, 3);
+        for i = 1:6
+            leg_length(i) = norm(legs(i,:));
+            leg_vectors(i,:)  = legs(i,:) / leg_length(i);
+        end
+
+        stewart.Leg.lenght = 1000*leg_length(1)/1.5;
+        stewart.Leg.shape.bot = [0 0; ...
+                            stewart.Leg.rad.bottom 0; ...
+                            stewart.Leg.rad.bottom stewart.Leg.lenght; ...
+                            stewart.Leg.rad.top stewart.Leg.lenght; ...
+                            stewart.Leg.rad.top 0.2*stewart.Leg.lenght; ...
+                            0 0.2*stewart.Leg.lenght];
+
+        %% Calculate revolute and cylindrical axes
+        rev1 = zeros(6, 3);
+        rev2 = zeros(6, 3);
+        cyl1 = zeros(6, 3);
+        for i = 1:6
+            rev1(i,:) = cross(leg_vectors(i,:), [0 0 1]);
+            rev1(i,:) = rev1(i,:) / norm(rev1(i,:));
+
+            rev2(i,:) = - cross(rev1(i,:), leg_vectors(i,:));
+            rev2(i,:) = rev2(i,:) / norm(rev2(i,:));
+
+            cyl1(i,:) = leg_vectors(i,:);
+        end
+
+
+        %% Coordinate systems
+        stewart.lower_leg = struct('rotation', eye(3));
+        stewart.upper_leg = struct('rotation', eye(3));
+
+        for i = 1:6
+            stewart.lower_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
+            stewart.upper_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
+        end
+
+        %% Position Matrix
+        % TODO
+        stewart.M_pos_base = stewart.pos_base + (height+(stewart.TP.thickness+stewart.Leg.sphere.top+stewart.SP.thickness.top+stewart.jacobian)*1e-3)*[zeros(6, 2),ones(6, 1)];
+
+        %% Compute Jacobian Matrix
+        % TODO
+%         aa = stewart.pos_top_tranform + (stewart.jacobian - stewart.TP.thickness - stewart.SP.height.top)*1e-3*[zeros(6, 2),ones(6, 1)];
+        bb = stewart.pos_top_tranform - (stewart.TP.thickness + stewart.SP.height.top)*1e-3*[zeros(6, 2),ones(6, 1)];
+        bb = bb - stewart.jacobian*1e-3*[zeros(6, 2),ones(6, 1)];
+        stewart.J = getJacobianMatrix(leg_vectors', bb');
+        
+        stewart.K = stewart.Leg.k.ax*stewart.J'*stewart.J;
+    end
+
+    %% Compute the Jacobian Matrix
+    function J  = getJacobianMatrix(RM, M_pos_base)
+        % RM         - [3x6] unit vector of each leg in the fixed frame
+        % M_pos_base - [3x6] vector of the leg connection at the top platform location in the fixed frame
+        J = zeros(6);
+
+        J(:, 1:3) = RM';
+        J(:, 4:6) = cross(M_pos_base, RM)';
+    end
+end
diff --git a/src/initializeMicroHexapod.m b/src/initializeMicroHexapod.m
deleted file mode 100644
index 5f1144d..0000000
--- a/src/initializeMicroHexapod.m
+++ /dev/null
@@ -1,92 +0,0 @@
-function [stewart] = initializeMicroHexapod()
-    %% Stewart Object
-    stewart = struct();
-    stewart.h        = 350; % Total height of the platform [mm]
-    stewart.jacobian = 435; % Point where the Jacobian is computed => Center of rotation [mm]
-
-    %% Bottom Plate
-    BP = struct();
-
-    BP.rad.int   = 110;   % Internal Radius [mm]
-    BP.rad.ext   = 207.5; % External Radius [mm]
-    BP.thickness = 26;    % Thickness [mm]
-    BP.leg.rad   = 175.5; % Radius where the legs articulations are positionned [mm]
-    BP.leg.ang   = 9.5;   % Angle Offset [deg]
-    BP.density   = 8000;  % Density of the material [kg/m^3]
-    BP.color     = [0.6 0.6 0.6]; % Color [rgb]
-    BP.shape     = [BP.rad.int BP.thickness; BP.rad.int 0; BP.rad.ext 0; BP.rad.ext BP.thickness];
-
-    %% Top Plate
-    TP = struct();
-
-    TP.rad.int   = 82;   % Internal Radius [mm]
-    TP.rad.ext   = 150;  % Internal Radius [mm]
-    TP.thickness = 26;   % Thickness [mm]
-    TP.leg.rad   = 118;  % Radius where the legs articulations are positionned [mm]
-    TP.leg.ang   = 12.1; % Angle Offset [deg]
-    TP.density   = 8000; % Density of the material [kg/m^3]
-    TP.color     = [0.6 0.6 0.6]; % Color [rgb]
-    TP.shape     = [TP.rad.int TP.thickness; TP.rad.int 0; TP.rad.ext 0; TP.rad.ext TP.thickness];
-
-    %% Leg
-    Leg = struct(); 
-
-    Leg.stroke     = 10e-3; % Maximum Stroke of each leg [m]
-    Leg.k.ax       = 5e7;   % Stiffness of each leg [N/m]
-    Leg.ksi.ax     = 3;     % Maximum amplification at resonance []
-    Leg.rad.bottom = 25;    % Radius of the cylinder of the bottom part [mm]
-    Leg.rad.top    = 17;    % Radius of the cylinder of the top part [mm]
-    Leg.density    = 8000;  % Density of the material [kg/m^3]
-    Leg.color.bottom  = [0.5 0.5 0.5]; % Color [rgb]
-    Leg.color.top     = [0.5 0.5 0.5]; % Color [rgb]
-
-    Leg.sphere.bottom = Leg.rad.bottom; % Size of the sphere at the end of the leg [mm]
-    Leg.sphere.top    = Leg.rad.top; % Size of the sphere at the end of the leg [mm]
-    Leg.m             = TP.density*((pi*(TP.rad.ext/1000)^2)*(TP.thickness/1000)-(pi*(TP.rad.int/1000^2))*(TP.thickness/1000))/6; % TODO [kg]
-    Leg = updateDamping(Leg);
-
-
-    %% Sphere
-    SP = struct();
-
-    SP.height.bottom  = 27; % [mm]
-    SP.height.top     = 27; % [mm]
-    SP.density.bottom = 8000; % [kg/m^3]
-    SP.density.top    = 8000; % [kg/m^3]
-    SP.color.bottom   = [0.6 0.6 0.6]; % [rgb]
-    SP.color.top      = [0.6 0.6 0.6]; % [rgb]
-    SP.k.ax           = 0; % [N*m/deg]
-    SP.ksi.ax         = 10;
-
-    SP.thickness.bottom = SP.height.bottom-Leg.sphere.bottom; % [mm]
-    SP.thickness.top    = SP.height.top-Leg.sphere.top; % [mm]
-    SP.rad.bottom       = Leg.sphere.bottom; % [mm]
-    SP.rad.top          = Leg.sphere.top; % [mm]
-    SP.m                = SP.density.bottom*2*pi*((SP.rad.bottom*1e-3)^2)*(SP.height.bottom*1e-3); % TODO [kg]
-
-    SP = updateDamping(SP);
-
-    %%
-    Leg.support.bottom = [0 SP.thickness.bottom; 0 0; SP.rad.bottom 0; SP.rad.bottom SP.height.bottom];
-    Leg.support.top    = [0 SP.thickness.top; 0 0; SP.rad.top 0; SP.rad.top SP.height.top];
-
-    %%
-    stewart.BP = BP;
-    stewart.TP = TP;
-    stewart.Leg = Leg;
-    stewart.SP = SP;
-
-    %%
-    stewart = initializeParameters(stewart);
-    
-    %%
-    save('./mat/hexapod.mat', 'stewart');
-
-    %%
-    function element = updateDamping(element)
-        field = fieldnames(element.k);
-        for i = 1:length(field)
-            element.c.(field{i}) = 1/element.ksi.(field{i})*sqrt(element.k.(field{i})/element.m);
-        end
-    end
-end
diff --git a/src/initializeNanoHexapod.m b/src/initializeNanoHexapod.m
deleted file mode 100644
index 4a63041..0000000
--- a/src/initializeNanoHexapod.m
+++ /dev/null
@@ -1,93 +0,0 @@
-function [stewart] = initializeNanoHexapod()
-    %% Stewart Object
-    stewart = struct();
-    stewart.h        = 90;    % Total height of the platform [mm]
-    stewart.jacobian = 174.5; % Point where the Jacobian is computed => Center of rotation [mm]
-
-    %% Bottom Plate
-    BP = struct();
-
-    BP.rad.int   = 0;   % Internal Radius [mm]
-    BP.rad.ext   = 150; % External Radius [mm]
-    BP.thickness = 10;  % Thickness [mm]
-    BP.leg.rad   = 100; % Radius where the legs articulations are positionned [mm]
-    BP.leg.ang   = 5;   % Angle Offset [deg]
-    BP.density   = 8000;% Density of the material [kg/m^3]
-    BP.color     = [0.7 0.7 0.7]; % Color [rgb]
-    BP.shape     = [BP.rad.int BP.thickness; BP.rad.int 0; BP.rad.ext 0; BP.rad.ext BP.thickness];
-
-    %% Top Plate
-    TP = struct();
-
-    TP.rad.int   = 0;   % Internal Radius [mm]
-    TP.rad.ext   = 100; % Internal Radius [mm]
-    TP.thickness = 10;  % Thickness [mm]
-    TP.leg.rad   = 90;  % Radius where the legs articulations are positionned [mm]
-    TP.leg.ang   = 5;   % Angle Offset [deg]
-    TP.density   = 8000;% Density of the material [kg/m^3]
-    TP.color     = [0.7 0.7 0.7]; % Color [rgb]
-    TP.shape     = [TP.rad.int TP.thickness; TP.rad.int 0; TP.rad.ext 0; TP.rad.ext TP.thickness];
-
-    %% Leg
-    Leg = struct(); 
-
-    Leg.stroke     = 80e-6; % Maximum Stroke of each leg [m]
-    Leg.k.ax       = 5e7;   % Stiffness of each leg [N/m]
-    Leg.ksi.ax     = 10;    % Maximum amplification at resonance []
-    Leg.rad.bottom = 12;    % Radius of the cylinder of the bottom part [mm]
-    Leg.rad.top    = 10;    % Radius of the cylinder of the top part [mm]
-    Leg.density    = 8000;  % Density of the material [kg/m^3]
-    Leg.color.bottom  = [0.5 0.5 0.5]; % Color [rgb]
-    Leg.color.top     = [0.5 0.5 0.5]; % Color [rgb]
-
-    Leg.sphere.bottom = Leg.rad.bottom; % Size of the sphere at the end of the leg [mm]
-    Leg.sphere.top    = Leg.rad.top; % Size of the sphere at the end of the leg [mm]
-    Leg.m             = TP.density*((pi*(TP.rad.ext/1000)^2)*(TP.thickness/1000)-(pi*(TP.rad.int/1000^2))*(TP.thickness/1000))/6; % TODO [kg]
-    Leg = updateDamping(Leg);
-
-
-    %% Sphere
-    SP = struct();
-
-    SP.height.bottom  = 15; % [mm]
-    SP.height.top     = 15; % [mm]
-    SP.density.bottom = 8000; % [kg/m^3]
-    SP.density.top    = 8000; % [kg/m^3]
-    SP.color.bottom   = [0.7 0.7 0.7]; % [rgb]
-    SP.color.top      = [0.7 0.7 0.7]; % [rgb]
-    SP.k.ax           = 0; % [N*m/deg]
-    SP.ksi.ax         = 3;
-
-    SP.thickness.bottom = SP.height.bottom-Leg.sphere.bottom; % [mm]
-    SP.thickness.top    = SP.height.top-Leg.sphere.top; % [mm]
-    SP.rad.bottom       = Leg.sphere.bottom; % [mm]
-    SP.rad.top          = Leg.sphere.top; % [mm]
-    SP.m                = SP.density.bottom*2*pi*((SP.rad.bottom*1e-3)^2)*(SP.height.bottom*1e-3); % TODO [kg]
-
-    SP = updateDamping(SP);
-
-    %%
-    Leg.support.bottom = [0 SP.thickness.bottom; 0 0; SP.rad.bottom 0; SP.rad.bottom SP.height.bottom];
-    Leg.support.top    = [0 SP.thickness.top; 0 0; SP.rad.top 0; SP.rad.top SP.height.top];
-
-    %%
-    stewart.BP = BP;
-    stewart.TP = TP;
-    stewart.Leg = Leg;
-    stewart.SP = SP;
-
-    %%
-    stewart = initializeParameters(stewart);
-
-    %%
-    save('./mat/stewart.mat', 'stewart')
-
-    %%
-    function element = updateDamping(element)
-        field = fieldnames(element.k);
-        for i = 1:length(field)
-            element.c.(field{i}) = 1/element.ksi.(field{i})*sqrt(element.k.(field{i})/element.m);
-        end
-    end
-
-end
diff --git a/src/initializeParameters.m b/src/initializeParameters.m
deleted file mode 100644
index a5e6826..0000000
--- a/src/initializeParameters.m
+++ /dev/null
@@ -1,80 +0,0 @@
-function [stewart] = initializeParameters(stewart)
-    %% Connection points on base and top plate w.r.t. World frame at the center of the base plate
-    stewart.pos_base = zeros(6, 3);
-    stewart.pos_top = zeros(6, 3);
-
-    alpha_b = stewart.BP.leg.ang*pi/180; % angle de décalage par rapport à 120 deg (pour positionner les supports bases)
-    alpha_t = stewart.TP.leg.ang*pi/180; % +- offset angle from 120 degree spacing on top
-
-    height = (stewart.h-stewart.BP.thickness-stewart.TP.thickness-stewart.Leg.sphere.bottom-stewart.Leg.sphere.top-stewart.SP.thickness.bottom-stewart.SP.thickness.top)*0.001; % TODO
-
-    radius_b = stewart.BP.leg.rad*0.001; % rayon emplacement support base
-    radius_t = stewart.TP.leg.rad*0.001; % top radius in meters
-
-    for i = 1:3
-      % base points
-      angle_m_b = (2*pi/3)* (i-1) - alpha_b;
-      angle_p_b = (2*pi/3)* (i-1) + alpha_b;
-      stewart.pos_base(2*i-1,:) =  [radius_b*cos(angle_m_b), radius_b*sin(angle_m_b), 0.0];
-      stewart.pos_base(2*i,:) = [radius_b*cos(angle_p_b), radius_b*sin(angle_p_b), 0.0];
-
-      % top points
-      % Top points are 60 degrees offset
-      angle_m_t = (2*pi/3)* (i-1) - alpha_t + 2*pi/6;
-      angle_p_t = (2*pi/3)* (i-1) + alpha_t + 2*pi/6;
-      stewart.pos_top(2*i-1,:) = [radius_t*cos(angle_m_t), radius_t*sin(angle_m_t), height];
-      stewart.pos_top(2*i,:) = [radius_t*cos(angle_p_t), radius_t*sin(angle_p_t), height];
-    end
-
-    % permute pos_top points so that legs are end points of base and top points
-    stewart.pos_top = [stewart.pos_top(6,:); stewart.pos_top(1:5,:)]; %6th point on top connects to 1st on bottom
-    stewart.pos_top_tranform = stewart.pos_top - height*[zeros(6, 2),ones(6, 1)];
-
-    %% leg vectors
-    legs = stewart.pos_top - stewart.pos_base;
-    leg_length = zeros(6, 1);
-    leg_vectors = zeros(6, 3);
-    for i = 1:6
-      leg_length(i) = norm(legs(i,:));
-      leg_vectors(i,:)  = legs(i,:) / leg_length(i);
-    end
-
-    stewart.Leg.lenght = 1000*leg_length(1)/1.5;
-    stewart.Leg.shape.bot = [0 0; ...
-                             stewart.Leg.rad.bottom 0; ...
-                             stewart.Leg.rad.bottom stewart.Leg.lenght; ...
-                             stewart.Leg.rad.top stewart.Leg.lenght; ...
-                             stewart.Leg.rad.top 0.2*stewart.Leg.lenght; ...
-                             0 0.2*stewart.Leg.lenght];
-
-    %% Calculate revolute and cylindrical axes
-    rev1 = zeros(6, 3);
-    rev2 = zeros(6, 3);
-    cyl1 = zeros(6, 3);
-    for i = 1:6
-      rev1(i,:) = cross(leg_vectors(i,:), [0 0 1]);
-      rev1(i,:) = rev1(i,:) / norm(rev1(i,:));
-
-      rev2(i,:) = - cross(rev1(i,:), leg_vectors(i,:));
-      rev2(i,:) = rev2(i,:) / norm(rev2(i,:));
-
-      cyl1(i,:) = leg_vectors(i,:);
-    end
-
-
-    %% Coordinate systems
-    stewart.lower_leg = struct('rotation', eye(3));
-    stewart.upper_leg = struct('rotation', eye(3));
-
-    for i = 1:6
-      stewart.lower_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
-      stewart.upper_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
-    end
-
-    %% Position Matrix
-    stewart.M_pos_base = stewart.pos_base + (height+(stewart.TP.thickness+stewart.Leg.sphere.top+stewart.SP.thickness.top+stewart.jacobian)*1e-3)*[zeros(6, 2),ones(6, 1)];
-
-    %% Compute Jacobian Matrix
-    aa = stewart.pos_top_tranform + (stewart.jacobian - stewart.TP.thickness - stewart.SP.height.top)*1e-3*[zeros(6, 2),ones(6, 1)];
-    stewart.J  = getJacobianMatrix(leg_vectors', aa');
-end
diff --git a/src/initializeSample.m b/src/initializeSample.m
index ae9d8e7..4bae940 100644
--- a/src/initializeSample.m
+++ b/src/initializeSample.m
@@ -1,10 +1,12 @@
 function [] = initializeSample(opts_param)
     %% Default values for opts
-    sample = struct('radius', 100,...
-                    'height', 300,...
-                    'mass',   50,...
-                    'offset', 0,...
-                    'color',  [0.9 0.1 0.1] ...
+    sample = struct( ...
+        'radius',     100, ... % radius of the cylinder [mm]
+        'height',     300, ... % height of the cylinder [mm]
+        'mass',       50,  ... % mass of the cylinder [kg]
+        'measheight', 150, ... % measurement point z-offset [mm]
+        'offset',     [0, 0, 0],   ... % offset position of the sample [mm]
+        'color',      [0.9 0.1 0.1] ...
     );
 
     %% Populate opts with input parameters
diff --git a/stewart_displacement.slx b/stewart_displacement.slx
deleted file mode 100644
index a914947..0000000
Binary files a/stewart_displacement.slx and /dev/null differ
diff --git a/stewart_identification.slx b/stewart_identification.slx
new file mode 100644
index 0000000..704f812
Binary files /dev/null and b/stewart_identification.slx differ
diff --git a/stewart_kinematics.slx b/stewart_kinematics.slx
new file mode 100644
index 0000000..35f47c5
Binary files /dev/null and b/stewart_kinematics.slx differ
diff --git a/stewart_simscape.slx b/stewart_simscape.slx
index f097f85..35f47c5 100644
Binary files a/stewart_simscape.slx and b/stewart_simscape.slx differ
diff --git a/stewart_without_sensing.slx b/stewart_without_sensing.slx
deleted file mode 100644
index 396c599..0000000
Binary files a/stewart_without_sensing.slx and /dev/null differ
diff --git a/studies/com.m b/studies/com.m
new file mode 100644
index 0000000..f1eadf1
--- /dev/null
+++ b/studies/com.m
@@ -0,0 +1,44 @@
+clear; close all; clc;
+
+%% System - center of mass in the plane of joints
+initializeHexapod(struct('actuator', 'piezo', 'jacobian', 150, 'density', 0.1));
+initializeSample(struct('mass', 50, 'height', 300, 'measheight', 150));
+
+%% Identification
+G_aligned = identifyPlant();
+
+%%
+initializeHexapod(struct('actuator', 'piezo', 'jacobian', 160, 'density', 0.1));
+initializeSample(struct('mass', 50, 'height', 300, 'measheight', 160));
+
+%% Identification
+G_com = identifyPlant();
+
+%%
+freqs = logspace(0, 3, 2000);
+
+%%
+bode_opts = bodeoptions;
+bode_opts.FreqUnits = 'Hz';
+bode_opts.MagUnits = 'abs';
+bode_opts.MagScale = 'log';
+bode_opts.PhaseVisible = 'off';
+
+%%
+figure;
+bode(G_aligned.G_cart, G_com.G_cart, 2*pi*freqs, bode_opts);
+
+exportFig('G_com', 'wide-tall', struct('path', 'studies'));
+
+
+%%
+initializeHexapod(struct('actuator', 'piezo', 'jacobian', 150, 'density', 0.1));
+initializeSample(struct('mass', 1, 'height', 300, 'measheight', 150));
+
+%% Identification
+G_massless = identifyPlant();
+
+
+%%
+figure;
+bode(G_aligned.G_cart, G_massless.G_cart, 2*pi*freqs, bode_opts);
diff --git a/studies/coupling.m b/studies/coupling.m
new file mode 100644
index 0000000..f7927bf
--- /dev/null
+++ b/studies/coupling.m
@@ -0,0 +1,41 @@
+clear; close all; clc;
+
+%% System - center of mass in the plane of joints
+initializeHexapod(struct('actuator', 'piezo', 'jacobian', -25, 'density', 0.1));
+initializeSample(struct('mass', 50, 'height', 1, 'measheight', -25, 'offset', [0, 0, -25.5]));
+
+%% Identification
+G_aligned = identifyPlant();
+
+%%
+freqs = logspace(0, 3, 2000);
+
+%%
+bode_opts = bodeoptions;
+bode_opts.FreqUnits = 'Hz';
+bode_opts.MagUnits = 'abs';
+bode_opts.MagScale = 'log';
+bode_opts.PhaseVisible = 'off';
+
+%%
+figure;
+bode(G_aligned.G_legs, 2*pi*freqs, bode_opts);
+
+
+%% Change height of stewart platform
+for height = [50, 70, 90, 110, 130]
+    initializeHexapod(struct('actuator', 'piezo', 'jacobian', -25, 'density', 0.1, 'height', height));
+    G.(['h_' num2str(height)]) = identifyPlant();
+end
+
+%%
+figure;
+bode( ...
+    G.h_50.G_legs, ...
+    G.h_70.G_legs, ...
+    G.h_90.G_legs, ...
+    G.h_110.G_legs, ...
+    G.h_130.G_legs, ...
+    2*pi*freqs, bode_opts);
+% legend({'60', '80', '100', '120', '140'})
+
diff --git a/studies/stiffness_matrix.m b/studies/stiffness_matrix.m
new file mode 100644
index 0000000..b3c6560
--- /dev/null
+++ b/studies/stiffness_matrix.m
@@ -0,0 +1,26 @@
+%%
+clear; close all; clc;
+
+%%
+K_iff = tf(zeros(6));
+save('./mat/controllers.mat', 'K_iff', '-append');
+
+%% Initialize System
+hexapod = initializeHexapod(struct('actuator', 'piezo', 'jacobian', 150));
+initializeSample(struct('mass', 50, 'height', 300, 'measheight', 150));
+
+%% Identify transfer functions
+G = identifyPlant();
+
+%% Run to obtain the computed Jacobian
+sim stewart_identification
+
+%% Compare the two Jacobian matrices
+J_rel = (J.data(:, :, 1)-hexapod.J)./hexapod.J;
+
+%% Compute the Stiffness Matrix
+K = hexapod.Leg.k.ax*hexapod.J'*hexapod.J;
+K_id = pinv(freqresp(G.G_cart, 0));
+
+K_rel = (K-K_id)./K;
+
diff --git a/study_architecture.m b/study_architecture.m
index 3769553..8210d92 100644
--- a/study_architecture.m
+++ b/study_architecture.m
@@ -1,12 +1,8 @@
-%% Script Description
-% 
-
 %%
 clear; close all; clc;
 
 %%
 run stewart_parameters.m
-format shortE
 
 %% Study the effect of the radius of the top platform position of the legs
 leg_radius = 50:1:120;