Made sure all Matlab scripts are working fine

matlab/ustation_1_kinematics.m

@@ -126,9 +126,9 @@ Ttot = Rty*Rry*Rrz;
 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_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
@@ -157,6 +157,6 @@ T_sim = [simout.y.R.Data(:,:,end), [simout.y.x.Data(end); simout.y.y.Data(end);
 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_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

@@ -77,7 +77,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);
 
 %% Spindle - X response

matlab/ustation_3_disturbances.m

@@ -139,7 +139,7 @@ xlim([-5, 5]); ylim([-0.4, 0.4]);
 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
@@ -259,7 +259,7 @@ yticks([-30, -15, 0, 15, 30]);
 % 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));
@@ -496,6 +496,6 @@ 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])
+xlim([0, 1]); ylim([-0.6, 0.6])
 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.
@@ -1309,9 +751,9 @@ Ttot = Rty*Rry*Rrz;
 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_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
@@ -1340,9 +782,9 @@ T_sim = [simout.y.R.Data(:,:,end), [simout.y.x.Data(end); simout.y.y.Data(end);
 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_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));
 #+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,7 +2010,7 @@ 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])
+xlim([0, 1]); ylim([-0.6, 0.6])
 leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
 leg.ItemTokenSize(1) = 15;
 #+end_src