add all files

A4-simscape-micro-station/src/circlefit.m

@@ -0,0 +1,21 @@
+function   [xc,yc,R,a] = circlefit(x,y)
+%
+%   [xc yx R] = circfit(x,y)
+%
+%   fits a circle  in x,y plane in a more accurate
+%   (less prone to ill condition )
+%  procedure than circfit2 but using more memory
+%  x,y are column vector where (x(i),y(i)) is a measured point
+%
+%  result is center point (yc,xc) and radius R
+%  an optional output is the vector of coeficient a
+% describing the circle's equation
+%
+%   x^2+y^2+a(1)*x+a(2)*y+a(3)=0
+%
+%  By:  Izhak bucher 25/oct /1991,
+    x=x(:); y=y(:);
+    a=[x y ones(size(x))]\[-(x.^2+y.^2)];
+    xc = -.5*a(1);
+    yc = -.5*a(2);
+    R  =  sqrt((a(1)^2+a(2)^2)/4-a(3));

A4-simscape-micro-station/src/computeJacobian.m

@@ -0,0 +1,35 @@
+  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;

A4-simscape-micro-station/src/computeJointsPose.m

@@ -0,0 +1,78 @@
+  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;

A4-simscape-micro-station/src/computeReferencePose.m

@@ -0,0 +1,77 @@
+  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

A4-simscape-micro-station/src/describeMicroStationSetup.m

@@ -0,0 +1,141 @@
+  function [] = describeMicroStationSetup()
+  % describeMicroStationSetup -
+  %
+  % Syntax: [] = describeMicroStationSetup()
+  %
+  % Inputs:
+  %    -  -
+  %
+  % Outputs:
+  %    -  -
+
+  load('./mat/nass_model_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_model_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.Fdy_x && args.Fdy_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_model_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/nass_model_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');

A4-simscape-micro-station/src/generateGeneralConfiguration.m

@@ -0,0 +1,39 @@
+  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;

A4-simscape-micro-station/src/initializeCylindricalPlatforms.m

@@ -0,0 +1,59 @@
+  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;

A4-simscape-micro-station/src/initializeCylindricalStruts.m

@@ -0,0 +1,71 @@
+  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;

A4-simscape-micro-station/src/initializeDisturbances.m

@@ -0,0 +1,211 @@
+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.Dw_x logical {mustBeNumericOrLogical} = true
+    % Ground Motion - Y direction
+    args.Dw_y logical {mustBeNumericOrLogical} = true
+    % Ground Motion - Z direction
+    args.Dw_z logical {mustBeNumericOrLogical} = true
+    % Translation Stage - X direction
+    args.Fdy_x logical {mustBeNumericOrLogical} = true
+    % Translation Stage - Z direction
+    args.Fdy_z logical {mustBeNumericOrLogical} = true
+    % Spindle - X direction
+    args.Frz_x logical {mustBeNumericOrLogical} = true
+    % Spindle - Y direction
+    args.Frz_y logical {mustBeNumericOrLogical} = true
+    % Spindle - Z direction
+    args.Frz_z logical {mustBeNumericOrLogical} = true
+end
+
+% Initialization of random numbers
+rng("shuffle");
+
+%% Ground Motion
+if args.enable
+    % Load the PSD of disturbance
+    load('ustation_disturbance_psd.mat', 'gm_dist')
+
+    % Frequency Data
+    Dw.f = gm_dist.f;
+    Dw.psd_x = gm_dist.pxx_x;
+    Dw.psd_y = gm_dist.pxx_y;
+    Dw.psd_z = gm_dist.pxx_z;
+
+    % Time data
+    Fs = 2*Dw.f(end);      % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
+    N  = 2*length(Dw.f);   % Number of Samples match the one of the wanted PSD
+    T0 = N/Fs;             % Signal Duration [s]
+    Dw.t = linspace(0, T0, N+1)'; % Time Vector [s]
+
+    % ASD representation of the ground motion
+    C = zeros(N/2,1);
+    for i = 1:N/2
+        C(i) = sqrt(Dw.psd_x(i)/T0);
+    end
+
+    if args.Dw_x
+        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)))];;
+        Dw.x = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m]
+    else
+        Dw.x = zeros(length(Dw.t), 1);
+    end
+
+    if args.Dw_y
+        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)))];;
+        Dw.y = N/sqrt(2)*ifft(Cx); % Ground Motion - y direction [m]
+    else
+        Dw.y = zeros(length(Dw.t), 1);
+    end
+
+    if args.Dw_y
+        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)))];;
+        Dw.z = N/sqrt(2)*ifft(Cx); % Ground Motion - z direction [m]
+    else
+        Dw.z = zeros(length(Dw.t), 1);
+    end
+
+else
+    Dw.t = [0,1]; % Time Vector [s]
+    Dw.x = [0,0]; % Ground Motion - X [m]
+    Dw.y = [0,0]; % Ground Motion - Y [m]
+    Dw.z = [0,0]; % Ground Motion - Z [m]
+end
+
+%% Translation stage
+if args.enable
+    % Load the PSD of disturbance
+    load('ustation_disturbance_psd.mat', 'dy_dist')
+
+    % Frequency Data
+    Dy.f = dy_dist.f;
+    Dy.psd_x = dy_dist.pxx_fx;
+    Dy.psd_z = dy_dist.pxx_fz;
+
+    % Time data
+    Fs = 2*Dy.f(end);      % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
+    N  = 2*length(Dy.f);   % Number of Samples match the one of the wanted PSD
+    T0 = N/Fs;             % Signal Duration [s]
+    Dy.t = linspace(0, T0, N+1)'; % Time Vector [s]
+
+    % ASD representation of the disturbance voice
+    C = zeros(N/2,1);
+    for i = 1:N/2
+        C(i) = sqrt(Dy.psd_x(i)/T0);
+    end
+
+    if args.Fdy_x
+        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)))];;
+        Dy.x = N/sqrt(2)*ifft(Cx); % Translation stage disturbances - X direction [N]
+    else
+        Dy.x = zeros(length(Dy.t), 1);
+    end
+
+    if args.Fdy_z
+        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)))];;
+        Dy.z = N/sqrt(2)*ifft(Cx); % Translation stage disturbances - Z direction [N]
+    else
+        Dy.z = zeros(length(Dy.t), 1);
+    end
+
+else
+    Dy.t = [0,1]; % Time Vector [s]
+    Dy.x = [0,0]; % Translation Stage disturbances - X [N]
+    Dy.z = [0,0]; % Translation Stage disturbances - Z [N]
+end
+
+%% Spindle
+if args.enable
+    % Load the PSD of disturbance
+    load('ustation_disturbance_psd.mat', 'rz_dist')
+
+    % Frequency Data
+    Rz.f = rz_dist.f;
+    Rz.psd_x = rz_dist.pxx_fx;
+    Rz.psd_y = rz_dist.pxx_fy;
+    Rz.psd_z = rz_dist.pxx_fz;
+
+    % Time data
+    Fs = 2*Rz.f(end);      % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
+    N  = 2*length(Rz.f);   % Number of Samples match the one of the wanted PSD
+    T0 = N/Fs;             % Signal Duration [s]
+    Rz.t = linspace(0, T0, N+1)'; % Time Vector [s]
+
+    % ASD representation of the disturbance voice
+    C = zeros(N/2,1);
+    for i = 1:N/2
+        C(i) = sqrt(Rz.psd_x(i)/T0);
+    end
+
+    if args.Frz_x
+        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)))];;
+        Rz.x = N/sqrt(2)*ifft(Cx); % spindle disturbances - X direction [N]
+    else
+        Rz.x = zeros(length(Rz.t), 1);
+    end
+
+    if args.Frz_y
+        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)))];;
+        Rz.y = N/sqrt(2)*ifft(Cx); % spindle disturbances - Y direction [N]
+    else
+        Rz.y = zeros(length(Rz.t), 1);
+    end
+
+    if args.Frz_z
+        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)))];;
+        Rz.z = N/sqrt(2)*ifft(Cx); % spindle disturbances - Z direction [N]
+    else
+        Rz.z = zeros(length(Rz.t), 1);
+    end
+
+else
+    Rz.t = [0,1]; % Time Vector [s]
+    Rz.x = [0,0]; % Spindle disturbances - X [N]
+    Rz.y = [0,0]; % Spindle disturbances - X [N]
+    Rz.z = [0,0]; % Spindle disturbances - Z [N]
+end
+
+u = zeros(100, 6);
+Fd = u;
+
+Dw.x   = Dw.x   - Dw.x(1);
+Dw.y   = Dw.y   - Dw.y(1);
+Dw.z   = Dw.z   - Dw.z(1);
+
+Dy.x   = Dy.x   - Dy.x(1);
+Dy.z   = Dy.z   - Dy.z(1);
+
+Rz.x   = Rz.x   - Rz.x(1);
+Rz.y   = Rz.y   - Rz.y(1);
+Rz.z   = Rz.z   - Rz.z(1);
+
+if exist('./mat', 'dir')
+    save('mat/nass_model_disturbances.mat', 'Dw', 'Dy', 'Rz', 'Fd', 'args');
+elseif exist('./matlab', 'dir')
+    save('matlab/mat/nass_model_disturbances.mat', 'Dw', 'Dy', 'Rz', 'Fd', 'args');
+end

A4-simscape-micro-station/src/initializeFramesPositions.m

@@ -0,0 +1,35 @@
+  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;

A4-simscape-micro-station/src/initializeGranite.m

@@ -0,0 +1,48 @@
+function [granite] = initializeGranite(args)
+
+    arguments
+        args.type          char    {mustBeMember(args.type,{'rigid', 'flexible', 'none'})} = 'flexible'
+        args.density (1,1) double {mustBeNumeric, mustBeNonnegative} = 2800 % Density [kg/m3]
+        args.K  (6,1) double {mustBeNumeric, mustBeNonnegative} = [5e9;   5e9;   5e9;   2.5e7; 2.5e7; 1e7] % [N/m]
+        args.C  (6,1) double {mustBeNumeric, mustBeNonnegative} = [4.0e5; 1.1e5; 9.0e5; 2e4;   2e4; 1e4] % [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.775 % 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;
+    end
+
+    granite.density = args.density; % [kg/m3]
+    granite.STEP    = 'granite.STEP';
+
+    % Z-offset for the initial position of the sample with respect to the granite top surface.
+    granite.sample_pos = args.sample_pos; % [m]
+
+    granite.K = args.K; % [N/m]
+    granite.C = args.C; % [N/(m/s)]
+
+    if exist('./mat', 'dir')
+        if exist('./mat/nass_model_stages.mat', 'file')
+            save('mat/nass_model_stages.mat', 'granite', '-append');
+        else
+            save('mat/nass_model_stages.mat', 'granite');
+        end
+    elseif exist('./matlab', 'dir')
+        if exist('./matlab/mat/nass_model_stages.mat', 'file')
+            save('matlab/mat/nass_model_stages.mat', 'granite', '-append');
+        else
+            save('matlab/mat/nass_model_stages.mat', 'granite');
+        end
+    end
+
+end

A4-simscape-micro-station/src/initializeGround.m

@@ -0,0 +1,35 @@
+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;
+
+    if exist('./mat', 'dir')
+        if exist('./mat/nass_model_stages.mat', 'file')
+            save('mat/nass_model_stages.mat', 'ground', '-append');
+        else
+            save('mat/nass_model_stages.mat', 'ground');
+        end
+    elseif exist('./matlab', 'dir')
+        if exist('./matlab/mat/nass_model_stages.mat', 'file')
+            save('matlab/mat/nass_model_stages.mat', 'ground', '-append');
+        else
+            save('matlab/mat/nass_model_stages.mat', 'ground');
+        end
+    end
+end

A4-simscape-micro-station/src/initializeInertialSensor.m

@@ -0,0 +1,48 @@
+  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;

A4-simscape-micro-station/src/initializeJointDynamics.m

@@ -0,0 +1,131 @@
+  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;

A4-simscape-micro-station/src/initializeLoggingConfiguration.m

@@ -0,0 +1,33 @@
+  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;
+
+if exist('./mat', 'dir')
+    if exist('./mat/nass_model_conf_log.mat', 'file')
+        save('mat/nass_model_conf_log.mat', 'conf_log', '-append');
+    else
+        save('mat/nass_model_conf_log.mat', 'conf_log');
+    end
+elseif exist('./matlab', 'dir')
+    if exist('./matlab/mat/nass_model_conf_log.mat', 'file')
+        save('matlab/mat/nass_model_conf_log.mat', 'conf_log', '-append');
+    else
+        save('matlab/mat/nass_model_conf_log.mat', 'conf_log');
+    end
+end

A4-simscape-micro-station/src/initializeMicroHexapod.m

@@ -0,0 +1,108 @@
+function [micro_hexapod] = initializeMicroHexapod(args)
+
+    arguments
+        args.type      char   {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = '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)
+    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');
+
+    switch args.type
+      case 'none'
+        stewart.type = 0;
+      case 'rigid'
+        stewart.type = 1;
+      case 'flexible'
+        stewart.type = 2;
+    end
+
+    micro_hexapod = stewart;
+    if exist('./mat', 'dir')
+        if exist('./mat/nass_model_stages.mat', 'file')
+            save('mat/nass_model_stages.mat', 'micro_hexapod', '-append');
+        else
+            save('mat/nass_model_stages.mat', 'micro_hexapod');
+        end
+    elseif exist('./matlab', 'dir')
+        if exist('./matlab/mat/nass_model_stages.mat', 'file')
+            save('matlab/mat/nass_model_stages.mat', 'micro_hexapod', '-append');
+        else
+            save('matlab/mat/nass_model_stages.mat', 'micro_hexapod');
+        end
+    end
+end

A4-simscape-micro-station/src/initializeReferences.m

@@ -0,0 +1,199 @@
+  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('nass_model_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));
+
+if exist('./mat', 'dir')
+    if exist('./mat/nass_model_references.mat', 'file')
+        save('mat/nass_model_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts', '-append');
+    else
+        save('mat/nass_model_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts');
+    end
+elseif exist('./matlab', 'dir')
+    if exist('./matlab/mat/nass_model_references.mat', 'file')
+        save('matlab/mat/nass_model_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts', '-append');
+    else
+        save('matlab/mat/nass_model_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts');
+    end
+end

A4-simscape-micro-station/src/initializeRy.m

@@ -0,0 +1,57 @@
+function [ry] = initializeRy(args)
+
+    arguments
+        args.type          char    {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
+        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;
+    end
+
+    % Ry - Guide for the tilt stage
+    ry.guide.density = 7800; % [kg/m3]
+    ry.guide.STEP    = 'Tilt_Guide.STEP';
+
+    % Ry - Rotor of the motor
+    ry.rotor.density = 2400; % [kg/m3]
+    ry.rotor.STEP    = 'Tilt_Motor_Axis.STEP';
+
+    % Ry - Motor
+    ry.motor.density = 3200; % [kg/m3]
+    ry.motor.STEP    = 'Tilt_Motor.STEP';
+
+    % Ry - Plateau Tilt
+    ry.stage.density = 7800; % [kg/m3]
+    ry.stage.STEP    = 'Tilt_Stage.STEP';
+
+    % Z-Offset so that the center of rotation matches the sample center;
+    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 exist('./mat', 'dir')
+        if exist('./mat/nass_model_stages.mat', 'file')
+            save('mat/nass_model_stages.mat', 'ry', '-append');
+        else
+            save('mat/nass_model_stages.mat', 'ry');
+        end
+    elseif exist('./matlab', 'dir')
+        if exist('./matlab/mat/nass_model_stages.mat', 'file')
+            save('matlab/mat/nass_model_stages.mat', 'ry', '-append');
+        else
+            save('matlab/mat/nass_model_stages.mat', 'ry');
+        end
+    end
+
+end

A4-simscape-micro-station/src/initializeRz.m

@@ -0,0 +1,47 @@
+function [rz] = initializeRz(args)
+
+    arguments
+        args.type    char    {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
+    end
+
+    rz = struct();
+
+    switch args.type
+      case 'none'
+        rz.type = 0;
+      case 'rigid'
+        rz.type = 1;
+      case 'flexible'
+        rz.type = 2;
+    end
+
+    % Spindle - Slip Ring
+    rz.slipring.density = 7800; % [kg/m3]
+    rz.slipring.STEP    = 'Spindle_Slip_Ring.STEP';
+
+    % Spindle - Rotor
+    rz.rotor.density    = 7800; % [kg/m3]
+    rz.rotor.STEP       = 'Spindle_Rotor.STEP';
+
+    % Spindle - Stator
+    rz.stator.density   = 7800; % [kg/m3]
+    rz.stator.STEP      = 'Spindle_Stator.STEP';
+
+    rz.K = [7e8; 7e8; 2e9; 1e7; 1e7; 1e7];
+    rz.C = [4e4; 4e4; 7e4; 1e4; 1e4; 1e4];
+
+    if exist('./mat', 'dir')
+        if exist('./mat/nass_model_stages.mat', 'file')
+            save('mat/nass_model_stages.mat', 'rz', '-append');
+        else
+            save('mat/nass_model_stages.mat', 'rz');
+        end
+    elseif exist('./matlab', 'dir')
+        if exist('./matlab/mat/nass_model_stages.mat', 'file')
+            save('matlab/mat/nass_model_stages.mat', 'rz', '-append');
+        else
+            save('matlab/mat/nass_model_stages.mat', 'rz');
+        end
+    end
+
+end

A4-simscape-micro-station/src/initializeSimscapeConfiguration.m

@@ -0,0 +1,27 @@
+  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
+
+if exist('./mat', 'dir')
+    if exist('./mat/nass_model_conf_simscape.mat', 'file')
+        save('mat/nass_model_conf_simscape.mat', 'conf_simscape', '-append');
+    else
+        save('mat/nass_model_conf_simscape.mat', 'conf_simscape');
+    end
+elseif exist('./matlab', 'dir')
+    if exist('./matlab/mat/nass_model_conf_simscape.mat', 'file')
+        save('matlab/mat/nass_model_conf_simscape.mat', 'conf_simscape', '-append');
+    else
+        save('matlab/mat/nass_model_conf_simscape.mat', 'conf_simscape');
+    end
+end

A4-simscape-micro-station/src/initializeStewartPlatform.m

@@ -0,0 +1,31 @@
+  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();

A4-simscape-micro-station/src/initializeStewartPose.m

@@ -0,0 +1,27 @@
+  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;

A4-simscape-micro-station/src/initializeStrutDynamics.m

@@ -0,0 +1,49 @@
+  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;

A4-simscape-micro-station/src/initializeTy.m

@@ -0,0 +1,71 @@
+function [ty] = initializeTy(args)
+
+    arguments
+        args.type      char   {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
+    end
+
+    ty = struct();
+
+    switch args.type
+      case 'none'
+        ty.type = 0;
+      case 'rigid'
+        ty.type = 1;
+      case 'flexible'
+        ty.type = 2;
+    end
+
+    % Ty Granite frame
+    ty.granite_frame.density = 7800; % [kg/m3] => 43kg
+    ty.granite_frame.STEP    = 'Ty_Granite_Frame.STEP';
+
+    % Guide Translation Ty
+    ty.guide.density         = 7800; % [kg/m3] => 76kg
+    ty.guide.STEP            = 'Ty_Guide.STEP';
+
+    % Ty - Guide_Translation12
+    ty.guide12.density       = 7800; % [kg/m3]
+    ty.guide12.STEP          = 'Ty_Guide_12.STEP';
+
+    % Ty - Guide_Translation11
+    ty.guide11.density       = 7800; % [kg/m3]
+    ty.guide11.STEP          = 'Ty_Guide_11.STEP';
+
+    % Ty - Guide_Translation22
+    ty.guide22.density       = 7800; % [kg/m3]
+    ty.guide22.STEP          = 'Ty_Guide_22.STEP';
+
+    % Ty - Guide_Translation21
+    ty.guide21.density       = 7800; % [kg/m3]
+    ty.guide21.STEP          = 'Ty_Guide_21.STEP';
+
+    % Ty - Plateau translation
+    ty.frame.density         = 7800; % [kg/m3]
+    ty.frame.STEP            = 'Ty_Stage.STEP';
+
+    % Ty Stator Part
+    ty.stator.density        = 5400; % [kg/m3]
+    ty.stator.STEP           = 'Ty_Motor_Stator.STEP';
+
+    % Ty Rotor Part
+    ty.rotor.density         = 5400; % [kg/m3]
+    ty.rotor.STEP            = 'Ty_Motor_Rotor.STEP';
+
+    ty.K = [2e8; 1e8; 2e8; 6e7; 9e7; 6e7]; % [N/m, N*m/rad]
+    ty.C = [8e4; 5e4; 8e4; 2e4; 3e4; 1e4]; % [N/(m/s), N*m/(rad/s)]
+
+    if exist('./mat', 'dir')
+        if exist('./mat/nass_model_stages.mat', 'file')
+            save('mat/nass_model_stages.mat', 'ty', '-append');
+        else
+            save('mat/nass_model_stages.mat', 'ty');
+        end
+    elseif exist('./matlab', 'dir')
+        if exist('./matlab/mat/nass_model_stages.mat', 'file')
+            save('matlab/mat/nass_model_stages.mat', 'ty', '-append');
+        else
+            save('matlab/mat/nass_model_stages.mat', 'ty');
+        end
+    end
+
+end

A4-simscape-micro-station/src/initializeZAxisAccelerometer.m

@@ -0,0 +1,31 @@
+  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;
+
+if exist('./mat', 'dir')
+    if exist('./mat/accelerometer_z_axis.mat', 'file')
+        save('mat/accelerometer_z_axis.mat', 'accelerometer', '-append');
+    else
+        save('mat/accelerometer_z_axis.mat', 'accelerometer');
+    end
+elseif exist('./matlab', 'dir')
+    if exist('./matlab/mat/accelerometer_z_axis.mat', 'file')
+        save('matlab/mat/accelerometer_z_axis.mat', 'accelerometer', '-append');
+    else
+        save('matlab/mat/accelerometer_z_axis.mat', 'accelerometer');
+    end
+end

A4-simscape-micro-station/src/initializeZAxisGeophone.m

@@ -0,0 +1,28 @@
+  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);
+
+if exist('./mat', 'dir')
+    if exist('./mat/geophone_z_axis.mat', 'file')
+        save('mat/geophone_z_axis.mat', 'geophone', '-append');
+    else
+        save('mat/geophone_z_axis.mat', 'geophone');
+    end
+elseif exist('./matlab', 'dir')
+    if exist('./matlab/mat/geophone_z_axis.mat', 'file')
+        save('matlab/mat/geophone_z_axis.mat', 'geophone', '-append');
+    else
+        save('matlab/mat/geophone_z_axis.mat', 'geophone');
+    end
+end

A4-simscape-micro-station/src/inverseKinematics.m

@@ -0,0 +1,36 @@
+  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;

A4-simscape-micro-station/ustation_1_kinematics.m

@@ -0,0 +1,90 @@
+%% ustation_1_kinematics.m
+
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+%% Path for functions, data and scripts
+addpath('./mat/'); % Path for Data
+addpath('./src/'); % Path for functions
+addpath('./STEPS/'); % Path for STEPS
+addpath('./subsystems/'); % Path for Subsystems Simulink files
+
+% Simulink Model name
+mdl = 'ustation_simscape';
+
+load('nass_model_conf_simulink.mat');
+
+%% Colors for the figures
+colors = colororder;
+
+%% Frequency Vector
+freqs = logspace(log10(10), log10(2e3), 1000);
+
+%% Stage setpoints
+Dy = 1e-3; % Translation Stage [m]
+Ry = 3*pi/180; % Tilt Stage [rad]
+Rz = 180*pi/180; % Spindle [rad]
+
+%% Stage individual Homogeneous transformations
+% 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-Station homogeneous transformation
+Ttot = Rty*Rry*Rrz;
+
+%% Compute translations and rotations (Euler angles) induced by the micro-station
+ustation_dx = Ttot(1,4);
+ustation_dy = Ttot(2,4);
+ustation_dz = Ttot(3,4);
+ustation_ry = atan2( Ttot(1, 3),                  sqrt(Ttot(1, 1)^2 + Ttot(1, 2)^2));
+ustation_rx = atan2(-Ttot(2, 3)/cos(ustation_ry), Ttot(3, 3)/cos(ustation_ry));
+ustation_rz = atan2(-Ttot(1, 2)/cos(ustation_ry), Ttot(1, 1)/cos(ustation_ry));
+
+%% Verification using the Simscape model
+% All stages are initialized as rigid bodies to avoid any guiding error
+initializeGround(      'type', 'rigid');
+initializeGranite(     'type', 'rigid');
+initializeTy(          'type', 'rigid');
+initializeRy(          'type', 'rigid');
+initializeRz(          'type', 'rigid');
+initializeMicroHexapod('type', 'rigid');
+
+initializeLoggingConfiguration('log', 'all');
+
+initializeReferences('Dy_amplitude', Dy, ...
+                     'Ry_amplitude', Ry, ...
+                     'Rz_amplitude', Rz);
+
+initializeDisturbances('enable', false);
+
+set_param(conf_simulink, 'StopTime', '0.5');
+
+% Simulation is performed
+sim(mdl);
+
+% Sample's motion is computed from "external metrology"
+T_sim = [simout.y.R.Data(:,:,end), [simout.y.x.Data(end); simout.y.y.Data(end); simout.y.z.Data(end)]; [0,0,0,1]];
+sim_dx = T_sim(1,4);
+sim_dy = T_sim(2,4);
+sim_dz = T_sim(3,4);
+sim_ry = atan2( T_sim(1, 3),             sqrt(T_sim(1, 1)^2 + T_sim(1, 2)^2));
+sim_rx = atan2(-T_sim(2, 3)/cos(sim_ry), T_sim(3, 3)/cos(sim_ry));
+sim_rz = atan2(-T_sim(1, 2)/cos(sim_ry), T_sim(1, 1)/cos(sim_ry));

A4-simscape-micro-station/ustation_2_modeling.m

@@ -0,0 +1,321 @@
+%% ustation_2_modeling.m
+
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+%% Path for functions, data and scripts
+addpath('./mat/'); % Path for Data
+addpath('./src/'); % Path for functions
+addpath('./STEPS/'); % Path for STEPS
+addpath('./subsystems/'); % Path for Subsystems Simulink files
+
+% Simulink Model name
+mdl = 'ustation_simscape';
+
+load('nass_model_conf_simulink.mat');
+
+%% Colors for the figures
+colors = colororder;
+
+%% Frequency Vector
+freqs = logspace(log10(10), log10(2e3), 1000);
+
+%% Indentify the model dynamics from the 3 hammer imapcts
+% To the motion of each solid body at their CoM
+
+% All stages are initialized
+initializeGround(      'type', 'rigid');
+initializeGranite(     'type', 'flexible');
+initializeTy(          'type', 'flexible');
+initializeRy(          'type', 'flexible');
+initializeRz(          'type', 'flexible');
+initializeMicroHexapod('type', 'flexible');
+
+initializeLoggingConfiguration('log', 'none');
+
+initializeReferences();
+initializeDisturbances('enable', false);
+
+% Input/Output definition
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/Micro-Station/Translation Stage/Flexible/F_hammer'],          1, 'openinput');  io_i = io_i + 1;
+io(io_i) = linio([mdl, '/Micro-Station/Granite/Flexible/accelerometer'],               1, 'openoutput'); io_i = io_i + 1;
+io(io_i) = linio([mdl, '/Micro-Station/Translation Stage/Flexible/accelerometer'],     1, 'openoutput'); io_i = io_i + 1;
+io(io_i) = linio([mdl, '/Micro-Station/Tilt Stage/Flexible/accelerometer'],            1, 'openoutput'); io_i = io_i + 1;
+io(io_i) = linio([mdl, '/Micro-Station/Spindle/Flexible/accelerometer'],               1, 'openoutput'); io_i = io_i + 1;
+io(io_i) = linio([mdl, '/Micro-Station/Micro Hexapod/Flexible/accelerometer'],         1, 'openoutput'); io_i = io_i + 1;
+
+% Run the linearization
+G_ms = linearize(mdl, io, 0);
+G_ms.InputName  = {'Fx', 'Fy', 'Fz'};
+G_ms.OutputName = {'gtop_x', 'gtop_y', 'gtop_z', 'gtop_rx', 'gtop_ry', 'gtop_rz', ...
+                   'ty_x', 'ty_y', 'ty_z', 'ty_rx', 'ty_ry', 'ty_rz', ...
+                   'ry_x', 'ry_y', 'ry_z', 'ry_rx', 'ry_ry', 'ry_rz', ...
+                   'rz_x', 'rz_y', 'rz_z', 'rz_rx', 'rz_ry', 'rz_rz', ...
+                   'hexa_x', 'hexa_y', 'hexa_z', 'hexa_rx', 'hexa_ry', 'hexa_rz'};
+
+% Load estimated FRF at CoM
+load('ustation_frf_matrix.mat', 'freqs');
+load('ustation_frf_com.mat', 'frfs_CoM');
+
+% Initialization of some variables to the figures
+dirs = {'x', 'y', 'z', 'rx', 'ry', 'rz'};
+stages = {'gbot', 'gtop', 'ty', 'ry', 'rz', 'hexa'};
+f = logspace(log10(10), log10(500), 1000);
+
+%% Spindle - X response
+n_stg = 5; % Measured Stage
+n_dir = 1; % Measured DoF: x, y, z, Rx, Ry, Rz
+n_exc = 1; % Hammer impact: x, y, z
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(frfs_CoM(6*(n_stg-1) + n_dir, n_exc, :))), 'DisplayName', 'Measurement');
+plot(f, abs(squeeze(freqresp(G_ms([stages{n_stg}, '_', dirs{n_dir}], ['F', dirs{n_exc}]), f, 'Hz'))), 'DisplayName', 'Model');
+text(50, 2e-2,{'$D_x/F_x$ - Spindle'},'VerticalAlignment','bottom','HorizontalAlignment','center')
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude [$m/s^2/N$]');
+leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 15;
+xlim([10, 200]);
+ylim([1e-6, 1e-1])
+
+%% Hexapod - Y response
+n_stg = 6; % Measured Stage
+n_dir = 2; % Measured DoF: x, y, z, Rx, Ry, Rz
+n_exc = 2; % Hammer impact: x, y, z
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(frfs_CoM(6*(n_stg-1) + n_dir, n_exc, :))), 'DisplayName', 'Measurement');
+plot(f, abs(squeeze(freqresp(G_ms([stages{n_stg}, '_', dirs{n_dir}], ['F', dirs{n_exc}]), f, 'Hz'))), 'DisplayName', 'Model');
+text(50, 2e-2,{'$D_y/F_y$ - Hexapod'},'VerticalAlignment','bottom','HorizontalAlignment','center')
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude [$m/s^2/N$]');
+leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 15;
+xlim([10, 200]);
+ylim([1e-6, 1e-1])
+
+%% Tilt stage - Z response
+n_stg = 4; % Measured Stage
+n_dir = 3; % Measured DoF: x, y, z, Rx, Ry, Rz
+n_exc = 3; % Hammer impact: x, y, z
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(frfs_CoM(6*(n_stg-1) + n_dir, n_exc, :))), 'DisplayName', 'Measurement');
+plot(f, abs(squeeze(freqresp(G_ms([stages{n_stg}, '_', dirs{n_dir}], ['F', dirs{n_exc}]), f, 'Hz'))), 'DisplayName', 'Model');
+text(50, 2e-2,{'$D_z/F_z$ - Tilt'},'VerticalAlignment','bottom','HorizontalAlignment','center')
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude [$m/s^2/N$]');
+leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 15;
+xlim([10, 200]);
+ylim([1e-6, 1e-1])
+
+% Positions and orientation of accelerometers
+% | *Num* | *Position* | *Orientation* | *Sensibility* | *Channels* |
+% |-------+------------+---------------+---------------+------------|
+% |     1 | [0, +A, 0] | [x, y, z]     | 1V/g          |        1-3 |
+% |     2 | [-B, 0, 0] | [x, y, z]     | 1V/g          |        4-6 |
+% |     3 | [0, -A, 0] | [x, y, z]     | 0.1V/g        |        7-9 |
+% |     4 | [+B, 0, 0] | [x, y, z]     | 1V/g          |      10-12 |
+%
+
+% Measuerment channels
+%
+% | Acc Number | Dir | Channel Number |
+% |------------+-----+----------------|
+% |          1 | x   |              1 |
+% |          1 | y   |              2 |
+% |          1 | z   |              3 |
+% |          2 | x   |              4 |
+% |          2 | y   |              5 |
+% |          2 | z   |              6 |
+% |          3 | x   |              7 |
+% |          3 | y   |              8 |
+% |          3 | z   |              9 |
+% |          4 | x   |             10 |
+% |          4 | y   |             11 |
+% |          4 | z   |             12 |
+% |     Hammer |     |             13 |
+
+% 10 measurements corresponding to 10 different Hammer impact locations
+% | *Num* | *Direction* | *Position* | Accelerometer position | Jacobian number |
+% |-------+-------------+------------+------------------------+-----------------|
+% |     1 | -Y          | [0, +A, 0] |                      1 |              -2 |
+% |     2 | -Z          | [0, +A, 0] |                      1 |              -3 |
+% |     3 | X           | [-B, 0, 0] |                      2 |               4 |
+% |     4 | -Z          | [-B, 0, 0] |                      2 |              -6 |
+% |     5 | Y           | [0, -A, 0] |                      3 |               8 |
+% |     6 | -Z          | [0, -A, 0] |                      3 |              -9 |
+% |     7 | -X          | [+B, 0, 0] |                      4 |             -10 |
+% |     8 | -Z          | [+B, 0, 0] |                      4 |             -12 |
+% |     9 | -X          | [0, -A, 0] |                      3 |              -7 |
+% |    10 | -X          | [0, +A, 0] |                      1 |              -1 |
+
+
+%% Jacobian for accelerometers
+% aX = Ja . aL
+d = 0.14; % [m]
+Ja = [1  0  0  0  0 -d;
+      0  1  0  0  0  0;
+      0  0  1  d  0  0;
+      1  0  0  0  0  0;
+      0  1  0  0  0 -d;
+      0  0  1  0  d  0;
+      1  0  0  0  0  d;
+      0  1  0  0  0  0;
+      0  0  1 -d  0  0;
+      1  0  0  0  0  0;
+      0  1  0  0  0  d;
+      0  0  1  0 -d  0];
+
+Ja_inv = pinv(Ja);
+
+%% Jacobian for hammer impacts
+% F = Jf' tau
+Jf = [0 -1  0  0  0  0;
+      0  0 -1 -d  0  0;
+      1  0  0  0  0  0;
+      0  0 -1  0 -d  0;
+      0  1  0  0  0  0;
+      0  0 -1  d  0  0;
+     -1  0  0  0  0  0;
+      0  0 -1  0  d  0;
+     -1  0  0  0  0 -d;
+     -1  0  0  0  0  d]';
+Jf_inv = pinv(Jf);
+
+%% Load raw measurement data
+% "Track1" to "Track12" are the 12 accelerometers
+% "Track13" is the instrumented hammer force sensor
+data = [
+    load('ustation_compliance_hammer_1.mat'), ...
+    load('ustation_compliance_hammer_2.mat'), ...
+    load('ustation_compliance_hammer_3.mat'), ...
+    load('ustation_compliance_hammer_4.mat'), ...
+    load('ustation_compliance_hammer_5.mat'), ...
+    load('ustation_compliance_hammer_6.mat'), ...
+    load('ustation_compliance_hammer_7.mat'), ...
+    load('ustation_compliance_hammer_8.mat'), ...
+    load('ustation_compliance_hammer_9.mat'), ...
+    load('ustation_compliance_hammer_10.mat')];
+
+%% Prepare the FRF computation
+Ts = 1e-3; % Sampling Time [s]
+Nfft = floor(1/Ts); % Number of points for the FFT computation
+win = ones(Nfft, 1); % Rectangular window
+[~, f] = tfestimate(data(1).Track13, data(1).Track1, win, [], Nfft, 1/Ts);
+
+% Samples taken before and after the hammer "impact"
+pre_n = floor(0.1/Ts);
+post_n = Nfft - pre_n - 1;
+
+%% Compute the FRFs for the 10 hammer impact locations.
+% The FRF from hammer force the 12 accelerometers are computed
+% for each of the 10 hammer impacts and averaged
+G_raw = zeros(12,10,length(f));
+
+for i = 1:10 % For each impact location
+    % Find the 10 impacts
+    [~, impacts_i] = find(diff(data(i).Track13 > 50)==1);
+    % Only keep the first 10 impacts if there are more than 10 impacts
+    if length(impacts_i)>10
+        impacts_i(11:end) = [];
+    end
+
+    % Average the FRF for the 10 impacts
+    for impact_i = impacts_i
+        i_start = impact_i - pre_n;
+        i_end = impact_i + post_n;
+        data_in = data(i).Track13(i_start:i_end); % [N]
+        % Remove hammer DC offset
+        data_in  = data_in  - mean(data_in(end-pre_n:end));
+        % Combine all outputs [m/s^2]
+        data_out = [data(i).Track1( i_start:i_end); ...
+                    data(i).Track2( i_start:i_end); ...
+                    data(i).Track3( i_start:i_end); ...
+                    data(i).Track4( i_start:i_end); ...
+                    data(i).Track5( i_start:i_end); ...
+                    data(i).Track6( i_start:i_end); ...
+                    data(i).Track7( i_start:i_end); ...
+                    data(i).Track8( i_start:i_end); ...
+                    data(i).Track9( i_start:i_end); ...
+                    data(i).Track10(i_start:i_end); ...
+                    data(i).Track11(i_start:i_end); ...
+                    data(i).Track12(i_start:i_end)];
+
+        [frf, ~] = tfestimate(data_in, data_out', win, [], Nfft, 1/Ts);
+        G_raw(:,i,:) = squeeze(G_raw(:,i,:)) + 0.1*frf'./(-(2*pi*f').^2);
+    end
+end
+
+%% Compute transfer function in cartesian frame using Jacobian matrices
+% FRF_cartesian = inv(Ja) * FRF * inv(Jf)
+FRF_cartesian = pagemtimes(Ja_inv, pagemtimes(G_raw, Jf_inv));
+
+%% Identification of the compliance of the micro-station
+
+% Initialize simulation with default parameters (flexible elements)
+initializeGround();
+initializeGranite();
+initializeTy();
+initializeRy();
+initializeRz();
+initializeMicroHexapod();
+
+initializeReferences();
+initializeSimscapeConfiguration();
+initializeLoggingConfiguration();
+
+% Input/Output definition
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/Micro-Station/Micro Hexapod/Flexible/Fm'], 1, 'openinput');       io_i = io_i + 1; % Direct Forces/Torques applied on the micro-hexapod top platform
+io(io_i) = linio([mdl, '/Micro-Station/Micro Hexapod/Flexible/Dm'], 1, 'output'); io_i = io_i + 1; % Absolute displacement of the top platform
+
+% Run the linearization
+Gm = linearize(mdl, io, 0);
+Gm.InputName  = {'Fmx', 'Fmy', 'Fmz', 'Mmx', 'Mmy', 'Mmz'};
+Gm.OutputName = {'Dx', 'Dy', 'Dz', 'Drx', 'Dry', 'Drz'};
+
+%% Extracted FRF of the compliance of the micro-station in the Cartesian frame from the Simscape model
+figure;
+hold on;
+plot(f, abs(squeeze(FRF_cartesian(1,1,:))), '-', 'color', [colors(1,:), 0.5], 'linewidth', 2.5, 'DisplayName', '$D_x/F_x$ - Measured')
+plot(f, abs(squeeze(FRF_cartesian(2,2,:))), '-', 'color', [colors(2,:), 0.5], 'linewidth', 2.5, 'DisplayName', '$D_y/F_y$ - Measured')
+plot(f, abs(squeeze(FRF_cartesian(3,3,:))), '-', 'color', [colors(3,:), 0.5], 'linewidth', 2.5, 'DisplayName', '$D_z/F_z$ - Measured')
+plot(f, abs(squeeze(freqresp(Gm(1,1), f, 'Hz'))), '-', 'color', colors(1,:), 'DisplayName', '$D_x/F_x$ - Model')
+plot(f, abs(squeeze(freqresp(Gm(2,2), f, 'Hz'))), '-', 'color', colors(2,:), 'DisplayName', '$D_y/F_y$ - Model')
+plot(f, abs(squeeze(freqresp(Gm(3,3), f, 'Hz'))), '-', 'color', colors(3,:), 'DisplayName', '$D_z/F_z$ - Model')
+hold off;
+leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
+leg.ItemTokenSize(1) = 15;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude [m/N]');
+xlim([20, 500]); ylim([2e-10, 1e-6]);
+xticks([20, 50, 100, 200, 500])
+
+%% Extracted FRF of the compliance of the micro-station in the Cartesian frame from the Simscape model
+figure;
+hold on;
+plot(f, abs(squeeze(FRF_cartesian(4,4,:))), '-', 'color', [colors(1,:), 0.5], 'linewidth', 2.5, 'DisplayName', '$R_x/M_x$ - Measured')
+plot(f, abs(squeeze(FRF_cartesian(5,5,:))), '-', 'color', [colors(2,:), 0.5], 'linewidth', 2.5, 'DisplayName', '$R_y/M_y$ - Measured')
+plot(f, abs(squeeze(FRF_cartesian(6,6,:))), '-', 'color', [colors(3,:), 0.5], 'linewidth', 2.5, 'DisplayName', '$R_z/M_z$ - Measured')
+plot(f, abs(squeeze(freqresp(Gm(4,4), f, 'Hz'))), '-', 'color', colors(1,:), 'DisplayName', '$R_x/M_x$ - Model')
+plot(f, abs(squeeze(freqresp(Gm(5,5), f, 'Hz'))), '-', 'color', colors(2,:), 'DisplayName', '$R_y/M_y$ - Model')
+plot(f, abs(squeeze(freqresp(Gm(6,6), f, 'Hz'))), '-', 'color', colors(3,:), 'DisplayName', '$R_z/M_z$ - Model')
+hold off;
+leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
+leg.ItemTokenSize(1) = 15;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude [rad/Nm]');
+xlim([20, 500]); ylim([2e-7, 1e-4]);
+xticks([20, 50, 100, 200, 500])

A4-simscape-micro-station/ustation_3_disturbances.m

@@ -0,0 +1,345 @@
+%% ustation_3_disturbances.m
+
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+%% Path for functions, data and scripts
+addpath('./mat/'); % Path for Data
+addpath('./src/'); % Path for functions
+addpath('./STEPS/'); % Path for STEPS
+addpath('./subsystems/'); % Path for Subsystems Simulink files
+
+% Simulink Model name
+mdl = 'ustation_simscape';
+
+load('nass_model_conf_simulink.mat');
+
+%% Colors for the figures
+colors = colororder;
+
+%% Frequency Vector
+freqs = logspace(log10(10), log10(2e3), 1000);
+
+%% Compute Floor Motion Spectral Density
+% Load floor motion data
+% velocity in [m/s] is measured in X, Y and Z directions
+load('ustation_ground_motion.mat', 'Ts', 'Fs', 'vel_x', 'vel_y', 'vel_z', 't');
+
+% Estimate ground displacement from measured velocity
+% This is done by integrating the motion
+gm_x = lsim(1/(s+0.1*2*pi), vel_x, t);
+gm_y = lsim(1/(s+0.1*2*pi), vel_y, t);
+gm_z = lsim(1/(s+0.1*2*pi), vel_z, t);
+
+Nfft = floor(2/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+
+[pxx_gm_vx, f_gm] = pwelch(vel_x, win, Noverlap, Nfft, 1/Ts);
+[pxx_gm_vy, ~]    = pwelch(vel_y, win, Noverlap, Nfft, 1/Ts);
+[pxx_gm_vz, ~]    = pwelch(vel_z, win, Noverlap, Nfft, 1/Ts);
+
+% Convert PSD in velocity to PSD in displacement
+pxx_gm_x = pxx_gm_vx./((2*pi*f_gm).^2);
+pxx_gm_y = pxx_gm_vy./((2*pi*f_gm).^2);
+pxx_gm_z = pxx_gm_vz./((2*pi*f_gm).^2);
+
+%% Measured ground motion
+figure;
+hold on;
+plot(t, 1e6*gm_x, 'DisplayName', '$D_{xf}$')
+plot(t, 1e6*gm_y, 'DisplayName', '$D_{yf}$')
+plot(t, 1e6*gm_z, 'DisplayName', '$D_{zf}$')
+hold off;
+xlabel('Time [s]');
+ylabel('Ground motion [$\mu$m]')
+xlim([0, 5]); ylim([-2, 2])
+leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 15;
+
+%% Ty errors
+% Setpoint is in [mm]
+% Vertical error is in [um]
+ty_errors = load('ustation_errors_ty.mat');
+
+% Compute best straight line to remove it from data
+average_error = mean(ty_errors.ty_z')';
+straight_line = average_error - detrend(average_error, 1);
+
+%% Measurement of the linear (vertical) deviation of the Translation stage
+figure;
+hold on;
+plot(ty_errors.setpoint, ty_errors.ty_z(:,1), '-', 'color', colors(1,:), 'DisplayName', 'Raw data')
+plot(ty_errors.setpoint, ty_errors.ty_z(:,2:end), '-', 'color', colors(1,:), 'HandleVisibility', 'off')
+plot(ty_errors.setpoint, straight_line, '--', 'color', colors(2,:), 'DisplayName', 'Linear fit')
+hold off;
+xlabel('$D_y$ position [mm]'); ylabel('Vertical error [$\mu$m]');
+xlim([-5, 5]); ylim([-8, 8]);
+legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+
+%% Measurement of the linear (vertical) deviation of the Translation stage - Remove best linear fit
+figure;
+plot(ty_errors.setpoint, ty_errors.ty_z - straight_line, 'k-')
+xlabel('$D_y$ position [mm]'); ylabel('Residual vertical error [$\mu$m]');
+xlim([-5, 5]); ylim([-0.4, 0.4]);
+
+%% Convert the data to frequency domain
+% Suppose a certain constant velocity scan
+delta_ty = (ty_errors.setpoint(end) - ty_errors.setpoint(1))/(length(ty_errors.setpoint) - 1); % [mm]
+ty_vel = 8*1.125; % [mm/s]
+Ts = delta_ty/ty_vel;
+Fs = 1/Ts;
+
+% Frequency Analysis
+Nfft = floor(length(ty_errors.setpoint)); % Number of frequency points
+win = hanning(Nfft); % Windowing
+Noverlap = floor(Nfft/2); % Overlap for frequency analysis
+
+% Comnpute the power spectral density
+[pxx_dy_dz, f_dy] = pwelch(1e-6*detrend(ty_errors.ty_z, 1), win, Noverlap, Nfft, Fs);
+pxx_dy_dz = mean(pxx_dy_dz')';
+
+% Having the PSD of the lateral error equal to the PSD of the vertical error
+% is a reasonable assumption (and verified in practice)
+pxx_dy_dx = pxx_dy_dz;
+
+%% Spindle Errors
+% Errors are already converted to x,y,z,Rx,Ry
+% Units are in [m] and [rad]
+spindle_errors = load('ustation_errors_spindle.mat');
+
+%% Measured radial errors of the Spindle
+figure;
+hold on;
+plot(1e6*spindle_errors.Dx(1:100:end), 1e6*spindle_errors.Dy(1:100:end), 'DisplayName', 'Raw data')
+% Compute best fitting circle
+[x0, y0, R] = circlefit(spindle_errors.Dx, spindle_errors.Dy);
+theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
+plot(1e6*(x0 + R*cos(theta)), 1e6*(y0 + R*sin(theta)), '--', 'DisplayName', 'Best Fit')
+hold off;
+xlabel('X displacement [$\mu$m]'); ylabel('Y displacement [$\mu$m]');
+axis equal
+xlim([-1, 1]); ylim([-1, 1]);
+xticks([-1, -0.5, 0, 0.5, 1]);
+yticks([-1, -0.5, 0, 0.5, 1]);
+leg = legend('location', 'none', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 15;
+
+%% Measured axial errors of the Spindle
+figure;
+plot(spindle_errors.deg(1:100:end)/360, 1e9*spindle_errors.Dz(1:100:end))
+xlabel('Rotation [turn]'); ylabel('Z displacement [nm]');
+axis square
+xlim([0,2]); ylim([-40, 40]);
+
+%% Measured tilt errors of the Spindle
+figure;
+plot(1e6*spindle_errors.Rx(1:100:end), 1e6*spindle_errors.Ry(1:100:end))
+xlabel('X angle [$\mu$rad]'); ylabel('Y angle [$\mu$rad]');
+axis equal
+xlim([-35, 35]); ylim([-35, 35]);
+xticks([-30, -15, 0, 15, 30]);
+yticks([-30, -15, 0, 15, 30]);
+
+%% Remove the circular fit from the data
+[x0, y0, R] = circlefit(spindle_errors.Dx, spindle_errors.Dy);
+
+% Search the best angular match
+fun = @(theta)rms((spindle_errors.Dx - (x0 + R*cos(pi/180*spindle_errors.deg+theta(1)))).^2 + (spindle_errors.Dy - (y0 - R*sin(pi/180*spindle_errors.deg+theta(1)))).^2);
+x0 = [0];
+delta_theta = fminsearch(fun, 0);
+
+% Compute the remaining error after removing the best circular fit
+spindle_errors.Dx_err = spindle_errors.Dx - (x0 + R*cos(pi/180*spindle_errors.deg+delta_theta));
+spindle_errors.Dy_err = spindle_errors.Dy - (y0 - R*sin(pi/180*spindle_errors.deg+delta_theta));
+
+%% Convert the data to frequency domain
+Ts = (spindle_errors.t(end) - spindle_errors.t(1))/(length(spindle_errors.t)-1); % [s]
+Fs = 1/Ts; % [Hz]
+
+% Frequency Analysis
+Nfft = floor(2.0*Fs); % Number of frequency points
+win = hanning(Nfft); % Windowing
+Noverlap = floor(Nfft/2); % Overlap for frequency analysis
+
+% Comnpute the power spectral density
+[pxx_rz_dz, f_rz] = pwelch(spindle_errors.Dz,     win, Noverlap, Nfft, Fs);
+[pxx_rz_dx, ~   ] = pwelch(spindle_errors.Dx_err, win, Noverlap, Nfft, Fs);
+[pxx_rz_dy, ~   ] = pwelch(spindle_errors.Dy_err, win, Noverlap, Nfft, Fs);
+
+%% Extract sensitivity to disturbances from the Simscape model
+% Initialize stages
+initializeGround();
+initializeGranite();
+initializeTy();
+initializeRy();
+initializeRz();
+initializeMicroHexapod();
+
+initializeDisturbances('enable', false);
+initializeSimscapeConfiguration('gravity', false);
+initializeLoggingConfiguration();
+
+% Configure inputs and outputs
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/Disturbances/Dwx'], 1, 'openinput'); io_i = io_i + 1; % Vertical Ground Motion
+io(io_i) = linio([mdl, '/Disturbances/Dwy'], 1, 'openinput'); io_i = io_i + 1; % Vertical Ground Motion
+io(io_i) = linio([mdl, '/Disturbances/Dwz'], 1, 'openinput'); io_i = io_i + 1; % Vertical Ground Motion
+io(io_i) = linio([mdl, '/Disturbances/Fdy_x'], 1, 'openinput'); io_i = io_i + 1; % Parasitic force Ty
+io(io_i) = linio([mdl, '/Disturbances/Fdy_z'], 1, 'openinput'); io_i = io_i + 1; % Parasitic force Ty
+io(io_i) = linio([mdl, '/Disturbances/Frz_x'], 1, 'openinput'); io_i = io_i + 1; % Parasitic force Rz
+io(io_i) = linio([mdl, '/Disturbances/Frz_y'], 1, 'openinput'); io_i = io_i + 1; % Parasitic force Rz
+io(io_i) = linio([mdl, '/Disturbances/Frz_z'], 1, 'openinput'); io_i = io_i + 1; % Parasitic force Rz
+io(io_i) = linio([mdl, '/Micro-Station/metrology_6dof/x'], 1, 'openoutput'); io_i = io_i + 1; % Relative motion - Hexapod/Granite
+io(io_i) = linio([mdl, '/Micro-Station/metrology_6dof/y'], 1, 'openoutput'); io_i = io_i + 1; % Relative motion - Hexapod/Granite
+io(io_i) = linio([mdl, '/Micro-Station/metrology_6dof/z'], 1, 'openoutput'); io_i = io_i + 1; % Relative motion - Hexapod/Granite
+
+% Run the linearization
+Gd = linearize(mdl, io, 0);
+Gd.InputName  = {'Dwx', 'Dwy', 'Dwz', 'Fdy_x', 'Fdy_z', 'Frz_x', 'Frz_y', 'Frz_z'};
+Gd.OutputName = {'Dx', 'Dy', 'Dz'};
+
+% The identified dynamics are then saved for further use.
+save('./mat/ustation_disturbance_sensitivity.mat', 'Gd')
+
+%% Sensitivity to Ground motion
+freqs = logspace(log10(10), log10(2e2), 1000);
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(Gd('Dx', 'Dwx'), freqs, 'Hz'))), 'DisplayName', '$D_x/D_{xf}$');
+plot(freqs, abs(squeeze(freqresp(Gd('Dy', 'Dwy'), freqs, 'Hz'))), 'DisplayName', '$D_y/D_{yf}$');
+plot(freqs, abs(squeeze(freqresp(Gd('Dz', 'Dwz'), freqs, 'Hz'))), 'DisplayName', '$D_z/D_{zf}$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
+leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 15;
+
+%% Sensitivity to Translation stage disturbance forces
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(Gd('Dx', 'Fdy_x'), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', '$D_x/F_{xD_y}$');
+plot(freqs, abs(squeeze(freqresp(Gd('Dz', 'Fdy_z'), freqs, 'Hz'))), 'color', colors(3,:), 'DisplayName', '$D_z/F_{zD_y}$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+ylim([1e-10, 1e-7]);
+leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 15;
+
+%% Sensitivity to Spindle disturbance forces
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(Gd('Dx', 'Frz_x'), freqs, 'Hz'))), 'DisplayName', '$D_x/F_{xR_z}$');
+plot(freqs, abs(squeeze(freqresp(Gd('Dy', 'Frz_y'), freqs, 'Hz'))), 'DisplayName', '$D_y/F_{yR_z}$');
+plot(freqs, abs(squeeze(freqresp(Gd('Dz', 'Frz_z'), freqs, 'Hz'))), 'DisplayName', '$D_z/F_{zR_z}$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+ylim([1e-10, 1e-7]);
+leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 15;
+
+%% Compute the PSD of the equivalent disturbance sources
+pxx_rz_fx = pxx_rz_dx./abs(squeeze(freqresp(Gd('Dx', 'Frz_x'), f_rz, 'Hz'))).^2;
+pxx_rz_fy = pxx_rz_dy./abs(squeeze(freqresp(Gd('Dy', 'Frz_y'), f_rz, 'Hz'))).^2;
+pxx_rz_fz = pxx_rz_dz./abs(squeeze(freqresp(Gd('Dz', 'Frz_z'), f_rz, 'Hz'))).^2;
+
+pxx_dy_fx = pxx_dy_dx./abs(squeeze(freqresp(Gd('Dx', 'Fdy_x'), f_dy, 'Hz'))).^2;
+pxx_dy_fz = pxx_dy_dz./abs(squeeze(freqresp(Gd('Dz', 'Fdy_z'), f_dy, 'Hz'))).^2;
+
+%% Save the PSD of the disturbance sources for further used
+% in the Simscape model
+
+% Ground motion
+min_f = 1; max_f = 100;
+gm_dist.f  = f_gm(f_gm < max_f & f_gm > min_f);
+gm_dist.pxx_x = pxx_gm_x(f_gm < max_f & f_gm > min_f);
+gm_dist.pxx_y = pxx_gm_y(f_gm < max_f & f_gm > min_f);
+gm_dist.pxx_z = pxx_gm_z(f_gm < max_f & f_gm > min_f);
+
+% Translation stage
+min_f = 0.5; max_f = 500;
+dy_dist.f  = f_dy(f_dy < max_f & f_dy > min_f);
+dy_dist.pxx_fx = pxx_dy_fx(f_dy < max_f & f_dy > min_f);
+dy_dist.pxx_fz = pxx_dy_fz(f_dy < max_f & f_dy > min_f);
+
+% Spindle
+min_f = 1; max_f = 500;
+rz_dist.f  = f_rz(f_rz < max_f & f_rz > min_f);
+rz_dist.pxx_fx = pxx_rz_fx(f_rz < max_f & f_rz > min_f);
+rz_dist.pxx_fy = pxx_rz_fy(f_rz < max_f & f_rz > min_f);
+rz_dist.pxx_fz = pxx_rz_fz(f_rz < max_f & f_rz > min_f);
+
+% The identified dynamics are then saved for further use.
+save('./mat/ustation_disturbance_psd.mat', 'rz_dist', 'dy_dist', 'gm_dist')
+
+%% Ground Motion
+figure;
+hold on;
+plot(f_gm, sqrt(pxx_gm_x), 'DisplayName', '$D_{wx}$');
+plot(f_gm, sqrt(pxx_gm_y), 'DisplayName', '$D_{wy}$');
+plot(f_gm, sqrt(pxx_gm_z), 'DisplayName', '$D_{wz}$');
+set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Spectral Density $\left[\frac{m}{\sqrt{Hz}}\right]$')
+xlim([1, 200]);
+leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 15;
+
+figure;
+hold on;
+plot(f_dy, sqrt(pxx_dy_fx), 'DisplayName', '$F_{xD_y}$');
+plot(f_dy, sqrt(pxx_dy_fz), 'DisplayName', '$F_{zD_y}$');
+set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Spectral Density $\left[\frac{N}{\sqrt{Hz}}\right]$')
+xlim([1, 200]); ylim([1e-3, 1e3]);
+leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 15;
+
+figure;
+hold on;
+plot(f_rz, sqrt(pxx_rz_fx), 'DisplayName', '$F_{xR_z}$');
+plot(f_rz, sqrt(pxx_rz_fy), 'DisplayName', '$F_{yR_z}$');
+plot(f_rz, sqrt(pxx_rz_fz), 'DisplayName', '$F_{zR_z}$');
+set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Spectral Density $\left[\frac{N}{\sqrt{Hz}}\right]$')
+xlim([1, 200]); ylim([1e-3, 1e3]);
+leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 15;
+
+%% Compute time domain disturbance signals
+initializeDisturbances();
+load('nass_model_disturbances.mat');
+
+figure;
+hold on;
+plot(Rz.t, Rz.x, 'DisplayName', '$F_{xR_z}$');
+plot(Rz.t, Rz.y, 'DisplayName', '$F_{yR_z}$');
+plot(Rz.t, Rz.z, 'DisplayName', '$F_{zR_z}$');
+xlabel('Time [s]'); ylabel('Amplitude [N]')
+xlim([0, 1]); ylim([-100, 100]);
+leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 15;
+
+figure;
+hold on;
+plot(Dy.t, Dy.x, 'DisplayName', '$F_{xD_y}$');
+plot(Dy.t, Dy.z, 'DisplayName', '$F_{zD_y}$');
+xlabel('Time [s]'); ylabel('Amplitude [N]')
+xlim([0, 1]); ylim([-60, 60])
+leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 15;
+
+figure;
+hold on;
+plot(Dw.t, 1e6*Dw.x, 'DisplayName', '$D_{xf}$');
+plot(Dw.t, 1e6*Dw.y, 'DisplayName', '$D_{yf}$');
+plot(Dw.t, 1e6*Dw.z, 'DisplayName', '$D_{zf}$');
+xlabel('Time [s]'); ylabel('Amplitude [$\mu$m]')
+xlim([0, 1]); ylim([-0.6, 0.6])
+leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 15;

A4-simscape-micro-station/ustation_4_experiments.m

@@ -0,0 +1,141 @@
+%% ustation_4_experiments.m
+
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+%% Path for functions, data and scripts
+addpath('./mat/'); % Path for Data
+addpath('./src/'); % Path for functions
+addpath('./STEPS/'); % Path for STEPS
+addpath('./subsystems/'); % Path for Subsystems Simulink files
+
+% Simulink Model name
+mdl = 'ustation_simscape';
+
+load('nass_model_conf_simulink.mat');
+
+%% Colors for the figures
+colors = colororder;
+
+%% Frequency Vector
+freqs = logspace(log10(10), log10(2e3), 1000);
+
+%% Tomography experiment
+% Sample is not centered with the rotation axis
+% This is done by offsetfing the micro-hexapod by 0.9um
+P_micro_hexapod = [0.9e-6; 0; 0]; % [m]
+
+set_param(conf_simulink, 'StopTime', '10');
+
+initializeGround();
+initializeGranite();
+initializeTy();
+initializeRy();
+initializeRz();
+initializeMicroHexapod('AP', P_micro_hexapod);
+
+initializeSimscapeConfiguration('gravity', false);
+initializeLoggingConfiguration('log', 'all');
+
+initializeDisturbances(...
+    'Dw_x',  true, ... % Ground Motion - X direction
+    'Dw_y',  true, ... % Ground Motion - Y direction
+    'Dw_z',  true, ... % Ground Motion - Z direction
+    'Fdy_x', false, ... % Translation Stage - X direction
+    'Fdy_z', false, ... % Translation Stage - Z direction
+    'Frz_x', true, ... % Spindle - X direction
+    'Frz_y', true, ... % Spindle - Y direction
+    'Frz_z', true);    % Spindle - Z direction
+
+initializeReferences(...
+    'Rz_type', 'rotating', ...
+    'Rz_period', 1, ...
+    'Dh_pos', [P_micro_hexapod; 0; 0; 0]);
+
+sim(mdl);
+exp_tomography = simout;
+
+%% Compare with the measurements
+spindle_errors = load('ustation_errors_spindle.mat');
+
+%% Measured radial errors of the Spindle
+figure;
+hold on;
+plot(1e6*spindle_errors.Dx(1:100:end), 1e6*spindle_errors.Dy(1:100:end), 'DisplayName', 'Measurements')
+plot(1e6*exp_tomography.y.x.Data, 1e6*exp_tomography.y.y.Data, 'DisplayName', 'Simulation')
+hold off;
+xlabel('X displacement [$\mu$m]'); ylabel('Y displacement [$\mu$m]');
+axis equal
+xlim([-1, 1]); ylim([-1, 1]);
+xticks([-1, -0.5, 0, 0.5, 1]);
+yticks([-1, -0.5, 0, 0.5, 1]);
+leg = legend('location', 'none', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 15;
+
+%% Measured axial errors of the Spindle
+figure;
+hold on;
+plot(spindle_errors.deg(1:100:end)/360, detrend(1e9*spindle_errors.Dz(1:100:end), 0), 'DisplayName', 'Measurements')
+plot(exp_tomography.y.z.Time, detrend(1e9*exp_tomography.y.z.Data, 0), 'DisplayName', 'Simulation')
+hold off;
+xlabel('Rotation [turn]'); ylabel('Z displacement [nm]');
+axis square
+xlim([0,2]); ylim([-40, 40]);
+leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 15;
+
+%% Translation stage latteral scans
+set_param(conf_simulink, 'StopTime', '2');
+
+initializeGround();
+initializeGranite();
+initializeTy();
+initializeRy();
+initializeRz();
+initializeMicroHexapod();
+
+initializeSimscapeConfiguration('gravity', false);
+initializeLoggingConfiguration('log', 'all');
+
+initializeDisturbances(...
+    'Dw_x',  true, ... % Ground Motion - X direction
+    'Dw_y',  true, ... % Ground Motion - Y direction
+    'Dw_z',  true, ... % Ground Motion - Z direction
+    'Fdy_x', true, ... % Translation Stage - X direction
+    'Fdy_z', true, ... % Translation Stage - Z direction
+    'Frz_x', false, ... % Spindle - X direction
+    'Frz_y', false, ... % Spindle - Y direction
+    'Frz_z', false);    % Spindle - Z direction
+
+initializeReferences(...
+    'Dy_type', 'triangular', ...
+    'Dy_amplitude', 4.5e-3, ...
+    'Dy_period', 2);
+
+sim(mdl);
+exp_latteral_scans = simout;
+
+% Load the experimentally measured errors
+ty_errors = load('ustation_errors_ty.mat');
+
+% Compute best straight line to remove it from data
+average_error = mean(ty_errors.ty_z')';
+straight_line = average_error - detrend(average_error, 1);
+
+% Only plot data for the scan from -4.5mm to 4.5mm
+dy_setpoint = 1e3*exp_latteral_scans.y.y.Data(exp_latteral_scans.y.y.time > 0.5 & exp_latteral_scans.y.y.time < 1.5);
+dz_error = detrend(1e6*exp_latteral_scans.y.z.Data(exp_latteral_scans.y.y.time > 0.5 & exp_latteral_scans.y.y.time < 1.5), 0);
+
+figure;
+hold on;
+plot(ty_errors.setpoint, detrend(ty_errors.ty_z(:,1) - straight_line, 0), 'color', colors(1,:), 'DisplayName', 'Measurement')
+% plot(ty_errors.setpoint, detrend(ty_errors.ty_z(:,[4,6]) - straight_line, 0), 'color', colors(1,:), 'HandleVisibility', 'off')
+plot(dy_setpoint, dz_error, 'color', colors(2,:), 'DisplayName', 'Simulation')
+hold off;
+xlabel('$D_y$ position [mm]'); ylabel('Vertical error [$\mu$m]');
+xlim([-5, 5]); ylim([-0.4, 0.4]);
+leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 15;