Version before Christophe's review

matlab/ustation_1_kinematics.m

@@ -0,0 +1,162 @@
+% Matlab Init                                              :noexport:ignore:
+
+%% 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);
+
+% Micro-Station Kinematics
+% <<ssec:ustation_kinematics>>
+
+% Each stage is described by two frames; one is attached to the fixed platform $\{A\}$ while the other is fixed to the mobile platform $\{B\}$.
+% At "rest" position, the two have the same pose and coincide with the point of interest ($O_A = O_B$).
+% An example of the tilt stage is shown in Figure ref:fig:ustation_stage_motion.
+% The mobile frame of the translation stage is equal to the fixed frame of the tilt stage: $\{B_{D_y}\} = \{A_{R_y}\}$.
+% Similarly, the mobile frame of the tilt stage is equal to the fixed frame of the spindle: $\{B_{R_y}\} = \{A_{R_z}\}$.
+
+% #+name: fig:ustation_stage_motion
+% #+caption: Example of the motion induced by the tilt-stage $R_y$. "Rest" position in shown in blue while a arbitrary position in shown in red. Parasitic motions are here magnified for clarity.
+% [[file:figs/ustation_stage_motion.png]]
+
+% The motion induced by a positioning stage can be described by a homogeneous transformation matrix from frame $\{A\}$ to frame $\{B\}$ as explain in Section ref:ssec:ustation_kinematics.
+% As any motion stage induces parasitic motion in all 6 DoF, the transformation matrix representing its induced motion can be written as in eqref:eq:ustation_translation_stage_errors.
+
+% \begin{equation}\label{eq:ustation_translation_stage_errors}
+% {}^A\mathbf{T}_B(D_x, D_y, D_z, \theta_x, \theta_y, \theta_z) =
+% \left[ \begin{array}{ccc|c}
+%     &   &   & D_x \\
+%     & \mathbf{R}_x(\theta_x) \mathbf{R}_y(\theta_y) \mathbf{R}_z(\theta_z) &   & D_y \\
+%     &   &   & D_z \cr
+%   \hline
+%   0 & 0 & 0 & 1
+% \end{array} \right]
+% \end{equation}
+
+% The homogeneous transformation matrix corresponding to the micro-station $\mathbf{T}_{\mu\text{-station}}$ is simply equal to the matrix multiplication of the homogeneous transformation matrices of the individual stages as shown in Equation eqref:eq:ustation_transformation_station.
+
+% \begin{equation}\label{eq:ustation_transformation_station}
+% \mathbf{T}_{\mu\text{-station}} = \mathbf{T}_{D_y} \cdot \mathbf{T}_{R_y} \cdot \mathbf{T}_{R_z} \cdot \mathbf{T}_{\mu\text{-hexapod}}
+% \end{equation}
+
+% $\mathbf{T}_{\mu\text{-station}}$ represents the pose of the sample (supposed to be rigidly fixed on top of the positioning-hexapod) with respect to the granite.
+
+% If the transformation matrices of the individual stages are each representing a perfect motion (i.e. the stages are supposed to have no parasitic motion), $\mathbf{T}_{\mu\text{-station}}$ then represent the pose setpoint of the sample with respect to the granite.
+% The transformation matrices for the translation stage, tilt stage, spindle, and positioning hexapod can be written as shown in Equation eqref:eq:ustation_transformation_matrices_stages.
+
+% \begin{equation}\label{eq:ustation_transformation_matrices_stages}
+% \begin{align}
+% \mathbf{T}_{D_y} &= \begin{bmatrix}
+%     1 & 0 & 0 & 0 \\
+%     0 & 1 & 0 & D_y \\
+%     0 & 0 & 1 & 0 \\
+%     0 & 0 & 0 & 1
+% \end{bmatrix} \quad
+% \mathbf{T}_{\mu\text{-hexapod}} =
+% \left[ \begin{array}{ccc|c}
+%     &   &   & D_{\mu x} \\
+%     & \mathbf{R}_x(\theta_{\mu x}) \mathbf{R}_y(\theta_{\mu y}) \mathbf{R}_{z}(\theta_{\mu z}) &   & D_{\mu y} \\
+%     &   &   & D_{\mu z} \cr
+%   \hline
+%   0 & 0 & 0 & 1
+% \end{array} \right] \\
+% \mathbf{T}_{R_z} &= \begin{bmatrix}
+%     \cos(\theta_z) & -\sin(\theta_z) & 0 & 0 \\
+%     \sin(\theta_z) & \cos(\theta_z) & 0 & 0 \\
+%     0 & 0 & 1 & 0 \\
+%     0 & 0 & 0 & 1
+% \end{bmatrix} \quad
+% \mathbf{T}_{R_y} = \begin{bmatrix}
+%     \cos(\theta_y) & 0 & \sin(\theta_y) & 0 \\
+%     0 & 1 & 0 & 0 \\
+%     -\sin(\theta_y) & 0 & \cos(\theta_y) & 0 \\
+%     0 & 0 & 0 & 1
+% \end{bmatrix}
+% \end{align}
+% \end{equation}
+
+
+%% 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));

matlab/ustation_2_modeling.m

@@ -0,0 +1,411 @@
+% Matlab Init                                              :noexport:ignore:
+
+%% 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);
+
+% Comparison with the measured dynamics
+% <<ssec:ustation_model_comp_dynamics>>
+
+% The dynamics of the micro-station was measured by placing accelerometers on each stage and by impacting the translation stage with an instrumented hammer in three directions.
+% The obtained FRFs were then projected at the CoM of each stage.
+
+% To gain a first insight into the accuracy of the obtained model, the FRFs from the hammer impacts to the acceleration of each stage were extracted from the Simscape model and compared with the measurements in Figure ref:fig:ustation_comp_com_response.
+
+% Even though there is some similarity between the model and the measurements (similar overall shapes and amplitudes), it is clear that the Simscape model does not accurately represent the complex micro-station dynamics.
+% Tuning the numerous model parameters to better match the measurements is a highly non-linear optimization problem that is difficult to solve in practice.
+
+
+%% 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])
+
+% Micro-station compliance
+% <<ssec:ustation_model_compliance>>
+
+% As discussed in the previous section, the dynamics of the micro-station is complex, and tuning the multi-body model parameters to obtain a perfect match is difficult.
+
+% When considering the NASS, the most important dynamical characteristics of the micro-station is its compliance, as it can affect the plant dynamics.
+% Therefore, the adopted strategy is to accurately model the micro-station compliance.
+
+% The micro-station compliance was experimentally measured using the setup illustrated in Figure ref:fig:ustation_compliance_meas.
+% Four 3-axis accelerometers were fixed to the micro-hexapod top platform.
+% The micro-hexapod top platform was impacted at 10 different points.
+% For each impact position, 10 impacts were performed to average and improve the data quality.
+
+% #+name: fig:ustation_compliance_meas
+% #+caption: Schematic of the measurement setup used to estimate the compliance of the micro-station. The top platform of the positioning hexapod is shown with four 3-axis accelerometers (shown in red) are on top. 10 hammer impacts are performed at different locations (shown in blue).
+% [[file:figs/ustation_compliance_meas.png]]
+
+% To convert the 12 acceleration signals $a_{\mathcal{L}} = [a_{1x}\ a_{1y}\ a_{1z}\ a_{2x}\ \dots\ a_{4z}]$ to the acceleration expressed in the frame $\{\mathcal{X}\}$ $a_{\mathcal{X}} = [a_{dx}\ a_{dy}\ a_{dz}\ a_{rx}\ a_{ry}\ a_{rz}]$, a Jacobian matrix $\mathbf{J}_a$ is written based on the positions and orientations of the accelerometers eqref:eq:ustation_compliance_acc_jacobian.
+
+% \begin{equation}\label{eq:ustation_compliance_acc_jacobian}
+% \mathbf{J}_a = \begin{bmatrix}
+% 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
+% \end{bmatrix}
+% \end{equation}
+
+% Then, the acceleration in the cartesian frame can be computed using eqref:eq:ustation_compute_cart_acc.
+
+% \begin{equation}\label{eq:ustation_compute_cart_acc}
+% a_{\mathcal{X}} = \mathbf{J}_a^\dagger \cdot a_{\mathcal{L}}
+% \end{equation}
+
+% Similar to what is done for the accelerometers, a Jacobian matrix $\mathbf{J}_F$ is computed eqref:eq:ustation_compliance_force_jacobian and used to convert the individual hammer forces $F_{\mathcal{L}}$ to force and torques $F_{\mathcal{X}}$ applied at the center of the micro-hexapod top plate (defined by frame $\{\mathcal{X}\}$ in Figure ref:fig:ustation_compliance_meas).
+
+% \begin{equation}\label{eq:ustation_compliance_force_jacobian}
+% \mathbf{J}_F = \begin{bmatrix}
+%  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
+% \end{bmatrix}
+% \end{equation}
+
+% The equivalent forces and torques applied at center of $\{\mathcal{X}\}$ are then computed using eqref:eq:ustation_compute_cart_force.
+
+% \begin{equation}\label{eq:ustation_compute_cart_force}
+% F_{\mathcal{X}} = \mathbf{J}_F^t \cdot F_{\mathcal{L}}
+% \end{equation}
+
+% Using the two Jacobian matrices, the FRF from the 10 hammer impacts to the 12 accelerometer outputs can be converted to the FRF from 6 forces/torques applied at the origin of frame $\{\mathcal{X}\}$ to the 6 linear/angular accelerations of the top platform expressed with respect to $\{\mathcal{X}\}$.
+% These FRFs were then used for comparison with the Simscape model.
+
+
+% 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));
+
+
+
+% The compliance of the micro-station multi-body model was extracted by computing the transfer function from forces/torques applied on the hexapod's top platform to the "absolute" motion of the top platform.
+% These results are compared with the measurements in Figure ref:fig:ustation_frf_compliance_model.
+% Considering the complexity of the micro-station compliance dynamics, the model compliance matches sufficiently well for the current application.
+
+
+%% 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], 'DisplayName', '$D_x/F_x$ - Measured')
+plot(f, abs(squeeze(FRF_cartesian(2,2,:))), '-', 'color', [colors(2,:), 0.5], 'DisplayName', '$D_y/F_y$ - Measured')
+plot(f, abs(squeeze(FRF_cartesian(3,3,:))), '-', 'color', [colors(3,:), 0.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], 'DisplayName', '$R_x/M_x$ - Measured')
+plot(f, abs(squeeze(FRF_cartesian(5,5,:))), '-', 'color', [colors(2,:), 0.5], 'DisplayName', '$R_y/M_y$ - Measured')
+plot(f, abs(squeeze(FRF_cartesian(6,6,:))), '-', 'color', [colors(3,:), 0.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])

matlab/ustation_3_disturbances.m

@@ -0,0 +1,501 @@
+% Matlab Init                                              :noexport:ignore:
+
+%% 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);
+
+% Ground Motion
+
+% The ground motion was measured by using a sensitive 3-axis geophone[fn:11] placed on the ground.
+% The generated voltages were recorded with a high resolution DAC, and converted to displacement using the Geophone sensitivity transfer function.
+% The obtained ground motion displacement is shown in Figure ref:fig:ustation_ground_disturbance.
+
+
+%% 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 Stage
+
+% To measure the positioning errors of the translation stage, the setup shown in Figure ref:fig:ustation_errors_ty_setup is used.
+% A special optical element (called a "straightness interferometer"[fn:9]) is fixed on top of the micro-station, while a laser source[fn:10] and a straightness reflector are fixed on the ground.
+% A similar setup was used to measure the horizontal deviation (i.e. in the $x$ direction), as well as the pitch and yaw errors of the translation stage.
+
+% #+name: fig:ustation_errors_ty_setup
+% #+caption: Experimental setup to measure the flatness (vertical deviation) of the translation stage
+% [[file:figs/ustation_errors_ty_setup.png]]
+
+% Six scans were performed between $-4.5\,mm$ and $4.5\,mm$.
+% The results for each individual scan are shown in Figure ref:fig:ustation_errors_dy_vertical.
+% The measurement axis may not be perfectly aligned with the translation stage axis; this, a linear fit is removed from the measurement.
+% The remaining vertical displacement is shown in Figure ref:fig:ustation_errors_dy_vertical_remove_mean.
+% A vertical error of $\pm300\,nm$ induced by the translation stage is expected.
+% Similar result is obtained for the $x$ lateral direction.
+
+
+%% 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('Vertical error [$\mu$m]');
+xlim([-5, 5]); ylim([-0.4, 0.4]);
+
+
+
+% #+name: fig:ustation_errors_dy
+% #+caption: Measurement of the linear (vertical) deviation of the Translation stage (\subref{fig:ustation_errors_dy_vertical}). A linear fit is then removed from the data (\subref{fig:ustation_errors_dy_vertical_remove_mean}).
+% #+attr_latex: :options [htbp]
+% #+begin_figure
+% #+attr_latex: :caption \subcaption{\label{fig:ustation_errors_dy_vertical}Measured vertical error}
+% #+attr_latex: :options {0.49\textwidth}
+% #+begin_subfigure
+% #+attr_latex: :width 0.95\linewidth
+% [[file:figs/ustation_errors_dy_vertical.png]]
+% #+end_subfigure
+% #+attr_latex: :caption \subcaption{\label{fig:ustation_errors_dy_vertical_remove_mean}Error after removing linear fit}
+% #+attr_latex: :options {0.49\textwidth}
+% #+begin_subfigure
+% #+attr_latex: :width 0.95\linewidth
+% [[file:figs/ustation_errors_dy_vertical_remove_mean.png]]
+% #+end_subfigure
+% #+end_figure
+
+
+%% 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
+
+% To measure the positioning errors induced by the Spindle, a "Spindle error analyzer"[fn:7] is used as shown in Figure ref:fig:ustation_rz_meas_lion_setup.
+% A specific target is fixed on top of the micro-station, which consists of two sphere with 1 inch diameter precisely aligned with the spindle rotation axis.
+% Five capacitive sensors[fn:8] are pointing at the two spheres, as shown in Figure ref:fig:ustation_rz_meas_lion_zoom.
+% From the 5 measured displacements $[d_1,\,d_2,\,d_3,\,d_4,\,d_5]$, the translations and rotations $[D_x,\,D_y,\,D_z,\,R_x,\,R_y]$ of the target can be estimated.
+
+% #+name: fig:ustation_rz_meas_lion_setup
+% #+caption: Experimental setup used to estimate the errors induced by the Spindle rotation (\subref{fig:ustation_rz_meas_lion}). The motion of the two reference spheres is measured using 5 capacitive sensors (\subref{fig:ustation_rz_meas_lion_zoom})
+% #+attr_latex: :options [htbp]
+% #+begin_figure
+% #+attr_latex: :caption \subcaption{\label{fig:ustation_rz_meas_lion}Micro-station and 5-DoF metrology}
+% #+attr_latex: :options {0.49\textwidth}
+% #+begin_subfigure
+% #+attr_latex: :width 0.9\linewidth
+% [[file:figs/ustation_rz_meas_lion.jpg]]
+% #+end_subfigure
+% #+attr_latex: :caption \subcaption{\label{fig:ustation_rz_meas_lion_zoom}Zoom on the metrology system}
+% #+attr_latex: :options {0.49\textwidth}
+% #+begin_subfigure
+% #+attr_latex: :width 0.9\linewidth
+% [[file:figs/ustation_rz_meas_lion_zoom.jpg]]
+% #+end_subfigure
+% #+end_figure
+
+% A measurement was performed during a constant rotational velocity of the spindle of 60rpm and during 10 turns.
+% The obtained results are shown in Figure ref:fig:ustation_errors_spindle.
+% A large fraction of the radial (Figure ref:fig:ustation_errors_spindle_radial) and tilt (Figure ref:fig:ustation_errors_spindle_tilt) errors is linked to the fact that the two spheres are not perfectly aligned with the rotation axis of the Spindle.
+% This is displayed by the dashed circle.
+% After removing the best circular fit from the data, the vibrations induced by the Spindle may be viewed as stochastic disturbances.
+% However, some misalignment between the "point-of-interest" of the sample and the rotation axis will be considered because the alignment is not perfect in practice.
+% The vertical motion induced by scanning the spindle is in the order of $\pm 30\,nm$ (Figure ref:fig:ustation_errors_spindle_axial).
+
+
+%% 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]);
+
+
+
+% #+name: fig:ustation_errors_spindle
+% #+caption: Measurement of the radial (\subref{fig:ustation_errors_spindle_radial}), axial (\subref{fig:ustation_errors_spindle_axial}) and tilt (\subref{fig:ustation_errors_spindle_tilt}) Spindle errors during a 60rpm spindle rotation. The circular best fit is shown by the dashed circle. It represents the misalignment of the spheres with the rotation axis.
+% #+attr_latex: :options [htbp]
+% #+begin_figure
+% #+attr_latex: :caption \subcaption{\label{fig:ustation_errors_spindle_radial}Radial errors}
+% #+attr_latex: :options {0.33\textwidth}
+% #+begin_subfigure
+% #+attr_latex: :width 0.9\linewidth
+% [[file:figs/ustation_errors_spindle_radial.png]]
+% #+end_subfigure
+% #+attr_latex: :caption \subcaption{\label{fig:ustation_errors_spindle_axial}Axial error}
+% #+attr_latex: :options {0.33\textwidth}
+% #+begin_subfigure
+% #+attr_latex: :width 0.9\linewidth
+% [[file:figs/ustation_errors_spindle_axial.png]]
+% #+end_subfigure
+% #+attr_latex: :caption \subcaption{\label{fig:ustation_errors_spindle_tilt}Tilt errors}
+% #+attr_latex: :options {0.33\textwidth}
+% #+begin_subfigure
+% #+attr_latex: :width 0.9\linewidth
+% [[file:figs/ustation_errors_spindle_tilt.png]]
+% #+end_subfigure
+% #+end_figure
+
+
+%% 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);
+
+% Sensitivity to disturbances
+% <<ssec:ustation_disturbances_sensitivity>>
+
+% To compute the disturbance source (i.e. forces) that induced the measured vibrations in Section ref:ssec:ustation_disturbances_meas, the transfer function from the disturbance sources to the stage vibration (i.e. the "sensitivity to disturbances") needs to be estimated.
+% This is achieved using the multi-body model presented in Section ref:sec:ustation_modeling.
+% The obtained transfer functions are shown in Figure ref:fig:ustation_model_sensitivity.
+
+
+%% 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;
+
+% Obtained disturbance sources
+% <<ssec:ustation_disturbances_results>>
+
+% From the measured effect of disturbances in Section ref:ssec:ustation_disturbances_meas and the sensitivity to disturbances extracted from the Simscape model in Section ref:ssec:ustation_disturbances_sensitivity, the power spectral density of the disturbance sources (i.e. forces applied in the stage's joint) can be estimated.
+% The obtained power spectral density of the disturbances are shown in Figure ref:fig:ustation_dist_sources.
+
+
+%% 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;
+
+
+
+% #+name: fig:ustation_dist_sources
+% #+caption: Measured spectral density of the micro-station disturbance sources. Ground motion (\subref{fig:ustation_dist_source_ground_motion}), translation stage (\subref{fig:ustation_dist_source_translation_stage}) and spindle (\subref{fig:ustation_dist_source_spindle}).
+% #+attr_latex: :options [htbp]
+% #+begin_figure
+% #+attr_latex: :caption \subcaption{\label{fig:ustation_dist_source_ground_motion}Ground Motion}
+% #+attr_latex: :options {0.33\textwidth}
+% #+begin_subfigure
+% #+attr_latex: :width 0.9\linewidth
+% [[file:figs/ustation_dist_source_ground_motion.png]]
+% #+end_subfigure
+% #+attr_latex: :caption \subcaption{\label{fig:ustation_dist_source_translation_stage}Translation Stage}
+% #+attr_latex: :options {0.33\textwidth}
+% #+begin_subfigure
+% #+attr_latex: :width 0.9\linewidth
+% [[file:figs/ustation_dist_source_translation_stage.png]]
+% #+end_subfigure
+% #+attr_latex: :caption \subcaption{\label{fig:ustation_dist_source_spindle}Spindle}
+% #+attr_latex: :options {0.33\textwidth}
+% #+begin_subfigure
+% #+attr_latex: :width 0.9\linewidth
+% [[file:figs/ustation_dist_source_spindle.png]]
+% #+end_subfigure
+% #+end_figure
+
+% The disturbances are characterized by their power spectral densities, as shown in Figure ref:fig:ustation_dist_sources.
+% However, to perform time domain simulations, disturbances must be represented by a time domain signal.
+% To generate stochastic time-domain signals with a specific power spectral densities, the discrete inverse Fourier transform is used, as explained in [[cite:&preumont94_random_vibrat_spect_analy chap. 12.11]].
+% Examples of the obtained time-domain disturbance signals are shown in Figure ref:fig:ustation_dist_sources_time.
+
+
+%% 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', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 15;

matlab/ustation_4_experiments.m

@@ -0,0 +1,164 @@
+% Matlab Init                                              :noexport:ignore:
+
+%% 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
+% <<sec:ustation_experiments_tomography>>
+
+% To simulate a tomography experiment, the setpoint of the Spindle is configured to perform a constant rotation with a rotational velocity of 60rpm.
+% Both ground motion and spindle vibration disturbances were simulated based on what was computed in Section ref:sec:ustation_disturbances.
+% A radial offset of $\approx 1\,\mu m$ between the "point-of-interest" and the spindle's rotation axis is introduced to represent what is experimentally observed.
+% During the 10 second simulation (i.e. 10 spindle turns), the position of the "point-of-interest" with respect to the granite was recorded.
+% Results are shown in Figure ref:fig:ustation_errors_model_spindle.
+% A good correlation with the measurements is observed both for radial errors (Figure ref:fig:ustation_errors_model_spindle_radial) and axial errors (Figure ref:fig:ustation_errors_model_spindle_axial).
+
+
+%% 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;
+
+% Raster Scans with the translation stage
+% <<sec:ustation_experiments_ty_scans>>
+
+% A second experiment was performed in which the translation stage was scanned at constant velocity.
+% The translation stage setpoint is configured to have a "triangular" shape with stroke of $\pm 4.5\, mm$.
+% Both ground motion and translation stage vibrations were included in the simulation.
+% Similar to what was performed for the tomography simulation, the PoI position with respect to the granite was recorded and compared with the experimental measurements in Figure ref:fig:ustation_errors_model_dy_vertical.
+% A similar error amplitude was observed, thus indicating that the multi-body model with the included disturbances accurately represented the micro-station behavior in typical scientific experiments.
+
+
+%% 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;

simscape-micro-station.org

@@ -322,564 +322,6 @@ CLOSED: [2024-11-06 Wed 16:29]
 
 PoI | Point of interest
 
-** Backup - Kinematics
-*** Micro-Station DoF table
-
-#+name: tab:ustation_dof_summary
-#+caption: Summary of the micro-station degrees-of-freedom
-#+attr_latex: :environment tabularx :width \linewidth :align lX
-#+attr_latex: :center t :booktabs t
-| *Stage*           | *Degrees of Freedom*                                  |
-|-------------------+-------------------------------------------------------|
-| Translation stage | $D_y = \pm 10\,mm$                                    |
-| Tilt stage        | $R_y = \pm 3\,\text{deg}$                             |
-| Spindle           | $R_z = 360\,\text{deg}$                               |
-| Micro Hexapod     | $D_{xyz} = \pm 10\,mm$, $R_{xyz} = \pm 3\,\text{deg}$ |
-
-*** Stage specifications
-
-Translation Stage
-| Axial Motion ($y$) | Radial Motion ($y-z$) | Rotation motion ($\theta-z$) |
-|--------------------+-----------------------+------------------------------|
-| $40nm$  repeat     | $20nm$                | $< 1.7 \mu rad$              |
-
-Tilt stage
-| Axial Error ($y$) | Radial Error ($z$) | Tilt error ($R_y$) |
-|-------------------+--------------------+--------------------|
-| $0.5\mu m$        | $10nm$             | $5 \mu rad$ repeat |
-
-Spindle
-| Radial Error ($x$-$y$) | Vertical Error ($z$) | Rz                         |
-|------------------------+----------------------+----------------------------|
-| $0.33\mu m$            | $0.07\mu m$          | $5\,\mu \text{rad}$ repeat |
-
-Micro Hexapod
-| Motion           | Stroke       | Repetability   | MIM          | Stiffness     |
-|------------------+--------------+----------------+--------------+---------------|
-| $T_{\mu_x}$      | $\pm10mm$    | $\pm1\mu m$    | $0.5\mu m$   | $>12N/\mu m$  |
-| $T_{\mu_y}$      | $\pm10mm$    | $\pm1\mu m$    | $0.5\mu m$   | $>12N/\mu m$  |
-| $T_{\mu_z}$      | $\pm10mm$    | $\pm1\mu m$    | $0.5\mu m$   | $>135N/\mu m$ |
-| $\theta_{\mu_x}$ | $\pm3 deg$   | $\pm5 \mu rad$ | $2.5\mu rad$ |               |
-| $\theta_{\mu_y}$ | $\pm3 deg$   | $\pm5 \mu rad$ | $2.5\mu rad$ |               |
-| $\theta_{\mu_z}$ | $\pm0.5 deg$ | $\pm5 \mu rad$ | $2.5\mu rad$ |               |
-
-*** Frames
-Let's define the following frames:
-- $\{W\}$ the frame that is *fixed to the granite* and its origin at the theoretical meeting point between the X-ray and the spindle axis.
-- $\{S\}$ the frame *attached to the sample* (in reality attached to the top platform of the nano-hexapod) with its origin at 175mm above the top platform of the nano-hexapod.
-  Its origin is $O_S$.
-- $\{T\}$ the theoretical wanted frame that correspond to the wanted pose of the frame $\{S\}$.
-  $\{T\}$ is computed from the wanted position of each stage. It is thus theoretical and does not correspond to a real position.
-  The origin of $T$ is $O_T$ and is the wanted position of the sample.
-
-Thus:
-- the *measurement* of the position of the sample corresponds to ${}^W O_S = \begin{bmatrix} {}^WP_{x,m} & {}^WP_{y,m} & {}^WP_{z,m} \end{bmatrix}^T$ in translation and to $\theta_m {}^W\mathbf{s}_m = \theta_m \cdot \begin{bmatrix} {}^Ws_{x,m} & {}^Ws_{y,m} & {}^Ws_{z,m} \end{bmatrix}^T$ in rotations
-- the *wanted position* of the sample expressed w.r.t. the granite is ${}^W O_T = \begin{bmatrix} {}^WP_{x,r} & {}^WP_{y,r} & {}^WP_{z,r} \end{bmatrix}^T$ in translation and to $\theta_r {}^W\mathbf{s}_r = \theta_r \cdot \begin{bmatrix} {}^Ws_{x,r} & {}^Ws_{y,r} & {}^Ws_{z,r} \end{bmatrix}^T$ in rotations
-*** Positioning Error with respect to the Granite
-The wanted position expressed with respect to the granite is ${}^WO_T$ and the measured position with respect to the granite is ${}^WO_S$, thus the *position error* expressed in $\{W\}$ is
-\[ {}^W E = {}^W O_T - {}^W O_S \]
-The same is true for rotations:
-\[ \theta_\epsilon {}^W\mathbf{s}_\epsilon = \theta_r {}^W\mathbf{s}_r - \theta_m {}^W\mathbf{s}_m \]
-
-#+begin_src matlab
-  WPe = WPr - WPm;
-#+end_src
-
-#+begin_quote
-Now we want to express this error in a frame attached to the *base of the nano-hexapod* with its origin at the same point where the Jacobian of the nano-hexapod is computed (175mm above the top platform + 90mm of total height of the nano-hexapod).
-
-Or maybe should we want to express this error with respect to the *top platform of the nano-hexapod*?
-We are measuring the position of the top-platform, and we don't know exactly the position of the bottom platform.
-We could compute the position of the bottom platform in two ways:
-- from the encoders of each stage
-- from the measurement of the nano-hexapod top platform + the internal metrology in the nano-hexapod (capacitive sensors e.g)
-
-A third option is to say that the maximum stroke of the nano-hexapod is so small that the error should no change to much by the change of base.
-#+end_quote
-
-*** Position Error Expressed in the Nano-Hexapod Frame
-We now want the position error to be expressed in $\{S\}$ (the frame attach to the sample) for control:
-\[ {}^S E = {}^S T_W \cdot {}^W E \]
-
-Thus we need to compute the homogeneous transformation ${}^ST_W$.
-Fortunately, this homogeneous transformation can be computed from the measurement of the sample position and orientation with respect to the granite.
-#+begin_src matlab
-  Trxm = [1 0         0;
-          0 cos(Rxm) -sin(Rxm);
-          0 sin(Rxm)  cos(Rxm)];
-  Trym = [ cos(Rym) 0 sin(Rym);
-           0        1 0;
-          -sin(Rym) 0 cos(Rym)];
-  Trzm = [cos(Rzm) -sin(Rzm) 0;
-          sin(Rzm)  cos(Rzm) 0;
-          0         0        1];
-
-  STw = [[ Trym*Trxm*Trzm , [Dxm; Dym; Dzm]]; 0 0 0 1];
-#+end_src
-
-Translation Error.
-#+begin_src matlab
-  SEm = STw * [WPe(1:3); 0];
-  SEm = SEm(1:3);
-#+end_src
-
-Rotation Error.
-#+begin_src matlab
-  SEr = STw * [WPe(4:6); 0];
-  SEr = SEr(1:3);
-#+end_src
-
-#+begin_src matlab
-  Etot = [SEm ; SEr]
-#+end_src
-
-*** Another try
-Let's denote:
-- $\{W\}$ the initial fixed frame
-- $\{R\}$ the reference frame corresponding to the wanted pose of the sample
-- $\{M\}$ the frame corresponding to the measured pose of the sample
-
-We have then computed:
-- ${}^WT_R$
-- ${}^WT_M$
-
-We have:
-\begin{align}
-  {}^MT_R &= {}^MT_W {}^WT_R \\
-          &= {}^WT_M^t {}^WT_R
-\end{align}
-
-#+begin_src matlab
-  MTr = STw'*Ttot;
-#+end_src
-
-Position error:
-#+begin_src matlab
-  MTr(1:3, 1:4)*[0; 0; 0; 1]
-#+end_src
-
-Orientation error:
-#+begin_src matlab
-  MTr(1:3, 1:3)
-#+end_src
-
-
-
-*** Measured Position of the Sample with respect to the Granite
-The measurement of the position of the sample using the metrology system gives the position and orientation of the sample with respect to the granite.
-#+begin_src matlab
-  % Measurements: Xm, Ym, Zm, Rx, Ry, Rz
-  Dxm = 0; % [m]
-  Dym = 0; % [m]
-  Dzm = 0; % [m]
-
-  Rxm = 0*pi/180; % [rad]
-  Rym = 0*pi/180; % [rad]
-  Rzm = 180*pi/180; % [rad]
-#+end_src
-
-Let's compute the corresponding orientation using screw axis.
-#+begin_src matlab
-  Trxm = [1 0         0;
-          0 cos(Rxm) -sin(Rxm);
-          0 sin(Rxm)  cos(Rxm)];
-  Trym = [ cos(Rym) 0 sin(Rym);
-           0        1 0;
-          -sin(Rym) 0 cos(Rym)];
-  Trzm = [cos(Rzm) -sin(Rzm) 0;
-          sin(Rzm)  cos(Rzm) 0;
-          0         0        1];
-
-  STw = [[ Trym*Trxm*Trzm , [Dxm; Dym; Dzm]]; 0 0 0 1];
-#+end_src
-
-We then obtain the orientation measurement in the form of screw coordinate $\theta_m ({}^Ws_{x,m},\ {}^Ws_{y,m},\ {}^Ws_{z,m})^T$ where:
-- $\theta_m = \cos^{-1} \frac{\text{Tr}(R) - 1}{2}$
-- ${}^W\mathbf{s}_m$ is the eigen vector of the rotation matrix $R$ corresponding to the eigen value $\lambda = 1$
-
-#+begin_src matlab
-  thetam = acos((trace(STw(1:3, 1:3))-1)/2); % [rad]
-  if thetam == 0
-    WSm = [0; 0; 0];
-  else
-    [V, D] = eig(STw(1:3, 1:3));
-    WSm = thetam*V(:, abs(diag(D) - 1) < eps(1));
-  end
-#+end_src
-
-#+begin_src matlab
-  WPm = [Dxm ; Dym ; Dzm ; WSm];
-#+end_src
-
-*** Get resonance frequencies
-#+begin_src matlab
-%% Initialize simulation with default parameters (flexible elements)
-initializeGround();
-initializeGranite();
-initializeTy();
-initializeRy();
-initializeRz();
-initializeMicroHexapod();
-
-initializeReferences();
-initializeDisturbances();
-initializeSimscapeConfiguration();
-initializeLoggingConfiguration();
-#+end_src
-
-And we identify the dynamics from forces/torques applied on the micro-hexapod top platform to the motion of the micro-hexapod top platform at the same point.
-
-#+begin_src matlab
-%% Identification of the compliance
-% 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'};
-#+end_src
-
-#+begin_src matlab
-modes_freq = imag(eig(Gm))/2/pi;
-modes_freq = sort(modes_freq(modes_freq>0));
-#+end_src
-
-#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
-data2orgtable([modes_freq(1:16), [11.9, 18.6, 37.8, 39.1, 56.3, 69.8, 72.5, 84.8, 91.3, 105.5, 106.6, 112.7, 124.2, 145.3, 150.5, 165.4]'], {'1', '2', '3', '4', '5', '6', '7', '8', '9', '10', '11', '12', '13', '14', '15', '16'}, {'Mode', 'Simscape', 'Modal analysis'}, ' %.1f ');
-#+end_src
-
-#+RESULTS:
-| Mode | Simscape | Modal analysis |
-|------+----------+----------------|
-|    1 |     11.7 |           11.9 |
-|    2 |     21.3 |           18.6 |
-|    3 |     26.1 |           37.8 |
-|    4 |     57.5 |           39.1 |
-|    5 |     60.6 |           56.3 |
-|    6 |     73.0 |           69.8 |
-|    7 |     97.9 |           72.5 |
-|    8 |    120.2 |           84.8 |
-|    9 |    126.2 |           91.3 |
-|   10 |    142.4 |          105.5 |
-|   11 |    155.9 |          106.6 |
-|   12 |    178.5 |          112.7 |
-|   13 |    179.3 |          124.2 |
-|   14 |    182.6 |          145.3 |
-|   15 |    223.6 |          150.5 |
-|   16 |    226.4 |          165.4 |
-
-*** Noise Budget
-<<ssec:ustation_disturbances_results>>
-
-- [ ] Compare the PSD of the Z relative motion of the sample due to all disturbances
-- [ ] Is it relevant here as it should be more relevant when doing control / with the nano-hexapod, here we just want to make sure that we have a good model!
-
-From the obtained spectral density of the disturbance sources, we can compute the resulting relative motion of the Hexapod with respect to the granite using the model.
-
-This is equivalent as doing the inverse that was done in the previous section.
-This is done in order to verify that this is coherent with the measurements.
-
-The power spectral density of the relative motion is computed below and the result is shown in Figure ref:fig:psd_effect_dist_verif.
-We can see that this is exactly the same as the Figure ref:fig:dist_effect_relative_motion.
-#+begin_src matlab
-psd_gm_d = gm.psd_gm.*abs(squeeze(freqresp(G('Dz', 'Dw')/s, gm.f, 'Hz'))).^2;
-psd_ty_d = tyz.psd_f.*abs(squeeze(freqresp(G('Dz', 'Fdy')/s, tyz.f, 'Hz'))).^2;
-psd_rz_d = rz.psd_f.*abs(squeeze(freqresp(G('Dz', 'Frz')/s, rz.f, 'Hz'))).^2;
-#+end_src
-
-#+begin_src matlab :exports none
-figure;
-hold on;
-plot(gm.f,  sqrt(psd_gm_d), 'DisplayName', 'Ground Motion');
-plot(tyz.f, sqrt(psd_ty_d), 'DisplayName', 'Ty');
-plot(rz.f,  sqrt(psd_rz_d), 'DisplayName', 'Rz');
-hold off;
-set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
-xlabel('Frequency [Hz]'); ylabel('ASD of the relative motion $\left[\frac{m}{\sqrt{Hz}}\right]$')
-legend('Location', 'southwest');
-xlim([2, 500]);
-#+end_src
-
-*** Time Domain Disturbances
-Let's initialize the time domain disturbances and load them.
-#+begin_src matlab
-initializeDisturbances();
-dist = load('nass_disturbances.mat');
-#+end_src
-
-The time domain disturbance signals are shown in Figure ref:fig:disturbances_time_domain.
-
-#+begin_src matlab :exports none
-figure;
-
-ax1 = subplot(2, 2, 1);
-hold on;
-plot(dist.t, dist.Dwx, 'DisplayName', '$D_{w,x}$')
-plot(dist.t, dist.Dwy, 'DisplayName', '$D_{w,y}$')
-plot(dist.t, dist.Dwz, 'DisplayName', '$D_{w,z}$')
-hold off;
-xlabel('Time [s]');
-ylabel('Ground Motion [m]');
-legend();
-
-ax2 = subplot(2, 2, 2);
-hold on;
-plot(dist.t, dist.Fdy_x, 'DisplayName', '$F_{ty,x}$')
-hold off;
-xlabel('Time [s]');
-ylabel('Ty Forces [N]');
-legend();
-
-ax3 = subplot(2, 2, 3);
-hold on;
-plot(dist.t, dist.Fdy_z, 'DisplayName', '$F_{ty,z}$')
-hold off;
-xlabel('Time [s]');
-ylabel('Ty Forces [N]');
-legend();
-
-ax4 = subplot(2, 2, 4);
-hold on;
-plot(dist.t, dist.Frz_z, 'DisplayName', '$F_{rz,z}$')
-hold off;
-xlabel('Time [s]');
-ylabel('Rz Forces [N]');
-legend();
-
-linkaxes([ax1,ax2,ax3,ax4], 'x');
-xlim([0, dist.t(end)]);
-#+end_src
-
-*** Time Domain Effect of Disturbances
-**** Initialization of the Experiment
-We initialize all the stages with the default parameters.
-#+begin_src matlab
-  initializeGround();
-  initializeGranite();
-  initializeTy();
-  initializeRy();
-  initializeRz();
-  initializeMicroHexapod();
-  initializeAxisc();
-  initializeMirror();
-#+end_src
-
-The nano-hexapod is a piezoelectric hexapod and the sample has a mass of 50kg.
-#+begin_src matlab
-  initializeNanoHexapod('type', 'rigid');
-  initializeSample('mass', 1);
-#+end_src
-
-#+begin_src matlab
-  initializeReferences();
-  initializeController('type', 'open-loop');
-  initializeSimscapeConfiguration('gravity', false);
-  initializeLoggingConfiguration('log', 'all');
-#+end_src
-
-#+begin_src matlab
-  load('mat/conf_simulink.mat');
-  set_param(conf_simulink, 'StopTime', '2');
-#+end_src
-
-**** Simulations
-
-No disturbances:
-#+begin_src matlab
-  initializeDisturbances('enable', false);
-  sim('nass_model');
-  sim_no = simout;
-#+end_src
-
-Ground Motion:
-#+begin_src matlab
-  initializeDisturbances('Fdy_x', false, 'Fdy_z', false, 'Frz_x', false, 'Frz_y', false, 'Frz_z', false);
-  sim('nass_model');
-  sim_gm = simout;
-#+end_src
-
-Translation Stage Vibrations:
-#+begin_src matlab
-  initializeDisturbances('Dwx', false, 'Dwy', false, 'Dwz', false, 'Frz_z', false);
-  sim('nass_model');
-  sim_ty = simout;
-#+end_src
-
-Rotation Stage Vibrations:
-#+begin_src matlab
-  initializeDisturbances('Dwx', false, 'Dwy', false, 'Dwz', false, 'Fdy_x', false, 'Fdy_z', false);
-  sim('nass_model');
-  sim_rz = simout;
-#+end_src
-
-**** Comparison
-Let's now compare the effect of those perturbations on the position error of the sample (Figure ref:fig:effect_disturbances_position_error)
-
-#+begin_src matlab :exports none
-  figure;
-  ax1 = subplot(2, 3, 1);
-  hold on;
-  plot(sim_no.Em.En.Time, sim_no.Em.En.Data(:, 1))
-  plot(sim_gm.Em.En.Time, sim_gm.Em.En.Data(:, 1))
-  plot(sim_ty.Em.En.Time, sim_ty.Em.En.Data(:, 1))
-  plot(sim_rz.Em.En.Time, sim_rz.Em.En.Data(:, 1))
-  hold off;
-  xlabel('Time [s]');
-  ylabel('Dx [m]');
-
-  ax2 = subplot(2, 3, 2);
-  hold on;
-  plot(sim_no.Em.En.Time, sim_no.Em.En.Data(:, 2))
-  plot(sim_gm.Em.En.Time, sim_gm.Em.En.Data(:, 2))
-  plot(sim_ty.Em.En.Time, sim_ty.Em.En.Data(:, 2))
-  plot(sim_rz.Em.En.Time, sim_rz.Em.En.Data(:, 2))
-  hold off;
-  xlabel('Time [s]');
-  ylabel('Dy [m]');
-
-  ax3 = subplot(2, 3, 3);
-  hold on;
-  plot(sim_no.Em.En.Time, sim_no.Em.En.Data(:, 3))
-  plot(sim_gm.Em.En.Time, sim_gm.Em.En.Data(:, 3))
-  plot(sim_ty.Em.En.Time, sim_ty.Em.En.Data(:, 3))
-  plot(sim_rz.Em.En.Time, sim_rz.Em.En.Data(:, 3))
-  hold off;
-  xlabel('Time [s]');
-  ylabel('Dz [m]');
-
-  ax4 = subplot(2, 3, 4);
-  hold on;
-  plot(sim_no.Em.En.Time, sim_no.Em.En.Data(:, 4))
-  plot(sim_gm.Em.En.Time, sim_gm.Em.En.Data(:, 4))
-  plot(sim_ty.Em.En.Time, sim_ty.Em.En.Data(:, 4))
-  plot(sim_rz.Em.En.Time, sim_rz.Em.En.Data(:, 4))
-  hold off;
-  xlabel('Time [s]');
-  ylabel('Rx [rad]');
-
-  ax5 = subplot(2, 3, 5);
-  hold on;
-  plot(sim_no.Em.En.Time, sim_no.Em.En.Data(:, 5))
-  plot(sim_gm.Em.En.Time, sim_gm.Em.En.Data(:, 5))
-  plot(sim_ty.Em.En.Time, sim_ty.Em.En.Data(:, 5))
-  plot(sim_rz.Em.En.Time, sim_rz.Em.En.Data(:, 5))
-  hold off;
-  xlabel('Time [s]');
-  ylabel('Ry [rad]');
-
-  ax6 = subplot(2, 3, 6);
-  hold on;
-  plot(sim_no.Em.En.Time, sim_no.Em.En.Data(:, 6), 'DisplayName', 'No')
-  plot(sim_gm.Em.En.Time, sim_gm.Em.En.Data(:, 6), 'DisplayName', 'Dw')
-  plot(sim_ty.Em.En.Time, sim_ty.Em.En.Data(:, 6), 'DisplayName', 'Ty')
-  plot(sim_rz.Em.En.Time, sim_rz.Em.En.Data(:, 6), 'DisplayName', 'Rz')
-  hold off;
-  xlabel('Time [s]');
-  ylabel('Rz [rad]');
-  legend();
-
-  linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');
-#+end_src
-
-#+header: :tangle no :exports results :results none :noweb yes
-#+begin_src matlab :var filepath="figs/effect_disturbances_position_error.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
-<<plt-matlab>>
-#+end_src
-
-#+name: fig:effect_disturbances_position_error
-#+caption: Effect of Perturbations on the position error ([[./figs/effect_disturbances_position_error.png][png]], [[./figs/effect_disturbances_position_error.pdf][pdf]])
-[[file:figs/effect_disturbances_position_error.png]]
-
-
-*** Power Spectral Density of the effect of the disturbances
-
-#+begin_src matlab
-figure;
-hold on;
-hold off;
-set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
-xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{\mu m}{\sqrt{Hz}}\right]$')
-#+end_src
-
-#+begin_src matlab
-figure;
-hold on;
-plot(f_rz, sqrt(pxx_rz_dx), 'DisplayName', '$D_x$')
-plot(f_rz, sqrt(pxx_rz_dy), 'DisplayName', '$D_y$')
-plot(f_rz, sqrt(pxx_rz_dz), 'DisplayName', '$D_z$')
-plot(f_dy, sqrt(pxx_dy_dz))
-hold off;
-set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
-xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{m}{\sqrt{Hz}}\right]$')
-xlim([1, 200]);
-legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
-#+end_src
-
-#+begin_src matlab :results none :exports results
-figure;
-hold on;
-plot(f_rz(f_rz>1&f_rz<200), 1e9*sqrt(flip(-cumtrapz(flip(f_rz(f_rz>1&f_rz<200)),flip(pxx_rz_dx(f_rz>1&f_rz<200))))), 'DisplayName', 'Spindle - $D_x$');
-plot(f_rz(f_rz>1&f_rz<200), 1e9*sqrt(flip(-cumtrapz(flip(f_rz(f_rz>1&f_rz<200)),flip(pxx_rz_dy(f_rz>1&f_rz<200))))), 'DisplayName', 'Spindle - $D_y$');
-plot(f_rz(f_rz>1&f_rz<200), 1e9*sqrt(flip(-cumtrapz(flip(f_rz(f_rz>1&f_rz<200)),flip(pxx_rz_dz(f_rz>1&f_rz<200))))), 'DisplayName', 'Spindle - $D_z$');
-plot(f_dy(f_dy>1&f_dy<200), 1e9*sqrt(flip(-cumtrapz(flip(f_dy(f_dy>1&f_dy<200)),flip(pxx_dy_dz(f_dy>1&f_dy<200))))), 'DisplayName', 'Spindle - $D_z$');
-hold off;
-set(gca, 'xscale', 'log');
-set(gca, 'yscale', 'log');
-xlabel('Frequency [Hz]'); ylabel('CAS [nm RMS]')
-legend('Location', 'southwest');
-xlim([1, 200]);
-#+end_src
-
-
-#+begin_src matlab
-gm  = load('matlab/mat/dist/psd_gm.mat', 'f', 'psd_gm');
-rz  = load('matlab/mat/dist/pxsp_r.mat', 'f', 'pxsp_r');
-tyz = load('matlab/mat/dist/pxz_ty_r.mat', 'f', 'pxz_ty_r');
-tyx = load('matlab/mat/dist/pxe_ty_r.mat', 'f', 'pxe_ty_r');
-#+end_src
-
-#+begin_src matlab :exports none
-gm.f  = gm.f(2:end);
-rz.f  = rz.f(2:end);
-tyz.f = tyz.f(2:end);
-tyx.f = tyx.f(2:end);
-
-gm.psd_gm  = gm.psd_gm(2:end); % PSD of Ground Motion [m^2/Hz]
-rz.pxsp_r  = rz.pxsp_r(2:end); % PSD of Relative Velocity [(m/s)^2/Hz]
-tyz.pxz_ty_r = tyz.pxz_ty_r(2:end); % PSD of Relative Velocity [(m/s)^2/Hz]
-tyx.pxe_ty_r = tyx.pxe_ty_r(2:end); % PSD of Relative Velocity [(m/s)^2/Hz]
-#+end_src
-
-Because some 50Hz and harmonics were present in the ground motion measurement, we remove these peaks with the following code:
-#+begin_src matlab
-f0s = [50, 100, 150, 200, 250, 350, 450];
-for f0 = f0s
-    i = find(gm.f > f0-0.5 & gm.f < f0+0.5);
-    gm.psd_gm(i) = linspace(gm.psd_gm(i(1)), gm.psd_gm(i(end)), length(i));
-end
-#+end_src
-
-We now compute the relative velocity between the hexapod and the granite due to ground motion.
-#+begin_src matlab
-gm.psd_rv = gm.psd_gm.*abs(squeeze(freqresp(G('Dz', 'Dw'), gm.f, 'Hz'))).^2;
-#+end_src
-
-#+begin_src matlab :exports none
-figure;
-hold on;
-plot(gm.f, sqrt(gm.psd_rv), 'DisplayName', 'Ground Motion');
-plot(tyz.f, sqrt(tyz.pxz_ty_r), 'DisplayName', 'Ty');
-plot(rz.f, sqrt(rz.pxsp_r), 'DisplayName', 'Rz');
-hold off;
-set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
-xlabel('Frequency [Hz]'); ylabel('ASD of the measured velocity $\left[\frac{m/s}{\sqrt{Hz}}\right]$')
-legend('Location', 'southwest');
-xlim([2, 500]);
-#+end_src
-
 * Introduction                                                        :ignore:
 
 From the start of this work, it became increasingly clear that an accurate micro-station model was necessary.
@@ -1310,8 +752,8 @@ 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(Ery), Ttot(3, 3)/cos(Ery));
-ustation_rz = atan2(-Ttot(1, 2)/cos(Ery), Ttot(1, 1)/cos(Ery));
+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
@@ -1341,8 +783,8 @@ 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(Ery), T_sim(3, 3)/cos(Ery));
-sim_rz = atan2(-T_sim(1, 2)/cos(Ery), T_sim(1, 1)/cos(Ery));
+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));
 #+end_src
 
 * Micro-Station Dynamics
@@ -1491,7 +933,7 @@ 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'}
+stages = {'gbot', 'gtop', 'ty', 'ry', 'rz', 'hexa'};
 f = logspace(log10(10), log10(500), 1000);
 #+end_src
 
@@ -2104,7 +1546,7 @@ exportFig('figs/ustation_errors_dy_vertical_remove_mean.pdf', 'width', 'half', '
 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
+Fs = 1/Ts;
 
 % Frequency Analysis
 Nfft = floor(length(ty_errors.setpoint)); % Number of frequency points
@@ -2242,7 +1684,7 @@ exportFig('figs/ustation_errors_spindle_tilt.pdf', 'width', 'third', 'height', '
 % 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)
+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));
@@ -2568,8 +2010,8 @@ 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.15, 0.15])
-leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+xlim([0, 1]); ylim([-0.6, 0.6])
+leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
 leg.ItemTokenSize(1) = 15;
 #+end_src