diff --git a/matlab/src/computeJacobian.m b/matlab/src/computeJacobian.m
deleted file mode 100644
index e4a37a8..0000000
--- a/matlab/src/computeJacobian.m
+++ /dev/null
@@ -1,35 +0,0 @@
-function [stewart] = computeJacobian(stewart)
-% computeJacobian -
-%
-% Syntax: [stewart] = computeJacobian(stewart)
-%
-% Inputs:
-%    - stewart - With at least the following fields:
-%      - geometry.As [3x6] - The 6 unit vectors for each strut expressed in {A}
-%      - geometry.Ab [3x6] - The 6 position of the joints bi expressed in {A}
-%      - actuators.K [6x1] - Total stiffness of the actuators
-%
-% Outputs:
-%    - stewart - With the 3 added field:
-%        - kinematics.J [6x6] - The Jacobian Matrix
-%        - kinematics.K [6x6] - The Stiffness Matrix
-%        - kinematics.C [6x6] - The Compliance Matrix
-
-assert(isfield(stewart.geometry, 'As'),   'stewart.geometry should have attribute As')
-As = stewart.geometry.As;
-
-assert(isfield(stewart.geometry, 'Ab'),   'stewart.geometry should have attribute Ab')
-Ab = stewart.geometry.Ab;
-
-assert(isfield(stewart.actuators, 'K'),   'stewart.actuators should have attribute K')
-Ki = stewart.actuators.K;
-
-J = [As' , cross(Ab, As)'];
-
-K = J'*diag(Ki)*J;
-
-C = inv(K);
-
-stewart.kinematics.J = J;
-stewart.kinematics.K = K;
-stewart.kinematics.C = C;
diff --git a/matlab/src/computeJointsPose.m b/matlab/src/computeJointsPose.m
deleted file mode 100644
index 3e31760..0000000
--- a/matlab/src/computeJointsPose.m
+++ /dev/null
@@ -1,78 +0,0 @@
-function [stewart] = computeJointsPose(stewart)
-% computeJointsPose -
-%
-% Syntax: [stewart] = computeJointsPose(stewart)
-%
-% Inputs:
-%    - stewart - A structure with the following fields
-%        - platform_F.Fa   [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
-%        - platform_M.Mb   [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
-%        - platform_F.FO_A [3x1] - Position of {A} with respect to {F}
-%        - platform_M.MO_B [3x1] - Position of {B} with respect to {M}
-%        - geometry.FO_M   [3x1] - Position of {M} with respect to {F}
-%
-% Outputs:
-%    - stewart - A structure with the following added fields
-%        - geometry.Aa    [3x6]   - The i'th column is the position of ai with respect to {A}
-%        - geometry.Ab    [3x6]   - The i'th column is the position of bi with respect to {A}
-%        - geometry.Ba    [3x6]   - The i'th column is the position of ai with respect to {B}
-%        - geometry.Bb    [3x6]   - The i'th column is the position of bi with respect to {B}
-%        - geometry.l     [6x1]   - The i'th element is the initial length of strut i
-%        - geometry.As    [3x6]   - The i'th column is the unit vector of strut i expressed in {A}
-%        - geometry.Bs    [3x6]   - The i'th column is the unit vector of strut i expressed in {B}
-%        - struts_F.l     [6x1]   - Length of the Fixed part of the i'th strut
-%        - struts_M.l     [6x1]   - Length of the Mobile part of the i'th strut
-%        - platform_F.FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F}
-%        - platform_M.MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M}
-
-assert(isfield(stewart.platform_F, 'Fa'),   'stewart.platform_F should have attribute Fa')
-Fa = stewart.platform_F.Fa;
-
-assert(isfield(stewart.platform_M, 'Mb'),   'stewart.platform_M should have attribute Mb')
-Mb = stewart.platform_M.Mb;
-
-assert(isfield(stewart.platform_F, 'FO_A'), 'stewart.platform_F should have attribute FO_A')
-FO_A = stewart.platform_F.FO_A;
-
-assert(isfield(stewart.platform_M, 'MO_B'), 'stewart.platform_M should have attribute MO_B')
-MO_B = stewart.platform_M.MO_B;
-
-assert(isfield(stewart.geometry,   'FO_M'), 'stewart.geometry should have attribute FO_M')
-FO_M = stewart.geometry.FO_M;
-
-Aa = Fa - repmat(FO_A, [1, 6]);
-Bb = Mb - repmat(MO_B, [1, 6]);
-
-Ab = Bb - repmat(-MO_B-FO_M+FO_A, [1, 6]);
-Ba = Aa - repmat( MO_B+FO_M-FO_A, [1, 6]);
-
-As = (Ab - Aa)./vecnorm(Ab - Aa); % As_i is the i'th vector of As
-
-l = vecnorm(Ab - Aa)';
-
-Bs = (Bb - Ba)./vecnorm(Bb - Ba);
-
-FRa = zeros(3,3,6);
-MRb = zeros(3,3,6);
-
-for i = 1:6
-  FRa(:,:,i) = [cross([0;1;0], As(:,i)) , cross(As(:,i), cross([0;1;0], As(:,i))) , As(:,i)];
-  FRa(:,:,i) = FRa(:,:,i)./vecnorm(FRa(:,:,i));
-
-  MRb(:,:,i) = [cross([0;1;0], Bs(:,i)) , cross(Bs(:,i), cross([0;1;0], Bs(:,i))) , Bs(:,i)];
-  MRb(:,:,i) = MRb(:,:,i)./vecnorm(MRb(:,:,i));
-end
-
-stewart.geometry.Aa = Aa;
-stewart.geometry.Ab = Ab;
-stewart.geometry.Ba = Ba;
-stewart.geometry.Bb = Bb;
-stewart.geometry.As = As;
-stewart.geometry.Bs = Bs;
-stewart.geometry.l  = l;
-
-stewart.struts_F.l  = l/2;
-stewart.struts_M.l  = l/2;
-
-stewart.platform_F.FRa = FRa;
-stewart.platform_M.MRb = MRb;
diff --git a/matlab/src/computeReferencePose.m b/matlab/src/computeReferencePose.m
deleted file mode 100644
index ea8de43..0000000
--- a/matlab/src/computeReferencePose.m
+++ /dev/null
@@ -1,77 +0,0 @@
-function [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)
-% computeReferencePose - Compute the homogeneous transformation matrix corresponding to the wanted pose of the sample
-%
-% Syntax: [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)
-%
-% Inputs:
-%    - Dy - Reference of the Translation Stage [m]
-%    - Ry - Reference of the Tilt Stage [rad]
-%    - Rz - Reference of the Spindle [rad]
-%    - Dh - Reference of the Micro Hexapod (Pitch, Roll, Yaw angles) [m, m, m, rad, rad, rad]
-%    - Dn - Reference of the Nano Hexapod [m, m, m, rad, rad, rad]
-%
-% Outputs:
-%    - WTr -
-
-  %% Translation Stage
-  Rty = [1 0 0 0;
-         0 1 0 Dy;
-         0 0 1 0;
-         0 0 0 1];
-
-  %% Tilt Stage - Pure rotating aligned with Ob
-  Rry = [ cos(Ry) 0 sin(Ry) 0;
-          0       1 0       0;
-         -sin(Ry) 0 cos(Ry) 0;
-          0       0 0       1];
-
-  %% Spindle - Rotation along the Z axis
-  Rrz = [cos(Rz) -sin(Rz) 0 0 ;
-         sin(Rz)  cos(Rz) 0 0 ;
-         0        0       1 0 ;
-         0        0       0 1 ];
-
-
-  %% Micro-Hexapod
-  Rhx = [1 0           0;
-         0 cos(Dh(4)) -sin(Dh(4));
-         0 sin(Dh(4))  cos(Dh(4))];
-
-  Rhy = [ cos(Dh(5)) 0 sin(Dh(5));
-         0           1 0;
-         -sin(Dh(5)) 0 cos(Dh(5))];
-
-  Rhz = [cos(Dh(6)) -sin(Dh(6)) 0;
-         sin(Dh(6))  cos(Dh(6)) 0;
-         0           0          1];
-
-  Rh = [1 0 0 Dh(1) ;
-        0 1 0 Dh(2) ;
-        0 0 1 Dh(3) ;
-        0 0 0 1 ];
-
-  Rh(1:3, 1:3) = Rhz*Rhy*Rhx;
-
-  %% Nano-Hexapod
-  Rnx = [1 0           0;
-         0 cos(Dn(4)) -sin(Dn(4));
-         0 sin(Dn(4))  cos(Dn(4))];
-
-  Rny = [ cos(Dn(5)) 0 sin(Dn(5));
-         0           1 0;
-         -sin(Dn(5)) 0 cos(Dn(5))];
-
-  Rnz = [cos(Dn(6)) -sin(Dn(6)) 0;
-         sin(Dn(6))  cos(Dn(6)) 0;
-         0           0          1];
-
-  Rn = [1 0 0 Dn(1) ;
-        0 1 0 Dn(2) ;
-        0 0 1 Dn(3) ;
-        0 0 0 1 ];
-
-  Rn(1:3, 1:3) = Rnz*Rny*Rnx;
-
-  %% Total Homogeneous transformation
-  WTr = Rty*Rry*Rrz*Rh*Rn;
-end
diff --git a/matlab/src/computeSampleError.m b/matlab/src/computeSampleError.m
deleted file mode 100644
index 5c42696..0000000
--- a/matlab/src/computeSampleError.m
+++ /dev/null
@@ -1,20 +0,0 @@
-function [MTr] = computeSampleError(WTm, WTr)
-% computeSampleError -
-%
-% Syntax: [MTr] = computeSampleError(WTm, WTr)
-%
-% Inputs:
-%    - WTm - Homoegeneous transformation that represent the
-%            wanted pose of the sample with respect to the granite
-%    - WTr - Homoegeneous transformation that represent the
-%            measured pose of the sample with respect to the granite
-%
-% Outputs:
-%    - MTr - Homoegeneous transformation that represent the
-%            wanted pose of the sample expressed in a frame
-%            attached to the top platform of the nano-hexapod
-
-MTr = zeros(4,4);
-
-MTr = [WTm(1:3,1:3)', -WTm(1:3,1:3)'*WTm(1:3,4) ; 0 0 0 1]*WTr;
-end
diff --git a/matlab/src/describeNassSetup.m b/matlab/src/describeNassSetup.m
deleted file mode 100644
index a5f9b26..0000000
--- a/matlab/src/describeNassSetup.m
+++ /dev/null
@@ -1,196 +0,0 @@
-function [] = describeNassSetup()
-% describeNassSetup -
-%
-% Syntax: [] = describeNassSetup()
-%
-% Inputs:
-%    -  -
-%
-% Outputs:
-%    -  -
-
-load('./mat/conf_simscape.mat', 'conf_simscape');
-
-fprintf('Simscape Configuration:\n');
-
-if conf_simscape.type == 1
-    fprintf('- Gravity is included\n');
-else
-    fprintf('- Gravity is not included\n');
-end
-
-fprintf('\n');
-
-load('./mat/nass_disturbances.mat', 'args');
-
-fprintf('Disturbances:\n');
-if ~args.enable
-    fprintf('- No disturbance is included\n');
-else
-    if args.Dwx && args.Dwy && args.Dwz
-        fprintf('- Ground motion\n');
-    end
-    if args.Fty_x && args.Fty_z
-        fprintf('- Vibrations of the Translation Stage\n');
-    end
-    if args.Frz_z
-        fprintf('- Vibrations of the Spindle\n');
-    end
-end
-fprintf('\n');
-
-load('./mat/nass_references.mat', 'args');
-
-fprintf('Reference Tracking:\n');
-fprintf('- Translation Stage:\n');
-switch args.Dy_type
-  case 'constant'
-    fprintf('  - Constant Position\n');
-    fprintf('  - Dy = %.0f [mm]\n', args.Dy_amplitude*1e3);
-  case 'triangular'
-    fprintf('  - Triangular Path\n');
-    fprintf('  - Amplitude = %.0f [mm]\n', args.Dy_amplitude*1e3);
-    fprintf('  - Period = %.0f [s]\n', args.Dy_period);
-  case 'sinusoidal'
-    fprintf('  - Sinusoidal Path\n');
-    fprintf('  - Amplitude = %.0f [mm]\n', args.Dy_amplitude*1e3);
-    fprintf('  - Period = %.0f [s]\n', args.Dy_period);
-end
-
-fprintf('- Tilt Stage:\n');
-switch args.Ry_type
-  case 'constant'
-    fprintf('  - Constant Position\n');
-    fprintf('  - Ry = %.0f [mm]\n', args.Ry_amplitude*1e3);
-  case 'triangular'
-    fprintf('  - Triangular Path\n');
-    fprintf('  - Amplitude = %.0f [mm]\n', args.Ry_amplitude*1e3);
-    fprintf('  - Period = %.0f [s]\n', args.Ry_period);
-  case 'sinusoidal'
-    fprintf('  - Sinusoidal Path\n');
-    fprintf('  - Amplitude = %.0f [mm]\n', args.Ry_amplitude*1e3);
-    fprintf('  - Period = %.0f [s]\n', args.Ry_period);
-end
-
-fprintf('- Spindle:\n');
-switch args.Rz_type
-  case 'constant'
-    fprintf('  - Constant Position\n');
-    fprintf('  - Rz = %.0f [deg]\n', 180/pi*args.Rz_amplitude);
-  case { 'rotating', 'rotating-not-filtered' }
-    fprintf('  - Rotating\n');
-    fprintf('  - Speed = %.0f [rpm]\n', 60/args.Rz_period);
-end
-
-
-fprintf('- Micro Hexapod:\n');
-switch args.Dh_type
-  case 'constant'
-    fprintf('  - Constant Position\n');
-    fprintf('  - Dh = %.0f, %.0f, %.0f [mm]\n',  args.Dh_pos(1), args.Dh_pos(2), args.Dh_pos(3));
-    fprintf('  - Rh = %.0f, %.0f, %.0f [deg]\n', args.Dh_pos(4), args.Dh_pos(5), args.Dh_pos(6));
-end
-
-fprintf('\n');
-
-load('./mat/controller.mat', 'controller');
-
-fprintf('Controller:\n');
-fprintf('- %s\n', controller.name);
-fprintf('\n');
-
-load('./mat/stages.mat', 'ground', 'granite', 'ty', 'ry', 'rz', 'micro_hexapod', 'axisc');
-
-fprintf('Micro Station:\n');
-
-if granite.type == 1 && ...
-        ty.type == 1 && ...
-        ry.type == 1 && ...
-        rz.type == 1 && ...
-        micro_hexapod.type == 1;
-    fprintf('- All stages are rigid\n');
-elseif granite.type == 2 && ...
-        ty.type == 2 && ...
-        ry.type == 2 && ...
-        rz.type == 2 && ...
-        micro_hexapod.type == 2;
-    fprintf('- All stages are flexible\n');
-else
-    if granite.type == 1 || granite.type == 4
-        fprintf('- Granite is rigid\n');
-    else
-        fprintf('- Granite is flexible\n');
-    end
-    if ty.type == 1 || ty.type == 4
-        fprintf('- Translation Stage is rigid\n');
-    else
-        fprintf('- Translation Stage is flexible\n');
-    end
-    if ry.type == 1 || ry.type == 4
-        fprintf('- Tilt Stage is rigid\n');
-    else
-        fprintf('- Tilt Stage is flexible\n');
-    end
-    if rz.type == 1 || rz.type == 4
-        fprintf('- Spindle is rigid\n');
-    else
-        fprintf('- Spindle is flexible\n');
-    end
-    if micro_hexapod.type == 1 || micro_hexapod.type == 4
-        fprintf('- Micro Hexapod is rigid\n');
-    else
-        fprintf('- Micro Hexapod is flexible\n');
-    end
-
-end
-
-fprintf('\n');
-
-load('./mat/stages.mat', 'mirror');
-
-fprintf('Reference Mirror:\n');
-
-if mirror.type == 2;
-    fprintf('- flexible fixation\n');
-    fprintf('- w = %.0f [Hz]\n', mirror.freq(1));
-else
-    fprintf('- rigidly attached to the nano-hexapod\n');
-end
-fprintf('- m = %.0f [kg]\n', mirror.mass);
-fprintf('\n');
-
-load('./mat/stages.mat', 'nano_hexapod');
-
-fprintf('Nano Hexapod:\n');
-
-if nano_hexapod.type == 0;
-    fprintf('- no included\n');
-elseif nano_hexapod.type == 1 || nano_hexapod.type == 3;
-    fprintf('- rigid\n');
-elseif nano_hexapod.type == 2;
-    fprintf('- flexible\n');
-    fprintf('- Ki = %.0g [N/m]\n', nano_hexapod.actuators.K(1));
-end
-
-fprintf('\n');
-
-load('./mat/stages.mat', 'sample');
-
-fprintf('Sample:\n');
-
-if sample.type == 0;
-    fprintf('- no included\n');
-elseif sample.type == 1 || sample.type == 3;
-    fprintf('- rigid\n');
-    fprintf('- mass = %.0f [kg]\n', sample.mass);
-    fprintf('- moment of inertia = %.2f, %.2f, %.2f [kg m2]\n', sample.inertia(1), sample.inertia(2), sample.inertia(3));
-elseif sample.type == 2;
-    fprintf('- flexible\n');
-    fprintf('- mass = %.0f [kg]\n', sample.mass);
-    fprintf('- moment of inertia = %.2f, %.2f, %.2f [kg m2]\n', sample.inertia(1), sample.inertia(2), sample.inertia(3));
-    % fprintf('- Kt = %.0g, %.0g, %.0g [N/m]\n', sample.K(1), sample.K(2), sample.K(3));
-    % fprintf('- Kr = %.0g, %.0g, %.0g [Nm/rad]\n', sample.K(4), sample.K(5), sample.K(6));
-    fprintf('- wt(x,y,z) = %.0f, %.0f, %.0f [Hz]\n', 1/2/pi*sqrt(sample.K(1)/sample.mass), 1/2/pi*sqrt(sample.K(1)/sample.mass), 1/2/pi*sqrt(sample.K(1)/sample.mass));
-    fprintf('- wr(x,y,z) = %.0f, %.0f, %.0f [Hz]\n', 1/2/pi*sqrt(sample.K(4)/sample.inertia(1)), 1/2/pi*sqrt(sample.K(5)/sample.inertia(2)), 1/2/pi*sqrt(sample.K(6)/sample.inertia(3)));
-end
-fprintf('\n');
diff --git a/matlab/src/describeStewartPlatform.m b/matlab/src/describeStewartPlatform.m
deleted file mode 100644
index 256fc60..0000000
--- a/matlab/src/describeStewartPlatform.m
+++ /dev/null
@@ -1,83 +0,0 @@
-function [] = describeStewartPlatform(stewart)
-% describeStewartPlatform - Display some text describing the current defined Stewart Platform
-%
-% Syntax: [] = describeStewartPlatform(args)
-%
-% Inputs:
-%    - stewart
-%
-% Outputs:
-
-arguments
-    stewart
-end
-
-fprintf('GEOMETRY:\n')
-fprintf('- The height between the fixed based and the top platform is %.3g [mm].\n', 1e3*stewart.geometry.H)
-
-if stewart.platform_M.MO_B(3) > 0
-  fprintf('- Frame {A} is located %.3g [mm] above the top platform.\n',  1e3*stewart.platform_M.MO_B(3))
-else
-  fprintf('- Frame {A} is located %.3g [mm] below the top platform.\n', - 1e3*stewart.platform_M.MO_B(3))
-end
-
-fprintf('- The initial length of the struts are:\n')
-fprintf('\t %.3g, %.3g, %.3g, %.3g, %.3g, %.3g [mm]\n', 1e3*stewart.geometry.l)
-fprintf('\n')
-
-fprintf('ACTUATORS:\n')
-if stewart.actuators.type == 1
-    fprintf('- The actuators are classical.\n')
-    fprintf('- The Stiffness and Damping of each actuators is:\n')
-    fprintf('\t k = %.0e [N/m] \t c = %.0e [N/(m/s)]\n', stewart.actuators.K(1), stewart.actuators.C(1))
-elseif stewart.actuators.type == 2
-    fprintf('- The actuators are mechanicaly amplified.\n')
-    fprintf('- The vertical stiffness and damping contribution of the piezoelectric stack is:\n')
-    fprintf('\t ka = %.0e [N/m] \t ca = %.0e [N/(m/s)]\n', stewart.actuators.Ka(1), stewart.actuators.Ca(1))
-    fprintf('- Vertical stiffness when the piezoelectric stack is removed is:\n')
-    fprintf('\t kr = %.0e [N/m] \t cr = %.0e [N/(m/s)]\n', stewart.actuators.Kr(1), stewart.actuators.Cr(1))
-end
-fprintf('\n')
-
-fprintf('JOINTS:\n')
-
-switch stewart.joints_F.type
-  case 1
-    fprintf('- The joints on the fixed based are universal joints\n')
-  case 2
-    fprintf('- The joints on the fixed based are spherical joints\n')
-  case 3
-    fprintf('- The joints on the fixed based are perfect universal joints\n')
-  case 4
-    fprintf('- The joints on the fixed based are perfect spherical joints\n')
-end
-
-switch stewart.joints_M.type
-  case 1
-    fprintf('- The joints on the mobile based are universal joints\n')
-  case 2
-    fprintf('- The joints on the mobile based are spherical joints\n')
-  case 3
-    fprintf('- The joints on the mobile based are perfect universal joints\n')
-  case 4
-    fprintf('- The joints on the mobile based are perfect spherical joints\n')
-end
-
-fprintf('- The position of the joints on the fixed based with respect to {F} are (in [mm]):\n')
-fprintf('\t % .3g \t % .3g \t % .3g\n', 1e3*stewart.platform_F.Fa)
-
-fprintf('- The position of the joints on the mobile based with respect to {M} are (in [mm]):\n')
-fprintf('\t % .3g \t % .3g \t % .3g\n', 1e3*stewart.platform_M.Mb)
-fprintf('\n')
-
-fprintf('KINEMATICS:\n')
-
-if isfield(stewart.kinematics, 'K')
-  fprintf('- The Stiffness matrix K is (in [N/m]):\n')
-  fprintf('\t % .0e \t % .0e \t % .0e \t % .0e \t % .0e \t % .0e\n', stewart.kinematics.K)
-end
-
-if isfield(stewart.kinematics, 'C')
-  fprintf('- The Damping matrix C is (in [m/N]):\n')
-  fprintf('\t % .0e \t % .0e \t % .0e \t % .0e \t % .0e \t % .0e\n', stewart.kinematics.C)
-end
diff --git a/matlab/src/displayArchitecture.m b/matlab/src/displayArchitecture.m
deleted file mode 100644
index 2513217..0000000
--- a/matlab/src/displayArchitecture.m
+++ /dev/null
@@ -1,240 +0,0 @@
-function [] = displayArchitecture(stewart, args)
-% displayArchitecture - 3D plot of the Stewart platform architecture
-%
-% Syntax: [] = displayArchitecture(args)
-%
-% Inputs:
-%    - stewart
-%    - args - Structure with the following fields:
-%        - AP   [3x1] - The wanted position of {B} with respect to {A}
-%        - ARB  [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
-%        - ARB  [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
-%        - F_color [color] - Color used for the Fixed elements
-%        - M_color [color] - Color used for the Mobile elements
-%        - L_color [color] - Color used for the Legs elements
-%        - frames    [true/false] - Display the Frames
-%        - legs      [true/false] - Display the Legs
-%        - joints    [true/false] - Display the Joints
-%        - labels    [true/false] - Display the Labels
-%        - platforms [true/false] - Display the Platforms
-%        - views     ['all', 'xy', 'yz', 'xz', 'default'] -
-%
-% Outputs:
-
-arguments
-    stewart
-    args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
-    args.ARB (3,3) double {mustBeNumeric} = eye(3)
-    args.F_color = [0 0.4470 0.7410]
-    args.M_color = [0.8500 0.3250 0.0980]
-    args.L_color = [0 0 0]
-    args.frames    logical {mustBeNumericOrLogical} = true
-    args.legs      logical {mustBeNumericOrLogical} = true
-    args.joints    logical {mustBeNumericOrLogical} = true
-    args.labels    logical {mustBeNumericOrLogical} = true
-    args.platforms logical {mustBeNumericOrLogical} = true
-    args.views     char    {mustBeMember(args.views,{'all', 'xy', 'xz', 'yz', 'default'})} = 'default'
-end
-
-assert(isfield(stewart.platform_F, 'FO_A'), 'stewart.platform_F should have attribute FO_A')
-FO_A = stewart.platform_F.FO_A;
-
-assert(isfield(stewart.platform_M, 'MO_B'), 'stewart.platform_M should have attribute MO_B')
-MO_B = stewart.platform_M.MO_B;
-
-assert(isfield(stewart.geometry, 'H'),   'stewart.geometry should have attribute H')
-H = stewart.geometry.H;
-
-assert(isfield(stewart.platform_F, 'Fa'),   'stewart.platform_F should have attribute Fa')
-Fa = stewart.platform_F.Fa;
-
-assert(isfield(stewart.platform_M, 'Mb'),   'stewart.platform_M should have attribute Mb')
-Mb = stewart.platform_M.Mb;
-
-if ~strcmp(args.views, 'all')
-  figure;
-else
-  f = figure('visible', 'off');
-end
-
-hold on;
-
-FTa = [eye(3), FO_A; ...
-       zeros(1,3), 1];
-ATb = [args.ARB, args.AP; ...
-       zeros(1,3), 1];
-BTm = [eye(3), -MO_B; ...
-       zeros(1,3), 1];
-
-FTm = FTa*ATb*BTm;
-
-d_unit_vector = H/4;
-
-d_label = H/20;
-
-Ff = [0, 0, 0];
-if args.frames
-  quiver3(Ff(1)*ones(1,3), Ff(2)*ones(1,3), Ff(3)*ones(1,3), ...
-          [d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', args.F_color)
-
-  if args.labels
-    text(Ff(1) + d_label, ...
-        Ff(2) + d_label, ...
-        Ff(3) + d_label, '$\{F\}$', 'Color', args.F_color);
-  end
-end
-
-if args.frames
-  quiver3(FO_A(1)*ones(1,3), FO_A(2)*ones(1,3), FO_A(3)*ones(1,3), ...
-          [d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', args.F_color)
-
-  if args.labels
-    text(FO_A(1) + d_label, ...
-         FO_A(2) + d_label, ...
-         FO_A(3) + d_label, '$\{A\}$', 'Color', args.F_color);
-  end
-end
-
-if args.platforms && stewart.platform_F.type == 1
-  theta = [0:0.01:2*pi+0.01]; % Angles [rad]
-  v = null([0; 0; 1]'); % Two vectors that are perpendicular to the circle normal
-  center = [0; 0; 0]; % Center of the circle
-  radius = stewart.platform_F.R; % Radius of the circle [m]
-
-  points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta));
-
-  plot3(points(1,:), ...
-        points(2,:), ...
-        points(3,:), '-', 'Color', args.F_color);
-end
-
-if args.joints
-  scatter3(Fa(1,:), ...
-           Fa(2,:), ...
-           Fa(3,:), 'MarkerEdgeColor', args.F_color);
-  if args.labels
-    for i = 1:size(Fa,2)
-      text(Fa(1,i) + d_label, ...
-           Fa(2,i), ...
-           Fa(3,i), sprintf('$a_{%i}$', i), 'Color', args.F_color);
-    end
-  end
-end
-
-Fm = FTm*[0; 0; 0; 1]; % Get the position of frame {M} w.r.t. {F}
-
-if args.frames
-  FM_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors
-  quiver3(Fm(1)*ones(1,3), Fm(2)*ones(1,3), Fm(3)*ones(1,3), ...
-          FM_uv(1,1:3), FM_uv(2,1:3), FM_uv(3,1:3), '-', 'Color', args.M_color)
-
-  if args.labels
-    text(Fm(1) + d_label, ...
-         Fm(2) + d_label, ...
-         Fm(3) + d_label, '$\{M\}$', 'Color', args.M_color);
-  end
-end
-
-FB = FO_A + args.AP;
-
-if args.frames
-  FB_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors
-  quiver3(FB(1)*ones(1,3), FB(2)*ones(1,3), FB(3)*ones(1,3), ...
-          FB_uv(1,1:3), FB_uv(2,1:3), FB_uv(3,1:3), '-', 'Color', args.M_color)
-
-  if args.labels
-    text(FB(1) - d_label, ...
-         FB(2) + d_label, ...
-         FB(3) + d_label, '$\{B\}$', 'Color', args.M_color);
-  end
-end
-
-if args.platforms && stewart.platform_M.type == 1
-  theta = [0:0.01:2*pi+0.01]; % Angles [rad]
-  v = null((FTm(1:3,1:3)*[0;0;1])'); % Two vectors that are perpendicular to the circle normal
-  center = Fm(1:3); % Center of the circle
-  radius = stewart.platform_M.R; % Radius of the circle [m]
-
-  points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta));
-
-  plot3(points(1,:), ...
-        points(2,:), ...
-        points(3,:), '-', 'Color', args.M_color);
-end
-
-if args.joints
-  Fb = FTm*[Mb;ones(1,6)];
-
-  scatter3(Fb(1,:), ...
-           Fb(2,:), ...
-           Fb(3,:), 'MarkerEdgeColor', args.M_color);
-
-  if args.labels
-    for i = 1:size(Fb,2)
-      text(Fb(1,i) + d_label, ...
-           Fb(2,i), ...
-           Fb(3,i), sprintf('$b_{%i}$', i), 'Color', args.M_color);
-    end
-  end
-end
-
-if args.legs
-  for i = 1:6
-    plot3([Fa(1,i), Fb(1,i)], ...
-          [Fa(2,i), Fb(2,i)], ...
-          [Fa(3,i), Fb(3,i)], '-', 'Color', args.L_color);
-
-    if args.labels
-      text((Fa(1,i)+Fb(1,i))/2 + d_label, ...
-           (Fa(2,i)+Fb(2,i))/2, ...
-           (Fa(3,i)+Fb(3,i))/2, sprintf('$%i$', i), 'Color', args.L_color);
-    end
-  end
-end
-
-switch args.views
-  case 'default'
-      view([1 -0.6 0.4]);
-  case 'xy'
-      view([0 0 1]);
-  case 'xz'
-      view([0 -1 0]);
-  case 'yz'
-      view([1 0 0]);
-end
-axis equal;
-axis off;
-
-if strcmp(args.views, 'all')
-  hAx = findobj('type', 'axes');
-
-  figure;
-  s1 = subplot(2,2,1);
-  copyobj(get(hAx(1), 'Children'), s1);
-  view([0 0 1]);
-  axis equal;
-  axis off;
-  title('Top')
-
-  s2 = subplot(2,2,2);
-  copyobj(get(hAx(1), 'Children'), s2);
-  view([1 -0.6 0.4]);
-  axis equal;
-  axis off;
-
-  s3 = subplot(2,2,3);
-  copyobj(get(hAx(1), 'Children'), s3);
-  view([1 0 0]);
-  axis equal;
-  axis off;
-  title('Front')
-
-  s4 = subplot(2,2,4);
-  copyobj(get(hAx(1), 'Children'), s4);
-  view([0 -1 0]);
-  axis equal;
-  axis off;
-  title('Side')
-
-  close(f);
-end
diff --git a/matlab/src/extractNodes.m b/matlab/src/extractNodes.m
index 8b0a09f..5c7e679 100644
--- a/matlab/src/extractNodes.m
+++ b/matlab/src/extractNodes.m
@@ -87,4 +87,4 @@ if length(n_xyz) > 0 && length(n_i) > 0
     int_xyz = n_xyz(logical(sum(n_i.*ones(1, length(int_i)) == int_i', 2)), :);
 else
     int_xyz = n_xyz;
-end
\ No newline at end of file
+end
diff --git a/matlab/src/forwardKinematicsApprox.m b/matlab/src/forwardKinematicsApprox.m
deleted file mode 100644
index a6f3c23..0000000
--- a/matlab/src/forwardKinematicsApprox.m
+++ /dev/null
@@ -1,34 +0,0 @@
-function [P, R] = forwardKinematicsApprox(stewart, args)
-% forwardKinematicsApprox - Computed the approximate pose of {B} with respect to {A} from the length of each strut and using
-%                           the Jacobian Matrix
-%
-% Syntax: [P, R] = forwardKinematicsApprox(stewart, args)
-%
-% Inputs:
-%    - stewart - A structure with the following fields
-%        - kinematics.J  [6x6] - The Jacobian Matrix
-%    - args - Can have the following fields:
-%        - dL [6x1] - Displacement of each strut [m]
-%
-% Outputs:
-%    - P  [3x1] - The estimated position of {B} with respect to {A}
-%    - R  [3x3] - The estimated rotation matrix that gives the orientation of {B} with respect to {A}
-
-arguments
-    stewart
-    args.dL (6,1) double {mustBeNumeric} = zeros(6,1)
-end
-
-assert(isfield(stewart.kinematics, 'J'),   'stewart.kinematics should have attribute J')
-J = stewart.kinematics.J;
-
-X = J\args.dL;
-
-P = X(1:3);
-
-theta = norm(X(4:6));
-s = X(4:6)/theta;
-
-R = [s(1)^2*(1-cos(theta)) + cos(theta) ,        s(1)*s(2)*(1-cos(theta)) - s(3)*sin(theta), s(1)*s(3)*(1-cos(theta)) + s(2)*sin(theta);
-     s(2)*s(1)*(1-cos(theta)) + s(3)*sin(theta), s(2)^2*(1-cos(theta)) + cos(theta),         s(2)*s(3)*(1-cos(theta)) - s(1)*sin(theta);
-     s(3)*s(1)*(1-cos(theta)) - s(2)*sin(theta), s(3)*s(2)*(1-cos(theta)) + s(1)*sin(theta), s(3)^2*(1-cos(theta)) + cos(theta)];
diff --git a/matlab/src/generateCubicConfiguration.m b/matlab/src/generateCubicConfiguration.m
deleted file mode 100644
index 596787b..0000000
--- a/matlab/src/generateCubicConfiguration.m
+++ /dev/null
@@ -1,50 +0,0 @@
-function [stewart] = generateCubicConfiguration(stewart, args)
-% generateCubicConfiguration - Generate a Cubic Configuration
-%
-% Syntax: [stewart] = generateCubicConfiguration(stewart, args)
-%
-% Inputs:
-%    - stewart - A structure with the following fields
-%        - geometry.H [1x1] - Total height of the platform [m]
-%    - args - Can have the following fields:
-%        - Hc  [1x1] - Height of the "useful" part of the cube [m]
-%        - FOc [1x1] - Height of the center of the cube with respect to {F} [m]
-%        - FHa [1x1] - Height of the plane joining the points ai with respect to the frame {F} [m]
-%        - MHb [1x1] - Height of the plane joining the points bi with respect to the frame {M} [m]
-%
-% Outputs:
-%    - stewart - updated Stewart structure with the added fields:
-%        - platform_F.Fa  [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
-%        - platform_M.Mb  [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
-
-arguments
-    stewart
-    args.Hc  (1,1) double {mustBeNumeric, mustBePositive} = 60e-3
-    args.FOc (1,1) double {mustBeNumeric} = 50e-3
-    args.FHa (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e-3
-    args.MHb (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e-3
-end
-
-assert(isfield(stewart.geometry, 'H'),   'stewart.geometry should have attribute H')
-H = stewart.geometry.H;
-
-sx = [ 2; -1; -1];
-sy = [ 0;  1; -1];
-sz = [ 1;  1;  1];
-
-R = [sx, sy, sz]./vecnorm([sx, sy, sz]);
-
-L = args.Hc*sqrt(3);
-
-Cc = R'*[[0;0;L],[L;0;L],[L;0;0],[L;L;0],[0;L;0],[0;L;L]] - [0;0;1.5*args.Hc];
-
-CCf = [Cc(:,1), Cc(:,3), Cc(:,3), Cc(:,5), Cc(:,5), Cc(:,1)]; % CCf(:,i) corresponds to the bottom cube's vertice corresponding to the i'th leg
-CCm = [Cc(:,2), Cc(:,2), Cc(:,4), Cc(:,4), Cc(:,6), Cc(:,6)]; % CCm(:,i) corresponds to the top cube's vertice corresponding to the i'th leg
-
-CSi = (CCm - CCf)./vecnorm(CCm - CCf);
-
-Fa = CCf + [0; 0; args.FOc] + ((args.FHa-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;
-Mb = CCf + [0; 0; args.FOc-H] + ((H-args.MHb-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;
-
-stewart.platform_F.Fa = Fa;
-stewart.platform_M.Mb = Mb;
diff --git a/matlab/src/generateDiagPidControl.m b/matlab/src/generateDiagPidControl.m
deleted file mode 100644
index 47a777a..0000000
--- a/matlab/src/generateDiagPidControl.m
+++ /dev/null
@@ -1,18 +0,0 @@
-function [K] = generateDiagPidControl(G, fs)
-    %%
-    pid_opts = pidtuneOptions(...
-        'PhaseMargin', 50, ...
-        'DesignFocus', 'disturbance-rejection');
-
-    %%
-    K = tf(zeros(6));
-
-    for i = 1:6
-        input_name = G.InputName(i);
-        output_name = G.OutputName(i);
-        K(i, i) = tf(pidtune(minreal(G(output_name, input_name)), 'PIDF', 2*pi*fs, pid_opts));
-    end
-
-    K.InputName = G.OutputName;
-    K.OutputName = G.InputName;
-end
diff --git a/matlab/src/generateGeneralConfiguration.m b/matlab/src/generateGeneralConfiguration.m
deleted file mode 100644
index fbb570d..0000000
--- a/matlab/src/generateGeneralConfiguration.m
+++ /dev/null
@@ -1,39 +0,0 @@
-function [stewart] = generateGeneralConfiguration(stewart, args)
-% generateGeneralConfiguration - Generate a Very General Configuration
-%
-% Syntax: [stewart] = generateGeneralConfiguration(stewart, args)
-%
-% Inputs:
-%    - args - Can have the following fields:
-%        - FH  [1x1] - Height of the position of the fixed joints with respect to the frame {F} [m]
-%        - FR  [1x1] - Radius of the position of the fixed joints in the X-Y [m]
-%        - FTh [6x1] - Angles of the fixed joints in the X-Y plane with respect to the X axis [rad]
-%        - MH  [1x1] - Height of the position of the mobile joints with respect to the frame {M} [m]
-%        - FR  [1x1] - Radius of the position of the mobile joints in the X-Y [m]
-%        - MTh [6x1] - Angles of the mobile joints in the X-Y plane with respect to the X axis [rad]
-%
-% Outputs:
-%    - stewart - updated Stewart structure with the added fields:
-%        - platform_F.Fa  [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
-%        - platform_M.Mb  [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
-
-arguments
-    stewart
-    args.FH  (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
-    args.FR  (1,1) double {mustBeNumeric, mustBePositive} = 115e-3;
-    args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180);
-    args.MH  (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
-    args.MR  (1,1) double {mustBeNumeric, mustBePositive} = 90e-3;
-    args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180);
-end
-
-Fa = zeros(3,6);
-Mb = zeros(3,6);
-
-for i = 1:6
-  Fa(:,i) = [args.FR*cos(args.FTh(i)); args.FR*sin(args.FTh(i));  args.FH];
-  Mb(:,i) = [args.MR*cos(args.MTh(i)); args.MR*sin(args.MTh(i)); -args.MH];
-end
-
-stewart.platform_F.Fa = Fa;
-stewart.platform_M.Mb = Mb;
diff --git a/matlab/src/generateShapedNoise.m b/matlab/src/generateShapedNoise.m
deleted file mode 100644
index 51748c2..0000000
--- a/matlab/src/generateShapedNoise.m
+++ /dev/null
@@ -1,53 +0,0 @@
-function [U_exc] = generateShapedNoise(args)
-% generateShapedNoise - Generate a Shaped Noise excitation signal
-%
-% Syntax: [U_exc] = generateShapedNoise(args)
-%
-% Inputs:
-%    - args - Optinal arguments:
-%        - Ts              - Sampling Time                                              - [s]
-%        - V_mean          - Mean value of the excitation voltage                       - [V]
-%        - V_exc           - Excitation Amplitude, could be numeric or TF               - [V rms]
-%        - t_start         - Time at which the noise begins                             - [s]
-%        - exc_duration    - Duration of the noise                                      - [s]
-%        - smooth_ends     - 'true' or 'false': smooth transition between 0 and V_mean  - [-]
-
-arguments
-    args.Ts              (1,1) double  {mustBeNumeric, mustBePositive} = 1e-4
-    args.V_mean          (1,1) double  {mustBeNumeric} = 0
-    args.V_exc                                         = 1
-    args.t_start         (1,1) double  {mustBeNumeric, mustBePositive} = 5
-    args.exc_duration    (1,1) double  {mustBeNumeric, mustBePositive} = 10
-    args.smooth_ends           logical {mustBeNumericOrLogical} = true
-end
-
-t_noise = 0:args.Ts:args.exc_duration;
-
-if isnumeric(args.V_exc)
-    V_noise = args.V_mean + args.V_exc*sqrt(1/args.Ts/2)*randn(length(t_noise), 1)';
-elseif isct(args.V_exc)
-    V_noise = args.V_mean + lsim(args.V_exc, sqrt(1/args.Ts/2)*randn(length(t_noise), 1), t_noise)';
-end
-
-t_smooth_start = args.Ts:args.Ts:args.t_start;
-
-V_smooth_start = zeros(size(t_smooth_start));
-V_smooth_end   = zeros(size(t_smooth_start));
-
-if args.smooth_ends
-    Vd_max = args.V_mean/(0.7*args.t_start);
-
-    V_d = zeros(size(t_smooth_start));
-    V_d(t_smooth_start < 0.2*args.t_start) = t_smooth_start(t_smooth_start < 0.2*args.t_start)*Vd_max/(0.2*args.t_start);
-    V_d(t_smooth_start > 0.2*args.t_start & t_smooth_start < 0.7*args.t_start) = Vd_max;
-    V_d(t_smooth_start > 0.7*args.t_start & t_smooth_start < 0.9*args.t_start) = Vd_max - (t_smooth_start(t_smooth_start > 0.7*args.t_start & t_smooth_start < 0.9*args.t_start) - 0.7*args.t_start)*Vd_max/(0.2*args.t_start);
-
-    V_smooth_start = cumtrapz(V_d)*args.Ts;
-
-    V_smooth_end = args.V_mean - V_smooth_start;
-end
-
-V_exc = [V_smooth_start, V_noise, V_smooth_end];
-t_exc = args.Ts*[0:1:length(V_exc)-1];
-
-U_exc = [t_exc; V_exc];
diff --git a/matlab/src/generateSinIncreasingAmpl.m b/matlab/src/generateSinIncreasingAmpl.m
deleted file mode 100644
index 4ce9170..0000000
--- a/matlab/src/generateSinIncreasingAmpl.m
+++ /dev/null
@@ -1,54 +0,0 @@
-function [U_exc] = generateSinIncreasingAmpl(args)
-% generateSinIncreasingAmpl - Generate Sinus with increasing amplitude
-%
-% Syntax: [U_exc] = generateSinIncreasingAmpl(args)
-%
-% Inputs:
-%    - args - Optinal arguments:
-%        - Ts              - Sampling Time                                              - [s]
-%        - V_mean          - Mean value of the excitation voltage                       - [V]
-%        - sin_ampls       - Excitation Amplitudes                                      - [V]
-%        - sin_freq        - Excitation Frequency                                       - [Hz]
-%        - sin_num         - Number of period for each amplitude                        - [-]
-%        - t_start         - Time at which the excitation begins                        - [s]
-%        - smooth_ends     - 'true' or 'false': smooth transition between 0 and V_mean  - [-]
-
-arguments
-    args.Ts              (1,1) double  {mustBeNumeric, mustBePositive} = 1e-4
-    args.V_mean          (1,1) double  {mustBeNumeric} = 0
-    args.sin_ampls             double  {mustBeNumeric, mustBePositive} = [0.1, 0.2, 0.3]
-    args.sin_period      (1,1) double  {mustBeNumeric, mustBePositive} = 1
-    args.sin_num         (1,1) double  {mustBeNumeric, mustBePositive, mustBeInteger} = 3
-    args.t_start         (1,1) double  {mustBeNumeric, mustBePositive} = 5
-    args.smooth_ends           logical {mustBeNumericOrLogical} = true
-end
-
-t_noise = 0:args.Ts:args.sin_period*args.sin_num;
-sin_exc = [];
-
-for sin_ampl = args.sin_ampls
-    sin_exc = [sin_exc, args.V_mean + sin_ampl*sin(2*pi/args.sin_period*t_noise)];
-end
-
-t_smooth_start = args.Ts:args.Ts:args.t_start;
-
-V_smooth_start = zeros(size(t_smooth_start));
-V_smooth_end   = zeros(size(t_smooth_start));
-
-if args.smooth_ends
-    Vd_max = args.V_mean/(0.7*args.t_start);
-
-    V_d = zeros(size(t_smooth_start));
-    V_d(t_smooth_start < 0.2*args.t_start) = t_smooth_start(t_smooth_start < 0.2*args.t_start)*Vd_max/(0.2*args.t_start);
-    V_d(t_smooth_start > 0.2*args.t_start & t_smooth_start < 0.7*args.t_start) = Vd_max;
-    V_d(t_smooth_start > 0.7*args.t_start & t_smooth_start < 0.9*args.t_start) = Vd_max - (t_smooth_start(t_smooth_start > 0.7*args.t_start & t_smooth_start < 0.9*args.t_start) - 0.7*args.t_start)*Vd_max/(0.2*args.t_start);
-
-    V_smooth_start = cumtrapz(V_d)*args.Ts;
-
-    V_smooth_end = args.V_mean - V_smooth_start;
-end
-
-V_exc = [V_smooth_start, sin_exc, V_smooth_end];
-t_exc = args.Ts*[0:1:length(V_exc)-1];
-
-U_exc = [t_exc; V_exc];
diff --git a/matlab/src/generateSpiralAngleTrajectory.m b/matlab/src/generateSpiralAngleTrajectory.m
deleted file mode 100644
index ac2d33c..0000000
--- a/matlab/src/generateSpiralAngleTrajectory.m
+++ /dev/null
@@ -1,35 +0,0 @@
-function [ref] = generateSpiralAngleTrajectory(args)
-% generateSpiralAngleTrajectory -
-%
-% Syntax: [ref] = generateSpiralAngleTrajectory(args)
-%
-% Inputs:
-%    - args
-%
-% Outputs:
-%    - ref - Reference Signal
-
-arguments
-    args.R_tot  (1,1) double {mustBeNumeric, mustBePositive} = 10e-6 % [rad]
-    args.n_turn (1,1) double {mustBeInteger, mustBePositive} = 5 % [-]
-    args.Ts     (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % [s]
-    args.t_turn (1,1) double {mustBeNumeric, mustBePositive} = 1 % [s]
-    args.t_end  (1,1) double {mustBeNumeric, mustBePositive} = 1 % [s]
-end
-
-time_s = 0:args.Ts:args.n_turn*args.t_turn;
-time_e = 0:args.Ts:args.t_end;
-
-Rx = sin(2*pi*time_s/args.t_turn).*(args.R_tot*time_s/(args.n_turn*args.t_turn));
-Ry = cos(2*pi*time_s/args.t_turn).*(args.R_tot*time_s/(args.n_turn*args.t_turn));
-
-Rx = [Rx, 0*time_e];
-Ry = [Ry, Ry(end) - Ry(end)*time_e/args.t_end];
-
-t = 0:args.Ts:args.Ts*(length(Rx) - 1);
-
-ref = zeros(length(Rx), 7);
-
-ref(:, 1) = t;
-ref(:, 5) = Rx;
-ref(:, 6) = Ry;
diff --git a/matlab/src/generateSweepExc.m b/matlab/src/generateSweepExc.m
deleted file mode 100644
index 252b8c5..0000000
--- a/matlab/src/generateSweepExc.m
+++ /dev/null
@@ -1,76 +0,0 @@
-function [U_exc] = generateSweepExc(args)
-% generateSweepExc - Generate a Sweep Sine excitation signal
-%
-% Syntax: [U_exc] = generateSweepExc(args)
-%
-% Inputs:
-%    - args - Optinal arguments:
-%        - Ts              - Sampling Time                                              - [s]
-%        - f_start         - Start frequency of the sweep                               - [Hz]
-%        - f_end           - End frequency of the sweep                                 - [Hz]
-%        - V_mean          - Mean value of the excitation voltage                       - [V]
-%        - V_exc           - Excitation Amplitude for the Sweep, could be numeric or TF - [V]
-%        - t_start         - Time at which the sweep begins                             - [s]
-%        - exc_duration    - Duration of the sweep                                      - [s]
-%        - sweep_type      - 'logarithmic' or 'linear'                                  - [-]
-%        - smooth_ends     - 'true' or 'false': smooth transition between 0 and V_mean  - [-]
-
-arguments
-    args.Ts              (1,1) double  {mustBeNumeric, mustBePositive} = 1e-4
-    args.f_start         (1,1) double  {mustBeNumeric, mustBePositive} = 1
-    args.f_end           (1,1) double  {mustBeNumeric, mustBePositive} = 1e3
-    args.V_mean          (1,1) double  {mustBeNumeric} = 0
-    args.V_exc                                         = 1
-    args.t_start         (1,1) double  {mustBeNumeric, mustBeNonnegative} = 5
-    args.exc_duration    (1,1) double  {mustBeNumeric, mustBePositive} = 10
-    args.sweep_type            char    {mustBeMember(args.sweep_type,{'log', 'lin'})} = 'lin'
-    args.smooth_ends           logical {mustBeNumericOrLogical} = true
-end
-
-t_sweep = 0:args.Ts:args.exc_duration;
-
-if strcmp(args.sweep_type, 'log')
-    V_exc = sin(2*pi*args.f_start * args.exc_duration/log(args.f_end/args.f_start) * (exp(log(args.f_end/args.f_start)*t_sweep/args.exc_duration) - 1));
-elseif strcmp(args.sweep_type, 'lin')
-    V_exc = sin(2*pi*(args.f_start + (args.f_end - args.f_start)/2/args.exc_duration*t_sweep).*t_sweep);
-else
-    error('sweep_type should either be equal to "log" or to "lin"');
-end
-
-if isnumeric(args.V_exc)
-    V_sweep = args.V_mean + args.V_exc*V_exc;
-elseif isct(args.V_exc)
-    if strcmp(args.sweep_type, 'log')
-        V_sweep = args.V_mean + abs(squeeze(freqresp(args.V_exc, args.f_start*(args.f_end/args.f_start).^(t_sweep/args.exc_duration), 'Hz')))'.*V_exc;
-    elseif strcmp(args.sweep_type, 'lin')
-        V_sweep = args.V_mean + abs(squeeze(freqresp(args.V_exc, args.f_start+(args.f_end-args.f_start)/args.exc_duration*t_sweep, 'Hz')))'.*V_exc;
-    end
-end
-
-if args.t_start > 0
-    t_smooth_start = args.Ts:args.Ts:args.t_start;
-
-    V_smooth_start = zeros(size(t_smooth_start));
-    V_smooth_end   = zeros(size(t_smooth_start));
-
-    if args.smooth_ends
-        Vd_max = args.V_mean/(0.7*args.t_start);
-
-        V_d = zeros(size(t_smooth_start));
-        V_d(t_smooth_start < 0.2*args.t_start) = t_smooth_start(t_smooth_start < 0.2*args.t_start)*Vd_max/(0.2*args.t_start);
-        V_d(t_smooth_start > 0.2*args.t_start & t_smooth_start < 0.7*args.t_start) = Vd_max;
-        V_d(t_smooth_start > 0.7*args.t_start & t_smooth_start < 0.9*args.t_start) = Vd_max - (t_smooth_start(t_smooth_start > 0.7*args.t_start & t_smooth_start < 0.9*args.t_start) - 0.7*args.t_start)*Vd_max/(0.2*args.t_start);
-
-        V_smooth_start = cumtrapz(V_d)*args.Ts;
-
-        V_smooth_end = args.V_mean - V_smooth_start;
-    end
-else
-    V_smooth_start = [];
-    V_smooth_end = [];
-end
-
-V_exc = [V_smooth_start, V_sweep, V_smooth_end];
-t_exc = args.Ts*[0:1:length(V_exc)-1];
-
-U_exc = [t_exc; V_exc];
diff --git a/matlab/src/generateXYZTrajectory.m b/matlab/src/generateXYZTrajectory.m
deleted file mode 100644
index 3c2e69f..0000000
--- a/matlab/src/generateXYZTrajectory.m
+++ /dev/null
@@ -1,48 +0,0 @@
-function [ref] = generateXYZTrajectory(args)
-% generateXYZTrajectory -
-%
-% Syntax: [ref] = generateXYZTrajectory(args)
-%
-% Inputs:
-%    - args
-%
-% Outputs:
-%    - ref - Reference Signal
-
-arguments
-    args.points double {mustBeNumeric} = zeros(2, 3) % [m]
-
-    args.ti    (1,1) double {mustBeNumeric, mustBeNonnegative} = 1 % Time to go to first point and after last point [s]
-    args.tw    (1,1) double {mustBeNumeric, mustBeNonnegative} = 0.5 % Time wait between each point [s]
-    args.tm    (1,1) double {mustBeNumeric, mustBeNonnegative} = 1 % Motion time between points [s]
-
-    args.Ts    (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % Sampling Time [s]
-end
-
-time_i = 0:args.Ts:args.ti;
-time_w = 0:args.Ts:args.tw;
-time_m = 0:args.Ts:args.tm;
-
-% Go to initial position
-xyz = (args.points(1,:))'*(time_i/args.ti);
-
-% Wait
-xyz = [xyz, xyz(:,end).*ones(size(time_w))];
-
-% Scans
-for i = 2:size(args.points, 1)
-    % Go to next point
-    xyz = [xyz, xyz(:,end) + (args.points(i,:)' - xyz(:,end))*(time_m/args.tm)];
-    % Wait a litle bit
-    xyz = [xyz, xyz(:,end).*ones(size(time_w))];
-end
-
-% End motion
-xyz = [xyz, xyz(:,end) - xyz(:,end)*(time_i/args.ti)];
-
-t = 0:args.Ts:args.Ts*(length(xyz) - 1);
-
-ref = zeros(length(xyz), 7);
-
-ref(:, 1) = t;
-ref(:, 2:4) = xyz';
diff --git a/matlab/src/generateYZScanTrajectory.m b/matlab/src/generateYZScanTrajectory.m
deleted file mode 100644
index e23b168..0000000
--- a/matlab/src/generateYZScanTrajectory.m
+++ /dev/null
@@ -1,83 +0,0 @@
-function [ref] = generateYZScanTrajectory(args)
-% generateYZScanTrajectory -
-%
-% Syntax: [ref] = generateYZScanTrajectory(args)
-%
-% Inputs:
-%    - args
-%
-% Outputs:
-%    - ref - Reference Signal
-
-arguments
-    args.y_tot (1,1) double {mustBeNumeric, mustBePositive} = 10e-6 % [m]
-    args.z_tot (1,1) double {mustBeNumeric, mustBePositive} = 10e-6 % [m]
-
-    args.n     (1,1) double {mustBeInteger, mustBePositive} = 10 % [-]
-
-    args.Ts    (1,1) double {mustBeNumeric, mustBePositive} = 1e-4 % [s]
-
-    args.ti    (1,1) double {mustBeNumeric, mustBeNonnegative} = 1 % [s]
-    args.tw    (1,1) double {mustBeNumeric, mustBeNonnegative} = 1 % [s]
-    args.ty    (1,1) double {mustBeNumeric, mustBeNonnegative} = 1 % [s]
-    args.tz    (1,1) double {mustBeNumeric, mustBeNonnegative} = 1 % [s]
-end
-
-time_i = 0:args.Ts:args.ti;
-time_w = 0:args.Ts:args.tw;
-time_y = 0:args.Ts:args.ty;
-time_z = 0:args.Ts:args.tz;
-
-% Go to initial position
-y = (time_i/args.ti)*(args.y_tot/2);
-
-% Wait
-y = [y, y(end)*ones(size(time_w))];
-
-% Scans
-for i = 1:args.n
-    if mod(i,2) == 0
-        y = [y, -(args.y_tot/2) + (time_y/args.ty)*args.y_tot];
-    else
-        y = [y,  (args.y_tot/2) - (time_y/args.ty)*args.y_tot];
-    end
-
-    if i < args.n
-        y = [y, y(end)*ones(size(time_z))];
-    end
-end
-
-% Wait a litle bit
-y = [y, y(end)*ones(size(time_w))];
-
-% End motion
-y = [y, y(end) - y(end)*time_i/args.ti];
-
-% Go to initial position
-z = (time_i/args.ti)*(args.z_tot/2);
-
-% Wait
-z = [z, z(end)*ones(size(time_w))];
-
-% Scans
-for i = 1:args.n
-    z = [z, z(end)*ones(size(time_y))];
-
-    if i < args.n
-        z = [z, z(end) - (time_z/args.tz)*args.z_tot/(args.n-1)];
-    end
-end
-
-% Wait a litle bit
-z = [z, z(end)*ones(size(time_w))];
-
-% End motion
-z = [z, z(end) - z(end)*time_i/args.ti];
-
-t = 0:args.Ts:args.Ts*(length(y) - 1);
-
-ref = zeros(length(y), 7);
-
-ref(:, 1) = t;
-ref(:, 3) = y;
-ref(:, 4) = z;
diff --git a/matlab/src/getJacobianNanoHexapod.m b/matlab/src/getJacobianNanoHexapod.m
deleted file mode 100644
index f660c78..0000000
--- a/matlab/src/getJacobianNanoHexapod.m
+++ /dev/null
@@ -1,34 +0,0 @@
-function [J] = getJacobianNanoHexapod(Hbm)
-% getJacobianNanoHexapod -
-%
-% Syntax: [J] = getJacobianNanoHexapod(Hbm)
-%
-% Inputs:
-%    - Hbm - Height of {B} w.r.t. {M} [m]
-%
-% Outputs:
-%    - J - Jacobian Matrix
-
-Fa = [[-86.05,  -74.78, 22.49],
-      [ 86.05,  -74.78, 22.49],
-      [ 107.79, -37.13, 22.49],
-      [ 21.74,  111.91, 22.49],
-      [-21.74,  111.91, 22.49],
-      [-107.79, -37.13, 22.49]]'*1e-3; % Ai w.r.t. {F} [m]
-
-Mb = [[-28.47, -106.25, -22.50],
-      [ 28.47, -106.25, -22.50],
-      [ 106.25, 28.47,  -22.50],
-      [ 77.78,  77.78,  -22.50],
-      [-77.78,  77.78,  -22.50],
-      [-106.25, 28.47,  -22.50]]'*1e-3; % Bi w.r.t. {M} [m]
-
-H = 95e-3; % Stewart platform height [m]
-Fb = Mb + [0; 0; H]; % Bi w.r.t. {F} [m]
-
-si = Fb - Fa;
-si = si./vecnorm(si); % Normalize
-
-Bb = Mb - [0; 0; Hbm];
-
-J = [si', cross(Bb, si)'];
diff --git a/matlab/src/getTransformationMatrixAcc.m b/matlab/src/getTransformationMatrixAcc.m
deleted file mode 100644
index cc4dd44..0000000
--- a/matlab/src/getTransformationMatrixAcc.m
+++ /dev/null
@@ -1,23 +0,0 @@
-function [M] = getTransformationMatrixAcc(Opm, Osm)
-% getTransformationMatrixAcc -
-%
-% Syntax: [M] = getTransformationMatrixAcc(Opm, Osm)
-%
-% Inputs:
-%    - Opm - Nx3 (N = number of accelerometer measurements) X,Y,Z position of accelerometers
-%    - Opm - Nx3 (N = number of accelerometer measurements) Unit vectors representing the accelerometer orientation
-%
-% Outputs:
-%    - M - Transformation Matrix
-
-M = zeros(length(Opm), 6);
-
-for i = 1:length(Opm)
-    Ri = [0,         Opm(3,i), -Opm(2,i);
-         -Opm(3,i),  0,         Opm(1,i);
-          Opm(2,i), -Opm(1,i),  0];
-    M(i, 1:3) = Osm(:,i)';
-    M(i, 4:6) = Osm(:,i)'*Ri;
-end
-
-end
diff --git a/matlab/src/initializeAmplifiedStrutDynamics.m b/matlab/src/initializeAmplifiedStrutDynamics.m
deleted file mode 100644
index a22bdb0..0000000
--- a/matlab/src/initializeAmplifiedStrutDynamics.m
+++ /dev/null
@@ -1,43 +0,0 @@
-function [stewart] = initializeAmplifiedStrutDynamics(stewart, args)
-% initializeAmplifiedStrutDynamics - Add Stiffness and Damping properties of each strut
-%
-% Syntax: [stewart] = initializeAmplifiedStrutDynamics(args)
-%
-% Inputs:
-%    - args - Structure with the following fields:
-%        - Ka [6x1] - Vertical stiffness contribution of the piezoelectric stack [N/m]
-%        - Ca [6x1] - Vertical damping contribution of the piezoelectric stack [N/(m/s)]
-%        - Kr [6x1] - Vertical (residual) stiffness when the piezoelectric stack is removed [N/m]
-%        - Cr [6x1] - Vertical (residual) damping when the piezoelectric stack is removed [N/(m/s)]
-%
-% Outputs:
-%    - stewart - updated Stewart structure with the added fields:
-%      - actuators.type = 2
-%      - actuators.K   [6x1] - Total Stiffness of each strut [N/m]
-%      - actuators.C   [6x1] - Total Damping of each strut [N/(m/s)]
-%      - actuators.Ka [6x1] - Vertical stiffness contribution of the piezoelectric stack [N/m]
-%      - actuators.Ca [6x1] - Vertical damping contribution of the piezoelectric stack [N/(m/s)]
-%      - actuators.Kr [6x1] - Vertical stiffness when the piezoelectric stack is removed [N/m]
-%      - actuators.Cr [6x1] - Vertical damping when the piezoelectric stack is removed [N/(m/s)]
-
-arguments
-    stewart
-    args.Kr (6,1) double {mustBeNumeric, mustBeNonnegative} = 5e6*ones(6,1)
-    args.Cr (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
-    args.Ka (6,1) double {mustBeNumeric, mustBeNonnegative} = 15e6*ones(6,1)
-    args.Ca (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
-end
-
-K = args.Ka + args.Kr;
-C = args.Ca + args.Cr;
-
-stewart.actuators.type = 2;
-
-stewart.actuators.Ka = args.Ka;
-stewart.actuators.Ca = args.Ca;
-
-stewart.actuators.Kr = args.Kr;
-stewart.actuators.Cr = args.Cr;
-
-stewart.actuators.K = K;
-stewart.actuators.C = K;
diff --git a/matlab/src/initializeAxisc.m b/matlab/src/initializeAxisc.m
deleted file mode 100644
index 9358a59..0000000
--- a/matlab/src/initializeAxisc.m
+++ /dev/null
@@ -1,34 +0,0 @@
-function [axisc] = initializeAxisc(args)
-
-arguments
-  args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
-end
-
-axisc = struct();
-
-switch args.type
-  case 'none'
-    axisc.type = 0;
-  case 'rigid'
-    axisc.type = 1;
-  case 'flexible'
-    axisc.type = 2;
-end
-
-% Structure
-axisc.structure.density = 3400; % [kg/m3]
-axisc.structure.STEP    = './STEPS/axisc/axisc_structure.STEP';
-
-% Wheel
-axisc.wheel.density     = 2700; % [kg/m3]
-axisc.wheel.STEP        = './STEPS/axisc/axisc_wheel.STEP';
-
-% Mass
-axisc.mass.density      = 7800; % [kg/m3]
-axisc.mass.STEP         = './STEPS/axisc/axisc_mass.STEP';
-
-% Gear
-axisc.gear.density      = 7800; % [kg/m3]
-axisc.gear.STEP         = './STEPS/axisc/axisc_gear.STEP';
-
-save('./mat/stages.mat', 'axisc', '-append');
diff --git a/matlab/src/initializeCedratPiezo.m b/matlab/src/initializeCedratPiezo.m
deleted file mode 100644
index a91dc8e..0000000
--- a/matlab/src/initializeCedratPiezo.m
+++ /dev/null
@@ -1,17 +0,0 @@
-function [cedrat] = initializeCedratPiezo()
-  %% Stewart Object
-  cedrat = struct();
-  cedrat.k = 10e7; % Linear Stiffness of each "blade" [N/m]
-  cedrat.ka = 10e7; % Linear Stiffness of the stack [N/m]
-
-  cedrat.c = 0.1*sqrt(1*cedrat.k); % [N/(m/s)]
-  cedrat.ca = 0.1*sqrt(1*cedrat.ka); % [N/(m/s)]
-
-  cedrat.L = 80; % Total Width of the Actuator[mm]
-  cedrat.H = 45; % Total Height of the Actuator [mm]
-  cedrat.L2 = sqrt((cedrat.L/2)^2 + (cedrat.H/2)^2); % Length of the elipsoidal sections [mm]
-  cedrat.alpha = 180/pi*atan2(cedrat.L/2, cedrat.H/2); % [deg]
-
-  %% Save
-  save('./mat/stages.mat', 'cedrat', '-append');
-end
diff --git a/matlab/src/initializeController.m b/matlab/src/initializeController.m
deleted file mode 100644
index 76571d8..0000000
--- a/matlab/src/initializeController.m
+++ /dev/null
@@ -1,39 +0,0 @@
-function [] = initializeController(args)
-
-arguments
-  args.type char {mustBeMember(args.type,{'open-loop', 'iff', 'dvf', 'hac-dvf', 'ref-track-L', 'ref-track-iff-L', 'cascade-hac-lac', 'hac-iff', 'stabilizing'})} = 'open-loop'
-end
-
-controller = struct();
-
-switch args.type
-  case 'open-loop'
-    controller.type = 1;
-    controller.name = 'Open-Loop';
-  case 'dvf'
-    controller.type = 2;
-    controller.name = 'Decentralized Direct Velocity Feedback';
-  case 'iff'
-    controller.type = 3;
-    controller.name = 'Decentralized Integral Force Feedback';
-  case 'hac-dvf'
-    controller.type = 4;
-    controller.name = 'HAC-DVF';
-  case 'ref-track-L'
-    controller.type = 5;
-    controller.name = 'Reference Tracking in the frame of the legs';
-  case 'ref-track-iff-L'
-    controller.type = 6;
-    controller.name = 'Reference Tracking in the frame of the legs + IFF';
-  case 'cascade-hac-lac'
-    controller.type = 7;
-    controller.name = 'Cascade Control + HAC-LAC';
-  case 'hac-iff'
-    controller.type = 8;
-    controller.name = 'HAC-IFF';
-  case 'stabilizing'
-    controller.type = 9;
-    controller.name = 'Stabilizing Controller';
-end
-
-save('./mat/controller.mat', 'controller');
diff --git a/matlab/src/initializeCylindricalPlatforms.m b/matlab/src/initializeCylindricalPlatforms.m
deleted file mode 100644
index 0b86fc1..0000000
--- a/matlab/src/initializeCylindricalPlatforms.m
+++ /dev/null
@@ -1,59 +0,0 @@
-function [stewart] = initializeCylindricalPlatforms(stewart, args)
-% initializeCylindricalPlatforms - Initialize the geometry of the Fixed and Mobile Platforms
-%
-% Syntax: [stewart] = initializeCylindricalPlatforms(args)
-%
-% Inputs:
-%    - args - Structure with the following fields:
-%        - Fpm [1x1] - Fixed Platform Mass [kg]
-%        - Fph [1x1] - Fixed Platform Height [m]
-%        - Fpr [1x1] - Fixed Platform Radius [m]
-%        - Mpm [1x1] - Mobile Platform Mass [kg]
-%        - Mph [1x1] - Mobile Platform Height [m]
-%        - Mpr [1x1] - Mobile Platform Radius [m]
-%
-% Outputs:
-%    - stewart - updated Stewart structure with the added fields:
-%      - platform_F [struct] - structure with the following fields:
-%        - type = 1
-%        - M [1x1] - Fixed Platform Mass [kg]
-%        - I [3x3] - Fixed Platform Inertia matrix [kg*m^2]
-%        - H [1x1] - Fixed Platform Height [m]
-%        - R [1x1] - Fixed Platform Radius [m]
-%      - platform_M [struct] - structure with the following fields:
-%        - M [1x1] - Mobile Platform Mass [kg]
-%        - I [3x3] - Mobile Platform Inertia matrix [kg*m^2]
-%        - H [1x1] - Mobile Platform Height [m]
-%        - R [1x1] - Mobile Platform Radius [m]
-
-arguments
-    stewart
-    args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
-    args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
-    args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 125e-3
-    args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1
-    args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
-    args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
-end
-
-I_F = diag([1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ...
-            1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ...
-            1/2 *args.Fpm * args.Fpr^2]);
-
-I_M = diag([1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
-            1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
-            1/2 *args.Mpm * args.Mpr^2]);
-
-stewart.platform_F.type = 1;
-
-stewart.platform_F.I = I_F;
-stewart.platform_F.M = args.Fpm;
-stewart.platform_F.R = args.Fpr;
-stewart.platform_F.H = args.Fph;
-
-stewart.platform_M.type = 1;
-
-stewart.platform_M.I = I_M;
-stewart.platform_M.M = args.Mpm;
-stewart.platform_M.R = args.Mpr;
-stewart.platform_M.H = args.Mph;
diff --git a/matlab/src/initializeCylindricalStruts.m b/matlab/src/initializeCylindricalStruts.m
deleted file mode 100644
index 2bb7257..0000000
--- a/matlab/src/initializeCylindricalStruts.m
+++ /dev/null
@@ -1,71 +0,0 @@
-function [stewart] = initializeCylindricalStruts(stewart, args)
-% initializeCylindricalStruts - Define the mass and moment of inertia of cylindrical struts
-%
-% Syntax: [stewart] = initializeCylindricalStruts(args)
-%
-% Inputs:
-%    - args - Structure with the following fields:
-%        - Fsm [1x1] - Mass of the Fixed part of the struts [kg]
-%        - Fsh [1x1] - Height of cylinder for the Fixed part of the struts [m]
-%        - Fsr [1x1] - Radius of cylinder for the Fixed part of the struts [m]
-%        - Msm [1x1] - Mass of the Mobile part of the struts [kg]
-%        - Msh [1x1] - Height of cylinder for the Mobile part of the struts [m]
-%        - Msr [1x1] - Radius of cylinder for the Mobile part of the struts [m]
-%
-% Outputs:
-%    - stewart - updated Stewart structure with the added fields:
-%      - struts_F [struct] - structure with the following fields:
-%        - M [6x1]   - Mass of the Fixed part of the struts [kg]
-%        - I [3x3x6] - Moment of Inertia for the Fixed part of the struts [kg*m^2]
-%        - H [6x1]   - Height of cylinder for the Fixed part of the struts [m]
-%        - R [6x1]   - Radius of cylinder for the Fixed part of the struts [m]
-%      - struts_M [struct] - structure with the following fields:
-%        - M [6x1]   - Mass of the Mobile part of the struts [kg]
-%        - I [3x3x6] - Moment of Inertia for the Mobile part of the struts [kg*m^2]
-%        - H [6x1]   - Height of cylinder for the Mobile part of the struts [m]
-%        - R [6x1]   - Radius of cylinder for the Mobile part of the struts [m]
-
-arguments
-    stewart
-    args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
-    args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
-    args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
-    args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
-    args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
-    args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
-end
-
-Fsm = ones(6,1).*args.Fsm;
-Fsh = ones(6,1).*args.Fsh;
-Fsr = ones(6,1).*args.Fsr;
-
-Msm = ones(6,1).*args.Msm;
-Msh = ones(6,1).*args.Msh;
-Msr = ones(6,1).*args.Msr;
-
-I_F = zeros(3, 3, 6); % Inertia of the "fixed" part of the strut
-I_M = zeros(3, 3, 6); % Inertia of the "mobile" part of the strut
-
-for i = 1:6
-  I_F(:,:,i) = diag([1/12 * Fsm(i) * (3*Fsr(i)^2 + Fsh(i)^2), ...
-                     1/12 * Fsm(i) * (3*Fsr(i)^2 + Fsh(i)^2), ...
-                     1/2  * Fsm(i) * Fsr(i)^2]);
-
-  I_M(:,:,i) = diag([1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ...
-                     1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ...
-                     1/2  * Msm(i) * Msr(i)^2]);
-end
-
-stewart.struts_M.type = 1;
-
-stewart.struts_M.I = I_M;
-stewart.struts_M.M = Msm;
-stewart.struts_M.R = Msr;
-stewart.struts_M.H = Msh;
-
-stewart.struts_F.type = 1;
-
-stewart.struts_F.I = I_F;
-stewart.struts_F.M = Fsm;
-stewart.struts_F.R = Fsr;
-stewart.struts_F.H = Fsh;
diff --git a/matlab/src/initializeDisturbances.m b/matlab/src/initializeDisturbances.m
deleted file mode 100644
index 70d6f87..0000000
--- a/matlab/src/initializeDisturbances.m
+++ /dev/null
@@ -1,135 +0,0 @@
-function [] = initializeDisturbances(args)
-% initializeDisturbances - Initialize the disturbances
-%
-% Syntax: [] = initializeDisturbances(args)
-%
-% Inputs:
-%    - args -
-
-arguments
-    % Global parameter to enable or disable the disturbances
-    args.enable logical {mustBeNumericOrLogical} = true
-    % Ground Motion - X direction
-    args.Dwx logical {mustBeNumericOrLogical} = true
-    % Ground Motion - Y direction
-    args.Dwy logical {mustBeNumericOrLogical} = true
-    % Ground Motion - Z direction
-    args.Dwz logical {mustBeNumericOrLogical} = true
-    % Translation Stage - X direction
-    args.Fty_x logical {mustBeNumericOrLogical} = true
-    % Translation Stage - Z direction
-    args.Fty_z logical {mustBeNumericOrLogical} = true
-    % Spindle - Z direction
-    args.Frz_z logical {mustBeNumericOrLogical} = true
-end
-
-load('./mat/dist_psd.mat', 'dist_f');
-
-dist_f.f = dist_f.f(2:end);
-dist_f.psd_gm = dist_f.psd_gm(2:end);
-dist_f.psd_ty = dist_f.psd_ty(2:end);
-dist_f.psd_rz = dist_f.psd_rz(2:end);
-
-Fs = 2*dist_f.f(end);      % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
-N  = 2*length(dist_f.f);   % Number of Samples match the one of the wanted PSD
-T0 = N/Fs;                 % Signal Duration [s]
-df = 1/T0;                 % Frequency resolution of the DFT [Hz]
-                           % Also equal to (dist_f.f(2)-dist_f.f(1))
-t = linspace(0, T0, N+1)'; % Time Vector [s]
-Ts = 1/Fs;                 % Sampling Time [s]
-
-phi = dist_f.psd_gm;
-C = zeros(N/2,1);
-for i = 1:N/2
-  C(i) = sqrt(phi(i)*df);
-end
-
-if args.Dwx && args.enable
-  rng(111);
-  theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
-  Cx = [0 ; C.*complex(cos(theta),sin(theta))];
-  Cx = [Cx; flipud(conj(Cx(2:end)))];;
-  Dwx = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m]
-else
-  Dwx = zeros(length(t), 1);
-end
-
-if args.Dwy && args.enable
-  rng(112);
-  theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
-  Cx = [0 ; C.*complex(cos(theta),sin(theta))];
-  Cx = [Cx; flipud(conj(Cx(2:end)))];;
-  Dwy = N/sqrt(2)*ifft(Cx); % Ground Motion - y direction [m]
-else
-  Dwy = zeros(length(t), 1);
-end
-
-if args.Dwy && args.enable
-  rng(113);
-  theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
-  Cx = [0 ; C.*complex(cos(theta),sin(theta))];
-  Cx = [Cx; flipud(conj(Cx(2:end)))];;
-  Dwz = N/sqrt(2)*ifft(Cx); % Ground Motion - z direction [m]
-else
-  Dwz = zeros(length(t), 1);
-end
-
-if args.Fty_x && args.enable
-  phi = dist_f.psd_ty; % TODO - we take here the vertical direction which is wrong but approximate
-  C = zeros(N/2,1);
-  for i = 1:N/2
-    C(i) = sqrt(phi(i)*df);
-  end
-  rng(121);
-  theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
-  Cx = [0 ; C.*complex(cos(theta),sin(theta))];
-  Cx = [Cx; flipud(conj(Cx(2:end)))];;
-  u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty x [N]
-  Fty_x = u;
-else
-  Fty_x = zeros(length(t), 1);
-end
-
-if args.Fty_z && args.enable
-  phi = dist_f.psd_ty;
-  C = zeros(N/2,1);
-  for i = 1:N/2
-    C(i) = sqrt(phi(i)*df);
-  end
-  rng(122);
-  theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
-  Cx = [0 ; C.*complex(cos(theta),sin(theta))];
-  Cx = [Cx; flipud(conj(Cx(2:end)))];;
-  u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty z [N]
-  Fty_z = u;
-else
-  Fty_z = zeros(length(t), 1);
-end
-
-if args.Frz_z && args.enable
-  phi = dist_f.psd_rz;
-  C = zeros(N/2,1);
-  for i = 1:N/2
-    C(i) = sqrt(phi(i)*df);
-  end
-  rng(131);
-  theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
-  Cx = [0 ; C.*complex(cos(theta),sin(theta))];
-  Cx = [Cx; flipud(conj(Cx(2:end)))];;
-  u = N/sqrt(2)*ifft(Cx); % Disturbance Force Rz z [N]
-  Frz_z = u;
-else
-  Frz_z = zeros(length(t), 1);
-end
-
-u = zeros(length(t), 6);
-Fd = u;
-
-Dwx    = Dwx   - Dwx(1);
-Dwy    = Dwy   - Dwy(1);
-Dwz    = Dwz   - Dwz(1);
-Fty_x  = Fty_x - Fty_x(1);
-Fty_z  = Fty_z - Fty_z(1);
-Frz_z  = Frz_z - Frz_z(1);
-
-save('./mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't', 'args');
diff --git a/matlab/src/initializeFramesPositions.m b/matlab/src/initializeFramesPositions.m
deleted file mode 100644
index f68b97d..0000000
--- a/matlab/src/initializeFramesPositions.m
+++ /dev/null
@@ -1,35 +0,0 @@
-function [stewart] = initializeFramesPositions(stewart, args)
-% initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M}
-%
-% Syntax: [stewart] = initializeFramesPositions(stewart, args)
-%
-% Inputs:
-%    - args - Can have the following fields:
-%        - H    [1x1] - Total Height of the Stewart Platform (height from {F} to {M}) [m]
-%        - MO_B [1x1] - Height of the frame {B} with respect to {M} [m]
-%
-% Outputs:
-%    - stewart - A structure with the following fields:
-%        - geometry.H      [1x1] - Total Height of the Stewart Platform [m]
-%        - geometry.FO_M   [3x1] - Position of {M} with respect to {F} [m]
-%        - platform_M.MO_B [3x1] - Position of {B} with respect to {M} [m]
-%        - platform_F.FO_A [3x1] - Position of {A} with respect to {F} [m]
-
-arguments
-    stewart
-    args.H    (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
-    args.MO_B (1,1) double {mustBeNumeric} = 50e-3
-end
-
-H = args.H; % Total Height of the Stewart Platform [m]
-
-FO_M = [0; 0; H]; % Position of {M} with respect to {F} [m]
-
-MO_B = [0; 0; args.MO_B]; % Position of {B} with respect to {M} [m]
-
-FO_A = MO_B + FO_M; % Position of {A} with respect to {F} [m]
-
-stewart.geometry.H      = H;
-stewart.geometry.FO_M   = FO_M;
-stewart.platform_M.MO_B = MO_B;
-stewart.platform_F.FO_A = FO_A;
diff --git a/matlab/src/initializeGranite.m b/matlab/src/initializeGranite.m
deleted file mode 100644
index 67c1e75..0000000
--- a/matlab/src/initializeGranite.m
+++ /dev/null
@@ -1,45 +0,0 @@
-function [granite] = initializeGranite(args)
-
-arguments
-  args.type          char    {mustBeMember(args.type,{'rigid', 'flexible', 'none', 'modal-analysis', 'init'})} = 'flexible'
-  args.Foffset       logical {mustBeNumericOrLogical} = false
-  args.density (1,1) double {mustBeNumeric, mustBeNonnegative} = 2800 % Density [kg/m3]
-  args.K  (3,1) double {mustBeNumeric, mustBeNonnegative} = [4e9; 3e8; 8e8] % [N/m]
-  args.C  (3,1) double {mustBeNumeric, mustBeNonnegative} = [4.0e5; 1.1e5; 9.0e5] % [N/(m/s)]
-  args.x0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the X direction [m]
-  args.y0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Y direction [m]
-  args.z0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Z direction [m]
-  args.sample_pos (1,1) double {mustBeNumeric} = 0.8 % Height of the measurment point [m]
-end
-
-granite = struct();
-
-switch args.type
-  case 'none'
-    granite.type = 0;
-  case 'rigid'
-    granite.type = 1;
-  case 'flexible'
-    granite.type = 2;
-  case 'modal-analysis'
-    granite.type = 3;
-  case 'init'
-    granite.type = 4;
-end
-
-granite.density = args.density; % [kg/m3]
-granite.STEP    = './STEPS/granite/granite.STEP';
-
-granite.sample_pos = args.sample_pos; % [m]
-
-granite.K = args.K; % [N/m]
-granite.C = args.C; % [N/(m/s)]
-
-if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
-  load('mat/Foffset.mat', 'Fgm');
-  granite.Deq = -Fgm'./granite.K;
-else
-  granite.Deq = zeros(6,1);
-end
-
-save('./mat/stages.mat', 'granite', '-append');
diff --git a/matlab/src/initializeGround.m b/matlab/src/initializeGround.m
deleted file mode 100644
index d9c4f2a..0000000
--- a/matlab/src/initializeGround.m
+++ /dev/null
@@ -1,22 +0,0 @@
-function [ground] = initializeGround(args)
-
-arguments
-  args.type char {mustBeMember(args.type,{'none', 'rigid'})} = 'rigid'
-  args.rot_point (3,1) double {mustBeNumeric} = zeros(3,1) % Rotation point for the ground motion [m]
-end
-
-ground = struct();
-
-switch args.type
-  case 'none'
-    ground.type = 0;
-  case 'rigid'
-    ground.type = 1;
-end
-
-ground.shape   = [2, 2, 0.5]; % [m]
-ground.density = 2800;        % [kg/m3]
-
-ground.rot_point = args.rot_point;
-
-save('./mat/stages.mat', 'ground', '-append');
diff --git a/matlab/src/initializeInertialSensor.m b/matlab/src/initializeInertialSensor.m
deleted file mode 100644
index a9509e4..0000000
--- a/matlab/src/initializeInertialSensor.m
+++ /dev/null
@@ -1,48 +0,0 @@
-function [stewart] = initializeInertialSensor(stewart, args)
-% initializeInertialSensor - Initialize the inertial sensor in each strut
-%
-% Syntax: [stewart] = initializeInertialSensor(args)
-%
-% Inputs:
-%    - args - Structure with the following fields:
-%        - type       - 'geophone', 'accelerometer', 'none'
-%        - mass [1x1] - Weight of the inertial mass [kg]
-%        - freq [1x1] - Cutoff frequency [Hz]
-%
-% Outputs:
-%    - stewart - updated Stewart structure with the added fields:
-%      - stewart.sensors.inertial
-%        - type    - 1 (geophone), 2 (accelerometer), 3 (none)
-%        - K [1x1] - Stiffness [N/m]
-%        - C [1x1] - Damping [N/(m/s)]
-%        - M [1x1] - Inertial Mass [kg]
-%        - G [1x1] - Gain
-
-arguments
-    stewart
-    args.type       char   {mustBeMember(args.type,{'geophone', 'accelerometer', 'none'})} = 'none'
-    args.mass (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e-2
-    args.freq (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e3
-end
-
-sensor = struct();
-
-switch args.type
-  case 'geophone'
-    sensor.type = 1;
-
-    sensor.M = args.mass;
-    sensor.K = sensor.M * (2*pi*args.freq)^2;
-    sensor.C = 2*sqrt(sensor.M * sensor.K);
-  case 'accelerometer'
-    sensor.type = 2;
-
-    sensor.M = args.mass;
-    sensor.K = sensor.M * (2*pi*args.freq)^2;
-    sensor.C = 2*sqrt(sensor.M * sensor.K);
-    sensor.G = -sensor.K/sensor.M;
-  case 'none'
-    sensor.type = 3;
-end
-
-stewart.sensors.inertial = sensor;
diff --git a/matlab/src/initializeJointDynamics.m b/matlab/src/initializeJointDynamics.m
deleted file mode 100644
index cc78b3a..0000000
--- a/matlab/src/initializeJointDynamics.m
+++ /dev/null
@@ -1,131 +0,0 @@
-function [stewart] = initializeJointDynamics(stewart, args)
-% initializeJointDynamics - Add Stiffness and Damping properties for the spherical joints
-%
-% Syntax: [stewart] = initializeJointDynamics(args)
-%
-% Inputs:
-%    - args - Structure with the following fields:
-%        - type_F - 'universal', 'spherical', 'universal_p', 'spherical_p'
-%        - type_M - 'universal', 'spherical', 'universal_p', 'spherical_p'
-%        - Kf_M [6x1] - Bending (Rx, Ry) Stiffness for each top joints [(N.m)/rad]
-%        - Kt_M [6x1] - Torsion (Rz) Stiffness for each top joints [(N.m)/rad]
-%        - Cf_M [6x1] - Bending (Rx, Ry) Damping of each top joint [(N.m)/(rad/s)]
-%        - Ct_M [6x1] - Torsion (Rz) Damping of each top joint [(N.m)/(rad/s)]
-%        - Kf_F [6x1] - Bending (Rx, Ry) Stiffness for each bottom joints [(N.m)/rad]
-%        - Kt_F [6x1] - Torsion (Rz) Stiffness for each bottom joints [(N.m)/rad]
-%        - Cf_F [6x1] - Bending (Rx, Ry) Damping of each bottom joint [(N.m)/(rad/s)]
-%        - Cf_F [6x1] - Torsion (Rz) Damping of each bottom joint [(N.m)/(rad/s)]
-%
-% Outputs:
-%    - stewart - updated Stewart structure with the added fields:
-%      - stewart.joints_F and stewart.joints_M:
-%        - type - 1 (universal), 2 (spherical), 3 (universal perfect), 4 (spherical perfect)
-%        - Kx, Ky, Kz [6x1] - Translation (Tx, Ty, Tz) Stiffness [N/m]
-%        - Kf [6x1] - Flexion (Rx, Ry) Stiffness [(N.m)/rad]
-%        - Kt [6x1] - Torsion (Rz) Stiffness [(N.m)/rad]
-%        - Cx, Cy, Cz [6x1] - Translation (Rx, Ry) Damping [N/(m/s)]
-%        - Cf [6x1] - Flexion (Rx, Ry) Damping [(N.m)/(rad/s)]
-%        - Cb [6x1] - Torsion (Rz) Damping [(N.m)/(rad/s)]
-
-arguments
-    stewart
-    args.type_F     char   {mustBeMember(args.type_F,{'universal', 'spherical', 'universal_p', 'spherical_p', 'universal_3dof', 'spherical_3dof', 'flexible'})} = 'universal'
-    args.type_M     char   {mustBeMember(args.type_M,{'universal', 'spherical', 'universal_p', 'spherical_p', 'universal_3dof', 'spherical_3dof', 'flexible'})} = 'spherical'
-    args.Kf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 33*ones(6,1)
-    args.Cf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
-    args.Kt_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 236*ones(6,1)
-    args.Ct_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1)
-    args.Kf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 33*ones(6,1)
-    args.Cf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
-    args.Kt_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 236*ones(6,1)
-    args.Ct_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1)
-    args.Ka_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.2e8*ones(6,1)
-    args.Ca_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
-    args.Kr_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.1e7*ones(6,1)
-    args.Cr_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
-    args.Ka_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.2e8*ones(6,1)
-    args.Ca_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
-    args.Kr_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.1e7*ones(6,1)
-    args.Cr_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
-    args.K_M        double {mustBeNumeric} = zeros(6,6)
-    args.M_M        double {mustBeNumeric} = zeros(6,6)
-    args.n_xyz_M    double {mustBeNumeric} = zeros(2,3)
-    args.xi_M       double {mustBeNumeric} = 0.1
-    args.step_file_M char {} = ''
-    args.K_F        double {mustBeNumeric} = zeros(6,6)
-    args.M_F        double {mustBeNumeric} = zeros(6,6)
-    args.n_xyz_F    double {mustBeNumeric} = zeros(2,3)
-    args.xi_F       double {mustBeNumeric} = 0.1
-    args.step_file_F char {} = ''
-end
-
-switch args.type_F
-  case 'universal'
-    stewart.joints_F.type = 1;
-  case 'spherical'
-    stewart.joints_F.type = 2;
-  case 'universal_p'
-    stewart.joints_F.type = 3;
-  case 'spherical_p'
-    stewart.joints_F.type = 4;
-  case 'flexible'
-    stewart.joints_F.type = 5;
-  case 'universal_3dof'
-    stewart.joints_F.type = 6;
-  case 'spherical_3dof'
-    stewart.joints_F.type = 7;
-end
-
-switch args.type_M
-  case 'universal'
-    stewart.joints_M.type = 1;
-  case 'spherical'
-    stewart.joints_M.type = 2;
-  case 'universal_p'
-    stewart.joints_M.type = 3;
-  case 'spherical_p'
-    stewart.joints_M.type = 4;
-  case 'flexible'
-    stewart.joints_M.type = 5;
-  case 'universal_3dof'
-    stewart.joints_M.type = 6;
-  case 'spherical_3dof'
-    stewart.joints_M.type = 7;
-end
-
-stewart.joints_M.Ka = args.Ka_M;
-stewart.joints_M.Kr = args.Kr_M;
-
-stewart.joints_F.Ka = args.Ka_F;
-stewart.joints_F.Kr = args.Kr_F;
-
-stewart.joints_M.Ca = args.Ca_M;
-stewart.joints_M.Cr = args.Cr_M;
-
-stewart.joints_F.Ca = args.Ca_F;
-stewart.joints_F.Cr = args.Cr_F;
-
-stewart.joints_M.Kf = args.Kf_M;
-stewart.joints_M.Kt = args.Kt_M;
-
-stewart.joints_F.Kf = args.Kf_F;
-stewart.joints_F.Kt = args.Kt_F;
-
-stewart.joints_M.Cf = args.Cf_M;
-stewart.joints_M.Ct = args.Ct_M;
-
-stewart.joints_F.Cf = args.Cf_F;
-stewart.joints_F.Ct = args.Ct_F;
-
-stewart.joints_F.M = args.M_F;
-stewart.joints_F.K = args.K_F;
-stewart.joints_F.n_xyz = args.n_xyz_F;
-stewart.joints_F.xi = args.xi_F;
-stewart.joints_F.xi = args.xi_F;
-stewart.joints_F.step_file = args.step_file_F;
-
-stewart.joints_M.M = args.M_M;
-stewart.joints_M.K = args.K_M;
-stewart.joints_M.n_xyz = args.n_xyz_M;
-stewart.joints_M.xi = args.xi_M;
-stewart.joints_M.step_file = args.step_file_M;
diff --git a/matlab/src/initializeLoggingConfiguration.m b/matlab/src/initializeLoggingConfiguration.m
deleted file mode 100644
index 5d46e58..0000000
--- a/matlab/src/initializeLoggingConfiguration.m
+++ /dev/null
@@ -1,21 +0,0 @@
-function [] = initializeLoggingConfiguration(args)
-
-arguments
-  args.log      char   {mustBeMember(args.log,{'none', 'all', 'forces'})} = 'none'
-  args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
-end
-
-conf_log = struct();
-
-switch args.log
-  case 'none'
-    conf_log.type = 0;
-  case 'all'
-    conf_log.type = 1;
-  case 'forces'
-    conf_log.type = 2;
-end
-
-conf_log.Ts = args.Ts;
-
-save('./mat/conf_log.mat', 'conf_log');
diff --git a/matlab/src/initializeMicroHexapod.m b/matlab/src/initializeMicroHexapod.m
deleted file mode 100644
index 643d81e..0000000
--- a/matlab/src/initializeMicroHexapod.m
+++ /dev/null
@@ -1,110 +0,0 @@
-function [micro_hexapod] = initializeMicroHexapod(args)
-
-arguments
-    args.type      char   {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init', 'compliance'})} = 'flexible'
-    % initializeFramesPositions
-    args.H    (1,1) double {mustBeNumeric, mustBePositive} = 350e-3
-    args.MO_B (1,1) double {mustBeNumeric} = 270e-3
-    % generateGeneralConfiguration
-    args.FH  (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
-    args.FR  (1,1) double {mustBeNumeric, mustBePositive} = 175.5e-3
-    args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180)
-    args.MH  (1,1) double {mustBeNumeric, mustBePositive} = 45e-3
-    args.MR  (1,1) double {mustBeNumeric, mustBePositive} = 118e-3
-    args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180)
-    % initializeStrutDynamics
-    args.Ki (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e7*ones(6,1)
-    args.Ci (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.4e3*ones(6,1)
-    % initializeCylindricalPlatforms
-    args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 10
-    args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3
-    args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 207.5e-3
-    args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 10
-    args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3
-    args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3
-    % initializeCylindricalStruts
-    args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 1
-    args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
-    args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3
-    args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 1
-    args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
-    args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3
-    % inverseKinematics
-    args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
-    args.ARB (3,3) double {mustBeNumeric} = eye(3)
-    % Force that stiffness of each joint should apply at t=0
-    args.Foffset      logical {mustBeNumericOrLogical} = false
-end
-
-stewart = initializeStewartPlatform();
-
-stewart = initializeFramesPositions(stewart, ...
-                                    'H', args.H, ...
-                                    'MO_B', args.MO_B);
-
-stewart = generateGeneralConfiguration(stewart, ...
-                                       'FH', args.FH, ...
-                                       'FR', args.FR, ...
-                                       'FTh', args.FTh, ...
-                                       'MH', args.MH, ...
-                                       'MR', args.MR, ...
-                                       'MTh', args.MTh);
-
-stewart = computeJointsPose(stewart);
-
-stewart = initializeStrutDynamics(stewart, ...
-                                  'K', args.Ki, ...
-                                  'C', args.Ci);
-
-stewart = initializeJointDynamics(stewart, ...
-                                  'type_F', 'universal_p', ...
-                                  'type_M', 'spherical_p');
-
-stewart = initializeCylindricalPlatforms(stewart, ...
-                                         'Fpm', args.Fpm, ...
-                                         'Fph', args.Fph, ...
-                                         'Fpr', args.Fpr, ...
-                                         'Mpm', args.Mpm, ...
-                                         'Mph', args.Mph, ...
-                                         'Mpr', args.Mpr);
-
-stewart = initializeCylindricalStruts(stewart, ...
-                                      'Fsm', args.Fsm, ...
-                                      'Fsh', args.Fsh, ...
-                                      'Fsr', args.Fsr, ...
-                                      'Msm', args.Msm, ...
-                                      'Msh', args.Msh, ...
-                                      'Msr', args.Msr);
-
-stewart = computeJacobian(stewart);
-
-stewart = initializeStewartPose(stewart, ...
-                              'AP', args.AP, ...
-                              'ARB', args.ARB);
-
-stewart = initializeInertialSensor(stewart, 'type', 'none');
-
-if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
-  load('mat/Foffset.mat', 'Fhm');
-  stewart.actuators.dLeq = -Fhm'./args.Ki;
-else
-  stewart.actuators.dLeq = zeros(6,1);
-end
-
-switch args.type
-  case 'none'
-    stewart.type = 0;
-  case 'rigid'
-    stewart.type = 1;
-  case 'flexible'
-    stewart.type = 2;
-  case 'modal-analysis'
-    stewart.type = 3;
-  case 'init'
-    stewart.type = 4;
-  case 'compliance'
-    stewart.type = 5;
-end
-
-micro_hexapod = stewart;
-save('./mat/stages.mat', 'micro_hexapod', '-append');
diff --git a/matlab/src/initializeMicroHexapodOld.m b/matlab/src/initializeMicroHexapodOld.m
deleted file mode 100644
index 41da308..0000000
--- a/matlab/src/initializeMicroHexapodOld.m
+++ /dev/null
@@ -1,196 +0,0 @@
-function [micro_hexapod] = initializeMicroHexapod(args)
-    arguments
-        args.rigid logical {mustBeNumericOrLogical} = false
-        args.AP    (3,1) double {mustBeNumeric} = zeros(3,1)
-        args.ARB   (3,3) double {mustBeNumeric} = eye(3)
-    end
-
-    %% Stewart Object
-    micro_hexapod = struct();
-    micro_hexapod.h        = 350; % Total height of the platform [mm]
-    micro_hexapod.jacobian = 270; % Distance from the top of the mobile platform to the Jacobian point [mm]
-
-    %% Bottom Plate - Mechanical Design
-    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 - Mechanical Design
-    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];
-
-    %% Struts
-    Leg = struct();
-
-    Leg.stroke     = 10e-3; % Maximum Stroke of each leg [m]
-    if args.rigid
-      Leg.k.ax     = 1e12; % Stiffness of each leg [N/m]
-    else
-      Leg.k.ax     = 2e7; % Stiffness of each leg [N/m]
-    end
-    Leg.ksi.ax     = 0.1;   % Modal damping ksi = 1/2*c/sqrt(km) []
-    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];
-
-    %%
-    micro_hexapod.BP = BP;
-    micro_hexapod.TP = TP;
-    micro_hexapod.Leg = Leg;
-    micro_hexapod.SP = SP;
-
-    %%
-    micro_hexapod = initializeParameters(micro_hexapod);
-
-    %% Setup equilibrium position of each leg
-    micro_hexapod.L0 = inverseKinematicsHexapod(micro_hexapod, args.AP, args.ARB);
-
-    %% Save
-    save('./mat/stages.mat', 'micro_hexapod', '-append');
-
-    %%
-    function [element] = updateDamping(element)
-      field = fieldnames(element.k);
-      for i = 1:length(field)
-        element.c.(field{i}) = 2*element.ksi.(field{i})*sqrt(element.k.(field{i})*element.m);
-      end
-    end
-
-    %%
-    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
-
-    %%
-    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/matlab/src/initializeMirror.m b/matlab/src/initializeMirror.m
deleted file mode 100644
index ca98db1..0000000
--- a/matlab/src/initializeMirror.m
+++ /dev/null
@@ -1,76 +0,0 @@
-function [] = initializeMirror(args)
-
-arguments
-    args.type        char   {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'rigid'
-    args.shape       char   {mustBeMember(args.shape,{'spherical', 'conical'})} = 'spherical'
-    args.angle (1,1) double {mustBeNumeric, mustBePositive} = 45 % [deg]
-    args.mass  (1,1) double {mustBeNumeric, mustBePositive} = 10 % [kg]
-    args.freq  (6,1) double {mustBeNumeric, mustBeNonnegative} = 200*ones(6,1) % [Hz]
-end
-
-mirror = struct();
-
-switch args.type
-  case 'none'
-    mirror.type = 0;
-  case 'rigid'
-    mirror.type = 1;
-  case 'flexible'
-    mirror.type = 2;
-end
-
-mirror.mass = args.mass;
-mirror.freq = args.freq;
-
-mirror.K = zeros(6,1);
-mirror.K(1:3) = mirror.mass * (2*pi*mirror.freq(1:3)).^2;
-
-mirror.C = zeros(6,1);
-mirror.C(1:3) = 0.2 * sqrt(mirror.K(1:3).*mirror.mass);
-
-mirror.Deq = zeros(6,1);
-
-mirror.h = 0.05; % Height of the mirror [m]
-
-mirror.thickness = 0.02; % Thickness of the plate supporting the sample [m]
-
-mirror.hole_rad = 0.125; % radius of the hole in the mirror [m]
-
-mirror.support_rad = 0.1; % radius of the support plate [m]
-
-% point of interest offset in z (above the top surfave) [m]
-switch args.type
-  case 'none'
-    mirror.jacobian = 0.205;
-  case 'rigid'
-    mirror.jacobian = 0.205 - mirror.h;
-  case 'flexible'
-    mirror.jacobian = 0.205 - mirror.h;
-end
-
-mirror.rad = 0.180; % radius of the mirror (at the bottom surface) [m]
-
-mirror.cone_length = mirror.rad*tand(args.angle)+mirror.h+mirror.jacobian; % Distance from Apex point of the cone to jacobian point
-
-mirror.shape = [...
-    mirror.support_rad+5e-3 mirror.h-mirror.thickness
-    mirror.hole_rad mirror.h-mirror.thickness; ...
-    mirror.hole_rad 0; ...
-    mirror.rad 0 ...
-];
-
-if strcmp(args.shape, 'spherical')
-    mirror.sphere_radius = sqrt((mirror.jacobian+mirror.h)^2+mirror.rad^2); % Radius of the sphere [mm]
-
-    for z = linspace(0, mirror.h, 101)
-        mirror.shape = [mirror.shape; sqrt(mirror.sphere_radius^2-(z-mirror.jacobian-mirror.h)^2) z];
-    end
-elseif strcmp(args.shape, 'conical')
-    mirror.shape = [mirror.shape; mirror.rad+mirror.h/tand(args.angle) mirror.h];
-else
-    error('Shape should be either conical or spherical');
-end
-
-mirror.shape = [mirror.shape; mirror.support_rad+5e-3 mirror.h];
-
-save('./mat/stages.mat', 'mirror', '-append');
diff --git a/matlab/src/initializeNanoHexapod.m b/matlab/src/initializeNanoHexapod.m
index 4a97fe4..71717b0 100644
--- a/matlab/src/initializeNanoHexapod.m
+++ b/matlab/src/initializeNanoHexapod.m
@@ -1,142 +1,290 @@
 function [nano_hexapod] = initializeNanoHexapod(args)
 
 arguments
-    args.type      char   {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'init'})} = 'flexible'
-    % initializeFramesPositions
-    args.H    (1,1) double {mustBeNumeric, mustBePositive} = 95e-3
-    args.MO_B (1,1) double {mustBeNumeric} = 170e-3
-    % generateGeneralConfiguration
-    args.FH  (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
-    args.FR  (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
-    args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180)
-    args.MH  (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
-    args.MR  (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
-    args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180)
-    % initializeStrutDynamics
-    args.actuator  char   {mustBeMember(args.actuator,{'piezo', 'lorentz', 'amplified'})} = 'piezo'
-    args.k1  (1,1) double {mustBeNumeric} = 1e6
-    args.ke  (1,1) double {mustBeNumeric} = 5e6
-    args.ka  (1,1) double {mustBeNumeric} = 60e6
-    args.c1  (1,1) double {mustBeNumeric} = 10
-    args.F_gain  (1,1) double {mustBeNumeric} = 1
-    args.k   (1,1) double {mustBeNumeric} = -1
-    args.c   (1,1) double {mustBeNumeric} = -1
-    % initializeJointDynamics
-    args.type_F     char   {mustBeMember(args.type_F,{'universal', 'spherical', 'universal_p', 'spherical_p', 'universal_3dof', 'spherical_3dof', 'flexible'})} = 'universal'
-    args.type_M     char   {mustBeMember(args.type_M,{'universal', 'spherical', 'universal_p', 'spherical_p', 'universal_3dof', 'spherical_3dof', 'flexible'})} = 'spherical'
-    args.Ka_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.2e8*ones(6,1)
-    args.Ca_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
-    args.Kr_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.1e7*ones(6,1)
-    args.Cr_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
-    args.Kf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 33*ones(6,1)
-    args.Cf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
-    args.Kt_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 236*ones(6,1)
-    args.Ct_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1)
-    args.Kf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 33*ones(6,1)
-    args.Cf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
-    args.Kt_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 236*ones(6,1)
-    args.Ct_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1)
-    args.Ka_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.2e8*ones(6,1)
-    args.Ca_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
-    args.Kr_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.1e7*ones(6,1)
-    args.Cr_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
-    % initializeCylindricalPlatforms
-    args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
-    args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
-    args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3
-    args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1
-    args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
-    args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 120e-3
-    % initializeCylindricalStruts
-    args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
-    args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
-    args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
-    args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
-    args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
-    args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
-    % inverseKinematics
-    args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
-    args.ARB (3,3) double {mustBeNumeric} = eye(3)
-    % Equilibrium position of each leg
-    args.dLeq (6,1) double {mustBeNumeric} = zeros(6,1)
-    % Force that stiffness of each joint should apply at t=0
-    args.Foffset      logical {mustBeNumericOrLogical} = false
+    %% Bottom Flexible Joints
+    args.flex_bot_type      char {mustBeMember(args.flex_bot_type,{'2dof', '3dof', '4dof', 'flexible'})} = '4dof'
+    args.flex_bot_kRx (6,1) double {mustBeNumeric} = ones(6,1)*5 % X bending stiffness [Nm/rad]
+    args.flex_bot_kRy (6,1) double {mustBeNumeric} = ones(6,1)*5 % Y bending stiffness [Nm/rad]
+    args.flex_bot_kRz (6,1) double {mustBeNumeric} = ones(6,1)*260 % Torsionnal stiffness [Nm/rad]
+    args.flex_bot_kz  (6,1) double {mustBeNumeric} = ones(6,1)*7e7 % Axial Stiffness [N/m]
+    args.flex_bot_cRx (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % X bending Damping [Nm/(rad/s)]
+    args.flex_bot_cRy (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Y bending Damping [Nm/(rad/s)]
+    args.flex_bot_cRz (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Torsionnal Damping [Nm/(rad/s)]
+    args.flex_bot_cz  (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Axial Damping [N/(m/s)]
+
+    %% Top Flexible Joints
+    args.flex_top_type      char {mustBeMember(args.flex_top_type,{'2dof', '3dof', '4dof', 'flexible'})} = '4dof'
+    args.flex_top_kRx (6,1) double {mustBeNumeric} = ones(6,1)*5 % X bending stiffness [Nm/rad]
+    args.flex_top_kRy (6,1) double {mustBeNumeric} = ones(6,1)*5 % Y bending stiffness [Nm/rad]
+    args.flex_top_kRz (6,1) double {mustBeNumeric} = ones(6,1)*260 % Torsionnal stiffness [Nm/rad]
+    args.flex_top_kz  (6,1) double {mustBeNumeric} = ones(6,1)*7e7 % Axial Stiffness [N/m]
+    args.flex_top_cRx (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % X bending Damping [Nm/(rad/s)]
+    args.flex_top_cRy (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Y bending Damping [Nm/(rad/s)]
+    args.flex_top_cRz (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Torsionnal Damping [Nm/(rad/s)]
+    args.flex_top_cz  (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Axial Damping [N/(m/s)]
+
+    %% Jacobian - Location of frame {A} and {B}
+    args.MO_B (1,1) double {mustBeNumeric} = 150e-3 % Height of {B} w.r.t. {M} [m]
+
+    %% Relative Motion Sensor
+    args.motion_sensor_type char {mustBeMember(args.motion_sensor_type,{'struts', 'plates'})} = 'struts'
+
+    %% Top Plate
+    args.top_plate_type char {mustBeMember(args.top_plate_type,{'rigid', 'flexible'})} = 'rigid'
+    args.top_plate_xi  (1,1) double {mustBeNumeric} = 0.01 % Damping Ratio
+
+    %% Actuators
+    args.actuator_type char {mustBeMember(args.actuator_type,{'2dof', 'flexible frame', 'flexible'})} = 'flexible'
+    args.actuator_Ga  (6,1) double {mustBeNumeric} = zeros(6,1) % Actuator gain [N/V]
+    args.actuator_Gs  (6,1) double {mustBeNumeric} = zeros(6,1) % Sensor gain [V/m]
+    % For 2DoF
+    args.actuator_k   (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*380000
+    args.actuator_ke  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*4952605
+    args.actuator_ka  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*2476302
+    args.actuator_c   (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*5
+    args.actuator_ce  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*100
+    args.actuator_ca  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*50
+    args.actuator_Leq (6,1) double {mustBeNumeric} = ones(6,1)*0.056 % [m]
+    % For Flexible Frame
+    args.actuator_ks (6,1) double {mustBeNumeric} = ones(6,1)*235e6 % Stiffness of one stack [N/m]
+    args.actuator_cs (6,1) double {mustBeNumeric} = ones(6,1)*1e1 % Stiffness of one stack [N/m]
+    % Misalignment
+    args.actuator_d_align (6,3) double {mustBeNumeric} = zeros(6,3) % [m]
+
+    args.actuator_xi  (1,1) double {mustBeNumeric} = 0.01 % Damping Ratio
+
+    %% Controller
+    args.controller_type char {mustBeMember(args.controller_type,{'none', 'iff', 'dvf', 'hac-iff-struts'})} = 'none'
 end
 
-stewart = initializeStewartPlatform();
+nano_hexapod = struct();
 
-stewart = initializeFramesPositions(stewart, 'H', args.H, 'MO_B', args.MO_B);
+nano_hexapod.flex_bot = struct();
 
-stewart = generateGeneralConfiguration(stewart, 'FH', args.FH, 'FR', args.FR, 'FTh', args.FTh, 'MH', args.MH, 'MR', args.MR, 'MTh', args.MTh);
-
-stewart = computeJointsPose(stewart);
-
-if args.k > 0 && args.c > 0
-    stewart = initializeStrutDynamics(stewart, 'type', 'classical', 'K', args.k*ones(6,1), 'C', args.c*ones(6,1));
-elseif args.k > 0
-    stewart = initializeStrutDynamics(stewart, 'type', 'classical', 'K', args.k*ones(6,1), 'C', 1.5*sqrt(args.k)*ones(6,1));
-elseif strcmp(args.actuator, 'piezo')
-    stewart = initializeStrutDynamics(stewart, 'type', 'classical', 'K', 1e7*ones(6,1), 'C', 1e2*ones(6,1));
-elseif strcmp(args.actuator, 'lorentz')
-    stewart = initializeStrutDynamics(stewart, 'type', 'classical', 'K', 1e4*ones(6,1), 'C', 1e2*ones(6,1));
-elseif strcmp(args.actuator, 'amplified')
-    stewart = initializeStrutDynamics(stewart, 'type', 'amplified', ...
-                                      'k1', args.k1*ones(6,1), ...
-                                      'c1', args.c1*ones(6,1), ...
-                                      'ka', args.ka*ones(6,1), ...
-                                      'ke', args.ke*ones(6,1), ...
-                                      'F_gain', args.F_gain*ones(6,1));
-else
-    error('args.actuator should be piezo, lorentz or amplified');
-end
-
-stewart = initializeJointDynamics(stewart, ...
-                                  'type_F', args.type_F, ...
-                                  'type_M', args.type_M, ...
-                                  'Kf_M', args.Kf_M, ...
-                                  'Cf_M', args.Cf_M, ...
-                                  'Kt_M', args.Kt_M, ...
-                                  'Ct_M', args.Ct_M, ...
-                                  'Kf_F', args.Kf_F, ...
-                                  'Cf_F', args.Cf_F, ...
-                                  'Kt_F', args.Kt_F, ...
-                                  'Ct_F', args.Ct_F, ...
-                                  'Ka_F', args.Ka_F, ...
-                                  'Ca_F', args.Ca_F, ...
-                                  'Kr_F', args.Kr_F, ...
-                                  'Cr_F', args.Cr_F, ...
-                                  'Ka_M', args.Ka_M, ...
-                                  'Ca_M', args.Ca_M, ...
-                                  'Kr_M', args.Kr_M, ...
-                                  'Cr_M', args.Cr_M);
-
-stewart = initializeCylindricalPlatforms(stewart, 'Fpm', args.Fpm, 'Fph', args.Fph, 'Fpr', args.Fpr, 'Mpm', args.Mpm, 'Mph', args.Mph, 'Mpr', args.Mpr);
-
-stewart = initializeCylindricalStruts(stewart, 'Fsm', args.Fsm, 'Fsh', args.Fsh, 'Fsr', args.Fsr, 'Msm', args.Msm, 'Msh', args.Msh, 'Msr', args.Msr);
-
-stewart = computeJacobian(stewart);
-
-stewart = initializeStewartPose(stewart, 'AP', args.AP, 'ARB', args.ARB);
-
-stewart = initializeInertialSensor(stewart, 'type', 'accelerometer');
-
-if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
-  load('mat/Foffset.mat', 'Fnm');
-  stewart.actuators.dLeq = -Fnm'./stewart.Ki;
-else
-  stewart.actuators.dLeq = args.dLeq;
-end
-
-switch args.type
-  case 'none'
-    stewart.type = 0;
-  case 'rigid'
-    stewart.type = 1;
+switch args.flex_bot_type
+  case '2dof'
+    nano_hexapod.flex_bot.type = 1;
+  case '3dof'
+    nano_hexapod.flex_bot.type = 2;
+  case '4dof'
+    nano_hexapod.flex_bot.type = 3;
   case 'flexible'
-    stewart.type = 2;
-  case 'init'
-    stewart.type = 4;
+    nano_hexapod.flex_bot.type = 4;
 end
 
-nano_hexapod = stewart;
-save('./mat/stages.mat', 'nano_hexapod', '-append');
+nano_hexapod.flex_bot.kRx = args.flex_bot_kRx; % X bending stiffness [Nm/rad]
+nano_hexapod.flex_bot.kRy = args.flex_bot_kRy; % Y bending stiffness [Nm/rad]
+nano_hexapod.flex_bot.kRz = args.flex_bot_kRz; % Torsionnal stiffness [Nm/rad]
+nano_hexapod.flex_bot.kz  = args.flex_bot_kz;  % Axial stiffness [N/m]
+
+nano_hexapod.flex_bot.cRx = args.flex_bot_cRx; % [Nm/(rad/s)]
+nano_hexapod.flex_bot.cRy = args.flex_bot_cRy; % [Nm/(rad/s)]
+nano_hexapod.flex_bot.cRz = args.flex_bot_cRz; % [Nm/(rad/s)]
+nano_hexapod.flex_bot.cz  = args.flex_bot_cz;  %[N/(m/s)]
+
+nano_hexapod.flex_top = struct();
+
+switch args.flex_top_type
+  case '2dof'
+    nano_hexapod.flex_top.type = 1;
+  case '3dof'
+    nano_hexapod.flex_top.type = 2;
+  case '4dof'
+    nano_hexapod.flex_top.type = 3;
+  case 'flexible'
+    nano_hexapod.flex_top.type = 4;
+end
+
+nano_hexapod.flex_top.kRx = args.flex_top_kRx; % X bending stiffness [Nm/rad]
+nano_hexapod.flex_top.kRy = args.flex_top_kRy; % Y bending stiffness [Nm/rad]
+nano_hexapod.flex_top.kRz = args.flex_top_kRz; % Torsionnal stiffness [Nm/rad]
+nano_hexapod.flex_top.kz  = args.flex_top_kz;  % Axial stiffness [N/m]
+
+nano_hexapod.flex_top.cRx = args.flex_top_cRx; % [Nm/(rad/s)]
+nano_hexapod.flex_top.cRy = args.flex_top_cRy; % [Nm/(rad/s)]
+nano_hexapod.flex_top.cRz = args.flex_top_cRz; % [Nm/(rad/s)]
+nano_hexapod.flex_top.cz  = args.flex_top_cz;  %[N/(m/s)]
+
+nano_hexapod.motion_sensor = struct();
+
+switch args.motion_sensor_type
+  case 'struts'
+    nano_hexapod.motion_sensor.type = 1;
+  case 'plates'
+    nano_hexapod.motion_sensor.type = 2;
+end
+
+nano_hexapod.actuator = struct();
+
+switch args.actuator_type
+  case '2dof'
+    nano_hexapod.actuator.type = 1;
+  case 'flexible frame'
+    nano_hexapod.actuator.type = 2;
+  case 'flexible'
+    nano_hexapod.actuator.type = 3;
+end
+
+%% Actuator gain [N/V]
+if all(args.actuator_Ga == 0)
+    switch args.actuator_type
+      case '2dof'
+        nano_hexapod.actuator.Ga = ones(6,1)*(-2.5796);
+      case 'flexible frame'
+        nano_hexapod.actuator.Ga = ones(6,1); % TODO
+      case 'flexible'
+        nano_hexapod.actuator.Ga = ones(6,1)*23.2;
+    end
+else
+    nano_hexapod.actuator.Ga = args.actuator_Ga; % Actuator gain [N/V]
+end
+
+%% Sensor gain [V/m]
+if all(args.actuator_Gs == 0)
+    switch args.actuator_type
+      case '2dof'
+        nano_hexapod.actuator.Gs = ones(6,1)*466664;
+      case 'flexible frame'
+        nano_hexapod.actuator.Gs = ones(6,1); % TODO
+      case 'flexible'
+        nano_hexapod.actuator.Gs = ones(6,1)*(-4898341);
+    end
+else
+    nano_hexapod.actuator.Gs = args.actuator_Gs; % Sensor gain [V/m]
+end
+
+switch args.actuator_type
+  case '2dof'
+    nano_hexapod.actuator.k  = args.actuator_k;  % [N/m]
+    nano_hexapod.actuator.ke = args.actuator_ke; % [N/m]
+    nano_hexapod.actuator.ka = args.actuator_ka; % [N/m]
+
+    nano_hexapod.actuator.c  = args.actuator_c;  % [N/(m/s)]
+    nano_hexapod.actuator.ce = args.actuator_ce; % [N/(m/s)]
+    nano_hexapod.actuator.ca = args.actuator_ca; % [N/(m/s)]
+
+    nano_hexapod.actuator.Leq = args.actuator_Leq; % [m]
+
+  case 'flexible frame'
+    nano_hexapod.actuator.K = readmatrix('APA300ML_b_mat_K.CSV'); % Stiffness Matrix
+    nano_hexapod.actuator.M = readmatrix('APA300ML_b_mat_M.CSV'); % Mass Matrix
+    nano_hexapod.actuator.P = extractNodes('APA300ML_b_out_nodes_3D.txt'); % Node coordinates [m]
+
+    nano_hexapod.actuator.ks = args.actuator_ks; % Stiffness of one stack [N/m]
+    nano_hexapod.actuator.cs = args.actuator_cs; % Damping of one stack [N/m]
+    nano_hexapod.actuator.xi = args.actuator_xi; % Damping ratio
+
+  case 'flexible'
+    nano_hexapod.actuator.K = readmatrix('full_APA300ML_K.CSV'); % Stiffness Matrix
+    nano_hexapod.actuator.M = readmatrix('full_APA300ML_M.CSV'); % Mass Matrix
+    nano_hexapod.actuator.P = extractNodes('full_APA300ML_out_nodes_3D.txt'); % Node coordiantes [m]
+
+    nano_hexapod.actuator.d_align = args.actuator_d_align; % Misalignment
+    nano_hexapod.actuator.xi = args.actuator_xi; % Damping ratio
+
+end
+
+nano_hexapod.geometry = struct();
+
+Fa = [[-86.05,  -74.78, 22.49],
+      [ 86.05,  -74.78, 22.49],
+      [ 107.79, -37.13, 22.49],
+      [ 21.74,  111.91, 22.49],
+      [-21.74,  111.91, 22.49],
+      [-107.79, -37.13, 22.49]]'*1e-3; % Ai w.r.t. {F} [m]
+
+Mb = [[-28.47, -106.25, -22.50],
+      [ 28.47, -106.25, -22.50],
+      [ 106.25, 28.47,  -22.50],
+      [ 77.78,  77.78,  -22.50],
+      [-77.78,  77.78,  -22.50],
+      [-106.25, 28.47,  -22.50]]'*1e-3; % Bi w.r.t. {M} [m]
+
+Fb = Mb + [0; 0; 95e-3]; % Bi w.r.t. {F} [m]
+
+si = Fb - Fa;
+si = si./vecnorm(si); % Normalize
+
+Fc = [[-29.362, -105.765, 52.605]
+      [ 29.362, -105.765, 52.605]
+      [ 106.276, 27.454,  52.605]
+      [ 76.914,  78.31,   52.605]
+      [-76.914,  78.31,   52.605]
+      [-106.276, 27.454,  52.605]]'*1e-3; % Meas pos w.r.t. {F}
+Mc = Fc - [0; 0; 95e-3]; % Meas pos w.r.t. {M}
+
+nano_hexapod.geometry.Fa = Fa;
+nano_hexapod.geometry.Fb = Fb;
+nano_hexapod.geometry.Fc = Fc;
+nano_hexapod.geometry.Mb = Mb;
+nano_hexapod.geometry.Mc = Mc;
+nano_hexapod.geometry.si = si;
+nano_hexapod.geometry.MO_B = args.MO_B;
+
+Bb = Mb - [0; 0; args.MO_B];
+
+nano_hexapod.geometry.J = [nano_hexapod.geometry.si', cross(Bb, nano_hexapod.geometry.si)'];
+
+switch args.motion_sensor_type
+  case 'struts'
+    nano_hexapod.geometry.Js = nano_hexapod.geometry.J;
+  case 'plates'
+    Bc = Mc - [0; 0; args.MO_B];
+    nano_hexapod.geometry.Js = [nano_hexapod.geometry.si', cross(Bc, nano_hexapod.geometry.si)'];
+end
+
+nano_hexapod.top_plate = struct();
+
+switch args.top_plate_type
+  case 'rigid'
+    nano_hexapod.top_plate.type = 1;
+  case 'flexible'
+    nano_hexapod.top_plate.type = 2;
+
+    nano_hexapod.top_plate.R_flex = ...
+       {[ 0.53191886726305  0.4795690716524  0.69790817745892
+         -0.29070157897799  0.8775041341865 -0.38141720787774
+         -0.79533320729697  0                0.60617249143351 ],
+        [ 0.53191886726305 -0.4795690716524 -0.69790817745892
+          0.29070157897799  0.8775041341865 -0.38141720787774
+          0.79533320729697  0                0.60617249143351 ],
+        [-0.01420448131633 -0.9997254079576 -0.01863709726680
+          0.60600604129104 -0.0234330681729  0.79511481512719
+         -0.79533320729697  0                0.60617249143351 ],
+        [-0.51771438594672 -0.5201563363051  0.67927108019212
+          0.31530446231304 -0.8540710660135 -0.41369760724945
+          0.79533320729697  0                0.60617249143351 ],
+        [-0.51771438594671  0.5201563363052 -0.67927108019211
+         -0.31530446231304 -0.8540710660135 -0.41369760724945
+         -0.79533320729697  0                0.60617249143351 ],
+        [-0.01420448131632  0.9997254079576  0.01863709726679
+         -0.60600604129104 -0.0234330681729  0.79511481512719
+          0.79533320729697  0                0.60617249143351 ] };
+
+
+    nano_hexapod.top_plate.R_enc = ...
+      { [-0.877504134186525 -0.479569071652412 0
+         0.479569071652412 -0.877504134186525 0
+                         0                  0 1 ],
+        [ 0.877504134186525 -0.479569071652413 0
+          0.479569071652413  0.877504134186525 0
+                          0                  0 1 ],
+        [ 0.023433068172945  0.999725407957606 0
+         -0.999725407957606  0.023433068172945 0
+                          0                  0 1 ],
+        [-0.854071066013566 -0.520156336305202 0
+          0.520156336305202 -0.854071066013566 0
+                          0                  0 1 ],
+        [ 0.854071066013574 -0.520156336305191 0
+          0.520156336305191  0.854071066013574 0
+                          0                  0 1 ],
+        [-0.023433068172958  0.999725407957606 0
+         -0.999725407957606 -0.023433068172958 0
+                          0                  0 1 ] };
+
+    nano_hexapod.top_plate.K = readmatrix('top_plate_K_6.CSV'); % Stiffness Matrix
+    nano_hexapod.top_plate.M = readmatrix('top_plate_M_6.CSV'); % Mass Matrix
+    nano_hexapod.top_plate.P = extractNodes('top_plate_out_nodes_3D_qua.txt'); % Node coordiantes [m]
+    nano_hexapod.top_plate.xi = args.top_plate_xi; % Damping ratio
+
+end
diff --git a/matlab/src/initializeNanoHexapodFinal.m b/matlab/src/initializeNanoHexapodFinal.m
deleted file mode 100644
index 6904c2e..0000000
--- a/matlab/src/initializeNanoHexapodFinal.m
+++ /dev/null
@@ -1,305 +0,0 @@
-function [nano_hexapod] = initializeNanoHexapodFinal(args)
-
-arguments
-    %% Bottom Flexible Joints
-    args.flex_bot_type      char {mustBeMember(args.flex_bot_type,{'2dof', '3dof', '4dof', 'flexible'})} = '4dof'
-    args.flex_bot_kRx (6,1) double {mustBeNumeric} = ones(6,1)*5 % X bending stiffness [Nm/rad]
-    args.flex_bot_kRy (6,1) double {mustBeNumeric} = ones(6,1)*5 % Y bending stiffness [Nm/rad]
-    args.flex_bot_kRz (6,1) double {mustBeNumeric} = ones(6,1)*260 % Torsionnal stiffness [Nm/rad]
-    args.flex_bot_kz  (6,1) double {mustBeNumeric} = ones(6,1)*7e7 % Axial Stiffness [N/m]
-    args.flex_bot_cRx (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % X bending Damping [Nm/(rad/s)]
-    args.flex_bot_cRy (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Y bending Damping [Nm/(rad/s)]
-    args.flex_bot_cRz (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Torsionnal Damping [Nm/(rad/s)]
-    args.flex_bot_cz  (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Axial Damping [N/(m/s)]
-
-    %% Top Flexible Joints
-    args.flex_top_type      char {mustBeMember(args.flex_top_type,{'2dof', '3dof', '4dof', 'flexible'})} = '4dof'
-    args.flex_top_kRx (6,1) double {mustBeNumeric} = ones(6,1)*5 % X bending stiffness [Nm/rad]
-    args.flex_top_kRy (6,1) double {mustBeNumeric} = ones(6,1)*5 % Y bending stiffness [Nm/rad]
-    args.flex_top_kRz (6,1) double {mustBeNumeric} = ones(6,1)*260 % Torsionnal stiffness [Nm/rad]
-    args.flex_top_kz  (6,1) double {mustBeNumeric} = ones(6,1)*7e7 % Axial Stiffness [N/m]
-    args.flex_top_cRx (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % X bending Damping [Nm/(rad/s)]
-    args.flex_top_cRy (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Y bending Damping [Nm/(rad/s)]
-    args.flex_top_cRz (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Torsionnal Damping [Nm/(rad/s)]
-    args.flex_top_cz  (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Axial Damping [N/(m/s)]
-
-    %% Jacobian - Location of frame {A} and {B}
-    args.MO_B (1,1) double {mustBeNumeric} = 150e-3 % Height of {B} w.r.t. {M} [m]
-
-    %% Relative Motion Sensor
-    args.motion_sensor_type char {mustBeMember(args.motion_sensor_type,{'struts', 'plates'})} = 'struts'
-
-    %% Top Plate
-    args.top_plate_type char {mustBeMember(args.top_plate_type,{'rigid', 'flexible'})} = 'rigid'
-    args.top_plate_xi  (1,1) double {mustBeNumeric} = 0.01 % Damping Ratio
-
-    %% Actuators
-    args.actuator_type char {mustBeMember(args.actuator_type,{'2dof', 'flexible frame', 'flexible'})} = 'flexible'
-    args.actuator_Ga  (6,1) double {mustBeNumeric} = zeros(6,1) % Actuator gain [N/V]
-    args.actuator_Gs  (6,1) double {mustBeNumeric} = zeros(6,1) % Sensor gain [V/m]
-    % For 2DoF
-    args.actuator_k   (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*380000
-    args.actuator_ke  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*4952605
-    args.actuator_ka  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*2476302
-    args.actuator_c   (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*5
-    args.actuator_ce  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*100
-    args.actuator_ca  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*50
-    args.actuator_Leq (6,1) double {mustBeNumeric} = ones(6,1)*0.056 % [m]
-    % For Flexible Frame
-    args.actuator_ks (6,1) double {mustBeNumeric} = ones(6,1)*235e6 % Stiffness of one stack [N/m]
-    args.actuator_cs (6,1) double {mustBeNumeric} = ones(6,1)*1e1 % Stiffness of one stack [N/m]
-    % Misalignment
-    args.actuator_d_align (6,3) double {mustBeNumeric} = zeros(6,3) % [m]
-
-    args.actuator_xi  (1,1) double {mustBeNumeric} = 0.01 % Damping Ratio
-
-    %% Controller
-    args.controller_type char {mustBeMember(args.controller_type,{'none', 'iff', 'dvf', 'hac-iff-struts'})} = 'none'
-end
-
-nano_hexapod = struct();
-
-nano_hexapod.flex_bot = struct();
-
-switch args.flex_bot_type
-  case '2dof'
-    nano_hexapod.flex_bot.type = 1;
-  case '3dof'
-    nano_hexapod.flex_bot.type = 2;
-  case '4dof'
-    nano_hexapod.flex_bot.type = 3;
-  case 'flexible'
-    nano_hexapod.flex_bot.type = 4;
-end
-
-nano_hexapod.flex_bot.kRx = args.flex_bot_kRx; % X bending stiffness [Nm/rad]
-nano_hexapod.flex_bot.kRy = args.flex_bot_kRy; % Y bending stiffness [Nm/rad]
-nano_hexapod.flex_bot.kRz = args.flex_bot_kRz; % Torsionnal stiffness [Nm/rad]
-nano_hexapod.flex_bot.kz  = args.flex_bot_kz;  % Axial stiffness [N/m]
-
-nano_hexapod.flex_bot.cRx = args.flex_bot_cRx; % [Nm/(rad/s)]
-nano_hexapod.flex_bot.cRy = args.flex_bot_cRy; % [Nm/(rad/s)]
-nano_hexapod.flex_bot.cRz = args.flex_bot_cRz; % [Nm/(rad/s)]
-nano_hexapod.flex_bot.cz  = args.flex_bot_cz;  %[N/(m/s)]
-
-nano_hexapod.flex_top = struct();
-
-switch args.flex_top_type
-  case '2dof'
-    nano_hexapod.flex_top.type = 1;
-  case '3dof'
-    nano_hexapod.flex_top.type = 2;
-  case '4dof'
-    nano_hexapod.flex_top.type = 3;
-  case 'flexible'
-    nano_hexapod.flex_top.type = 4;
-end
-
-nano_hexapod.flex_top.kRx = args.flex_top_kRx; % X bending stiffness [Nm/rad]
-nano_hexapod.flex_top.kRy = args.flex_top_kRy; % Y bending stiffness [Nm/rad]
-nano_hexapod.flex_top.kRz = args.flex_top_kRz; % Torsionnal stiffness [Nm/rad]
-nano_hexapod.flex_top.kz  = args.flex_top_kz;  % Axial stiffness [N/m]
-
-nano_hexapod.flex_top.cRx = args.flex_top_cRx; % [Nm/(rad/s)]
-nano_hexapod.flex_top.cRy = args.flex_top_cRy; % [Nm/(rad/s)]
-nano_hexapod.flex_top.cRz = args.flex_top_cRz; % [Nm/(rad/s)]
-nano_hexapod.flex_top.cz  = args.flex_top_cz;  %[N/(m/s)]
-
-nano_hexapod.motion_sensor = struct();
-
-switch args.motion_sensor_type
-  case 'struts'
-    nano_hexapod.motion_sensor.type = 1;
-  case 'plates'
-    nano_hexapod.motion_sensor.type = 2;
-end
-
-nano_hexapod.actuator = struct();
-
-switch args.actuator_type
-  case '2dof'
-    nano_hexapod.actuator.type = 1;
-  case 'flexible frame'
-    nano_hexapod.actuator.type = 2;
-  case 'flexible'
-    nano_hexapod.actuator.type = 3;
-end
-
-%% Actuator gain [N/V]
-if all(args.actuator_Ga == 0)
-    switch args.actuator_type
-      case '2dof'
-        nano_hexapod.actuator.Ga = ones(6,1)*(-2.5796);
-      case 'flexible frame'
-        nano_hexapod.actuator.Ga = ones(6,1); % TODO
-      case 'flexible'
-        nano_hexapod.actuator.Ga = ones(6,1)*23.2;
-    end
-else
-    nano_hexapod.actuator.Ga = args.actuator_Ga; % Actuator gain [N/V]
-end
-
-%% Sensor gain [V/m]
-if all(args.actuator_Gs == 0)
-    switch args.actuator_type
-      case '2dof'
-        nano_hexapod.actuator.Gs = ones(6,1)*466664;
-      case 'flexible frame'
-        nano_hexapod.actuator.Gs = ones(6,1); % TODO
-      case 'flexible'
-        nano_hexapod.actuator.Gs = ones(6,1)*(-4898341);
-    end
-else
-    nano_hexapod.actuator.Gs = args.actuator_Gs; % Sensor gain [V/m]
-end
-
-switch args.actuator_type
-  case '2dof'
-    nano_hexapod.actuator.k  = args.actuator_k;  % [N/m]
-    nano_hexapod.actuator.ke = args.actuator_ke; % [N/m]
-    nano_hexapod.actuator.ka = args.actuator_ka; % [N/m]
-
-    nano_hexapod.actuator.c  = args.actuator_c;  % [N/(m/s)]
-    nano_hexapod.actuator.ce = args.actuator_ce; % [N/(m/s)]
-    nano_hexapod.actuator.ca = args.actuator_ca; % [N/(m/s)]
-
-    nano_hexapod.actuator.Leq = args.actuator_Leq; % [m]
-
-  case 'flexible frame'
-    nano_hexapod.actuator.K = readmatrix('APA300ML_b_mat_K.CSV'); % Stiffness Matrix
-    nano_hexapod.actuator.M = readmatrix('APA300ML_b_mat_M.CSV'); % Mass Matrix
-    nano_hexapod.actuator.P = extractNodes('APA300ML_b_out_nodes_3D.txt'); % Node coordinates [m]
-
-    nano_hexapod.actuator.ks = args.actuator_ks; % Stiffness of one stack [N/m]
-    nano_hexapod.actuator.cs = args.actuator_cs; % Damping of one stack [N/m]
-    nano_hexapod.actuator.xi = args.actuator_xi; % Damping ratio
-
-  case 'flexible'
-    nano_hexapod.actuator.K = readmatrix('full_APA300ML_K.CSV'); % Stiffness Matrix
-    nano_hexapod.actuator.M = readmatrix('full_APA300ML_M.CSV'); % Mass Matrix
-    nano_hexapod.actuator.P = extractNodes('full_APA300ML_out_nodes_3D.txt'); % Node coordiantes [m]
-
-    nano_hexapod.actuator.d_align = args.actuator_d_align; % Misalignment
-    nano_hexapod.actuator.xi = args.actuator_xi; % Damping ratio
-
-end
-
-nano_hexapod.geometry = struct();
-
-Fa = [[-86.05,  -74.78, 22.49],
-      [ 86.05,  -74.78, 22.49],
-      [ 107.79, -37.13, 22.49],
-      [ 21.74,  111.91, 22.49],
-      [-21.74,  111.91, 22.49],
-      [-107.79, -37.13, 22.49]]'*1e-3; % Ai w.r.t. {F} [m]
-
-Mb = [[-28.47, -106.25, -22.50],
-      [ 28.47, -106.25, -22.50],
-      [ 106.25, 28.47,  -22.50],
-      [ 77.78,  77.78,  -22.50],
-      [-77.78,  77.78,  -22.50],
-      [-106.25, 28.47,  -22.50]]'*1e-3; % Bi w.r.t. {M} [m]
-
-Fb = Mb + [0; 0; 95e-3]; % Bi w.r.t. {F} [m]
-
-si = Fb - Fa;
-si = si./vecnorm(si); % Normalize
-
-Fc = [[-29.362, -105.765, 52.605]
-      [ 29.362, -105.765, 52.605]
-      [ 106.276, 27.454,  52.605]
-      [ 76.914,  78.31,   52.605]
-      [-76.914,  78.31,   52.605]
-      [-106.276, 27.454,  52.605]]'*1e-3; % Meas pos w.r.t. {F}
-Mc = Fc - [0; 0; 95e-3]; % Meas pos w.r.t. {M}
-
-nano_hexapod.geometry.Fa = Fa;
-nano_hexapod.geometry.Fb = Fb;
-nano_hexapod.geometry.Fc = Fc;
-nano_hexapod.geometry.Mb = Mb;
-nano_hexapod.geometry.Mc = Mc;
-nano_hexapod.geometry.si = si;
-nano_hexapod.geometry.MO_B = args.MO_B;
-
-Bb = Mb - [0; 0; args.MO_B];
-
-nano_hexapod.geometry.J = [nano_hexapod.geometry.si', cross(Bb, nano_hexapod.geometry.si)'];
-
-switch args.motion_sensor_type
-  case 'struts'
-    nano_hexapod.geometry.Js = nano_hexapod.geometry.J;
-  case 'plates'
-    Bc = Mc - [0; 0; args.MO_B];
-    nano_hexapod.geometry.Js = [nano_hexapod.geometry.si', cross(Bc, nano_hexapod.geometry.si)'];
-end
-
-nano_hexapod.top_plate = struct();
-
-switch args.top_plate_type
-  case 'rigid'
-    nano_hexapod.top_plate.type = 1;
-  case 'flexible'
-    nano_hexapod.top_plate.type = 2;
-
-    nano_hexapod.top_plate.R_flex = ...
-       {[ 0.53191886726305  0.4795690716524  0.69790817745892
-         -0.29070157897799  0.8775041341865 -0.38141720787774
-         -0.79533320729697  0                0.60617249143351 ],
-        [ 0.53191886726305 -0.4795690716524 -0.69790817745892
-          0.29070157897799  0.8775041341865 -0.38141720787774
-          0.79533320729697  0                0.60617249143351 ],
-        [-0.01420448131633 -0.9997254079576 -0.01863709726680
-          0.60600604129104 -0.0234330681729  0.79511481512719
-         -0.79533320729697  0                0.60617249143351 ],
-        [-0.51771438594672 -0.5201563363051  0.67927108019212
-          0.31530446231304 -0.8540710660135 -0.41369760724945
-          0.79533320729697  0                0.60617249143351 ],
-        [-0.51771438594671  0.5201563363052 -0.67927108019211
-         -0.31530446231304 -0.8540710660135 -0.41369760724945
-         -0.79533320729697  0                0.60617249143351 ],
-        [-0.01420448131632  0.9997254079576  0.01863709726679
-         -0.60600604129104 -0.0234330681729  0.79511481512719
-          0.79533320729697  0                0.60617249143351 ] };
-
-
-    nano_hexapod.top_plate.R_enc = ...
-      { [-0.877504134186525 -0.479569071652412 0
-         0.479569071652412 -0.877504134186525 0
-                         0                  0 1 ],
-        [ 0.877504134186525 -0.479569071652413 0
-          0.479569071652413  0.877504134186525 0
-                          0                  0 1 ],
-        [ 0.023433068172945  0.999725407957606 0
-         -0.999725407957606  0.023433068172945 0
-                          0                  0 1 ],
-        [-0.854071066013566 -0.520156336305202 0
-          0.520156336305202 -0.854071066013566 0
-                          0                  0 1 ],
-        [ 0.854071066013574 -0.520156336305191 0
-          0.520156336305191  0.854071066013574 0
-                          0                  0 1 ],
-        [-0.023433068172958  0.999725407957606 0
-         -0.999725407957606 -0.023433068172958 0
-                          0                  0 1 ] };
-
-    nano_hexapod.top_plate.K = readmatrix('top_plate_K_6.CSV'); % Stiffness Matrix
-    nano_hexapod.top_plate.M = readmatrix('top_plate_M_6.CSV'); % Mass Matrix
-    nano_hexapod.top_plate.P = extractNodes('top_plate_out_nodes_3D_qua.txt'); % Node coordiantes [m]
-    nano_hexapod.top_plate.xi = args.top_plate_xi; % Damping ratio
-
-end
-
-switch args.controller_type
-  case 'none'
-    nano_hexapod.controller.type = 1;
-  case 'iff'
-    nano_hexapod.controller.type = 2;
-  case 'dvf'
-    nano_hexapod.controller.type = 3;
-  case 'hac-iff-struts'
-    nano_hexapod.controller.type = 4;
-end
-
-if nargout == 0
-  save('./mat/stages.mat', 'nano_hexapod', '-append');
-end
diff --git a/matlab/src/initializePosError.m b/matlab/src/initializePosError.m
deleted file mode 100644
index e942c6f..0000000
--- a/matlab/src/initializePosError.m
+++ /dev/null
@@ -1,28 +0,0 @@
-function [] = initializePosError(args)
-% initializePosError - Initialize the position errors
-%
-% Syntax: [] = initializePosError(args)
-%
-% Inputs:
-%    - args -
-
-arguments
-    args.error    logical {mustBeNumericOrLogical} = false
-    args.Dy (1,1) double  {mustBeNumeric} = 0 % [m]
-    args.Ry (1,1) double  {mustBeNumeric} = 0 % [m]
-    args.Rz (1,1) double  {mustBeNumeric} = 0 % [m]
-end
-
-pos_error = struct();
-
-if args.error
-  pos_error.type = 1;
-else
-  pos_error.type = 0;
-end
-
-pos_error.Dy = args.Dy;
-pos_error.Ry = args.Ry;
-pos_error.Rz = args.Rz;
-
-save('./mat/pos_error.mat', 'pos_error');
diff --git a/matlab/src/initializeReferences.m b/matlab/src/initializeReferences.m
deleted file mode 100644
index c0471c1..0000000
--- a/matlab/src/initializeReferences.m
+++ /dev/null
@@ -1,230 +0,0 @@
-function [ref] = initializeReferences(args)
-
-arguments
-    % Sampling Frequency [s]
-    args.Ts           (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
-    % Maximum simulation time [s]
-    args.Tmax         (1,1) double {mustBeNumeric, mustBePositive} = 100
-    % Either "constant" / "triangular" / "sinusoidal"
-    args.Dy_type      char {mustBeMember(args.Dy_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
-    % Amplitude of the displacement [m]
-    args.Dy_amplitude (1,1) double {mustBeNumeric} = 0
-    % Period of the displacement [s]
-    args.Dy_period    (1,1) double {mustBeNumeric, mustBePositive} = 1
-    % Either "constant" / "triangular" / "sinusoidal"
-    args.Ry_type      char {mustBeMember(args.Ry_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
-    % Amplitude [rad]
-    args.Ry_amplitude (1,1) double {mustBeNumeric} = 0
-    % Period of the displacement [s]
-    args.Ry_period    (1,1) double {mustBeNumeric, mustBePositive} = 1
-    % Either "constant" / "rotating"
-    args.Rz_type      char {mustBeMember(args.Rz_type,{'constant', 'rotating', 'rotating-not-filtered'})} = 'constant'
-    % Initial angle [rad]
-    args.Rz_amplitude (1,1) double {mustBeNumeric} = 0
-    % Period of the rotating [s]
-    args.Rz_period    (1,1) double {mustBeNumeric, mustBePositive} = 1
-    % For now, only constant is implemented
-    args.Dh_type      char {mustBeMember(args.Dh_type,{'constant'})} = 'constant'
-    % Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles)
-    args.Dh_pos       (6,1) double {mustBeNumeric} = zeros(6, 1), ...
-    % For now, only constant is implemented
-    args.Rm_type      char {mustBeMember(args.Rm_type,{'constant'})} = 'constant'
-    % Initial position of the two masses
-    args.Rm_pos       (2,1) double {mustBeNumeric} = [0; pi]
-    % For now, only constant is implemented
-    args.Dn_type      char {mustBeMember(args.Dn_type,{'constant'})} = 'constant'
-    % Initial position [m,m,m,rad,rad,rad] of the top platform
-    args.Dn_pos       (6,1) double {mustBeNumeric} = zeros(6,1)
-end
-
-%% Set Sampling Time
-Ts = args.Ts;
-Tmax = args.Tmax;
-
-%% Low Pass Filter to filter out the references
-s = zpk('s');
-w0 = 2*pi*10;
-xi = 1;
-H_lpf = 1/(1 + 2*xi/w0*s + s^2/w0^2);
-
-%% Translation stage - Dy
-t = 0:Ts:Tmax; % Time Vector [s]
-Dy   = zeros(length(t), 1);
-Dyd  = zeros(length(t), 1);
-Dydd = zeros(length(t), 1);
-switch args.Dy_type
-  case 'constant'
-    Dy(:)   = args.Dy_amplitude;
-    Dyd(:)  = 0;
-    Dydd(:) = 0;
-  case 'triangular'
-    % This is done to unsure that we start with no displacement
-    Dy_raw = args.Dy_amplitude*sawtooth(2*pi*t/args.Dy_period,1/2);
-    i0 = find(t>=args.Dy_period/4,1);
-    Dy(1:end-i0+1) = Dy_raw(i0:end);
-    Dy(end-i0+2:end) = Dy_raw(end); % we fix the last value
-
-    % The signal is filtered out
-    Dy   = lsim(H_lpf,     Dy, t);
-    Dyd  = lsim(H_lpf*s,   Dy, t);
-    Dydd = lsim(H_lpf*s^2, Dy, t);
-  case 'sinusoidal'
-    Dy(:) = args.Dy_amplitude*sin(2*pi/args.Dy_period*t);
-    Dyd   = args.Dy_amplitude*2*pi/args.Dy_period*cos(2*pi/args.Dy_period*t);
-    Dydd  = -args.Dy_amplitude*(2*pi/args.Dy_period)^2*sin(2*pi/args.Dy_period*t);
-  otherwise
-    warning('Dy_type is not set correctly');
-end
-
-Dy = struct('time', t, 'signals', struct('values', Dy), 'deriv', Dyd, 'dderiv', Dydd);
-
-%% Tilt Stage - Ry
-t = 0:Ts:Tmax; % Time Vector [s]
-Ry   = zeros(length(t), 1);
-Ryd  = zeros(length(t), 1);
-Rydd = zeros(length(t), 1);
-
-switch args.Ry_type
-  case 'constant'
-    Ry(:) = args.Ry_amplitude;
-    Ryd(:)   = 0;
-    Rydd(:)  = 0;
-  case 'triangular'
-    Ry_raw = args.Ry_amplitude*sawtooth(2*pi*t/args.Ry_period,1/2);
-    i0 = find(t>=args.Ry_period/4,1);
-    Ry(1:end-i0+1) = Ry_raw(i0:end);
-    Ry(end-i0+2:end) = Ry_raw(end); % we fix the last value
-
-    % The signal is filtered out
-    Ry   = lsim(H_lpf,     Ry, t);
-    Ryd  = lsim(H_lpf*s,   Ry, t);
-    Rydd = lsim(H_lpf*s^2, Ry, t);
-  case 'sinusoidal'
-    Ry(:) = args.Ry_amplitude*sin(2*pi/args.Ry_period*t);
-
-    Ryd  = args.Ry_amplitude*2*pi/args.Ry_period*cos(2*pi/args.Ry_period*t);
-    Rydd = -args.Ry_amplitude*(2*pi/args.Ry_period)^2*sin(2*pi/args.Ry_period*t);
-  otherwise
-    warning('Ry_type is not set correctly');
-end
-
-Ry = struct('time', t, 'signals', struct('values', Ry), 'deriv', Ryd, 'dderiv', Rydd);
-
-%% Spindle - Rz
-t = 0:Ts:Tmax; % Time Vector [s]
-Rz   = zeros(length(t), 1);
-Rzd  = zeros(length(t), 1);
-Rzdd = zeros(length(t), 1);
-
-switch args.Rz_type
-  case 'constant'
-    Rz(:)   = args.Rz_amplitude;
-    Rzd(:)  = 0;
-    Rzdd(:) = 0;
-  case 'rotating-not-filtered'
-    Rz(:) = 2*pi/args.Rz_period*t;
-
-    % The signal is filtered out
-    Rz(:)   = 2*pi/args.Rz_period*t;
-    Rzd(:)  = 2*pi/args.Rz_period;
-    Rzdd(:) = 0;
-
-    % We add the angle offset
-    Rz = Rz + args.Rz_amplitude;
-
-  case 'rotating'
-    Rz(:) = 2*pi/args.Rz_period*t;
-
-    % The signal is filtered out
-    Rz   = lsim(H_lpf,     Rz, t);
-    Rzd  = lsim(H_lpf*s,   Rz, t);
-    Rzdd = lsim(H_lpf*s^2, Rz, t);
-
-    % We add the angle offset
-    Rz = Rz + args.Rz_amplitude;
-  otherwise
-    warning('Rz_type is not set correctly');
-end
-
-Rz = struct('time', t, 'signals', struct('values', Rz), 'deriv', Rzd, 'dderiv', Rzdd);
-
-%% Micro-Hexapod
-t = [0, Ts];
-Dh = zeros(length(t), 6);
-Dhl = zeros(length(t), 6);
-
-switch args.Dh_type
-  case 'constant'
-    Dh = [args.Dh_pos, args.Dh_pos];
-
-    load('mat/stages.mat', 'micro_hexapod');
-
-    AP = [args.Dh_pos(1) ; args.Dh_pos(2) ; args.Dh_pos(3)];
-
-    tx = args.Dh_pos(4);
-    ty = args.Dh_pos(5);
-    tz = args.Dh_pos(6);
-
-    ARB = [cos(tz) -sin(tz) 0;
-           sin(tz)  cos(tz) 0;
-           0        0       1]*...
-          [ cos(ty) 0 sin(ty);
-            0       1 0;
-           -sin(ty) 0 cos(ty)]*...
-          [1 0        0;
-           0 cos(tx) -sin(tx);
-           0 sin(tx)  cos(tx)];
-
-    [~, Dhl] = inverseKinematics(micro_hexapod, 'AP', AP, 'ARB', ARB);
-    Dhl = [Dhl, Dhl];
-  otherwise
-    warning('Dh_type is not set correctly');
-end
-
-Dh = struct('time', t, 'signals', struct('values', Dh));
-Dhl = struct('time', t, 'signals', struct('values', Dhl));
-
-%% Axis Compensation - Rm
-t = [0, Ts];
-
-Rm = [args.Rm_pos, args.Rm_pos];
-Rm = struct('time', t, 'signals', struct('values', Rm));
-
-%% Nano-Hexapod
-t = [0, Ts];
-Dn = zeros(length(t), 6);
-
-switch args.Dn_type
-  case 'constant'
-    Dn = [args.Dn_pos, args.Dn_pos];
-
-    load('mat/stages.mat', 'nano_hexapod');
-
-    AP = [args.Dn_pos(1) ; args.Dn_pos(2) ; args.Dn_pos(3)];
-
-    tx = args.Dn_pos(4);
-    ty = args.Dn_pos(5);
-    tz = args.Dn_pos(6);
-
-    ARB = [cos(tz) -sin(tz) 0;
-           sin(tz)  cos(tz) 0;
-           0        0       1]*...
-          [ cos(ty) 0 sin(ty);
-            0       1 0;
-           -sin(ty) 0 cos(ty)]*...
-          [1 0        0;
-           0 cos(tx) -sin(tx);
-           0 sin(tx)  cos(tx)];
-
-    [~, Dnl] = inverseKinematics(nano_hexapod, 'AP', AP, 'ARB', ARB);
-    Dnl = [Dnl, Dnl];
-  otherwise
-    warning('Dn_type is not set correctly');
-end
-
-Dn = struct('time', t, 'signals', struct('values', Dn));
-Dnl = struct('time', t, 'signals', struct('values', Dnl));
-
-%% Save
-    save('./mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Dnl', 'args', 'Ts');
-end
diff --git a/matlab/src/initializeRy.m b/matlab/src/initializeRy.m
deleted file mode 100644
index b156199..0000000
--- a/matlab/src/initializeRy.m
+++ /dev/null
@@ -1,54 +0,0 @@
-function [ry] = initializeRy(args)
-
-arguments
-  args.type          char    {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
-  args.Foffset       logical {mustBeNumericOrLogical} = false
-  args.Ry_init (1,1) double  {mustBeNumeric} = 0
-end
-
-ry = struct();
-
-switch args.type
-  case 'none'
-    ry.type = 0;
-  case 'rigid'
-    ry.type = 1;
-  case 'flexible'
-    ry.type = 2;
-  case 'modal-analysis'
-    ry.type = 3;
-  case 'init'
-    ry.type = 4;
-end
-
-% Ry - Guide for the tilt stage
-ry.guide.density = 7800; % [kg/m3]
-ry.guide.STEP    = './STEPS/ry/Tilt_Guide.STEP';
-
-% Ry - Rotor of the motor
-ry.rotor.density = 2400; % [kg/m3]
-ry.rotor.STEP    = './STEPS/ry/Tilt_Motor_Axis.STEP';
-
-% Ry - Motor
-ry.motor.density = 3200; % [kg/m3]
-ry.motor.STEP    = './STEPS/ry/Tilt_Motor.STEP';
-
-% Ry - Plateau Tilt
-ry.stage.density = 7800; % [kg/m3]
-ry.stage.STEP    = './STEPS/ry/Tilt_Stage.STEP';
-
-ry.z_offset = 0.58178; % [m]
-
-ry.Ry_init = args.Ry_init; % [rad]
-
-ry.K = [3.8e8; 4e8; 3.8e8; 1.2e8; 6e4; 1.2e8];
-ry.C = [1e5;   1e5; 1e5;   3e4;   1e3; 3e4];
-
-if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
-  load('mat/Foffset.mat', 'Fym');
-  ry.Deq = -Fym'./ry.K;
-else
-  ry.Deq = zeros(6,1);
-end
-
-save('./mat/stages.mat', 'ry', '-append');
diff --git a/matlab/src/initializeRz.m b/matlab/src/initializeRz.m
deleted file mode 100644
index 164e92f..0000000
--- a/matlab/src/initializeRz.m
+++ /dev/null
@@ -1,45 +0,0 @@
-function [rz] = initializeRz(args)
-
-arguments
-  args.type    char    {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
-  args.Foffset logical {mustBeNumericOrLogical} = false
-end
-
-rz = struct();
-
-switch args.type
-  case 'none'
-    rz.type = 0;
-  case 'rigid'
-    rz.type = 1;
-  case 'flexible'
-    rz.type = 2;
-  case 'modal-analysis'
-    rz.type = 3;
-  case 'init'
-    rz.type = 4;
-end
-
-% Spindle - Slip Ring
-rz.slipring.density = 7800; % [kg/m3]
-rz.slipring.STEP    = './STEPS/rz/Spindle_Slip_Ring.STEP';
-
-% Spindle - Rotor
-rz.rotor.density    = 7800; % [kg/m3]
-rz.rotor.STEP       = './STEPS/rz/Spindle_Rotor.STEP';
-
-% Spindle - Stator
-rz.stator.density   = 7800; % [kg/m3]
-rz.stator.STEP      = './STEPS/rz/Spindle_Stator.STEP';
-
-rz.K = [7e8; 7e8; 2e9; 1e7; 1e7; 1e7];
-rz.C = [4e4; 4e4; 7e4; 1e4; 1e4; 1e4];
-
-if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
-  load('mat/Foffset.mat', 'Fzm');
-  rz.Deq = -Fzm'./rz.K;
-else
-  rz.Deq = zeros(6,1);
-end
-
-save('./mat/stages.mat', 'rz', '-append');
diff --git a/matlab/src/initializeSample.m b/matlab/src/initializeSample.m
deleted file mode 100644
index bc4a757..0000000
--- a/matlab/src/initializeSample.m
+++ /dev/null
@@ -1,51 +0,0 @@
-function [sample] = initializeSample(args)
-
-arguments
-  args.type         char    {mustBeMember(args.type,{'rigid', 'flexible', 'none', 'init'})} = 'flexible'
-  args.radius (1,1) double  {mustBeNumeric, mustBePositive} = 0.1 % [m]
-  args.height (1,1) double  {mustBeNumeric, mustBePositive} = 0.3 % [m]
-  args.mass   (1,1) double  {mustBeNumeric, mustBePositive} = 50 % [kg]
-  args.freq   (6,1) double  {mustBeNumeric, mustBePositive} = 100*ones(6,1) % [Hz]
-  args.offset (1,1) double  {mustBeNumeric} = 0 % [m]
-  args.Foffset      logical {mustBeNumericOrLogical} = false
-end
-
-sample = struct();
-
-switch args.type
-  case 'none'
-    sample.type = 0;
-  case 'rigid'
-    sample.type = 1;
-  case 'flexible'
-    sample.type = 2;
-  case 'init'
-    sample.type = 3;
-end
-
-sample.radius = args.radius; % [m]
-sample.height = args.height; % [m]
-sample.mass   = args.mass; % [kg]
-sample.offset = args.offset; % [m]
-
-sample.inertia = [1/12 * sample.mass * (3*sample.radius^2 + sample.height^2); ...
-                  1/12 * sample.mass * (3*sample.radius^2 + sample.height^2); ...
-                  1/2  * sample.mass * sample.radius^2];
-
-sample.K = zeros(6, 1);
-sample.C = zeros(6, 1);
-
-sample.K(1:3) = sample.mass .* (2*pi * args.freq(1:3)).^2; % [N/m]
-sample.C(1:3) = 0.1 * sqrt(sample.K(1:3)*sample.mass); % [N/(m/s)]
-
-sample.K(4:6) = sample.inertia .* (2*pi * args.freq(4:6)).^2; % [N/m]
-sample.C(4:6) = 0.1 * sqrt(sample.K(4:6).*sample.inertia); % [N/(m/s)]
-
-if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
-  load('mat/Foffset.mat', 'Fsm');
-  sample.Deq = -Fsm'./sample.K;
-else
-  sample.Deq = zeros(6,1);
-end
-
-save('./mat/stages.mat', 'sample', '-append');
diff --git a/matlab/src/initializeSimscapeConfiguration.m b/matlab/src/initializeSimscapeConfiguration.m
deleted file mode 100644
index ba93c9f..0000000
--- a/matlab/src/initializeSimscapeConfiguration.m
+++ /dev/null
@@ -1,15 +0,0 @@
-function [] = initializeSimscapeConfiguration(args)
-
-arguments
-  args.gravity logical {mustBeNumericOrLogical} = true
-end
-
-conf_simscape = struct();
-
-if args.gravity
-  conf_simscape.type = 1;
-else
-  conf_simscape.type = 2;
-end
-
-save('./mat/conf_simscape.mat', 'conf_simscape');
diff --git a/matlab/src/initializeStewartPlatform.m b/matlab/src/initializeStewartPlatform.m
deleted file mode 100644
index 0545b4d..0000000
--- a/matlab/src/initializeStewartPlatform.m
+++ /dev/null
@@ -1,31 +0,0 @@
-function [stewart] = initializeStewartPlatform()
-% initializeStewartPlatform - Initialize the stewart structure
-%
-% Syntax: [stewart] = initializeStewartPlatform(args)
-%
-% Outputs:
-%    - stewart - A structure with the following sub-structures:
-%      - platform_F -
-%      - platform_M -
-%      - joints_F   -
-%      - joints_M   -
-%      - struts_F   -
-%      - struts_M   -
-%      - actuators  -
-%      - geometry   -
-%      - properties -
-
-stewart = struct();
-stewart.platform_F = struct();
-stewart.platform_M = struct();
-stewart.joints_F   = struct();
-stewart.joints_M   = struct();
-stewart.struts_F   = struct();
-stewart.struts_M   = struct();
-stewart.actuators  = struct();
-stewart.sensors    = struct();
-stewart.sensors.inertial = struct();
-stewart.sensors.force    = struct();
-stewart.sensors.relative = struct();
-stewart.geometry   = struct();
-stewart.kinematics = struct();
diff --git a/matlab/src/initializeStewartPose.m b/matlab/src/initializeStewartPose.m
deleted file mode 100644
index 325b501..0000000
--- a/matlab/src/initializeStewartPose.m
+++ /dev/null
@@ -1,27 +0,0 @@
-function [stewart] = initializeStewartPose(stewart, args)
-% initializeStewartPose - Determine the initial stroke in each leg to have the wanted pose
-%                         It uses the inverse kinematic
-%
-% Syntax: [stewart] = initializeStewartPose(stewart, args)
-%
-% Inputs:
-%    - stewart - A structure with the following fields
-%        - Aa   [3x6] - The positions ai expressed in {A}
-%        - Bb   [3x6] - The positions bi expressed in {B}
-%    - args - Can have the following fields:
-%        - AP   [3x1] - The wanted position of {B} with respect to {A}
-%        - ARB  [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
-%
-% Outputs:
-%    - stewart - updated Stewart structure with the added fields:
-%      - actuators.Leq [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
-
-arguments
-    stewart
-    args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
-    args.ARB (3,3) double {mustBeNumeric} = eye(3)
-end
-
-[Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB);
-
-stewart.actuators.Leq = dLi;
diff --git a/matlab/src/initializeStrutDynamics.m b/matlab/src/initializeStrutDynamics.m
deleted file mode 100644
index 69dc518..0000000
--- a/matlab/src/initializeStrutDynamics.m
+++ /dev/null
@@ -1,49 +0,0 @@
-function [stewart] = initializeStrutDynamics(stewart, args)
-% initializeStrutDynamics - Add Stiffness and Damping properties of each strut
-%
-% Syntax: [stewart] = initializeStrutDynamics(args)
-%
-% Inputs:
-%    - args - Structure with the following fields:
-%        - K [6x1] - Stiffness of each strut [N/m]
-%        - C [6x1] - Damping of each strut [N/(m/s)]
-%
-% Outputs:
-%    - stewart - updated Stewart structure with the added fields:
-%      - actuators.type = 1
-%      - actuators.K [6x1] - Stiffness of each strut [N/m]
-%      - actuators.C [6x1] - Damping of each strut [N/(m/s)]
-
-arguments
-    stewart
-    args.type char {mustBeMember(args.type,{'classical', 'amplified'})} = 'classical'
-    args.K (6,1) double {mustBeNumeric, mustBeNonnegative} = 20e6*ones(6,1)
-    args.C (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e1*ones(6,1)
-    args.k1 (6,1) double {mustBeNumeric} = 1e6*ones(6,1)
-    args.ke (6,1) double {mustBeNumeric} = 5e6*ones(6,1)
-    args.ka (6,1) double {mustBeNumeric} = 60e6*ones(6,1)
-    args.c1 (6,1) double {mustBeNumeric} = 10*ones(6,1)
-    args.F_gain (6,1) double {mustBeNumeric} = 1*ones(6,1)
-    args.me (6,1) double {mustBeNumeric} = 0.01*ones(6,1)
-    args.ma (6,1) double {mustBeNumeric} = 0.01*ones(6,1)
-end
-
-if strcmp(args.type, 'classical')
-    stewart.actuators.type = 1;
-elseif strcmp(args.type, 'amplified')
-    stewart.actuators.type = 2;
-end
-
-stewart.actuators.K = args.K;
-stewart.actuators.C = args.C;
-
-stewart.actuators.k1 = args.k1;
-stewart.actuators.c1 = args.c1;
-
-stewart.actuators.ka = args.ka;
-stewart.actuators.ke = args.ke;
-
-stewart.actuators.F_gain = args.F_gain;
-
-stewart.actuators.ma = args.ma;
-stewart.actuators.me = args.me;
diff --git a/matlab/src/initializeTy.m b/matlab/src/initializeTy.m
deleted file mode 100644
index 19bc37d..0000000
--- a/matlab/src/initializeTy.m
+++ /dev/null
@@ -1,69 +0,0 @@
-function [ty] = initializeTy(args)
-
-arguments
-  args.type      char   {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
-  args.Foffset logical {mustBeNumericOrLogical} = false
-end
-
-ty = struct();
-
-switch args.type
-  case 'none'
-    ty.type = 0;
-  case 'rigid'
-    ty.type = 1;
-  case 'flexible'
-    ty.type = 2;
-  case 'modal-analysis'
-    ty.type = 3;
-  case 'init'
-    ty.type = 4;
-end
-
-% Ty Granite frame
-ty.granite_frame.density = 7800; % [kg/m3] => 43kg
-ty.granite_frame.STEP    = './STEPS/Ty/Ty_Granite_Frame.STEP';
-
-% Guide Translation Ty
-ty.guide.density         = 7800; % [kg/m3] => 76kg
-ty.guide.STEP            = './STEPS/ty/Ty_Guide.STEP';
-
-% Ty - Guide_Translation12
-ty.guide12.density       = 7800; % [kg/m3]
-ty.guide12.STEP          = './STEPS/Ty/Ty_Guide_12.STEP';
-
-% Ty - Guide_Translation11
-ty.guide11.density       = 7800; % [kg/m3]
-ty.guide11.STEP          = './STEPS/ty/Ty_Guide_11.STEP';
-
-% Ty - Guide_Translation22
-ty.guide22.density       = 7800; % [kg/m3]
-ty.guide22.STEP          = './STEPS/ty/Ty_Guide_22.STEP';
-
-% Ty - Guide_Translation21
-ty.guide21.density       = 7800; % [kg/m3]
-ty.guide21.STEP          = './STEPS/Ty/Ty_Guide_21.STEP';
-
-% Ty - Plateau translation
-ty.frame.density         = 7800; % [kg/m3]
-ty.frame.STEP            = './STEPS/ty/Ty_Stage.STEP';
-
-% Ty Stator Part
-ty.stator.density        = 5400; % [kg/m3]
-ty.stator.STEP           = './STEPS/ty/Ty_Motor_Stator.STEP';
-
-% Ty Rotor Part
-ty.rotor.density         = 5400; % [kg/m3]
-ty.rotor.STEP            = './STEPS/ty/Ty_Motor_Rotor.STEP';
-
-ty.K = [2e8; 1e8; 2e8; 6e7; 9e7; 6e7]; % [N/m, N*m/rad]
-ty.C = [8e4; 5e4; 8e4; 2e4; 3e4; 2e4]; % [N/(m/s), N*m/(rad/s)]
-
-if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
-  load('mat/Foffset.mat', 'Ftym');
-  ty.Deq = -Ftym'./ty.K;
-else
-  ty.Deq = zeros(6,1);
-end
-
-save('./mat/stages.mat', 'ty', '-append');
diff --git a/matlab/src/initializeZAxisAccelerometer.m b/matlab/src/initializeZAxisAccelerometer.m
deleted file mode 100644
index 9d17056..0000000
--- a/matlab/src/initializeZAxisAccelerometer.m
+++ /dev/null
@@ -1,21 +0,0 @@
-function [accelerometer] = initializeZAxisAccelerometer(args)
-    arguments
-        args.mass (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % [kg]
-        args.freq (1,1) double {mustBeNumeric, mustBePositive} = 5e3  % [Hz]
-    end
-
-    %%
-    accelerometer.m = args.mass;
-
-    %% The Stiffness is set to have the damping resonance frequency
-    accelerometer.k = accelerometer.m * (2*pi*args.freq)^2;
-
-    %% We set the damping value to have critical damping
-    accelerometer.c = 2*sqrt(accelerometer.m * accelerometer.k);
-
-    %% Gain correction of the accelerometer to have a unity gain until the resonance
-    accelerometer.gain = -accelerometer.k/accelerometer.m;
-
-    %% Save
-    save('./mat/accelerometer_z_axis.mat', 'accelerometer');
-end
diff --git a/matlab/src/initializeZAxisGeophone.m b/matlab/src/initializeZAxisGeophone.m
deleted file mode 100644
index 8612793..0000000
--- a/matlab/src/initializeZAxisGeophone.m
+++ /dev/null
@@ -1,18 +0,0 @@
-function [geophone] = initializeZAxisGeophone(args)
-    arguments
-        args.mass (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % [kg]
-        args.freq (1,1) double {mustBeNumeric, mustBePositive} = 1    % [Hz]
-    end
-
-    %%
-    geophone.m = args.mass;
-
-    %% The Stiffness is set to have the damping resonance frequency
-    geophone.k = geophone.m * (2*pi*args.freq)^2;
-
-    %% We set the damping value to have critical damping
-    geophone.c = 2*sqrt(geophone.m * geophone.k);
-
-    %% Save
-    save('./mat/geophone_z_axis.mat', 'geophone');
-end
diff --git a/matlab/src/inverseKinematics.m b/matlab/src/inverseKinematics.m
deleted file mode 100644
index c1c0618..0000000
--- a/matlab/src/inverseKinematics.m
+++ /dev/null
@@ -1,36 +0,0 @@
-function [Li, dLi] = inverseKinematics(stewart, args)
-% inverseKinematics - Compute the needed length of each strut to have the wanted position and orientation of {B} with respect to {A}
-%
-% Syntax: [stewart] = inverseKinematics(stewart)
-%
-% Inputs:
-%    - stewart - A structure with the following fields
-%        - geometry.Aa   [3x6] - The positions ai expressed in {A}
-%        - geometry.Bb   [3x6] - The positions bi expressed in {B}
-%        - geometry.l    [6x1] - Length of each strut
-%    - args - Can have the following fields:
-%        - AP   [3x1] - The wanted position of {B} with respect to {A}
-%        - ARB  [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
-%
-% Outputs:
-%    - Li   [6x1] - The 6 needed length of the struts in [m] to have the wanted pose of {B} w.r.t. {A}
-%    - dLi  [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
-
-arguments
-    stewart
-    args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
-    args.ARB (3,3) double {mustBeNumeric} = eye(3)
-end
-
-assert(isfield(stewart.geometry, 'Aa'),   'stewart.geometry should have attribute Aa')
-Aa = stewart.geometry.Aa;
-
-assert(isfield(stewart.geometry, 'Bb'),   'stewart.geometry should have attribute Bb')
-Bb = stewart.geometry.Bb;
-
-assert(isfield(stewart.geometry, 'l'),   'stewart.geometry should have attribute l')
-l = stewart.geometry.l;
-
-Li = sqrt(args.AP'*args.AP + diag(Bb'*Bb) + diag(Aa'*Aa) - (2*args.AP'*Aa)' + (2*args.AP'*(args.ARB*Bb))' - diag(2*(args.ARB*Bb)'*Aa));
-
-dLi = Li-l;
diff --git a/matlab/src/prepareLinearizeIdentification.m b/matlab/src/prepareLinearizeIdentification.m
deleted file mode 100644
index 3488e7f..0000000
--- a/matlab/src/prepareLinearizeIdentification.m
+++ /dev/null
@@ -1,27 +0,0 @@
-function [] = prepareLinearizeIdentification(args)
-
-arguments
-    args.nass_actuator       char   {mustBeMember(args.nass_actuator,{'piezo', 'lorentz'})} = 'piezo'
-    args.sample_mass   (1,1) double {mustBeNumeric, mustBePositive} = 50 % [kg]
-end
-
-initializeGround();
-initializeGranite();
-initializeTy();
-initializeRy();
-initializeRz();
-initializeMicroHexapod();
-initializeAxisc();
-initializeMirror();
-
-initializeNanoHexapod('actuator', args.nass_actuator);
-initializeSample('mass', args.sample_mass);
-
-initializeReferences();
-initializeDisturbances('enable', false);
-
-initializeController('type', 'open-loop');
-
-initializeSimscapeConfiguration('gravity', true);
-
-initializeLoggingConfiguration('log', 'none');
diff --git a/matlab/src/prepareTomographyExperiment.m b/matlab/src/prepareTomographyExperiment.m
deleted file mode 100644
index 441044d..0000000
--- a/matlab/src/prepareTomographyExperiment.m
+++ /dev/null
@@ -1,29 +0,0 @@
-function [] = prepareTomographyExperiment(args)
-
-arguments
-    args.nass_actuator       char   {mustBeMember(args.nass_actuator,{'piezo', 'lorentz'})} = 'piezo'
-    args.sample_mass   (1,1) double {mustBeNumeric, mustBePositive} = 50 % [kg]
-    args.Rz_period     (1,1) double {mustBeNumeric, mustBePositive} = 1 % [s]
-end
-
-initializeGround();
-initializeGranite();
-initializeTy();
-initializeRy();
-initializeRz();
-initializeMicroHexapod();
-initializeAxisc();
-initializeMirror();
-
-initializeNanoHexapod('actuator', args.nass_actuator);
-initializeSample('mass', args.sample_mass);
-
-initializeReferences('Rz_type', 'rotating', 'Rz_period', args.Rz_period);
-
-initializeDisturbances();
-
-initializeController('type', 'open-loop');
-
-initializeSimscapeConfiguration('gravity', true);
-
-initializeLoggingConfiguration('log', 'all');
diff --git a/matlab/src/project_shutdown.m b/matlab/src/project_shutdown.m
deleted file mode 100644
index 5bb8d70..0000000
--- a/matlab/src/project_shutdown.m
+++ /dev/null
@@ -1,5 +0,0 @@
-
-
-% When the project closes, it runs the =project_shutdown.m= script defined below.
-
-Simulink.fileGenControl('reset');
diff --git a/matlab/src/project_startup.m b/matlab/src/project_startup.m
deleted file mode 100644
index c635e75..0000000
--- a/matlab/src/project_startup.m
+++ /dev/null
@@ -1,18 +0,0 @@
-
-
-% When the project opens, a startup script is ran.
-% The startup script is defined below and is exported to the =project_startup.m= script.
-
-project = simulinkproject;
-projectRoot = project.RootFolder;
-
-myCacheFolder = fullfile(projectRoot, '.SimulinkCache');
-myCodeFolder = fullfile(projectRoot, '.SimulinkCode');
-
-Simulink.fileGenControl('set',...
-    'CacheFolder', myCacheFolder,...
-    'CodeGenFolder', myCodeFolder,...
-    'createDir', true);
-
-%% Load the Simscape Configuration
-load('mat/conf_simulink.mat');
diff --git a/matlab/test_nhexa_1_suspended_table.m b/matlab/test_nhexa_1_suspended_table.m
index cf189e0..f87e3bb 100644
--- a/matlab/test_nhexa_1_suspended_table.m
+++ b/matlab/test_nhexa_1_suspended_table.m
@@ -20,7 +20,7 @@ device_type = 'None'; % On top of vibration table
 payload_num = 0; % No Payload
 
 % Simulink Model name
-mdl = 'test_bench_nano_hexapod';
+mdl = 'test_nhexa_simscape';
 
 %% Colors for the figures
 colors = colororder;
diff --git a/matlab/test_nhexa_3_model.m b/matlab/test_nhexa_3_model.m
index 098b086..3cad41a 100644
--- a/matlab/test_nhexa_3_model.m
+++ b/matlab/test_nhexa_3_model.m
@@ -21,7 +21,7 @@ device_type = 'None'; % On top of vibration table
 payload_num = 0; % No Payload
 
 % Simulink Model name
-mdl = 'test_bench_nano_hexapod';
+mdl = 'test_nhexa_simscape';
 
 %% Colors for the figures
 colors = colororder;
@@ -31,66 +31,73 @@ freqs = logspace(log10(10), log10(2e3), 1000);
 
 % Extract transfer function matrices from the Simscape Model      :noexport:
 
-%% Extract the transfer function matrix from the Simscape model
-% Initialization of the Simscape model
-table_type = 'Suspended'; % On top of vibration table
-device_type = 'Hexapod'; % Nano-Hexapod
-payload_num = 0; % No Payload
+%% Set to true only if all the dynamics should again computed
+% from the simscape model
+extract_simscape_dynamics = false
+if extract_simscape_dynamics
 
-n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
-                                       'flex_top_type', '4dof', ...
-                                       'motion_sensor_type', 'plates', ...
-                                       'actuator_type', '2dof');
+    %% Extract the transfer function matrix from the Simscape model
+    % Initialization of the Simscape model
+    table_type = 'Suspended'; % On top of vibration table
+    device_type = 'Hexapod'; % Nano-Hexapod
+    payload_num = 0; % No Payload
 
-% Identify the FRF matrix from u to [de,Vs]
-clear io; io_i = 1;
-io(io_i) = linio([mdl, '/u'],  1, 'openinput');  io_i = io_i + 1; % Actuator Inputs
-io(io_i) = linio([mdl, '/de'], 1, 'openoutput'); io_i = io_i + 1; % Encoders
-io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Encoders
+    n_hexapod = initializeNanoHexapod('flex_bot_type', '4dof', ...
+                                      'flex_top_type', '4dof', ...
+                                      'motion_sensor_type', 'plates', ...
+                                      'actuator_type', '2dof');
 
-G_de = {};
-G_Vs = {};
+    % Identify the FRF matrix from u to [de,Vs]
+    clear io; io_i = 1;
+    io(io_i) = linio([mdl, '/u'],  1, 'openinput');  io_i = io_i + 1; % Actuator Inputs
+    io(io_i) = linio([mdl, '/de'], 1, 'openoutput'); io_i = io_i + 1; % Encoders
+    io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Encoders
+
+    G_de = {};
+    G_Vs = {};
+
+    for i = [0:3]
+        payload_num = i; % Change the payload on the nano-hexapod
+        G = exp(-s*1e-4)*linearize(mdl, io, 0.0);
+        G.InputName  = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
+        G.OutputName = {'de1', 'de2', 'de3', 'de4', 'de5', 'de6', ...
+                        'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'};
+        G_de(i+1) = {G({'de1', 'de2', 'de3', 'de4', 'de5', 'de6'},:)};
+        G_Vs(i+1) = {G({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'},:)};
+    end
+
+    % Save the identified plants
+    save('./mat/test_nhexa_simscape_masses.mat', 'G_Vs', 'G_de')
+
+    %% The same identification is performed, but this time with
+    % "flexible" model of the APA
+    table_type = 'Suspended'; % On top of vibration table
+    device_type = 'Hexapod'; % Nano-Hexapod
+    payload_num = 0; % No Payload
+
+    n_hexapod = initializeNanoHexapod('flex_bot_type', '4dof', ...
+                                      'flex_top_type', '4dof', ...
+                                      'motion_sensor_type', 'plates', ...
+                                      'actuator_type', 'flexible');
+
+    G_de = {};
+    G_Vs = {};
+
+    for i = [0:3]
+        payload_num = i; % Change the payload on the nano-hexapod
+        G = exp(-s*1e-4)*linearize(mdl, io, 0.0);
+        G.InputName  = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
+        G.OutputName = {'de1', 'de2', 'de3', 'de4', 'de5', 'de6', ...
+                        'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'};
+        G_de(i+1) = {G({'de1', 'de2', 'de3', 'de4', 'de5', 'de6'},:)};
+        G_Vs(i+1) = {G({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'},:)};
+    end
+
+    % Save the identified plants
+    save('./mat/test_nhexa_simscape_flexible_masses.mat', 'G_Vs', 'G_de')
 
-for i = [0:3]
-    payload_num = i; % Change the payload on the nano-hexapod
-    G = exp(-s*1e-4)*linearize(mdl, io, 0.0);
-    G.InputName  = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
-    G.OutputName = {'de1', 'de2', 'de3', 'de4', 'de5', 'de6', ...
-                    'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'};
-    G_de(i+1) = {G({'de1', 'de2', 'de3', 'de4', 'de5', 'de6'},:)};
-    G_Vs(i+1) = {G({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'},:)};
 end
 
-% Save the identified plants
-save('./mat/test_nhexa_simscape_masses.mat', 'G_Vs', 'G_de')
-
-%% The same identification is performed, but this time with
-% "flexible" model of the APA
-table_type = 'Suspended'; % On top of vibration table
-device_type = 'Hexapod'; % Nano-Hexapod
-payload_num = 0; % No Payload
-
-n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
-                                       'flex_top_type', '4dof', ...
-                                       'motion_sensor_type', 'plates', ...
-                                       'actuator_type', 'flexible');
-
-G_de = {};
-G_Vs = {};
-
-for i = [0:3]
-    payload_num = i; % Change the payload on the nano-hexapod
-    G = exp(-s*1e-4)*linearize(mdl, io, 0.0);
-    G.InputName  = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
-    G.OutputName = {'de1', 'de2', 'de3', 'de4', 'de5', 'de6', ...
-                    'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'};
-    G_de(i+1) = {G({'de1', 'de2', 'de3', 'de4', 'de5', 'de6'},:)};
-    G_Vs(i+1) = {G({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'},:)};
-end
-
-% Save the identified plants
-save('./mat/test_nhexa_simscape_flexible_masses.mat', 'G_Vs', 'G_de')
-
 % Nano-Hexapod model dynamics
 % <<ssec:test_nhexa_comp_model>>
 
diff --git a/matlab/test_bench_nano_hexapod.slx b/matlab/test_nhexa_simscape.slx
similarity index 100%
rename from matlab/test_bench_nano_hexapod.slx
rename to matlab/test_nhexa_simscape.slx
diff --git a/test-bench-nano-hexapod.org b/test-bench-nano-hexapod.org
index d5790cb..5a5dd7a 100644
--- a/test-bench-nano-hexapod.org
+++ b/test-bench-nano-hexapod.org
@@ -1109,82 +1109,93 @@ This is checked in Section ref:ssec:test_nhexa_comp_model_masses.
 #+end_src
 
 ** Extract transfer function matrices from the Simscape Model      :noexport:
-#+begin_src matlab
-%% Extract the transfer function matrix from the Simscape model
-% Initialization of the Simscape model
-table_type = 'Suspended'; % On top of vibration table
-device_type = 'Hexapod'; % Nano-Hexapod
-payload_num = 0; % No Payload
-
-n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
-                                       'flex_top_type', '4dof', ...
-                                       'motion_sensor_type', 'plates', ...
-                                       'actuator_type', '2dof');
-
-% Identify the FRF matrix from u to [de,Vs]
-clear io; io_i = 1;
-io(io_i) = linio([mdl, '/u'],  1, 'openinput');  io_i = io_i + 1; % Actuator Inputs
-io(io_i) = linio([mdl, '/de'], 1, 'openoutput'); io_i = io_i + 1; % Encoders
-io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Encoders
-
-G_de = {};
-G_Vs = {};
-
-for i = [0:3]
-    payload_num = i; % Change the payload on the nano-hexapod
-    G = exp(-s*1e-4)*linearize(mdl, io, 0.0);
-    G.InputName  = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
-    G.OutputName = {'de1', 'de2', 'de3', 'de4', 'de5', 'de6', ...
-                    'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'};
-    G_de(i+1) = {G({'de1', 'de2', 'de3', 'de4', 'de5', 'de6'},:)};
-    G_Vs(i+1) = {G({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'},:)};
-end
-#+end_src
-
-#+begin_src matlab :exports none :tangle no
-% Save the identified plants
-save('matlab/mat/test_nhexa_simscape_masses.mat', 'G_Vs', 'G_de')
-#+end_src
-
 #+begin_src matlab :eval no
-% Save the identified plants
-save('./mat/test_nhexa_simscape_masses.mat', 'G_Vs', 'G_de')
+%% Set to true only if all the dynamics should again computed
+% from the simscape model
+extract_simscape_dynamics = false;
+if extract_simscape_dynamics
 #+end_src
 
 #+begin_src matlab
-%% The same identification is performed, but this time with
-% "flexible" model of the APA
-table_type = 'Suspended'; % On top of vibration table
-device_type = 'Hexapod'; % Nano-Hexapod
-payload_num = 0; % No Payload
+    %% Extract the transfer function matrix from the Simscape model
+    % Initialization of the Simscape model
+    table_type = 'Suspended'; % On top of vibration table
+    device_type = 'Hexapod'; % Nano-Hexapod
+    payload_num = 0; % No Payload
 
-n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
-                                       'flex_top_type', '4dof', ...
-                                       'motion_sensor_type', 'plates', ...
-                                       'actuator_type', 'flexible');
+    n_hexapod = initializeNanoHexapod('flex_bot_type', '4dof', ...
+                                      'flex_top_type', '4dof', ...
+                                      'motion_sensor_type', 'plates', ...
+                                      'actuator_type', '2dof');
 
-G_de = {};
-G_Vs = {};
+    % Identify the FRF matrix from u to [de,Vs]
+    clear io; io_i = 1;
+    io(io_i) = linio([mdl, '/u'],  1, 'openinput');  io_i = io_i + 1; % Actuator Inputs
+    io(io_i) = linio([mdl, '/de'], 1, 'openoutput'); io_i = io_i + 1; % Encoders
+    io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Encoders
 
-for i = [0:3]
-    payload_num = i; % Change the payload on the nano-hexapod
-    G = exp(-s*1e-4)*linearize(mdl, io, 0.0);
-    G.InputName  = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
-    G.OutputName = {'de1', 'de2', 'de3', 'de4', 'de5', 'de6', ...
-                    'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'};
-    G_de(i+1) = {G({'de1', 'de2', 'de3', 'de4', 'de5', 'de6'},:)};
-    G_Vs(i+1) = {G({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'},:)};
-end
+    G_de = {};
+    G_Vs = {};
+
+    for i = [0:3]
+        payload_num = i; % Change the payload on the nano-hexapod
+        G = exp(-s*1e-4)*linearize(mdl, io, 0.0);
+        G.InputName  = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
+        G.OutputName = {'de1', 'de2', 'de3', 'de4', 'de5', 'de6', ...
+                        'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'};
+        G_de(i+1) = {G({'de1', 'de2', 'de3', 'de4', 'de5', 'de6'},:)};
+        G_Vs(i+1) = {G({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'},:)};
+    end
 #+end_src
 
 #+begin_src matlab :exports none :tangle no
-% Save the identified plants
-save('matlab/mat/test_nhexa_simscape_flexible_masses.mat', 'G_Vs', 'G_de')
+    % Save the identified plants
+    save('matlab/mat/test_nhexa_simscape_masses.mat', 'G_Vs', 'G_de')
 #+end_src
 
 #+begin_src matlab :eval no
-% Save the identified plants
-save('./mat/test_nhexa_simscape_flexible_masses.mat', 'G_Vs', 'G_de')
+    % Save the identified plants
+    save('./mat/test_nhexa_simscape_masses.mat', 'G_Vs', 'G_de')
+#+end_src
+
+#+begin_src matlab
+    %% The same identification is performed, but this time with
+    % "flexible" model of the APA
+    table_type = 'Suspended'; % On top of vibration table
+    device_type = 'Hexapod'; % Nano-Hexapod
+    payload_num = 0; % No Payload
+
+    n_hexapod = initializeNanoHexapod('flex_bot_type', '4dof', ...
+                                      'flex_top_type', '4dof', ...
+                                      'motion_sensor_type', 'plates', ...
+                                      'actuator_type', 'flexible');
+
+    G_de = {};
+    G_Vs = {};
+
+    for i = [0:3]
+        payload_num = i; % Change the payload on the nano-hexapod
+        G = exp(-s*1e-4)*linearize(mdl, io, 0.0);
+        G.InputName  = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
+        G.OutputName = {'de1', 'de2', 'de3', 'de4', 'de5', 'de6', ...
+                        'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'};
+        G_de(i+1) = {G({'de1', 'de2', 'de3', 'de4', 'de5', 'de6'},:)};
+        G_Vs(i+1) = {G({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'},:)};
+    end
+#+end_src
+
+#+begin_src matlab :exports none :tangle no
+    % Save the identified plants
+    save('matlab/mat/test_nhexa_simscape_flexible_masses.mat', 'G_Vs', 'G_de')
+#+end_src
+
+#+begin_src matlab :eval no
+    % Save the identified plants
+    save('./mat/test_nhexa_simscape_flexible_masses.mat', 'G_Vs', 'G_de')
+#+end_src
+
+#+begin_src matlab :eval no
+end
 #+end_src
 
 ** Nano-Hexapod model dynamics
@@ -1773,7 +1784,7 @@ device_type = 'None'; % On top of vibration table
 payload_num = 0; % No Payload
 
 % Simulink Model name
-mdl = 'test_bench_nano_hexapod';
+mdl = 'test_nhexa_simscape';
 #+end_src
 
 ** Initialize other elements
@@ -1786,6 +1797,394 @@ colors = colororder;
 freqs = logspace(log10(10), log10(2e3), 1000);
 #+END_SRC
 
+** =initializeNanoHexapod=
+#+begin_src matlab :tangle matlab/src/initializeNanoHexapod.m :comments none :mkdirp yes :eval no
+function [nano_hexapod] = initializeNanoHexapod(args)
+
+arguments
+    %% Bottom Flexible Joints
+    args.flex_bot_type      char {mustBeMember(args.flex_bot_type,{'2dof', '3dof', '4dof', 'flexible'})} = '4dof'
+    args.flex_bot_kRx (6,1) double {mustBeNumeric} = ones(6,1)*5 % X bending stiffness [Nm/rad]
+    args.flex_bot_kRy (6,1) double {mustBeNumeric} = ones(6,1)*5 % Y bending stiffness [Nm/rad]
+    args.flex_bot_kRz (6,1) double {mustBeNumeric} = ones(6,1)*260 % Torsionnal stiffness [Nm/rad]
+    args.flex_bot_kz  (6,1) double {mustBeNumeric} = ones(6,1)*7e7 % Axial Stiffness [N/m]
+    args.flex_bot_cRx (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % X bending Damping [Nm/(rad/s)]
+    args.flex_bot_cRy (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Y bending Damping [Nm/(rad/s)]
+    args.flex_bot_cRz (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Torsionnal Damping [Nm/(rad/s)]
+    args.flex_bot_cz  (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Axial Damping [N/(m/s)]
+
+    %% Top Flexible Joints
+    args.flex_top_type      char {mustBeMember(args.flex_top_type,{'2dof', '3dof', '4dof', 'flexible'})} = '4dof'
+    args.flex_top_kRx (6,1) double {mustBeNumeric} = ones(6,1)*5 % X bending stiffness [Nm/rad]
+    args.flex_top_kRy (6,1) double {mustBeNumeric} = ones(6,1)*5 % Y bending stiffness [Nm/rad]
+    args.flex_top_kRz (6,1) double {mustBeNumeric} = ones(6,1)*260 % Torsionnal stiffness [Nm/rad]
+    args.flex_top_kz  (6,1) double {mustBeNumeric} = ones(6,1)*7e7 % Axial Stiffness [N/m]
+    args.flex_top_cRx (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % X bending Damping [Nm/(rad/s)]
+    args.flex_top_cRy (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Y bending Damping [Nm/(rad/s)]
+    args.flex_top_cRz (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Torsionnal Damping [Nm/(rad/s)]
+    args.flex_top_cz  (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Axial Damping [N/(m/s)]
+
+    %% Jacobian - Location of frame {A} and {B}
+    args.MO_B (1,1) double {mustBeNumeric} = 150e-3 % Height of {B} w.r.t. {M} [m]
+
+    %% Relative Motion Sensor
+    args.motion_sensor_type char {mustBeMember(args.motion_sensor_type,{'struts', 'plates'})} = 'struts'
+
+    %% Top Plate
+    args.top_plate_type char {mustBeMember(args.top_plate_type,{'rigid', 'flexible'})} = 'rigid'
+    args.top_plate_xi  (1,1) double {mustBeNumeric} = 0.01 % Damping Ratio
+
+    %% Actuators
+    args.actuator_type char {mustBeMember(args.actuator_type,{'2dof', 'flexible frame', 'flexible'})} = 'flexible'
+    args.actuator_Ga  (6,1) double {mustBeNumeric} = zeros(6,1) % Actuator gain [N/V]
+    args.actuator_Gs  (6,1) double {mustBeNumeric} = zeros(6,1) % Sensor gain [V/m]
+    % For 2DoF
+    args.actuator_k   (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*380000
+    args.actuator_ke  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*4952605
+    args.actuator_ka  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*2476302
+    args.actuator_c   (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*5
+    args.actuator_ce  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*100
+    args.actuator_ca  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*50
+    args.actuator_Leq (6,1) double {mustBeNumeric} = ones(6,1)*0.056 % [m]
+    % For Flexible Frame
+    args.actuator_ks (6,1) double {mustBeNumeric} = ones(6,1)*235e6 % Stiffness of one stack [N/m]
+    args.actuator_cs (6,1) double {mustBeNumeric} = ones(6,1)*1e1 % Stiffness of one stack [N/m]
+    % Misalignment
+    args.actuator_d_align (6,3) double {mustBeNumeric} = zeros(6,3) % [m]
+
+    args.actuator_xi  (1,1) double {mustBeNumeric} = 0.01 % Damping Ratio
+
+    %% Controller
+    args.controller_type char {mustBeMember(args.controller_type,{'none', 'iff', 'dvf', 'hac-iff-struts'})} = 'none'
+end
+
+nano_hexapod = struct();
+
+nano_hexapod.flex_bot = struct();
+
+switch args.flex_bot_type
+  case '2dof'
+    nano_hexapod.flex_bot.type = 1;
+  case '3dof'
+    nano_hexapod.flex_bot.type = 2;
+  case '4dof'
+    nano_hexapod.flex_bot.type = 3;
+  case 'flexible'
+    nano_hexapod.flex_bot.type = 4;
+end
+
+nano_hexapod.flex_bot.kRx = args.flex_bot_kRx; % X bending stiffness [Nm/rad]
+nano_hexapod.flex_bot.kRy = args.flex_bot_kRy; % Y bending stiffness [Nm/rad]
+nano_hexapod.flex_bot.kRz = args.flex_bot_kRz; % Torsionnal stiffness [Nm/rad]
+nano_hexapod.flex_bot.kz  = args.flex_bot_kz;  % Axial stiffness [N/m]
+
+nano_hexapod.flex_bot.cRx = args.flex_bot_cRx; % [Nm/(rad/s)]
+nano_hexapod.flex_bot.cRy = args.flex_bot_cRy; % [Nm/(rad/s)]
+nano_hexapod.flex_bot.cRz = args.flex_bot_cRz; % [Nm/(rad/s)]
+nano_hexapod.flex_bot.cz  = args.flex_bot_cz;  %[N/(m/s)]
+
+nano_hexapod.flex_top = struct();
+
+switch args.flex_top_type
+  case '2dof'
+    nano_hexapod.flex_top.type = 1;
+  case '3dof'
+    nano_hexapod.flex_top.type = 2;
+  case '4dof'
+    nano_hexapod.flex_top.type = 3;
+  case 'flexible'
+    nano_hexapod.flex_top.type = 4;
+end
+
+nano_hexapod.flex_top.kRx = args.flex_top_kRx; % X bending stiffness [Nm/rad]
+nano_hexapod.flex_top.kRy = args.flex_top_kRy; % Y bending stiffness [Nm/rad]
+nano_hexapod.flex_top.kRz = args.flex_top_kRz; % Torsionnal stiffness [Nm/rad]
+nano_hexapod.flex_top.kz  = args.flex_top_kz;  % Axial stiffness [N/m]
+
+nano_hexapod.flex_top.cRx = args.flex_top_cRx; % [Nm/(rad/s)]
+nano_hexapod.flex_top.cRy = args.flex_top_cRy; % [Nm/(rad/s)]
+nano_hexapod.flex_top.cRz = args.flex_top_cRz; % [Nm/(rad/s)]
+nano_hexapod.flex_top.cz  = args.flex_top_cz;  %[N/(m/s)]
+
+nano_hexapod.motion_sensor = struct();
+
+switch args.motion_sensor_type
+  case 'struts'
+    nano_hexapod.motion_sensor.type = 1;
+  case 'plates'
+    nano_hexapod.motion_sensor.type = 2;
+end
+
+nano_hexapod.actuator = struct();
+
+switch args.actuator_type
+  case '2dof'
+    nano_hexapod.actuator.type = 1;
+  case 'flexible frame'
+    nano_hexapod.actuator.type = 2;
+  case 'flexible'
+    nano_hexapod.actuator.type = 3;
+end
+
+%% Actuator gain [N/V]
+if all(args.actuator_Ga == 0)
+    switch args.actuator_type
+      case '2dof'
+        nano_hexapod.actuator.Ga = ones(6,1)*(-2.5796);
+      case 'flexible frame'
+        nano_hexapod.actuator.Ga = ones(6,1); % TODO
+      case 'flexible'
+        nano_hexapod.actuator.Ga = ones(6,1)*23.2;
+    end
+else
+    nano_hexapod.actuator.Ga = args.actuator_Ga; % Actuator gain [N/V]
+end
+
+%% Sensor gain [V/m]
+if all(args.actuator_Gs == 0)
+    switch args.actuator_type
+      case '2dof'
+        nano_hexapod.actuator.Gs = ones(6,1)*466664;
+      case 'flexible frame'
+        nano_hexapod.actuator.Gs = ones(6,1); % TODO
+      case 'flexible'
+        nano_hexapod.actuator.Gs = ones(6,1)*(-4898341);
+    end
+else
+    nano_hexapod.actuator.Gs = args.actuator_Gs; % Sensor gain [V/m]
+end
+
+switch args.actuator_type
+  case '2dof'
+    nano_hexapod.actuator.k  = args.actuator_k;  % [N/m]
+    nano_hexapod.actuator.ke = args.actuator_ke; % [N/m]
+    nano_hexapod.actuator.ka = args.actuator_ka; % [N/m]
+
+    nano_hexapod.actuator.c  = args.actuator_c;  % [N/(m/s)]
+    nano_hexapod.actuator.ce = args.actuator_ce; % [N/(m/s)]
+    nano_hexapod.actuator.ca = args.actuator_ca; % [N/(m/s)]
+
+    nano_hexapod.actuator.Leq = args.actuator_Leq; % [m]
+
+  case 'flexible frame'
+    nano_hexapod.actuator.K = readmatrix('APA300ML_b_mat_K.CSV'); % Stiffness Matrix
+    nano_hexapod.actuator.M = readmatrix('APA300ML_b_mat_M.CSV'); % Mass Matrix
+    nano_hexapod.actuator.P = extractNodes('APA300ML_b_out_nodes_3D.txt'); % Node coordinates [m]
+
+    nano_hexapod.actuator.ks = args.actuator_ks; % Stiffness of one stack [N/m]
+    nano_hexapod.actuator.cs = args.actuator_cs; % Damping of one stack [N/m]
+    nano_hexapod.actuator.xi = args.actuator_xi; % Damping ratio
+
+  case 'flexible'
+    nano_hexapod.actuator.K = readmatrix('full_APA300ML_K.CSV'); % Stiffness Matrix
+    nano_hexapod.actuator.M = readmatrix('full_APA300ML_M.CSV'); % Mass Matrix
+    nano_hexapod.actuator.P = extractNodes('full_APA300ML_out_nodes_3D.txt'); % Node coordiantes [m]
+
+    nano_hexapod.actuator.d_align = args.actuator_d_align; % Misalignment
+    nano_hexapod.actuator.xi = args.actuator_xi; % Damping ratio
+
+end
+
+nano_hexapod.geometry = struct();
+
+Fa = [[-86.05,  -74.78, 22.49],
+      [ 86.05,  -74.78, 22.49],
+      [ 107.79, -37.13, 22.49],
+      [ 21.74,  111.91, 22.49],
+      [-21.74,  111.91, 22.49],
+      [-107.79, -37.13, 22.49]]'*1e-3; % Ai w.r.t. {F} [m]
+
+Mb = [[-28.47, -106.25, -22.50],
+      [ 28.47, -106.25, -22.50],
+      [ 106.25, 28.47,  -22.50],
+      [ 77.78,  77.78,  -22.50],
+      [-77.78,  77.78,  -22.50],
+      [-106.25, 28.47,  -22.50]]'*1e-3; % Bi w.r.t. {M} [m]
+
+Fb = Mb + [0; 0; 95e-3]; % Bi w.r.t. {F} [m]
+
+si = Fb - Fa;
+si = si./vecnorm(si); % Normalize
+
+Fc = [[-29.362, -105.765, 52.605]
+      [ 29.362, -105.765, 52.605]
+      [ 106.276, 27.454,  52.605]
+      [ 76.914,  78.31,   52.605]
+      [-76.914,  78.31,   52.605]
+      [-106.276, 27.454,  52.605]]'*1e-3; % Meas pos w.r.t. {F}
+Mc = Fc - [0; 0; 95e-3]; % Meas pos w.r.t. {M}
+
+nano_hexapod.geometry.Fa = Fa;
+nano_hexapod.geometry.Fb = Fb;
+nano_hexapod.geometry.Fc = Fc;
+nano_hexapod.geometry.Mb = Mb;
+nano_hexapod.geometry.Mc = Mc;
+nano_hexapod.geometry.si = si;
+nano_hexapod.geometry.MO_B = args.MO_B;
+
+Bb = Mb - [0; 0; args.MO_B];
+
+nano_hexapod.geometry.J = [nano_hexapod.geometry.si', cross(Bb, nano_hexapod.geometry.si)'];
+
+switch args.motion_sensor_type
+  case 'struts'
+    nano_hexapod.geometry.Js = nano_hexapod.geometry.J;
+  case 'plates'
+    Bc = Mc - [0; 0; args.MO_B];
+    nano_hexapod.geometry.Js = [nano_hexapod.geometry.si', cross(Bc, nano_hexapod.geometry.si)'];
+end
+
+nano_hexapod.top_plate = struct();
+
+switch args.top_plate_type
+  case 'rigid'
+    nano_hexapod.top_plate.type = 1;
+  case 'flexible'
+    nano_hexapod.top_plate.type = 2;
+
+    nano_hexapod.top_plate.R_flex = ...
+       {[ 0.53191886726305  0.4795690716524  0.69790817745892
+         -0.29070157897799  0.8775041341865 -0.38141720787774
+         -0.79533320729697  0                0.60617249143351 ],
+        [ 0.53191886726305 -0.4795690716524 -0.69790817745892
+          0.29070157897799  0.8775041341865 -0.38141720787774
+          0.79533320729697  0                0.60617249143351 ],
+        [-0.01420448131633 -0.9997254079576 -0.01863709726680
+          0.60600604129104 -0.0234330681729  0.79511481512719
+         -0.79533320729697  0                0.60617249143351 ],
+        [-0.51771438594672 -0.5201563363051  0.67927108019212
+          0.31530446231304 -0.8540710660135 -0.41369760724945
+          0.79533320729697  0                0.60617249143351 ],
+        [-0.51771438594671  0.5201563363052 -0.67927108019211
+         -0.31530446231304 -0.8540710660135 -0.41369760724945
+         -0.79533320729697  0                0.60617249143351 ],
+        [-0.01420448131632  0.9997254079576  0.01863709726679
+         -0.60600604129104 -0.0234330681729  0.79511481512719
+          0.79533320729697  0                0.60617249143351 ] };
+
+
+    nano_hexapod.top_plate.R_enc = ...
+      { [-0.877504134186525 -0.479569071652412 0
+         0.479569071652412 -0.877504134186525 0
+                         0                  0 1 ],
+        [ 0.877504134186525 -0.479569071652413 0
+          0.479569071652413  0.877504134186525 0
+                          0                  0 1 ],
+        [ 0.023433068172945  0.999725407957606 0
+         -0.999725407957606  0.023433068172945 0
+                          0                  0 1 ],
+        [-0.854071066013566 -0.520156336305202 0
+          0.520156336305202 -0.854071066013566 0
+                          0                  0 1 ],
+        [ 0.854071066013574 -0.520156336305191 0
+          0.520156336305191  0.854071066013574 0
+                          0                  0 1 ],
+        [-0.023433068172958  0.999725407957606 0
+         -0.999725407957606 -0.023433068172958 0
+                          0                  0 1 ] };
+
+    nano_hexapod.top_plate.K = readmatrix('top_plate_K_6.CSV'); % Stiffness Matrix
+    nano_hexapod.top_plate.M = readmatrix('top_plate_M_6.CSV'); % Mass Matrix
+    nano_hexapod.top_plate.P = extractNodes('top_plate_out_nodes_3D_qua.txt'); % Node coordiantes [m]
+    nano_hexapod.top_plate.xi = args.top_plate_xi; % Damping ratio
+
+end
+#+end_src
+
+** =extractNodes=
+#+begin_src matlab :tangle matlab/src/extractNodes.m :comments none :mkdirp yes :eval no
+function [int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes(filename)
+% extractNodes -
+%
+% Syntax: [n_xyz, nodes] = extractNodes(filename)
+%
+% Inputs:
+%    - filename - relative or absolute path of the file that contains the Matrix
+%
+% Outputs:
+%    - n_xyz -
+%    - nodes - table containing the node numbers and corresponding dof of the interfaced DoFs
+
+arguments
+    filename
+end
+
+fid = fopen(filename,'rt');
+
+if fid == -1
+    error('Error opening the file');
+end
+
+n_xyz = []; % Contains nodes coordinates
+n_i = []; % Contains nodes indices
+
+n_num = []; % Contains node numbers
+n_dof = {}; % Contains node directions
+
+while 1
+    % Read a line
+    nextline = fgetl(fid);
+
+    % End of the file
+    if ~isstr(nextline), break, end
+
+    % Line just before the list of nodes coordinates
+    if contains(nextline, 'NODE') && ...
+            contains(nextline, 'X') && ...
+            contains(nextline, 'Y') && ...
+            contains(nextline, 'Z')
+
+        while 1
+            nextline = fgetl(fid);
+
+            if nextline < 0, break, end
+
+            c = sscanf(nextline, ' %f');
+
+            if isempty(c), break, end
+
+            n_xyz = [n_xyz; c(2:4)'];
+            n_i = [n_i; c(1)];
+        end
+    end
+
+    if nextline < 0, break, end
+
+    % Line just before the list of node DOF
+    if contains(nextline, 'NODE  LABEL')
+
+        while 1
+            nextline = fgetl(fid);
+
+            if nextline < 0, break, end
+
+            c = sscanf(nextline, ' %d %s');
+
+            if isempty(c), break, end
+
+            n_num = [n_num; c(1)];
+
+            n_dof{length(n_dof)+1} = char(c(2:end)');
+        end
+
+        nodes = table(n_num, string(n_dof'), 'VariableNames', {'node_i', 'node_dof'});
+    end
+
+    if nextline < 0, break, end
+end
+
+fclose(fid);
+
+int_i = unique(nodes.('node_i')); % indices of interface nodes
+
+% Extract XYZ coordinates of only the interface nodes
+if length(n_xyz) > 0 && length(n_i) > 0
+    int_xyz = n_xyz(logical(sum(n_i.*ones(1, length(int_i)) == int_i', 2)), :);
+else
+    int_xyz = n_xyz;
+end
+#+end_src
+
 * Footnotes
 
 [fn:7]PCB 356B18. Sensitivity is $1\,V/g$, measurement range is $\pm 5\,g$ and bandwidth is $0.5$ to $5\,\text{kHz}$.