tdehaeze/phd-test-bench-id31
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add tangled matlab files

Browse Source
This commit is contained in:
Thomas Dehaeze 2025-02-04 14:10:54 +01:00
parent af1e27b4c8
commit 5c23347a7e
9 changed files with 3528 additions and 107 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
matlab/mat/nass_model_controller.mat
View File

Binary file not shown.

BIN
matlab/mat/nass_model_disturbances.mat
View File

Binary file not shown.

60
matlab/src/initializeStewartPlatform.m
View File

@ -1,31 +1,31 @@
function [stewart] = initializeStewartPlatform()
% initializeStewartPlatform - Initialize the stewart structure
%
% Syntax: [stewart] = initializeStewartPlatform(args)
%
% Outputs:
% - stewart - A structure with the following sub-structures:
% - platform_F -
% - platform_M -
% - joints_F -
% - joints_M -
% - struts_F -
% - struts_M -
% - actuators -
% - geometry -
% - properties -
function [stewart] = initializeStewartPlatform()
% initializeStewartPlatform - Initialize the stewart structure
%
% Syntax: [stewart] = initializeStewartPlatform(args)
%
% Outputs:
% - stewart - A structure with the following sub-structures:
% - platform_F -
% - platform_M -
% - joints_F -
% - joints_M -
% - struts_F -
% - struts_M -
% - actuators -
% - geometry -
% - properties -
stewart = struct();
stewart.platform_F = struct();
stewart.platform_M = struct();
stewart.joints_F = struct();
stewart.joints_M = struct();
stewart.struts_F = struct();
stewart.struts_M = struct();
stewart.actuators = struct();
stewart.sensors = struct();
stewart.sensors.inertial = struct();
stewart.sensors.force = struct();
stewart.sensors.relative = struct();
stewart.geometry = struct();
stewart.kinematics = struct();
stewart = struct();
stewart.platform_F = struct();
stewart.platform_M = struct();
stewart.joints_F = struct();
stewart.joints_M = struct();
stewart.struts_F = struct();
stewart.struts_M = struct();
stewart.actuators = struct();
stewart.sensors = struct();
stewart.sensors.inertial = struct();
stewart.sensors.force = struct();
stewart.sensors.relative = struct();
stewart.geometry = struct();
stewart.kinematics = struct();

302
matlab/test_id31_1_metrology.m Normal file
View File

@ -0,0 +1,302 @@
%% 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
%% Data directory
data_dir = './mat/';
%% Colors for the figures
colors = colororder;
%% Frequency Vector
freqs = logspace(log10(1), log10(2e3), 1000);
%% Sampling Time
Ts = 1e-4;
%% Specifications for Experiments
specs_dz_peak = 50; % [nm]
specs_dy_peak = 100; % [nm]
specs_ry_peak = 0.85; % [urad]
specs_dz_rms = 15; % [nm RMS]
specs_dy_rms = 30; % [nm RMS]
specs_ry_rms = 0.25; % [urad RMS]
% Metrology Kinematics
% <<ssec:test_id31_metrology_kinematics>>
% The developed short-stroke metrology system is schematically shown in Figure ref:fig:test_id31_metrology_kinematics.
% The point of interest is indicated by the blue frame $\{B\}$, which is located $H = 150\,mm$ above the nano-hexapod's top platform.
% The spheres have a diameter $d = 25.4\,mm$, and indicated dimensions are $l_1 = 60\,mm$ and $l_2 = 16.2\,mm$.
% In order to compute the pose of the $\{B\}$ frame with respect to the granite (i.e. with respect to the fixed interferometer heads), the measured (small) displacements $[d_1,\ d_2,\ d_3,\ d_4,\ d_5]$ by the interferometers are first written as a function of the (small) linear and angular motion of the $\{B\}$ frame $[D_x,\ D_y,\ D_z,\ R_x,\ R_y]$ eqref:eq:test_id31_metrology_kinematics.
% \begin{equation}\label{eq:test_id31_metrology_kinematics}
% d_1 = D_y - l_2 R_x, \quad d_2 = D_y + l_1 R_x, \quad d_3 = -D_x - l_2 R_y, \quad d_4 = -D_x + l_1 R_y, \quad d_5 = -D_z
% \end{equation}
% #+attr_latex: :options [b]{0.48\linewidth}
% #+begin_minipage
% #+name: fig:test_id31_metrology_kinematics
% #+caption: Schematic of the measurement system. Measured distances are indicated by red arrows.
% #+attr_latex: :scale 1 :float nil
% [[file:figs/test_id31_metrology_kinematics.png]]
% #+end_minipage
% \hfill
% #+attr_latex: :options [b]{0.48\linewidth}
% #+begin_minipage
% #+name: fig:test_id31_align_top_sphere_comparators
% #+attr_latex: :width \linewidth :float nil
% #+caption: The top sphere is aligned with the rotation axis of the spindle using two probes.
% [[file:figs/test_id31_align_top_sphere_comparators.jpg]]
% #+end_minipage
% The five equations eqref:eq:test_id31_metrology_kinematics can be written in a matrix form, and then inverted to have the pose of the $\{B\}$ frame as a linear combination of the measured five distances by the interferometers eqref:eq:test_id31_metrology_kinematics_inverse.
% \begin{equation}\label{eq:test_id31_metrology_kinematics_inverse}
% \begin{bmatrix}
% D_x \\ D_y \\ D_z \\ R_x \\ R_y
% \end{bmatrix} = {\underbrace{\begin{bmatrix}
% 0 & 1 & 0 & -l_2 & 0 \\
% 0 & 1 & 0 & l_1 & 0 \\
% -1 & 0 & 0 & 0 & -l_2 \\
% -1 & 0 & 0 & 0 & l_1 \\
% 0 & 0 & -1 & 0 & 0
% \end{bmatrix}}_{\bm{J_d}}}^{-1} \cdot \begin{bmatrix}
% d_1 \\ d_2 \\ d_3 \\ d_4 \\ d_5
% \end{bmatrix}
% \end{equation}
%% Geometrical parameters of the metrology system
H = 150e-3;
l1 = (150-48-42)*1e-3;
l2 = (76.2+48+42-150)*1e-3;
% Computation of the Transformation matrix
Hm = [ 0 1 0 -l2 0;
0 1 0 l1 0;
-1 0 0 0 -l2;
-1 0 0 0 l1;
0 0 -1 0 0];
% Fine Alignment of reference spheres using interferometers
% <<ssec:test_id31_metrology_sphere_fine_alignment>>
% Thanks to the first alignment of the two reference spheres with the spindle axis (Section ref:ssec:test_id31_metrology_sphere_rought_alignment) and to the fine adjustment of the interferometers orientations (Section ref:ssec:test_id31_metrology_alignment), the spindle can perform complete rotations while still having interference for all five interferometers.
% This metrology can therefore be used to better align the axis defined by the two spheres' center with the spindle axis.
% The alignment process is made by few iterations.
% First, the spindle is scanned and the alignment errors are recorded.
% From the errors, the motion of the micro-hexapod to better align the spheres with the spindle axis is computed and the micro-hexapod is positioned accordingly.
% Then, the spindle is scanned again, and the new alignment errors are recorded.
% This iterative process is first performed for angular errors (Figure ref:fig:test_id31_metrology_align_rx_ry) and then for lateral errors (Figure ref:fig:test_id31_metrology_align_dx_dy).
% The remaining errors after alignment is in the order of $\pm5\,\mu\text{rad}$ in $R_x$ and $R_y$ orientations, $\pm 1\,\mu m$ in $D_x$ and $D_y$ directions and less than $0.1\,\mu m$ vertically.
%% Angular alignment
% Load Data
data_it0 = h5scan(data_dir, 'alignment', 'h1rx_h1ry', 1);
data_it1 = h5scan(data_dir, 'alignment', 'h1rx_h1ry_0002', 3);
data_it2 = h5scan(data_dir, 'alignment', 'h1rx_h1ry_0002', 5);
% Offset wrong points
i_it0 = find(abs(data_it0.Rx_int_filtered(2:end)-data_it0.Rx_int_filtered(1:end-1))>1e-5);
data_it0.Rx_int_filtered(i_it0+1:end) = data_it0.Rx_int_filtered(i_it0+1:end) + data_it0.Rx_int_filtered(i_it0) - data_it0.Rx_int_filtered(i_it0+1);
i_it1 = find(abs(data_it1.Rx_int_filtered(2:end)-data_it1.Rx_int_filtered(1:end-1))>1e-5);
data_it1.Rx_int_filtered(i_it1+1:end) = data_it1.Rx_int_filtered(i_it1+1:end) + data_it1.Rx_int_filtered(i_it1) - data_it1.Rx_int_filtered(i_it1+1);
i_it2 = find(abs(data_it2.Rx_int_filtered(2:end)-data_it2.Rx_int_filtered(1:end-1))>1e-5);
data_it2.Rx_int_filtered(i_it2+1:end) = data_it2.Rx_int_filtered(i_it2+1:end) + data_it2.Rx_int_filtered(i_it2) - data_it2.Rx_int_filtered(i_it2+1);
% Compute circle fit and get radius
[~, ~, R_it0, ~] = circlefit(1e6*data_it0.Rx_int_filtered, 1e6*data_it0.Ry_int_filtered);
[~, ~, R_it1, ~] = circlefit(1e6*data_it1.Rx_int_filtered, 1e6*data_it1.Ry_int_filtered);
[~, ~, R_it2, ~] = circlefit(1e6*data_it2.Rx_int_filtered, 1e6*data_it2.Ry_int_filtered);
%% Rx/Ry alignment of the spheres using the micro-station
figure;
hold on;
plot(1e6*data_it0.Rx_int_filtered, 1e6*data_it0.Ry_int_filtered, '-', ...
'DisplayName', sprintf('$R_0 = %.0f \\mu$rad', R_it0))
plot(1e6*data_it1.Rx_int_filtered, 1e6*data_it1.Ry_int_filtered, '-', ...
'DisplayName', sprintf('$R_1 = %.0f \\mu$rad', R_it1))
plot(1e6*data_it2.Rx_int_filtered, 1e6*data_it2.Ry_int_filtered, '-', 'color', colors(5,:), ...
'DisplayName', sprintf('$R_2 = %.0f \\mu$rad', R_it2))
hold off;
xlabel('$R_x$ [$\mu$rad]'); ylabel('$R_y$ [$\mu$rad]');
axis equal
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
xlim([-600, 300]);
ylim([-100, 800]);
%% Eccentricity alignment
% Load Data
data_it0 = h5scan(data_dir, 'alignment', 'h1rx_h1ry_0002', 5);
data_it1 = h5scan(data_dir, 'alignment', 'h1dx_h1dy', 1);
% Offset wrong points
i_it0 = find(abs(data_it0.Dy_int_filtered(2:end)-data_it0.Dy_int_filtered(1:end-1))>1e-5);
data_it0.Dy_int_filtered(i_it0+1:end) = data_it0.Dy_int_filtered(i_it0+1:end) + data_it0.Dy_int_filtered(i_it0) - data_it0.Dy_int_filtered(i_it0+1);
% Compute circle fit and get radius
[~, ~, R_it0, ~] = circlefit(1e6*data_it0.Dx_int_filtered, 1e6*data_it0.Dy_int_filtered);
[~, ~, R_it1, ~] = circlefit(1e6*data_it1.Dx_int_filtered, 1e6*data_it1.Dy_int_filtered);
%% Dx/Dy alignment of the spheres using the micro-station
figure;
hold on;
plot(1e6*data_it0.Dx_int_filtered, 1e6*data_it0.Dy_int_filtered, '-', ...
'DisplayName', sprintf('$R_0 = %.0f \\mu$m', R_it0))
plot(1e6*data_it1.Dx_int_filtered, 1e6*data_it1.Dy_int_filtered, '-', ...
'DisplayName', sprintf('$R_1 = %.0f \\mu$m', R_it1))
hold off;
xlabel('$D_x$ [$\mu$m]'); ylabel('$D_y$ [$\mu$m]');
axis equal
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
xlim([-1, 21]);
ylim([-8, 14]);
% Estimated measurement volume
% <<ssec:test_id31_metrology_acceptance>>
% Because the interferometers are pointing to spheres and not flat surfaces, the lateral acceptance is limited.
% In order to estimate the metrology acceptance, the micro-hexapod is used to perform three accurate scans of $\pm 1\,mm$, respectively along the $x$, $y$ and $z$ axes.
% During these scans, the 5 interferometers are recorded individually, and the ranges in which each interferometer has enough coupling efficiency to be able to measure the displacement are estimated.
% Results are summarized in Table ref:tab:test_id31_metrology_acceptance.
% The obtained lateral acceptance for pure displacements in any direction is estimated to be around $+/-0.5\,mm$, which is enough for the current application as it is well above the micro-station errors to be actively corrected by the NASS.
%% Estimated acceptance of the metrology
% This is estimated by moving the spheres using the micro-hexapod
% Dx
data_dx = h5scan(data_dir, 'metrology_acceptance_new_align', 'dx', 1);
dx_acceptance = zeros(5,1);
for i = [1:size(dx_acceptance, 1)]
% Find range in which the interferometers are measuring displacement
dx_di = diff(data_dx.(sprintf('d%i', i))) == 0;
if sum(dx_di) > 0
dx_acceptance(i) = data_dx.h1tx(find(dx_di(501:end), 1) + 500) - ...
data_dx.h1tx(find(flip(dx_di(1:500)), 1));
else
dx_acceptance(i) = data_dx.h1tx(end) - data_dx.h1tx(1);
end
end
% Dy
data_dy = h5scan(data_dir, 'metrology_acceptance_new_align', 'dy', 1);
dy_acceptance = zeros(5,1);
for i = [1:size(dy_acceptance, 1)]
% Find range in which the interferometers are measuring displacement
dy_di = diff(data_dy.(sprintf('d%i', i))) == 0;
if sum(dy_di) > 0
dy_acceptance(i) = data_dy.h1ty(find(dy_di(501:end), 1) + 500) - ...
data_dy.h1ty(find(flip(dy_di(1:500)), 1));
else
dy_acceptance(i) = data_dy.h1ty(end) - data_dy.h1ty(1);
end
end
% Dz
data_dz = h5scan(data_dir, 'metrology_acceptance_new_align', 'dz', 1);
dz_acceptance = zeros(5,1);
for i = [1:size(dz_acceptance, 1)]
% Find range in which the interferometers are measuring displacement
dz_di = diff(data_dz.(sprintf('d%i', i))) == 0;
if sum(dz_di) > 0
dz_acceptance(i) = data_dz.h1tz(find(dz_di(501:end), 1) + 500) - ...
data_dz.h1tz(find(flip(dz_di(1:500)), 1));
else
dz_acceptance(i) = data_dz.h1tz(end) - data_dz.h1tz(1);
end
end
% Estimated measurement errors
% <<ssec:test_id31_metrology_errors>>
% When using the NASS, the accuracy of the sample's positioning is determined by the accuracy of the external metrology.
% However, the validation of the nano-hexapod, the associated instrumentation and the control architecture is independent of the accuracy of the metrology system.
% Only the bandwidth and noise characteristics of the external metrology are important.
% Yet, some elements effecting the accuracy of the metrology are discussed here.
% First, the "metrology kinematics" (discussed in Section ref:ssec:test_id31_metrology_kinematics) is only approximate (i.e. valid for very small displacements).
% This can be easily seen when performing lateral $[D_x,\,D_y]$ scans using the micro-hexapod while recording the vertical interferometer (Figure ref:fig:test_id31_xy_map_sphere).
% As the interferometer is pointing to a sphere and not to a plane, lateral motion of the sphere is seen as a vertical motion by the top interferometer.
% Then, the reference spheres have some deviations with respect to an ideal sphere [fn:6].
% They are initially meant to be used with capacitive sensors which are integrating the shape errors over large surfaces.
% When using interferometers, the size of the "light spot" on the sphere surface is a circle with a diameter approximately equal to $50\,\mu m$, and therefore the measurement is more sensitive to shape errors with small features.
% As the light from the interferometer is travelling through air (as opposed to being in vacuum), the measured distance is sensitive to any variation in the refractive index of the air.
% Therefore, any variation of air temperature, pressure or humidity will induce measurement errors.
% For instance, for a measurement length of $40\,mm$, a temperature variation of $0.1\,{}^oC$ (which is typical for the ID31 experimental hutch) induces an errors in the distance measurement of $\approx 4\,nm$.
% Interferometers are also affected by noise [[cite:&watchi18_review_compac_inter]].
% The effect of the noise on the translation and rotation measurements is estimated in Figure ref:fig:test_id31_interf_noise.
%% Interferometer noise estimation
data = load("test_id31_interf_noise.mat");
Ts = 1e-4;
Nfft = floor(5/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);
[pxx_int, f] = pwelch(detrend(data.d, 0), win, Noverlap, Nfft, 1/Ts);
% Uncorrelated noise: square root of the sum of the squares
pxx_cart = pxx_int*sum(inv(Hm).^2, 2)';
rms_dxy = sqrt(trapz(f(f>1), pxx_cart((f>1),1))); % < 0.3 nm RMS
rms_dz = sqrt(trapz(f(f>1), pxx_cart((f>1),3))); % < 0.3 nm RMS
rms_rxy = sqrt(trapz(f(f>1), pxx_cart((f>1),4))); % 5 nrad RMS
figure;
hold on;
plot(f, sqrt(pxx_cart(:,1)), 'DisplayName', sprintf('$D_{x,y}$, %.1f nmRMS', rms_dxy));
plot(f, sqrt(pxx_cart(:,3)), 'DisplayName', sprintf('$D_{z}$, %.1f nmRMS', rms_dz));
plot(f, sqrt(pxx_cart(:,4)), 'DisplayName', sprintf('$R_{x,y}$, %.1f nradRMS', rms_rxy));
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{nm,\ nrad}{\sqrt{Hz}}\right]$')
xlim([1, 1e3]); ylim([1e-3, 1]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
%% X-Y scan with the micro-hexapod, and record of the vertical interferometer
data = h5scan(data_dir, 'metrology_acceptance', 'after_int_align_meshXY', 1);
x = 1e3*detrend(data.h1tx, 0); % [um]
y = 1e3*detrend(data.h1ty, 0); % [um]
z = 1e6*data.Dz_int_filtered - max(data.Dz_int_filtered); % [um]
mdl = scatteredInterpolant(x, y, z);
[xg, yg] = meshgrid(unique(x), unique(y));
zg = mdl(xg, yg);
% Fit a sphere to the data
[sphere_center,sphere_radius] = sphereFit(1e-3*[x, y, z]);
%% XY mapping of the Z measurement by the interferometer
figure;
[~,c] = contour3(xg,yg,zg,30);
c.LineWidth = 3;
xlabel('$D_x$ [$\mu$m]');
ylabel('$D_y$ [$\mu$m]');
zlabel('$D_z$ [$\mu$m]');
zlim([-1, 0]);
xticks(-100:50:100); yticks(-100:50:100); zticks(-1:0.2:0);

793
matlab/test_id31_2_open_loop_plant.m Normal file
View File

@ -0,0 +1,793 @@
% Matlab Init :noexport:ignore:
%% test_id31_2_open_loop_plant.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
%% Data directory
data_dir = './mat/';
% Simulink Model name
mdl = 'nass_model_id31';
%% Colors for the figures
colors = colororder;
%% Frequency Vector
freqs = logspace(log10(1), log10(2e3), 1000);
%% Sampling Time
Ts = 1e-4;
%% Specifications for Experiments
specs_dz_peak = 50; % [nm]
specs_dy_peak = 100; % [nm]
specs_ry_peak = 0.85; % [urad]
specs_dz_rms = 15; % [nm RMS]
specs_dy_rms = 30; % [nm RMS]
specs_ry_rms = 0.25; % [urad RMS]
% Open-Loop Plant Identification
% <<ssec:test_id31_open_loop_plant_first_id>>
% The plant dynamics is first identified for a fixed spindle angle (at $0\,\text{deg}$) and without any payload.
% The model dynamics is also identified in the same conditions.
% A first comparison between the model and the measured dynamics is done in Figure ref:fig:test_id31_first_id.
% A good match can be observed for the diagonal dynamics (except the high frequency modes which are not modeled).
% However, the coupling for the transfer function from command signals $\bm{u}$ to the estimated strut motion from the external metrology $\bm{\epsilon\mathcal{L}}$ is larger than expected (Figure ref:fig:test_id31_first_id_int).
% The experimental time delay estimated from the FRF (Figure ref:fig:test_id31_first_id_int) is larger than expected.
% After investigation, it was found that the additional delay was due to a digital processing unit[fn:3] that was used to get the interferometers' signals in the Speedgoat.
% This issue was later solved.
%% Identify the plant dynamics using the Simscape model
% Initialize each Simscape model elements
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeNanoHexapod('flex_bot_type', '2dof', ...
'flex_top_type', '3dof', ...
'motion_sensor_type', 'plates', ...
'actuator_type', '2dof');
initializeSample('type', '0');
initializeSimscapeConfiguration('gravity', false);
initializeDisturbances('enable', false);
initializeLoggingConfiguration('log', 'none');
initializeController('type', 'open-loop');
initializeReferences();
% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs [V]
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Vs'); io_i = io_i + 1; % Force Sensors voltages [V]
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL'); io_i = io_i + 1; % Position Errors [m]
% With no payload
Gm = exp(-1e-4*s)*linearize(mdl, io);
Gm.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
Gm.OutputName = {'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6', ...
'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'};
%% Identify the plant from experimental data
% Load identification data
data = load('2023-08-08_16-17_ol_plant_m0_Wz0.mat');
% Frequency analysis parameters
Ts = 1e-4; % Sampling Time [s]
Nfft = floor(2.0/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);
[~, f] = tfestimate(data.uL1.id_plant, data.uL1.e_L1, win, Noverlap, Nfft, 1/Ts);
G_iff = zeros(length(f), 6, 6); % Force sensor outputs
G_int = zeros(length(f), 6, 6); % Estimated strut errors
for i_strut = 1:6
Vs = [data.(sprintf("uL%i", i_strut)).Vs1 ; data.(sprintf("uL%i", i_strut)).Vs2 ; data.(sprintf("uL%i", i_strut)).Vs3 ; data.(sprintf("uL%i", i_strut)).Vs4 ; data.(sprintf("uL%i", i_strut)).Vs5 ; data.(sprintf("uL%i", i_strut)).Vs6]';
eL = [data.(sprintf("uL%i", i_strut)).e_L1 ; data.(sprintf("uL%i", i_strut)).e_L2 ; data.(sprintf("uL%i", i_strut)).e_L3 ; data.(sprintf("uL%i", i_strut)).e_L4 ; data.(sprintf("uL%i", i_strut)).e_L5 ; data.(sprintf("uL%i", i_strut)).e_L6]';
dL = [data.(sprintf("uL%i", i_strut)).dL1 ; data.(sprintf("uL%i", i_strut)).dL2 ; data.(sprintf("uL%i", i_strut)).dL3 ; data.(sprintf("uL%i", i_strut)).dL4 ; data.(sprintf("uL%i", i_strut)).dL5 ; data.(sprintf("uL%i", i_strut)).dL6]';
G_iff(:,:,i_strut) = tfestimate(data.(sprintf("uL%i", i_strut)).id_plant, Vs, win, Noverlap, Nfft, 1/Ts);
G_int(:,:,i_strut) = tfestimate(data.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
end
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
figure;
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
for i = 1:5
for j = i+1:6
plot(f, abs(G_int(:, i, j)), 'color', [colors(1,:), 0.2], ...
'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
'HandleVisibility', 'off');
end
end
plot(f, abs(G_int(:, 1, 1)), 'color', [colors(1,:)], ...
'DisplayName', '$-\epsilon\mathcal{L}_i/u_i$ meas');
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
'DisplayName', '$-\epsilon\mathcal{L}_i/u_i$ model');
for i = 2:6
plot(f, abs(G_int(:,i, i)), 'color', [colors(1,:)], ...
'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
'HandleVisibility', 'off');
end
plot(f, abs(G_int(:, 1, 2)), 'color', [colors(1,:), 0.2], ...
'DisplayName', '$-\epsilon\mathcal{L}_i/u_j$ meas');
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', 1), sprintf('u%i', 2)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
'DisplayName', '$-\epsilon\mathcal{L}_i/u_j$ model');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
ylim([2e-9, 2e-4]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;
ax2 = nexttile;
hold on;
for i = 1:6
plot(f, 180/pi*angle(G_int(:,i, i)), 'color', [colors(1,:)]);
end
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)]);
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-90, 180])
linkaxes([ax1,ax2],'x');
xlim([1, 1e3]);
%% Comparison between the measured dynamics and the model dynamics - Force Sensors
figure;
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
for i = 1:5
for j = i+1:6
plot(f, abs(G_iff(:, i, j)), 'color', [colors(1,:), 0.2], ...
'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('Vs%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
'HandleVisibility', 'off');
end
end
plot(f, abs(G_iff(:,1, 1)), 'color', [colors(1,:)], ...
'DisplayName', '$V_{s,i}/u_i$ meas');
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('Vs%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
'DisplayName', '$V_{s,i}/u_i$ model');
for i = 2:6
plot(f, abs(G_iff(:,i, i)), 'color', [colors(1,:)], ...
'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('Vs%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
'HandleVisibility', 'off');
end
plot(f, abs(G_iff(:, 1, 2)), 'color', [colors(1,:), 0.2], ...
'DisplayName', '$V_{s,i}/u_j$ meas');
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('Vs%i', 1), sprintf('u%i', 2)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
'DisplayName', '$V_{s,i}/u_j$ model');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
ylim([5e-5, 4e1]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;
ax2 = nexttile;
hold on;
for i = 1:6
plot(f, 180/pi*angle(G_iff(:,i, i)), 'color', [colors(1,:)]);
end
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm(sprintf('Vs%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)]);
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-90, 180])
linkaxes([ax1,ax2],'x');
xlim([1, 1e3]);
% Better Angular Alignment
% <<ssec:test_id31_open_loop_plant_rz_alignment>>
% One possible explanation of the increased coupling observed in Figure ref:fig:test_id31_first_id_int is the poor alignment between the external metrology axes (i.e. the interferometer supports) and the nano-hexapod axes.
% To estimate this alignment, a decentralized low-bandwidth feedback controller based on the nano-hexapod encoders was implemented.
% This allowed to perform two straight movements of the nano-hexapod along its $x$ and $y$ axes.
% During these two movements, the external metrology measurement was recorded and are shown in Figure ref:fig:test_id31_Rz_align_error.
% It was found that there is a misalignment of 2.7 degrees (rotation along the vertical axis) between the interferometer axes and nano-hexapod axes.
% This was corrected by adding an offset to the spindle angle.
% After alignment, the same movement was performed using the nano-hexapod while recording the signal of the external metrology.
% Results shown in Figure ref:fig:test_id31_Rz_align_correct are indeed indicating much better alignment.
%% Load Data
data_1_dx = h5scan(data_dir, 'align_int_enc_Rz', 'tx_first_scan', 2);
data_1_dy = h5scan(data_dir, 'align_int_enc_Rz', 'tx_first_scan', 3);
data_2_dx = h5scan(data_dir, 'align_int_enc_Rz', 'verif-after-correct-offset', 1);
data_2_dy = h5scan(data_dir, 'align_int_enc_Rz', 'verif-after-correct-offset', 2);
% Estimation of Rz misalignment
p1 = polyfit(data_1_dx.Dx_int_filtered, data_1_dx.Dy_int_filtered, 1);
p2 = polyfit(data_1_dy.Dx_int_filtered, data_1_dy.Dy_int_filtered, 1);
Rz_error = (atan(p1(1)) + atan(p2(1))-pi/2)/2; % ~3 degrees
%% Estimation of the Rz misalignment
figure;
hold on;
plot(1e6*data_1_dx.Dx_int_filtered, 1e6*data_1_dx.Dy_int_filtered, 'color', colors(2,:), 'DisplayName', 'Measurement')
plot(1e6*data_1_dy.Dx_int_filtered, 1e6*data_1_dy.Dy_int_filtered, 'color', colors(2,:), 'HandleVisibility', 'off')
plot( 1e6*[-10:10]*cos(Rz_error), 1e6*[-10:10]*sin(Rz_error), 'k--', 'DisplayName', sprintf('$\\epsilon_{R_z} = %.1f$ deg', Rz_error*180/pi))
plot(-1e6*[-10:10]*sin(Rz_error), 1e6*[-10:10]*cos(Rz_error), 'k--', 'HandleVisibility', 'off')
hold off;
xlabel('Interf $D_x$ [$\mu$m]');
ylabel('Interf $D_y$ [$\mu$m]');
axis equal
xlim([-10, 10]); ylim([-10, 10]);
xticks([-10:5:10]); yticks([-10:5:10]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
% Estimation of Rz misalignment after correcting the Rz angle
p1 = polyfit(data_2_dx.Dx_int_filtered, data_2_dx.Dy_int_filtered, 1);
p2 = polyfit(data_2_dy.Dx_int_filtered, data_2_dy.Dy_int_filtered, 1);
Rz_error = (atan(p1(1)) + atan(p2(1))-pi/2)/2; % ~0.2 degrees
%% Estimation of the Rz misalignment after correcting the Rz offset
figure;
hold on;
plot(1e6*data_2_dx.Dx_int_filtered, 1e6*data_2_dx.Dy_int_filtered, 'color', colors(5,:), 'DisplayName', 'Measurement')
plot(1e6*data_2_dy.Dx_int_filtered, 1e6*data_2_dy.Dy_int_filtered, 'color', colors(5,:), 'HandleVisibility', 'off')
plot( 1e6*[-10:10]*cos(Rz_error), 1e6*[-10:10]*sin(Rz_error), 'k--', 'DisplayName', sprintf('$\\epsilon_{R_z} = %.1f$ deg', Rz_error*180/pi))
plot(-1e6*[-10:10]*sin(Rz_error), 1e6*[-10:10]*cos(Rz_error), 'k--', 'HandleVisibility', 'off')
hold off;
xlabel('Interf $D_x$ [$\mu$m]');
ylabel('Interf $D_y$ [$\mu$m]');
axis equal
xlim([-10, 10]); ylim([-10, 10]);
xticks([-10:5:10]); yticks([-10:5:10]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
% #+name: fig:test_id31_Rz_align_error
% #+caption: Measurement of the Nano-Hexapod axes in the frame of the external metrology. Before alignment (\subref{fig:test_id31_Rz_align_error}) and after alignment (\subref{fig:test_id31_Rz_align_correct}).
% #+attr_latex: :options [htbp]
% #+begin_figure
% #+attr_latex: :caption \subcaption{\label{fig:test_id31_Rz_align_error}Before alignment}
% #+attr_latex: :options {0.49\textwidth}
% #+begin_subfigure
% #+attr_latex: :scale 1
% [[file:figs/test_id31_Rz_align_error.png]]
% #+end_subfigure
% #+attr_latex: :caption \subcaption{\label{fig:test_id31_Rz_align_correct}After alignment}
% #+attr_latex: :options {0.49\textwidth}
% #+begin_subfigure
% #+attr_latex: :scale 1
% [[file:figs/test_id31_Rz_align_correct.png]]
% #+end_subfigure
% #+end_figure
% The plant dynamics was identified again after the fine alignment and is compared with the model dynamics in Figure ref:fig:test_id31_first_id_int_better_rz_align.
% Compared to the initial identification shown in Figure ref:fig:test_id31_first_id_int, the obtained coupling has decreased and is now close to the coupling obtained with the multi-body model.
% At low frequency (below $10\,\text{Hz}$) all the off-diagonal elements have an amplitude $\approx 100$ times lower compared to the diagonal elements, indicating that a low bandwidth feedback controller can be implemented in a decentralized way (i.e. $6$ SISO controllers).
% Between $650\,\text{Hz}$ and $1000\,\text{Hz}$, several modes can be observed that are due to flexible modes of the top platform and modes of the two spheres adjustment mechanism.
% The flexible modes of the top platform can be passively damped while the modes of the two reference spheres should not be present in the final application.
%% Identification of the plant after Rz alignment
data_align = load('2023-08-17_17-37_ol_plant_m0_Wz0_new_Rz_align.mat');
G_int_align = zeros(length(f), 6, 6);
for i_strut = 1:6
eL = [data_align.(sprintf("uL%i", i_strut)).e_L1 ; data_align.(sprintf("uL%i", i_strut)).e_L2 ; data_align.(sprintf("uL%i", i_strut)).e_L3 ; data_align.(sprintf("uL%i", i_strut)).e_L4 ; data_align.(sprintf("uL%i", i_strut)).e_L5 ; data_align.(sprintf("uL%i", i_strut)).e_L6]';
G_int_align(:,:,i_strut) = tfestimate(data_align.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
end
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
nexttile();
hold on;
for i = 1:5
for j = i+1:6
plot(f, abs(G_int_align(:, i, j)), 'color', [colors(1,:), 0.2], ...
'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
'HandleVisibility', 'off');
end
end
plot(f, abs(G_int_align(:, 1, 1)), 'color', [colors(1,:)], ...
'DisplayName', '$\epsilon\mathcal{L}_i/u_i$ meas');
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
'DisplayName', '$\epsilon\mathcal{L}_i/u_i$ model');
for i = 2:6
plot(f, abs(G_int_align(:,i, i)), 'color', [colors(1,:)], ...
'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
'HandleVisibility', 'off');
end
plot(f, abs(G_int_align(:, 1, 2)), 'color', [colors(1,:), 0.2], ...
'DisplayName', '$\epsilon\mathcal{L}_i/u_j$ meas');
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', 1), sprintf('u%i', 2)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
'DisplayName', '$\epsilon\mathcal{L}_i/u_j$ model');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
xlim([1, 1e3]); ylim([2e-9, 2e-4]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;
% Effect of Payload Mass
% <<ssec:test_id31_open_loop_plant_mass>>
% In order to see how the system dynamics changes with the payload, open-loop identification was performed for four payload conditions that are shown in Figure ref:fig:test_id31_picture_masses.
% The obtained direct terms are compared with the model dynamics in Figure ref:fig:test_nhexa_comp_simscape_diag_masses.
% It is shown that the model dynamics well predicts the measured dynamics for all payload conditions.
% Therefore the model can be used for model-based control is necessary.
% It is interesting to note that the anti-resonances in the force sensor plant are now appearing as minimum-phase, as the model predicts (Figure ref:fig:test_id31_comp_simscape_iff_diag_masses).
% #+name: fig:test_id31_picture_masses
% #+caption: The four tested payload conditions. (\subref{fig:test_id31_picture_mass_m0}) without payload. (\subref{fig:test_id31_picture_mass_m1}) with $13\,\text{kg}$ payload. (\subref{fig:test_id31_picture_mass_m2}) with $26\,\text{kg}$ payload. (\subref{fig:test_id31_picture_mass_m3}) with $39\,\text{kg}$ payload.
% #+attr_latex: :options [htbp]
% #+begin_figure
% #+attr_latex: :caption \subcaption{\label{fig:test_id31_picture_mass_m0}$m=0\,\text{kg}$}
% #+attr_latex: :options {0.24\textwidth}
% #+begin_subfigure
% #+attr_latex: :width 0.99\linewidth
% [[file:figs/test_id31_picture_mass_m0.jpg]]
% #+end_subfigure
% #+attr_latex: :caption \subcaption{\label{fig:test_id31_picture_mass_m1}$m=13\,\text{kg}$}
% #+attr_latex: :options {0.24\textwidth}
% #+begin_subfigure
% #+attr_latex: :width 0.99\linewidth
% [[file:figs/test_id31_picture_mass_m1.jpg]]
% #+end_subfigure
% #+attr_latex: :caption \subcaption{\label{fig:test_id31_picture_mass_m2}$m=26\,\text{kg}$}
% #+attr_latex: :options {0.24\textwidth}
% #+begin_subfigure
% #+attr_latex: :width 0.99\linewidth
% [[file:figs/test_id31_picture_mass_m2.jpg]]
% #+end_subfigure
% #+attr_latex: :caption \subcaption{\label{fig:test_id31_picture_mass_m3}$m=39\,\text{kg}$}
% #+attr_latex: :options {0.24\textwidth}
% #+begin_subfigure
% #+attr_latex: :width 0.99\linewidth
% [[file:figs/test_id31_picture_mass_m3.jpg]]
% #+end_subfigure
% #+end_figure
%% Identify the model dynamics for all payload conditions
% Initialize each Simscape model elements
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeNanoHexapod('flex_bot_type', '2dof', ...
'flex_top_type', '3dof', ...
'motion_sensor_type', 'plates', ...
'actuator_type', '2dof');
initializeSample('type', '0');
initializeSimscapeConfiguration('gravity', false);
initializeDisturbances('enable', false);
initializeLoggingConfiguration('log', 'none');
initializeController('type', 'open-loop');
initializeReferences();
% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Vs'); io_i = io_i + 1; % Force Sensors
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL'); io_i = io_i + 1; % Position Errors
initializeSample('type', '0');
Gm_m0_Wz0 = linearize(mdl, io);
Gm_m0_Wz0.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
Gm_m0_Wz0.OutputName = {'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6', ...
'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'};
initializeSample('type', '1');
Gm_m1_Wz0 = linearize(mdl, io);
Gm_m1_Wz0.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
Gm_m1_Wz0.OutputName = {'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6', ...
'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'};
initializeSample('type', '2');
Gm_m2_Wz0 = linearize(mdl, io);
Gm_m2_Wz0.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
Gm_m2_Wz0.OutputName = {'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6', ...
'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'};
initializeSample('type', '3');
Gm_m3_Wz0 = linearize(mdl, io);
Gm_m3_Wz0.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
Gm_m3_Wz0.OutputName = {'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6', ...
'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'};
%% Identify the plant from experimental data - All payloads
% Load identification data
data_m0_Wz0 = load('2023-08-08_16-17_ol_plant_m0_Wz0.mat');
data_m1_Wz0 = load('2023-08-08_18-57_ol_plant_m1_Wz0.mat');
data_m2_Wz0 = load('2023-08-08_17-12_ol_plant_m2_Wz0.mat');
data_m3_Wz0 = load('2023-08-08_18-20_ol_plant_m3_Wz0.mat');
% Sampling Time [s]
Ts = 1e-4;
% Hannning Windows
Nfft = floor(2.0/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);
% And we get the frequency vector
[~, f] = tfestimate(data_m0_Wz0.uL1.id_plant, data_m0_Wz0.uL1.e_L1, win, Noverlap, Nfft, 1/Ts);
% No payload
G_iff_m0_Wz0 = zeros(length(f), 6, 6);
for i_strut = 1:6
eL = [data_m0_Wz0.(sprintf("uL%i", i_strut)).Vs1 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).Vs2 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).Vs3 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).Vs4 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).Vs5 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).Vs6]';
G_iff_m0_Wz0(:,:,i_strut) = tfestimate(data_m0_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
end
G_int_m0_Wz0 = zeros(length(f), 6, 6);
for i_strut = 1:6
eL = [data_m0_Wz0.(sprintf("uL%i", i_strut)).e_L1 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).e_L2 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).e_L3 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).e_L4 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).e_L5 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).e_L6]';
G_int_m0_Wz0(:,:,i_strut) = tfestimate(data_m0_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
end
% 1 "payload layer"
G_iff_m1_Wz0 = zeros(length(f), 6, 6);
for i_strut = 1:6
eL = [data_m1_Wz0.(sprintf("uL%i", i_strut)).Vs1 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).Vs2 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).Vs3 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).Vs4 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).Vs5 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).Vs6]';
G_iff_m1_Wz0(:,:,i_strut) = tfestimate(data_m1_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
end
G_int_m1_Wz0 = zeros(length(f), 6, 6);
for i_strut = 1:6
eL = [data_m1_Wz0.(sprintf("uL%i", i_strut)).e_L1 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).e_L2 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).e_L3 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).e_L4 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).e_L5 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).e_L6]';
G_int_m1_Wz0(:,:,i_strut) = tfestimate(data_m1_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
end
% 2 "payload layers"
G_iff_m2_Wz0 = zeros(length(f), 6, 6);
for i_strut = 1:6
eL = [data_m2_Wz0.(sprintf("uL%i", i_strut)).Vs1 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).Vs2 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).Vs3 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).Vs4 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).Vs5 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).Vs6]';
G_iff_m2_Wz0(:,:,i_strut) = tfestimate(data_m2_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
end
G_int_m2_Wz0 = zeros(length(f), 6, 6);
for i_strut = 1:6
eL = [data_m2_Wz0.(sprintf("uL%i", i_strut)).e_L1 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).e_L2 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).e_L3 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).e_L4 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).e_L5 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).e_L6]';
G_int_m2_Wz0(:,:,i_strut) = tfestimate(data_m2_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
end
% 3 "payload layers"
G_iff_m3_Wz0 = zeros(length(f), 6, 6);
for i_strut = 1:6
eL = [data_m3_Wz0.(sprintf("uL%i", i_strut)).Vs1 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).Vs2 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).Vs3 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).Vs4 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).Vs5 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).Vs6]';
G_iff_m3_Wz0(:,:,i_strut) = tfestimate(data_m3_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
end
G_int_m3_Wz0 = zeros(length(f), 6, 6);
for i_strut = 1:6
eL = [data_m3_Wz0.(sprintf("uL%i", i_strut)).e_L1 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).e_L2 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).e_L3 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).e_L4 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).e_L5 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).e_L6]';
G_int_m3_Wz0(:,:,i_strut) = tfestimate(data_m3_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
end
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
figure;
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(f, abs(G_int_m0_Wz0(:, 1, 1)), 'color', [colors(1,:), 0.5], ...
'DisplayName', 'Meas (0kg)');
for i = 2:6
plot(f, abs(G_int_m0_Wz0(:,i, i)), 'color', [colors(1,:), 0.5], ...
'HandleVisibility', 'off')
end
plot(f, abs(G_int_m1_Wz0(:, 1, 1)), 'color', [colors(2,:), 0.5], ...
'DisplayName', 'Meas (13kg)');
for i = 2:6
plot(f, abs(G_int_m1_Wz0(:,i, i)), 'color', [colors(2,:), 0.5], ...
'HandleVisibility', 'off')
end
plot(f, abs(G_int_m2_Wz0(:, 1, 1)), 'color', [colors(3,:), 0.5], ...
'DisplayName', 'Meas (26kg)');
for i = 2:6
plot(f, abs(G_int_m2_Wz0(:,i, i)), 'color', [colors(3,:), 0.5], ...
'HandleVisibility', 'off')
end
plot(f, abs(G_int_m3_Wz0(:, 1, 1)), 'color', [colors(4,:), 0.5], ...
'DisplayName', 'Meas (39kg)');
for i = 2:6
plot(f, abs(G_int_m3_Wz0(:,i, i)), 'color', [colors(4,:), 0.5], ...
'HandleVisibility', 'off')
end
plot(freqs, abs(squeeze(freqresp(Gm_m0_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:), ...
'DisplayName', 'Model (0kg)');
plot(freqs, abs(squeeze(freqresp(Gm_m1_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:), ...
'DisplayName', 'Model (13kg)');
plot(freqs, abs(squeeze(freqresp(Gm_m2_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:), ...
'DisplayName', 'Model (26kg)');
plot(freqs, abs(squeeze(freqresp(Gm_m3_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:), ...
'DisplayName', 'Model (39kg)');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-8, 5e-4]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;
ax2 = nexttile;
hold on;
for i =1:6
plot(f, 180/pi*angle(G_int_m0_Wz0(:,i, i)), 'color', [colors(1,:), 0.5]);
end
for i =1:6
plot(f, 180/pi*angle(G_int_m1_Wz0(:,i, i)), 'color', [colors(2,:), 0.5]);
end
for i =1:6
plot(f, 180/pi*angle(G_int_m2_Wz0(:,i, i)), 'color', [colors(3,:), 0.5]);
end
for i =1:6
plot(f, 180/pi*angle(G_int_m3_Wz0(:,i, i)), 'color', [colors(4,:), 0.5]);
end
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:))
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m1_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:))
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m2_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:))
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m3_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-90, 180])
linkaxes([ax1,ax2],'x');
xlim([10, 5e2]);
xticks([10, 20, 50, 100, 200, 500])
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
figure;
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(f, abs(G_iff_m0_Wz0(:, 1, 1)), 'color', [colors(1,:), 0.5], ...
'DisplayName', 'Meas (0kg)');
for i = 2:6
plot(f, abs(G_iff_m0_Wz0(:,i, i)), 'color', [colors(1,:), 0.5], ...
'HandleVisibility', 'off')
end
plot(f, abs(G_iff_m1_Wz0(:, 1, 1)), 'color', [colors(2,:), 0.5], ...
'DisplayName', 'Meas (13kg)');
for i = 2:6
plot(f, abs(G_iff_m1_Wz0(:,i, i)), 'color', [colors(2,:), 0.5], ...
'HandleVisibility', 'off')
end
plot(f, abs(G_iff_m2_Wz0(:, 1, 1)), 'color', [colors(3,:), 0.5], ...
'DisplayName', 'Meas (26kg)');
for i = 2:6
plot(f, abs(G_iff_m2_Wz0(:,i, i)), 'color', [colors(3,:), 0.5], ...
'HandleVisibility', 'off')
end
plot(f, abs(G_iff_m3_Wz0(:, 1, 1)), 'color', [colors(4,:), 0.5], ...
'DisplayName', 'Meas (39kg)');
for i = 2:6
plot(f, abs(G_iff_m3_Wz0(:,i, i)), 'color', [colors(4,:), 0.5], ...
'HandleVisibility', 'off')
end
plot(freqs, abs(squeeze(freqresp(Gm_m0_Wz0('Vs1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:), ...
'DisplayName', 'Model (0kg)');
plot(freqs, abs(squeeze(freqresp(Gm_m1_Wz0('Vs1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:), ...
'DisplayName', 'Model (13kg)');
plot(freqs, abs(squeeze(freqresp(Gm_m2_Wz0('Vs1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:), ...
'DisplayName', 'Model (26kg)');
plot(freqs, abs(squeeze(freqresp(Gm_m3_Wz0('Vs1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:), ...
'DisplayName', 'Model (39kg)');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-2, 4e1]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;
ax2 = nexttile;
hold on;
for i =1:6
plot(f, 180/pi*angle(G_iff_m0_Wz0(:,i, i)), 'color', [colors(1,:), 0.5]);
end
for i =1:6
plot(f, 180/pi*angle(G_iff_m1_Wz0(:,i, i)), 'color', [colors(2,:), 0.5]);
end
for i =1:6
plot(f, 180/pi*angle(G_iff_m2_Wz0(:,i, i)), 'color', [colors(3,:), 0.5]);
end
for i =1:6
plot(f, 180/pi*angle(G_iff_m3_Wz0(:,i, i)), 'color', [colors(4,:), 0.5]);
end
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0_Wz0('Vs1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:))
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m1_Wz0('Vs1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:))
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m2_Wz0('Vs1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:))
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m3_Wz0('Vs1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-90, 180])
linkaxes([ax1,ax2],'x');
xlim([10, 5e2]);
xticks([10, 20, 50, 100, 200, 500])
% Effect of Spindle Rotation
% <<ssec:test_id31_open_loop_plant_rotation>>
% To verify that all the kinematics in Figure ref:fig:test_id31_block_schematic_plant are correct and to check whether the system dynamics is affected by Spindle rotation of not, three identification experiments were performed: no spindle rotation, spindle rotation at $36\,\text{deg}/s$ and at $180\,\text{deg}/s$.
% The comparison of the obtained dynamics from command signal $u$ to estimated strut error $\epsilon\mathcal{L}$ is done in Figure ref:fig:test_id31_effect_rotation.
% Both direct terms (Figure ref:fig:test_id31_effect_rotation_direct) and coupling terms (Figure ref:fig:test_id31_effect_rotation_coupling) are unaffected by the rotation.
% The same can be observed for the dynamics from the command signal to the encoders and to the force sensors.
% This confirms that the rotation has no significant effect on the plant dynamics.
% This also indicates that the metrology kinematics is correct and is working in real time.
%% Identify the model dynamics with Spindle rotation
initializeSample('type', '0');
initializeReferences(...
'Rz_type', 'rotating', ...
'Rz_period', 360/36); % 36 deg/s, 6rpm
Gm_m0_Wz36 = linearize(mdl, io, 0.1);
Gm_m0_Wz36.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
Gm_m0_Wz36.OutputName = {'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6', ...
'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'};
initializeReferences(...
'Rz_type', 'rotating', ...
'Rz_period', 360/180); % 180 deg/s, 30rpm
Gm_m0_Wz180 = linearize(mdl, io, 0.1);
Gm_m0_Wz180.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
Gm_m0_Wz180.OutputName = {'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6', ...
'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'};
%% Identify the plant from experimental data - Effect of rotation
% Load identification data
data_m0_Wz36 = load('2023-08-08_16-28_ol_plant_m0_Wz36.mat');
data_m0_Wz180 = load('2023-08-08_16-45_ol_plant_m0_Wz180.mat');
% Spindle Rotation at 36 deg/s
G_iff_m0_Wz36 = zeros(length(f), 6, 6);
for i_strut = 1:6
eL = [data_m0_Wz36.(sprintf("uL%i", i_strut)).Vs1 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).Vs2 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).Vs3 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).Vs4 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).Vs5 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).Vs6]';
G_iff_m0_Wz36(:,:,i_strut) = tfestimate(data_m0_Wz36.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
end
G_int_m0_Wz36 = zeros(length(f), 6, 6);
for i_strut = 1:6
eL = [data_m0_Wz36.(sprintf("uL%i", i_strut)).e_L1 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).e_L2 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).e_L3 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).e_L4 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).e_L5 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).e_L6]';
G_int_m0_Wz36(:,:,i_strut) = tfestimate(data_m0_Wz36.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
end
% Spindle Rotation at 180 deg/s
G_iff_m0_Wz180 = zeros(length(f), 6, 6);
for i_strut = 1:6
eL = [data_m0_Wz180.(sprintf("uL%i", i_strut)).Vs1 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).Vs2 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).Vs3 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).Vs4 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).Vs5 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).Vs6]';
G_iff_m0_Wz180(:,:,i_strut) = tfestimate(data_m0_Wz180.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
end
G_int_m0_Wz180 = zeros(length(f), 6, 6);
for i_strut = 1:6
eL = [data_m0_Wz180.(sprintf("uL%i", i_strut)).e_L1 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).e_L2 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).e_L3 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).e_L4 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).e_L5 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).e_L6]';
G_int_m0_Wz180(:,:,i_strut) = tfestimate(data_m0_Wz180.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
end
% The identified dynamics are then saved for further use.
save('./mat/test_id31_simscape_open_loop_plants.mat', 'Gm_m0_Wz0', 'Gm_m0_Wz36', 'Gm_m0_Wz180', 'Gm_m1_Wz0', 'Gm_m2_Wz0', 'Gm_m3_Wz0');
save('./mat/test_id31_identified_open_loop_plants.mat', 'G_int_m0_Wz0', 'G_int_m0_Wz36', 'G_int_m0_Wz180', 'G_int_m1_Wz0', 'G_int_m2_Wz0', 'G_int_m3_Wz0', ...
'G_iff_m0_Wz0', 'G_iff_m0_Wz36', 'G_iff_m0_Wz180', 'G_iff_m1_Wz0', 'G_iff_m2_Wz0', 'G_iff_m3_Wz0', 'f');
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
nexttile();
hold on;
plot(f, abs(G_int_m0_Wz0(:, 1, 1)), 'color', [colors(1,:), 0.5], ...
'DisplayName', '$\Omega_z = 0$');
plot(f, abs(G_int_m0_Wz36(:, 1, 1)), 'color', [colors(2,:), 0.5], ...
'DisplayName', '$\Omega_z = 36$ deg/s');
plot(f, abs(G_int_m0_Wz180(:, 1, 1)), 'color', [colors(3,:), 0.5], ...
'DisplayName', '$\Omega_z = 180$ deg/s');
for i = 2:6
plot(f, abs(G_int_m0_Wz0(:,i, i)), 'color', [colors(1,:), 0.5], ...
'HandleVisibility', 'off')
plot(f, abs(G_int_m0_Wz36(:,i, i)), 'color', [colors(2,:), 0.5], ...
'HandleVisibility', 'off')
plot(f, abs(G_int_m0_Wz180(:,i, i)), 'color', [colors(3,:), 0.5], ...
'HandleVisibility', 'off')
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xlim([10, 1e3]); ylim([1e-8, 2e-4])
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
nexttile();
hold on;
plot(f, abs(G_int_m0_Wz0(:, 1, 2)), 'color', [colors(1,:), 0.5], ...
'DisplayName', '$\Omega_z = 0$');
plot(f, abs(G_int_m0_Wz36(:, 1, 2)), 'color', [colors(2,:), 0.5], ...
'DisplayName', '$\Omega_z = 36$ deg/s');
plot(f, abs(G_int_m0_Wz180(:, 1, 2)), 'color', [colors(3,:), 0.5], ...
'DisplayName', '$\Omega_z = 180$ deg/s');
for i = 1:5
for j = i+1:6
plot(f, abs(G_int_m0_Wz0(:, i, j)), 'color', [colors(1,:), 0.5], ...
'HandleVisibility', 'off');
plot(f, abs(G_int_m0_Wz36(:, i, j)), 'color', [colors(2,:), 0.5], ...
'HandleVisibility', 'off');
plot(f, abs(G_int_m0_Wz180(:, i, j)), 'color', [colors(3,:), 0.5], ...
'HandleVisibility', 'off');
end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xlim([10, 1e3]); ylim([1e-8, 2e-4])

561
matlab/test_id31_3_iff.m Normal file
View File

@ -0,0 +1,561 @@
% Matlab Init :noexport:ignore:
%% test_id31_3_iff.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
%% Data directory
data_dir = './mat/';
% Simulink Model name
mdl = 'nass_model_id31';
%% Colors for the figures
colors = colororder;
%% Frequency Vector
freqs = logspace(log10(1), log10(2e3), 1000);
%% Sampling Time
Ts = 1e-4;
%% Specifications for Experiments
specs_dz_peak = 50; % [nm]
specs_dy_peak = 100; % [nm]
specs_ry_peak = 0.85; % [urad]
specs_dz_rms = 15; % [nm RMS]
specs_dy_rms = 30; % [nm RMS]
specs_ry_rms = 0.25; % [urad RMS]
% IFF Plant
% <<ssec:test_id31_iff_plant>>
% As the multi-body model is going to be used to evaluate the stability of the IFF controller and to optimize the achievable damping, it is first checked whether this model accurately represents the system dynamics.
% In the previous section (Figure ref:fig:test_id31_comp_simscape_iff_diag_masses), it was shown that the model well captures the dynamics from each actuator to its collocated force sensor, and that for all considered payloads.
% Nevertheless, it is also important to well model the coupling in the system.
% To very that, instead of comparing the 36 elements of the $6 \times 6$ frequency response matrix from $\bm{u}$ to $\bm{V_s}$, only 6 elements are compared in Figure ref:fig:test_id31_comp_simscape_Vs.
% Similar results are obtained for all other 30 elements and for the different tested payload conditions.
% This confirms that the multi-body model can be used to tune the IFF controller.
% Load identified FRF for IFF Plant and Multi-Body Model
load('test_id31_identified_open_loop_plants.mat', 'G_iff_m0_Wz0', 'G_iff_m1_Wz0', 'G_iff_m2_Wz0', 'G_iff_m3_Wz0', 'f');
load('test_id31_simscape_open_loop_plants.mat', 'Gm_m0_Wz0', 'Gm_m1_Wz0', 'Gm_m2_Wz0', 'Gm_m3_Wz0');
figure;
tiledlayout(2, 3, 'TileSpacing', 'tight', 'Padding', 'tight');
ax1 = nexttile();
hold on;
plot(f, abs(G_iff_m0_Wz0(:, 1, 1)));
plot(freqs, abs(squeeze(freqresp(Gm_m0_Wz0('Vs1', 'u1'), freqs, 'Hz'))));
text(12, 4e1, '$V_{s1}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/V]');
yticks([1e-2, 1e-1, 1e0, 1e1]);
ax2 = nexttile();
hold on;
plot(f, abs(G_iff_m0_Wz0(:, 2, 1)));
plot(freqs, abs(squeeze(freqresp(Gm_m0_Wz0('Vs2', 'u1'), freqs, 'Hz'))));
text(12, 4e1, '$V_{s2}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
ax3 = nexttile();
hold on;
plot(f, abs(G_iff_m0_Wz0(:, 3, 1)), ...
'DisplayName', 'Measurements');
plot(freqs, abs(squeeze(freqresp(Gm_m0_Wz0('Vs3', 'u1'), freqs, 'Hz'))), ...
'DisplayName', 'Model (2-DoF APA)');
text(12, 4e1, '$V_{s3}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ax4 = nexttile();
hold on;
plot(f, abs(G_iff_m0_Wz0(:, 4, 1)));
plot(freqs, abs(squeeze(freqresp(Gm_m0_Wz0('Vs4', 'u1'), freqs, 'Hz'))));
text(12, 4e1, '$V_{s4}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
xticks([10, 20, 50, 100, 200])
yticks([1e-2, 1e-1, 1e0, 1e1]);
ax5 = nexttile();
hold on;
plot(f, abs(G_iff_m0_Wz0(:, 5, 1)));
plot(freqs, abs(squeeze(freqresp(Gm_m0_Wz0('Vs5', 'u1'), freqs, 'Hz'))));
text(12, 4e1, '$V_{s5}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xticks([10, 20, 50, 100, 200])
ax6 = nexttile();
hold on;
plot(f, abs(G_iff_m0_Wz0(:, 6, 1)));
plot(freqs, abs(squeeze(freqresp(Gm_m0_Wz0('Vs6', 'u1'), freqs, 'Hz'))));
text(12, 4e1, '$V_{s6}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
xticks([10, 20, 50, 100, 200])
linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'xy');
xlim([10, 5e2]); ylim([1e-2, 5e1]);
% IFF Controller
% <<ssec:test_id31_iff_controller>>
% A decentralized IFF controller was designed such that it adds damping to the suspension modes of the nano-hexapod for all considered payloads.
% The frequency of the suspension modes are ranging from $\approx 30\,\text{Hz}$ to $\approx 250\,\text{Hz}$ (Figure ref:fig:test_id31_comp_simscape_iff_diag_masses), and therefore the IFF controller should provide integral action in this frequency range.
% A second order high pass filter (cut-off frequency of $10\,\text{Hz}$) was added to limit the low frequency gain eqref:eq:test_id31_Kiff.
% \begin{equation}\label{eq:test_id31_Kiff}
% K_{\text{IFF}} = g_0 \cdot \underbrace{\frac{1}{s}}_{\text{int}} \cdot \underbrace{\frac{s^2/\omega_z^2}{s^2/\omega_z^2 + 2\xi_z s /\omega_z + 1}}_{\text{2nd order LPF}},\quad \left(g_0 = -100,\ \omega_z = 2\pi10\,\text{rad/s},\ \xi_z = 0.7\right)
% \end{equation}
% The bode plot of the decentralized IFF controller is shown in Figure ref:fig:test_id31_Kiff_bode_plot and the "decentralized loop-gains" for all considered payload masses are shown in Figure ref:fig:test_id31_Kiff_loop_gain.
% It can be seen that the loop-gain is larger than $1$ around suspension modes indicating that some damping should be added to the suspension modes.
%% IFF Controller Design
% Second order high pass filter
wz = 2*pi*10;
xiz = 0.7;
Ghpf = (s^2/wz^2)/(s^2/wz^2 + 2*xiz*s/wz + 1);
% IFF Controller
Kiff = -1e2 * ... % Gain
1/(0.01*2*pi + s) * ... % LPF: provides integral action
Ghpf * ... % 2nd order HPF (limit low frequency gain)
eye(6); % Diagonal 6x6 controller (i.e. decentralized)
Kiff.InputName = {'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'};
Kiff.OutputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
% The designed IFF controller is saved
save('./mat/test_id31_K_iff.mat', 'Kiff');
%% Bode plot of the designed decentralized IFF controller
figure;
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(f, abs(squeeze(freqresp(Kiff(1,1), f, 'Hz'))), 'color', colors(1,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
ylim([1e-2, 1e1]);
ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(squeeze(freqresp(Kiff(1,1), f, 'Hz'))), 'color', colors(1,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-180, 180])
linkaxes([ax1,ax2],'x');
xlim([1, 1e3]);
%% Loop gain for the decentralized IFF controller
Kiff_frf = squeeze(freqresp(Kiff(1,1), f, 'Hz'));
figure;
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(f, abs(G_iff_m0_Wz0(:, 1, 1).*Kiff_frf), 'color', colors(1,:), ...
'DisplayName', '$m = 0$ kg');
plot(f, abs(G_iff_m1_Wz0(:, 1, 1).*Kiff_frf), 'color', colors(2,:), ...
'DisplayName', '$m = 13$ kg');
plot(f, abs(G_iff_m2_Wz0(:, 1, 1).*Kiff_frf), 'color', colors(3,:), ...
'DisplayName', '$m = 26$ kg');
plot(f, abs(G_iff_m3_Wz0(:, 1, 1).*Kiff_frf), 'color', colors(4,:), ...
'DisplayName', '$m = 39$ kg');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
ylim([1e-2, 1e1]);
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(-G_iff_m0_Wz0(:,1,1).*Kiff_frf), 'color', colors(1,:));
plot(f, 180/pi*angle(-G_iff_m1_Wz0(:,1,1).*Kiff_frf), 'color', colors(2,:));
plot(f, 180/pi*angle(-G_iff_m2_Wz0(:,1,1).*Kiff_frf), 'color', colors(3,:));
plot(f, 180/pi*angle(-G_iff_m3_Wz0(:,1,1).*Kiff_frf), 'color', colors(4,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-180, 180])
linkaxes([ax1,ax2],'x');
xlim([1, 1e3]);
% #+name: fig:test_id31_Kiff
% #+caption: Bode plot of the decentralized IFF controller (\subref{fig:test_id31_Kiff_bode_plot}). The decentralized controller $K_{\text{IFF}}$ multiplied by the identified dynamics from $u_1$ to $V_{s1}$ for all payloads are shown in (\subref{fig:test_id31_Kiff_loop_gain})
% #+attr_latex: :options [htbp]
% #+begin_figure
% #+attr_latex: :caption \subcaption{\label{fig:test_id31_Kiff_bode_plot}Bode plot of $K_{\text{IFF}}$}
% #+attr_latex: :options {0.49\textwidth}
% #+begin_subfigure
% #+attr_latex: :width 0.95\linewidth
% [[file:figs/test_id31_Kiff_bode_plot.png]]
% #+end_subfigure
% #+attr_latex: :caption \subcaption{\label{fig:test_id31_Kiff_loop_gain}Decentralized Loop gains}
% #+attr_latex: :options {0.49\textwidth}
% #+begin_subfigure
% #+attr_latex: :width 0.95\linewidth
% [[file:figs/test_id31_Kiff_loop_gain.png]]
% #+end_subfigure
% #+end_figure
% To estimate the added damping, a root-locus plot is computed using the multi-body model (Figure ref:fig:test_id31_iff_root_locus_m0).
% It can be seen that for all considered payloads, the poles are bounded to the "left-half plane" indicating that the decentralized IFF is robust.
% The closed-loop poles for the chosen value of the gain are displayed by black crosses.
% It can be seen that while damping can be added for all payloads (as compared to the open-loop case), the optimal value of the gain is different for each payload.
% For low payload masses, a higher value of the IFF controller gain could lead to better damping.
% However, in this study, it was chosen to implement a fix (i.e. non-adaptive) decentralized IFF controller.
%% Root Locus for IFF
gains = logspace(-2, 2, 100);
Gm_iff_m0 = Gm_m0_Wz0({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'});
Gm_iff_m1 = Gm_m1_Wz0({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'});
Gm_iff_m2 = Gm_m2_Wz0({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'});
Gm_iff_m3 = Gm_m3_Wz0({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'});
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
nexttile();
hold on;
plot(real(pole(Gm_iff_m0)), imag(pole(Gm_iff_m0)), 'x', 'color', colors(1,:), ...
'DisplayName', '$g = 0$');
plot(real(tzero(Gm_iff_m0)), imag(tzero(Gm_iff_m0)), 'o', 'color', colors(1,:), ...
'HandleVisibility', 'off');
for g = gains
clpoles = pole(feedback(Gm_iff_m0, g*Kiff, +1));
plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
'HandleVisibility', 'off');
end
% Optimal gain
clpoles = pole(feedback(Gm_iff_m0, Kiff, +1));
plot(real(clpoles), imag(clpoles), 'kx', ...
'DisplayName', '$g_{opt}$');
hold off;
axis equal;
xlim([-600, 0]); ylim([0, 1500]);
xticks([-600:300:0]);
yticks([0:300:1500]);
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
xlabel('Real part'); ylabel('Imaginary part');
%% description
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
nexttile();
hold on;
plot(real(pole(Gm_iff_m1)), imag(pole(Gm_iff_m1)), 'x', 'color', colors(2,:), ...
'DisplayName', '$g = 0$');
plot(real(tzero(Gm_iff_m1)), imag(tzero(Gm_iff_m1)), 'o', 'color', colors(2,:), ...
'HandleVisibility', 'off');
for g = gains
clpoles = pole(feedback(Gm_iff_m1, g*Kiff, +1));
plot(real(clpoles), imag(clpoles), '.', 'color', colors(2,:), ...
'HandleVisibility', 'off');
end
% Optimal gain
clpoles = pole(feedback(Gm_iff_m1, Kiff, +1));
plot(real(clpoles), imag(clpoles), 'kx', ...
'DisplayName', '$g_{opt}$');
hold off;
axis equal;
xlim([-200, 0]); ylim([0, 500]);
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
xlabel('Real part'); ylabel('Imaginary part');
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
nexttile();
hold on;
plot(real(pole(Gm_iff_m2)), imag(pole(Gm_iff_m2)), 'x', 'color', colors(3,:), ...
'DisplayName', '$g = 0$');
plot(real(tzero(Gm_iff_m2)), imag(tzero(Gm_iff_m2)), 'o', 'color', colors(3,:), ...
'HandleVisibility', 'off');
for g = gains
clpoles = pole(feedback(Gm_iff_m2, g*Kiff, +1));
plot(real(clpoles), imag(clpoles), '.', 'color', colors(3,:), ...
'HandleVisibility', 'off');
end
% Optimal gain
clpoles = pole(feedback(Gm_iff_m2, Kiff, +1));
plot(real(clpoles), imag(clpoles), 'kx', ...
'DisplayName', '$g_{opt}$');
hold off;
axis equal;
xlim([-200, 0]); ylim([0, 500]);
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
xlabel('Real part'); ylabel('Imaginary part');
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
nexttile();
hold on;
plot(real(pole(Gm_iff_m3)), imag(pole(Gm_iff_m3)), 'x', 'color', colors(4,:), ...
'DisplayName', '$g = 0$');
plot(real(tzero(Gm_iff_m3)), imag(tzero(Gm_iff_m3)), 'o', 'color', colors(4,:), ...
'HandleVisibility', 'off');
for g = gains
clpoles = pole(feedback(Gm_iff_m3, g*Kiff, +1));
plot(real(clpoles), imag(clpoles), '.', 'color', colors(4,:), ...
'HandleVisibility', 'off');
end
% Optimal gain
clpoles = pole(feedback(Gm_iff_m3, Kiff, +1));
plot(real(clpoles), imag(clpoles), 'kx', ...
'DisplayName', '$g_{opt}$');
hold off;
axis equal;
xlim([-200, 0]); ylim([0, 500]);
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
xlabel('Real part'); ylabel('Imaginary part');
% Damped Plant
% <<ssec:test_id31_iff_perf>>
% As the model is accurately modelling the system dynamics, it can be used to estimate the damped plant, i.e. the transfer functions from $\bm{u}^\prime$ to $\bm{\mathcal{L}}$.
% The obtained damped plants are compared to the open-loop plants in Figure ref:fig:test_id31_comp_ol_iff_plant_model.
% The peak amplitudes corresponding to the suspension modes are approximately reduced by a factor $10$ for all considered payloads, showing the effectiveness of the decentralized IFF control strategy.
% In order to experimentally validate the Decentralized IFF controller, it was implemented and the damped plants (i.e. the transfer function from $\bm{u}^\prime$ to $\bm{\epsilon\mathcal{L}}$) were identified for all payload conditions.
% The obtained frequency response functions are compared with the model in Figure ref:fig:test_id31_hac_plant_effect_mass verifying the good correlation between the predicted damped plant using the multi-body model and the experimental results.
%% Estimate damped plant from Multi-Body model
Gm_hac_m0_Wz0 = feedback(Gm_m0_Wz0, Kiff, 'name', +1);
Gm_hac_m1_Wz0 = feedback(Gm_m1_Wz0, Kiff, 'name', +1);
Gm_hac_m2_Wz0 = feedback(Gm_m2_Wz0, Kiff, 'name', +1);
Gm_hac_m3_Wz0 = feedback(Gm_m3_Wz0, Kiff, 'name', +1);
% Check Stability
if not(isstable(Gm_hac_m0_Wz0) && isstable(Gm_hac_m1_Wz0) && isstable(Gm_hac_m2_Wz0) && isstable(Gm_hac_m3_Wz0))
warning("One of the damped system with decentralized IFF is not stable");
end
% The estimated damped plants from the multi-body model are saved
save('./mat/test_id31_simscape_damped_plants.mat', 'Gm_hac_m0_Wz0', 'Gm_hac_m1_Wz0', 'Gm_hac_m2_Wz0', 'Gm_hac_m3_Wz0');
%% Comparison of the open-loop plants and the estimated damped plant with IFF
figure;
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(Gm_m0_Wz0('eL1', 'u1'), freqs, 'Hz'))), 'color', [colors(1,:), 0.3], ...
'DisplayName', '$-\epsilon\mathcal{L}_{1}/u_1$ - 0 kg');
plot(freqs, abs(squeeze(freqresp(Gm_m1_Wz0('eL1', 'u1'), freqs, 'Hz'))), 'color', [colors(2,:), 0.3], ...
'DisplayName', '$-\epsilon\mathcal{L}_{1}/u_1$ - 13 kg');
plot(freqs, abs(squeeze(freqresp(Gm_m2_Wz0('eL1', 'u1'), freqs, 'Hz'))), 'color', [colors(3,:), 0.3], ...
'DisplayName', '$-\epsilon\mathcal{L}_{1}/u_1$ - 26 kg');
plot(freqs, abs(squeeze(freqresp(Gm_m3_Wz0('eL1', 'u1'), freqs, 'Hz'))), 'color', [colors(4,:), 0.3], ...
'DisplayName', '$-\epsilon\mathcal{L}_{1}/u_1$ - 39 kg');
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0_Wz0('eL1', 'u1'), freqs, 'Hz'))), 'color', colors(1,:), ...
'DisplayName', '$-\epsilon\mathcal{L}_{1}/u_1^\prime$ - 0 kg');
plot(freqs, abs(squeeze(freqresp(Gm_hac_m1_Wz0('eL1', 'u1'), freqs, 'Hz'))), 'color', colors(2,:), ...
'DisplayName', '$-\epsilon\mathcal{L}_{1}/u_1^\prime$ - 13 kg');
plot(freqs, abs(squeeze(freqresp(Gm_hac_m2_Wz0('eL1', 'u1'), freqs, 'Hz'))), 'color', colors(3,:), ...
'DisplayName', '$-\epsilon\mathcal{L}_{1}/u_1^\prime$ - 26 kg');
plot(freqs, abs(squeeze(freqresp(Gm_hac_m3_Wz0('eL1', 'u1'), freqs, 'Hz'))), 'color', colors(4,:), ...
'DisplayName', '$-\epsilon\mathcal{L}_{1}/u_1^\prime$ - 39 kg');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-7, 4e-4]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;
ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0_Wz0('eL1','u1'), freqs, 'Hz'))), 'color', [colors(1,:), 0.3]);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m1_Wz0('eL1','u1'), freqs, 'Hz'))), 'color', [colors(2,:), 0.3]);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m2_Wz0('eL1','u1'), freqs, 'Hz'))), 'color', [colors(3,:), 0.3]);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m3_Wz0('eL1','u1'), freqs, 'Hz'))), 'color', [colors(4,:), 0.3]);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_hac_m0_Wz0('eL1','u1'), freqs, 'Hz')))), 'color', colors(1,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_hac_m1_Wz0('eL1','u1'), freqs, 'Hz')))), 'color', colors(2,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_hac_m2_Wz0('eL1','u1'), freqs, 'Hz')))), 'color', colors(3,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_hac_m3_Wz0('eL1','u1'), freqs, 'Hz')))), 'color', colors(4,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-20, 200])
linkaxes([ax1,ax2],'x');
xlim([1, 1e3]);
%% Identification of the damped Plant (transfer function from u' to dL)
% Load identification data
data_m0 = load('2023-08-17_17-53_damp_plant_m0_Wz0.mat');
data_m1 = load('2023-08-10_16-01_damp_plant_m1_Wz0.mat');
data_m2 = load('2023-08-10_17-28_damp_plant_m2_Wz0.mat');
data_m3 = load('2023-08-10_18-16_damp_plant_m3_Wz0.mat');
% Hannning Windows
Ts = 1e-4; % Sampling Time [s]
Nfft = floor(2.0/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);
% And we get the frequency vector
[~, f] = tfestimate(data_m0.uL1.id_plant, data_m0.uL1.e_L1, win, Noverlap, Nfft, 1/Ts);
% Identification without any payload
G_hac_m0_Wz0 = zeros(length(f), 6, 6);
for i_strut = 1:6
eL = [data_m0.(sprintf("uL%i", i_strut)).e_L1 ; data_m0.(sprintf("uL%i", i_strut)).e_L2 ; data_m0.(sprintf("uL%i", i_strut)).e_L3 ; data_m0.(sprintf("uL%i", i_strut)).e_L4 ; data_m0.(sprintf("uL%i", i_strut)).e_L5 ; data_m0.(sprintf("uL%i", i_strut)).e_L6]';
G_hac_m0_Wz0(:,:,i_strut) = tfestimate(data_m0.(sprintf("uL%i", i_strut)).id_plant, -detrend(eL, 0), win, Noverlap, Nfft, 1/Ts);
end
% Identification with 1 "payload layer"
G_hac_m1_Wz0 = zeros(length(f), 6, 6);
for i_strut = 1:6
eL = [data_m1.(sprintf("uL%i", i_strut)).e_L1 ; data_m1.(sprintf("uL%i", i_strut)).e_L2 ; data_m1.(sprintf("uL%i", i_strut)).e_L3 ; data_m1.(sprintf("uL%i", i_strut)).e_L4 ; data_m1.(sprintf("uL%i", i_strut)).e_L5 ; data_m1.(sprintf("uL%i", i_strut)).e_L6]';
G_hac_m1_Wz0(:,:,i_strut) = tfestimate(data_m1.(sprintf("uL%i", i_strut)).id_plant, -detrend(eL, 0), win, Noverlap, Nfft, 1/Ts);
end
% Identification with 2 "payload layers"
G_hac_m2_Wz0 = zeros(length(f), 6, 6);
for i_strut = 1:6
eL = [data_m2.(sprintf("uL%i", i_strut)).e_L1 ; data_m2.(sprintf("uL%i", i_strut)).e_L2 ; data_m2.(sprintf("uL%i", i_strut)).e_L3 ; data_m2.(sprintf("uL%i", i_strut)).e_L4 ; data_m2.(sprintf("uL%i", i_strut)).e_L5 ; data_m2.(sprintf("uL%i", i_strut)).e_L6]';
G_hac_m2_Wz0(:,:,i_strut) = tfestimate(data_m2.(sprintf("uL%i", i_strut)).id_plant, -detrend(eL, 0), win, Noverlap, Nfft, 1/Ts);
end
% Identification with 3 "payload layers"
G_hac_m3_Wz0 = zeros(length(f), 6, 6);
for i_strut = 1:6
eL = [data_m3.(sprintf("uL%i", i_strut)).e_L1 ; data_m3.(sprintf("uL%i", i_strut)).e_L2 ; data_m3.(sprintf("uL%i", i_strut)).e_L3 ; data_m3.(sprintf("uL%i", i_strut)).e_L4 ; data_m3.(sprintf("uL%i", i_strut)).e_L5 ; data_m3.(sprintf("uL%i", i_strut)).e_L6]';
G_hac_m3_Wz0(:,:,i_strut) = tfestimate(data_m3.(sprintf("uL%i", i_strut)).id_plant, -detrend(eL, 0), win, Noverlap, Nfft, 1/Ts);
end
% The identified dynamics are then saved for further use.
save('./mat/test_id31_identified_damped_plants.mat', 'G_hac_m0_Wz0', 'G_hac_m1_Wz0', 'G_hac_m2_Wz0', 'G_hac_m3_Wz0', 'f');
%% Comparison of the identified HAC plant and the HAC plant extracted from the simscape model
figure;
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(f, abs(G_hac_m0_Wz0(:, 1, 1)), 'color', [colors(1,:), 0.2], ...
'DisplayName', '$m = 0$ kg');
for i = 2:6
plot(f, abs(G_hac_m0_Wz0(:,i, i)), 'color', [colors(1,:), 0.2], ...
'HandleVisibility', 'off')
end
plot(f, abs(G_hac_m1_Wz0(:, 1, 1)), 'color', [colors(2,:), 0.2], ...
'DisplayName', '$m = 13$ kg');
for i = 2:6
plot(f, abs(G_hac_m1_Wz0(:,i, i)), 'color', [colors(2,:), 0.2], ...
'HandleVisibility', 'off')
end
plot(f, abs(G_hac_m2_Wz0(:, 1, 1)), 'color', [colors(3,:), 0.2], ...
'DisplayName', '$m = 26$ kg');
for i = 2:6
plot(f, abs(G_hac_m2_Wz0(:,i, i)), 'color', [colors(3,:), 0.2], ...
'HandleVisibility', 'off')
end
plot(f, abs(G_hac_m3_Wz0(:, 1, 1)), 'color', [colors(4,:), 0.2], ...
'DisplayName', '$m = 39$ kg');
for i = 2:6
plot(f, abs(G_hac_m3_Wz0(:,i, i)), 'color', [colors(4,:), 0.2], ...
'HandleVisibility', 'off')
end
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:), ...
'DisplayName', '$m = 0$ kg (model)');
plot(freqs, abs(squeeze(freqresp(Gm_hac_m1_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:), ...
'DisplayName', '$m = 13$ kg (model)');
plot(freqs, abs(squeeze(freqresp(Gm_hac_m2_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:), ...
'DisplayName', '$m = 26$ kg (model)');
plot(freqs, abs(squeeze(freqresp(Gm_hac_m3_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:), ...
'DisplayName', '$m = 39$ kg (model)');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
ylim([2e-7, 3e-5]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;
ax2 = nexttile;
hold on;
plot(f, 180/pi*unwrapphase(angle(G_hac_m0_Wz0(:,1,1)), f), 'color', [colors(1,:), 0.2]);
for i = 2:6
plot(f, 180/pi*unwrapphase(angle(G_hac_m0_Wz0(:,i, i)), f), 'color', [colors(1,:), 0.2]);
end
plot(f, 180/pi*unwrapphase(angle(G_hac_m1_Wz0(:,1,1)), f), 'color', [colors(2,:), 0.2]);
for i = 2:6
plot(f, 180/pi*unwrapphase(angle(G_hac_m1_Wz0(:,i, i)), f), 'color', [colors(2,:), 0.2]);
end
plot(f, 180/pi*unwrapphase(angle(G_hac_m2_Wz0(:,1,1)), f), 'color', [colors(3,:), 0.2]);
for i = 2:6
plot(f, 180/pi*unwrapphase(angle(G_hac_m2_Wz0(:,i, i)), f), 'color', [colors(3,:), 0.2]);
end
plot(f, 180/pi*unwrapphase(angle(G_hac_m3_Wz0(:,1,1)), f), 'color', [colors(4,:), 0.2]);
for i = 2:6
plot(f, 180/pi*unwrapphase(angle(G_hac_m3_Wz0(:,i, i)), f), 'color', [colors(4,:), 0.2]);
end
plot(freqs, 180/pi*unwrapphase(angle(squeeze(freqresp(-exp(-3e-4*s)*Gm_hac_m0_Wz0('eL1', 'u1'), freqs, 'Hz'))), f), '--', 'color', colors(1,:));
plot(freqs, 180/pi*unwrapphase(angle(squeeze(freqresp(-exp(-3e-4*s)*Gm_hac_m1_Wz0('eL1', 'u1'), freqs, 'Hz'))), f), '--', 'color', colors(2,:));
plot(freqs, 180/pi*unwrapphase(angle(squeeze(freqresp(-exp(-3e-4*s)*Gm_hac_m2_Wz0('eL1', 'u1'), freqs, 'Hz'))), f), '--', 'color', colors(3,:));
plot(freqs, 180/pi*unwrapphase(angle(squeeze(freqresp(-exp(-3e-4*s)*Gm_hac_m3_Wz0('eL1', 'u1'), freqs, 'Hz'))), f), '--', 'color', colors(4,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-270, 20])
linkaxes([ax1,ax2],'x');
xlim([1, 5e2]);

583
matlab/test_id31_4_hac.m Normal file
View File

@ -0,0 +1,583 @@
% Matlab Init :noexport:ignore:
%% test_id31_4_hac.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
%% Data directory
data_dir = './mat/';
% Simulink Model name
mdl = 'nass_model_id31';
%% Colors for the figures
colors = colororder;
%% Frequency Vector
freqs = logspace(log10(1), log10(2e3), 1000);
%% Sampling Time
Ts = 1e-4;
%% Specifications for Experiments
specs_dz_peak = 50; % [nm]
specs_dy_peak = 100; % [nm]
specs_ry_peak = 0.85; % [urad]
specs_dz_rms = 15; % [nm RMS]
specs_dy_rms = 30; % [nm RMS]
specs_ry_rms = 0.25; % [urad RMS]
% Damped Plant
% <<ssec:test_id31_iff_hac_plant>>
% To verify whether the multi body model accurately represents the measured damped dynamics, both direct terms and coupling terms corresponding to the first actuator are compared in Figure ref:fig:test_id31_comp_simscape_hac.
% Considering the complexity of the system's dynamics, the model can be considered to well represent the system's dynamics, and can therefore be used to tune the feedback controller and evaluate its performances.
% Load the estimated damped plant from the multi-body model
load('test_id31_simscape_damped_plants.mat', 'Gm_hac_m0_Wz0', 'Gm_hac_m1_Wz0', 'Gm_hac_m2_Wz0', 'Gm_hac_m3_Wz0');
% Load the measured damped plants
load('test_id31_identified_damped_plants.mat', 'G_hac_m0_Wz0', 'G_hac_m1_Wz0', 'G_hac_m2_Wz0', 'G_hac_m3_Wz0', 'f');
% Load the undamped plant for comparison
load('test_id31_identified_open_loop_plants.mat', 'G_int_m0_Wz0', 'G_int_m1_Wz0', 'G_int_m2_Wz0', 'G_int_m3_Wz0', 'f');
figure;
tiledlayout(2, 3, 'TileSpacing', 'tight', 'Padding', 'tight');
ax1 = nexttile();
hold on;
plot(f, abs(G_hac_m0_Wz0(:, 1, 1)));
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0_Wz0('eL1', 'u1'), freqs, 'Hz'))));
text(12, 3e-5, '$\epsilon_{\mathcal{L}1}/u_1^\prime$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/V]');
yticks([1e-7, 1e-6, 1e-5]);
ax2 = nexttile();
hold on;
plot(f, abs(G_hac_m0_Wz0(:, 2, 1)));
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0_Wz0('eL2', 'u1'), freqs, 'Hz'))));
text(12, 3e-5, '$\epsilon_{\mathcal{L}2}/u_1^\prime$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
ax3 = nexttile();
hold on;
plot(f, abs(G_hac_m0_Wz0(:, 3, 1)))
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0_Wz0('eL3', 'u1'), freqs, 'Hz'))))
text(12, 3e-5, '$\epsilon_{\mathcal{L}3}/u_1^\prime$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
ax4 = nexttile();
hold on;
plot(f, abs(G_hac_m0_Wz0(:, 4, 1)));
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0_Wz0('eL4', 'u1'), freqs, 'Hz'))));
text(12, 3e-5, '$\epsilon_{\mathcal{L}4}/u_1^\prime$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
xticks([10, 20, 50, 100, 200])
yticks([1e-7, 1e-6, 1e-5]);
ax5 = nexttile();
hold on;
plot(f, abs(G_hac_m0_Wz0(:, 5, 1)));
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0_Wz0('eL5', 'u1'), freqs, 'Hz'))));
text(12, 3e-5, '$\epsilon_{\mathcal{L}5}/u_1^\prime$', 'Horiz','left', 'Vert','top')
hold off;
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xticks([10, 20, 50, 100, 200])
ax6 = nexttile();
hold on;
plot(f, abs(G_hac_m0_Wz0(:, 6, 1)), ...
'DisplayName', 'Measurements');
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0_Wz0('eL6', 'u1'), freqs, 'Hz'))), ...
'DisplayName', 'Model (2-DoF APA)');
text(12, 3e-5, '$\epsilon_{\mathcal{L}6}/u_1^\prime$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
xticks([10, 20, 50, 100, 200])
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'xy');
xlim([10, 5e2]); ylim([1e-7, 4e-5]);
% #+name: fig:test_id31_comp_simscape_hac
% #+caption: Comparison of the measured (in blue) and modeled (in red) frequency transfer functions from the first control signal ($u_1^\prime$) of the damped plant to the estimated errors ($\epsilon_{\mathcal{L}_i}$) in the frame of the six struts by the external metrology
% #+RESULTS:
% [[file:figs/test_id31_comp_simscape_hac.png]]
% The challenge here is to tune an high authority controller such that it is robust to the change of dynamics due to different payloads being used.
% Doing that without using the HAC-LAC strategy would require to design a controller which gives good performances for all the undamped dynamics (blue curves in Figure ref:fig:test_id31_comp_all_undamped_damped_plants), which is a very complex control problem.
% With the HAC-LAC strategy, the designed controller instead has to be be robust to all the damped dynamics (red curves in Figure ref:fig:test_id31_comp_all_undamped_damped_plants), which is easier from a control perspective.
% This is one of the key benefit of using the HAC-LAC strategy.
%% Comparison of all the undamped FRF and all the damped FRF
figure;
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(f, abs(G_int_m0_Wz0(:,1,1)), 'color', [colors(1,:), 0.5], 'DisplayName', 'Undamped - $\epsilon\mathcal{L}_i/u_i$');
plot(f, abs(G_hac_m0_Wz0(:,1,1)), 'color', [colors(2,:), 0.5], 'DisplayName', 'damped - $\epsilon\mathcal{L}_i/u_i^\prime$');
for i = 1:6
plot(f, abs(G_int_m0_Wz0(:,i, i)), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
plot(f, abs(G_int_m1_Wz0(:,i, i)), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
plot(f, abs(G_int_m2_Wz0(:,i, i)), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
plot(f, abs(G_int_m3_Wz0(:,i, i)), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
end
for i = 1:6
plot(f, abs(G_hac_m0_Wz0(:,i, i)), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
plot(f, abs(G_hac_m1_Wz0(:,i, i)), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
plot(f, abs(G_hac_m2_Wz0(:,i, i)), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
plot(f, abs(G_hac_m3_Wz0(:,i, i)), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ylim([2e-7, 4e-4]);
ax2 = nexttile;
hold on;
for i =1:6
plot(f, 180/pi*unwrapphase(angle(-G_int_m0_Wz0(:,i, i)), f), 'color', [colors(1,:), 0.5]);
plot(f, 180/pi*unwrapphase(angle(-G_int_m1_Wz0(:,i, i)), f), 'color', [colors(1,:), 0.5]);
plot(f, 180/pi*unwrapphase(angle(-G_int_m2_Wz0(:,i, i)), f), 'color', [colors(1,:), 0.5]);
plot(f, 180/pi*unwrapphase(angle(-G_int_m3_Wz0(:,i, i)), f), 'color', [colors(1,:), 0.5]);
end
for i = 1:6
plot(f, 180/pi*unwrapphase(angle(G_hac_m0_Wz0(:,i, i)), f), 'color', [colors(2,:), 0.5]);
plot(f, 180/pi*unwrapphase(angle(G_hac_m1_Wz0(:,i, i)), f), 'color', [colors(2,:), 0.5]);
plot(f, 180/pi*unwrapphase(angle(G_hac_m2_Wz0(:,i, i)), f), 'color', [colors(2,:), 0.5]);
plot(f, 180/pi*unwrapphase(angle(G_hac_m3_Wz0(:,i, i)), f), 'color', [colors(2,:), 0.5]);
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-270, 20])
linkaxes([ax1,ax2],'x');
xlim([1, 5e2]);
% Interaction Analysis
% <<sec:test_id31_hac_interaction_analysis>>
% As the control strategy here is to apply a diagonal control in the frame of the struts, it is important to determine the frequency at which multivariable effects become significant, as this represents a critical limitation of the control approach.
% To conduct this interaction analysis, the acrfull:rga $\bm{\Lambda_G}$ is first computed using eqref:eq:test_id31_rga for the plant dynamics identified with the multiple payload masses.
% \begin{equation}\label{eq:test_id31_rga}
% \bm{\Lambda_G}(\omega) = \bm{G}(j\omega) \star \left(\bm{G}(j\omega)^{-1}\right)^{T}, \quad (\star \text{ means element wise multiplication})
% \end{equation}
% Then, acrshort:rga numbers are computed using eqref:eq:test_id31_rga_number and are use as a metric for interaction [[cite:&skogestad07_multiv_feedb_contr chapt. 3.4]].
% \begin{equation}\label{eq:test_id31_rga_number}
% \text{RGA number}(\omega) = \|\bm{\Lambda_G}(\omega) - \bm{I}\|_{\text{sum}}
% \end{equation}
% The obtained acrshort:rga numbers are compared in Figure ref:fig:test_id31_hac_rga_number.
% The results indicates that higher payload masses increase the coupling when implementing control in the strut reference frame (i.e., decentralized approach).
% This indicates that it is progressively more challenging to achieve high bandwidth performance as the payload mass increases.
% This behavior can be attributed to the fundamental approach of implementing control in the frame of the struts.
% Indeed, above the suspension modes of the nano-hexapod, the induced motion by the piezoelectric actuators is no longer dictated by the kinematics but rather by the inertia of the different parts.
% This design choice, while beneficial for system simplicity, introduces inherent limitations in the system's ability to handle larger masses at high frequency.
%% Interaction Analysis - RGA Number
rga_m0 = zeros(1,size(G_hac_m0_Wz0,1));
for i = 1:length(rga_m0)
rga_m0(i) = sum(sum(abs(inv(squeeze(G_hac_m0_Wz0(i,:,:)).').*squeeze(G_hac_m0_Wz0(i,:,:)) - eye(6))));
end
rga_m1 = zeros(1,size(G_hac_m1_Wz0,1));
for i = 1:length(rga_m1)
rga_m1(i) = sum(sum(abs(inv(squeeze(G_hac_m1_Wz0(i,:,:)).').*squeeze(G_hac_m1_Wz0(i,:,:)) - eye(6))));
end
rga_m2 = zeros(1,size(G_hac_m2_Wz0,1));
for i = 1:length(rga_m2)
rga_m2(i) = sum(sum(abs(inv(squeeze(G_hac_m2_Wz0(i,:,:)).').*squeeze(G_hac_m2_Wz0(i,:,:)) - eye(6))));
end
rga_m3 = zeros(1,size(G_hac_m3_Wz0,1));
for i = 1:length(rga_m3)
rga_m3(i) = sum(sum(abs(inv(squeeze(G_hac_m3_Wz0(i,:,:)).').*squeeze(G_hac_m3_Wz0(i,:,:)) - eye(6))));
end
%% RGA-number for the damped plants - Comparison of all the payload conditions
figure;
hold on;
plot(f, rga_m0, 'DisplayName', '$m = 0$ kg')
plot(f, rga_m1, 'DisplayName', '$m = 13$ kg')
plot(f, rga_m2, 'DisplayName', '$m = 26$ kg')
plot(f, rga_m3, 'DisplayName', '$m = 39$ kg')
xlabel('Frequency [Hz]'); ylabel('RGA number');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlim([1, 1e2]); ylim([0, 10]);
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;
% Robust Controller Design
% <<ssec:test_id31_iff_hac_controller>>
% A diagonal controller was designed to be robust to change of payloads, which means that every damped plants shown in Figure ref:fig:test_id31_comp_all_undamped_damped_plants should be considered during the controller design.
% For this controller design, a crossover frequency of $5\,\text{Hz}$ was chosen to limit multivariable effects as explain in Section ref:sec:test_id31_hac_interaction_analysis.
% One integrator is added to increase the low frequency gain, a lead is added around $5\,\text{Hz}$ to increase the stability margins and a first order low pass filter with a cut-off frequency of $30\,\text{Hz}$ is added to improve the robustness to dynamical uncertainty at high frequency.
% The controller transfer function is shown in eqref:eq:test_id31_robust_hac.
% \begin{equation}\label{eq:test_id31_robust_hac}
% K_{\text{HAC}}(s) = g_0 \cdot \underbrace{\frac{\omega_c}{s}}_{\text{int}} \cdot \underbrace{\frac{1}{\sqrt{\alpha}}\frac{1 + \frac{s}{\omega_c/\sqrt{\alpha}}}{1 + \frac{s}{\omega_c\sqrt{\alpha}}}}_{\text{lead}} \cdot \underbrace{\frac{1}{1 + \frac{s}{\omega_0}}}_{\text{LPF}}, \quad \left( \omega_c = 2\pi5\,\text{rad/s},\ \alpha = 2,\ \omega_0 = 2\pi30\,\text{rad/s} \right)
% \end{equation}
% The obtained "decentralized" loop-gains (i.e. the diagonal element of the controller times the diagonal terms of the plant) are shown in Figure ref:fig:test_id31_hac_loop_gain.
% Closed-loop stability is verified by computing the characteristic Loci (Figure ref:fig:test_id31_hac_characteristic_loci).
% However, small stability margins are observed for the highest mass, indicating that some multivariable effects are in play.
%% HAC Design
% Wanted crossover
wc = 2*pi*5; % [rad/s]
% Integrator
H_int = wc/s;
% Lead to increase phase margin
a = 2; % Amount of phase lead / width of the phase lead / high frequency gain
H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
% Low Pass filter to increase robustness
H_lpf = 1/(1 + s/2/pi/30);
% Gain to have unitary crossover at 5Hz
[~, i_f] = min(abs(f - wc/2/pi));
H_gain = 1./abs(G_hac_m0_Wz0(i_f, 1, 1));
% Decentralized HAC
Khac = H_gain * ... % Gain
H_int * ... % Integrator
H_lpf * ... % Low Pass filter
eye(6); % 6x6 Diagonal
% The designed HAC controller is saved
save('./mat/test_id31_K_hac_robust.mat', 'Khac');
%% Decentralized Loop gain for the High Authority Controller
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(f(2:end), abs(G_hac_m0_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f(2:end), 'Hz'))), 'color', colors(1,:), 'DisplayName', '$0$ kg');
plot(f(2:end), abs(G_hac_m1_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f(2:end), 'Hz'))), 'color', colors(2,:), 'DisplayName', '$13$ kg');
plot(f(2:end), abs(G_hac_m2_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f(2:end), 'Hz'))), 'color', colors(3,:), 'DisplayName', '$26$ kg');
plot(f(2:end), abs(G_hac_m3_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f(2:end), 'Hz'))), 'color', colors(4,:), 'DisplayName', '$39$ kg');
xline(5, '--', 'linewidth', 1, 'color', [0,0,0,0.2], 'HandleVisibility', 'off')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
ylim([1e-5, 1e2]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;
ax2 = nexttile;
hold on;
plot(f(2:end), 180/pi*angle(G_hac_m0_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f(2:end), 'Hz'))), 'color', colors(1,:));
plot(f(2:end), 180/pi*angle(G_hac_m1_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f(2:end), 'Hz'))), 'color', colors(2,:));
plot(f(2:end), 180/pi*angle(G_hac_m2_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f(2:end), 'Hz'))), 'color', colors(3,:));
plot(f(2:end), 180/pi*angle(G_hac_m3_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f(2:end), 'Hz'))), 'color', colors(4,:));
xline(5, '--', 'linewidth', 1, 'color', [0,0,0,0.2])
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-180, 180])
linkaxes([ax1,ax2],'x');
xlim([1, 1e3]);
%% Compute the Eigenvalues of the loop gain
Ldet = zeros(4, 6, length(f));
% Loop gain
Lmimo = pagemtimes(permute(G_hac_m0_Wz0, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
for i_f = 2:length(f)
Ldet(1,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
end
Lmimo = pagemtimes(permute(G_hac_m1_Wz0, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
for i_f = 2:length(f)
Ldet(2,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
end
Lmimo = pagemtimes(permute(G_hac_m2_Wz0, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
for i_f = 2:length(f)
Ldet(3,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
end
Lmimo = pagemtimes(permute(G_hac_m3_Wz0, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
for i_f = 2:length(f)
Ldet(4,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
end
%% Plot of the eigenvalues of L in the complex plane
figure;
hold on;
plot(real(squeeze(Ldet(1, 1,:))), imag(squeeze(Ldet(1, 1,:))), ...
'.', 'color', colors(1, :), ...
'DisplayName', '$m = 0$ kg');
plot(real(squeeze(Ldet(2, 1,:))), imag(squeeze(Ldet(2, 1,:))), ...
'.', 'color', colors(2, :), ...
'DisplayName', '$m = 13$ kg');
plot(real(squeeze(Ldet(3, 1,:))), imag(squeeze(Ldet(3, 1,:))), ...
'.', 'color', colors(3, :), ...
'DisplayName', '$m = 26$ kg');
plot(real(squeeze(Ldet(4, 1,:))), imag(squeeze(Ldet(4, 1,:))), ...
'.', 'color', colors(4, :), ...
'DisplayName', '$m = 39$ kg');
for i_mass = 1:4
plot(real(squeeze(Ldet(i_mass, 1,:))), -imag(squeeze(Ldet(i_mass, 1,:))), ...
'.', 'color', colors(i_mass, :), ...
'HandleVisibility', 'off');
for i = 1:6
plot(real(squeeze(Ldet(i_mass, i,:))), imag(squeeze(Ldet(i_mass, i,:))), ...
'.', 'color', colors(i_mass, :), ...
'HandleVisibility', 'off');
plot(real(squeeze(Ldet(i_mass, i,:))), -imag(squeeze(Ldet(i_mass, i,:))), ...
'.', 'color', colors(i_mass, :), ...
'HandleVisibility', 'off');
end
end
plot(-1, 0, 'kx', 'HandleVisibility', 'off');
hold off;
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
xlabel('Real'); ylabel('Imag');
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;
axis square
xlim([-1.5, 0.5]); ylim([-1, 1]);
% Performance estimation with simulation of Tomography scans
% <<ssec:test_id31_iff_hac_perf>>
% To estimate the performances that can be expected with this HAC-LAC architecture and the designed controller, simulations of tomography experiments were performed[fn:4].
% The rotational velocity was set to $180\,\text{deg/s}$, and no payload was added on top of the nano-hexapod.
% An open-loop simulation and a closed-loop simulation were performed and compared in Figure ref:fig:test_id31_tomo_m0_30rpm_robust_hac_iff_sim.
% The obtained closed-loop positioning accuracy was found to comply with the requirements as it succeeded to keep the point of interest on the beam (Figure ref:fig:test_id31_tomo_m0_30rpm_robust_hac_iff_sim_yz).
%% Tomography experiment
% Sample is not centered with the rotation axis
% This is done by offsetfing the micro-hexapod by 0.9um
P_micro_hexapod = [2.5e-6; 0; -0.3e-6]; % [m]
open(mdl);
set_param(mdl, 'StopTime', '3'); % 6 turns at 180deg/s (30rpm)
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod('AP', P_micro_hexapod);
initializeNanoHexapod('flex_bot_type', '2dof', ...
'flex_top_type', '3dof', ...
'motion_sensor_type', 'plates', ...
'actuator_type', '2dof');
initializeSample('type', '0');
initializeSimscapeConfiguration('gravity', false);
initializeLoggingConfiguration('log', 'all', 'Ts', 1e-4);
initializeController('type', 'open-loop');
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', 360/180, ... % 180deg/s, 30rpm
'Dh_pos', [P_micro_hexapod; 0; 0; 0]);
% Open-Loop Simulation
sim(mdl);
exp_tomo_ol_m0_Wz180 = simout;
% Closed-Loop Simulation
load('test_id31_K_iff.mat', 'Kiff');
load('test_id31_K_hac_robust.mat', 'Khac');
initializeController('type', 'hac-iff');
initializeSample('type', '0');
sim(mdl);
exp_tomo_cl_m0_Wz180 = simout;
% Save the simulation results
save('./mat/test_id31_exp_tomo_ol_cl_30rpm_sim.mat', 'exp_tomo_ol_m0_Wz180', 'exp_tomo_cl_m0_Wz180');
%% Simulation of tomography experiment - no payload, 30rpm - XY errors
figure;
hold on;
plot(1e6*exp_tomo_ol_m0_Wz180.y.x.Data, 1e6*exp_tomo_ol_m0_Wz180.y.y.Data, 'DisplayName', 'OL')
plot(1e6*exp_tomo_cl_m0_Wz180.y.x.Data(1:2e3), 1e6*exp_tomo_cl_m0_Wz180.y.y.Data(1:2e3), 'color', colors(3,:), 'HandleVisibility', 'off')
plot(1e6*exp_tomo_cl_m0_Wz180.y.x.Data(2e3:end), 1e6*exp_tomo_cl_m0_Wz180.y.y.Data(2e3:end), 'color', colors(2,:), 'DisplayName', 'CL')
hold off;
xlabel('$D_x$ [$\mu$m]'); ylabel('$D_y$ [$\mu$m]');
axis equal
xlim([-3, 3]); ylim([-3, 3]);
xticks([-3:1:3]);
yticks([-3:1:3]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
%% Simulation of tomography experiment - no payload, 30rpm - YZ errors
figure;
tiledlayout(2, 1, 'TileSpacing', 'compact', 'Padding', 'None');
ax1 = nexttile();
hold on;
plot(1e6*exp_tomo_ol_m0_Wz180.y.y.Data, 1e6*exp_tomo_ol_m0_Wz180.y.z.Data, 'DisplayName', 'OL')
plot(1e6*exp_tomo_cl_m0_Wz180.y.y.Data(1:2e3), 1e6*exp_tomo_cl_m0_Wz180.y.z.Data(1:2e3), 'color', colors(3,:), 'HandleVisibility', 'off')
plot(1e6*exp_tomo_cl_m0_Wz180.y.y.Data(2e3:end), 1e6*exp_tomo_cl_m0_Wz180.y.z.Data(2e3:end), 'color', colors(2,:), 'DisplayName', 'CL')
hold off;
xlabel('$D_y$ [$\mu$m]'); ylabel('$D_z$ [$\mu$m]');
axis equal
xlim([-3, 3]); ylim([-0.6, 0.6]);
xticks([-3:1:3]);
yticks([-3:0.3:3]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ax2 = nexttile();
hold on;
plot(1e9*exp_tomo_cl_m0_Wz180.y.y.Data(2e3:end), 1e9*exp_tomo_cl_m0_Wz180.y.z.Data(2e3:end), 'color', colors(2,:), 'DisplayName', 'CL')
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
plot(100*cos(theta), 50*sin(theta), 'k--', 'DisplayName', 'Beam size')
hold off;
xlabel('$D_y$ [nm]'); ylabel('$D_z$ [nm]');
axis equal
xlim([-300, 300]); ylim([-100, 100]);
% xticks([-3:1:3]);
% yticks([-3:1:3]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
% Robustness estimation with simulation of Tomography scans
% <<ssec:test_id31_iff_hac_robustness>>
% To verify the robustness to the change of payload mass, four simulations of tomography experiments were performed with payloads as shown Figure ref:fig:test_id31_picture_masses (i.e. $0\,kg$, $13\,kg$, $26\,kg$ and $39\,kg$).
% This time, the rotational velocity was set at $6\,\text{deg/s}$, as it is the typical rotational velocity for heavy samples.
% The closed-loop systems were stable for all payload conditions, indicating good control robustness.
% However, the positioning errors are getting worse as the payload mass increases, especially in the lateral $D_y$ direction, as shown in Figure ref:fig:test_id31_hac_tomography_Wz36_simulation.
% Yet it was decided that this controller will be tested experimentally, and improved if necessary.
%% Simulation of tomography experiments at 1RPM with all payloads
% Configuration
open(mdl);
set_param(mdl, 'StopTime', '2'); % 30 degrees at 1rpm
initializeLoggingConfiguration('log', 'all', 'Ts', 1e-3);
initializeController('type', 'hac-iff');
initializeReferences(...
'Rz_type', 'rotating', ...
'Rz_period', 360/6, ... % 6deg/s, 1 rpm
'Dh_pos', [P_micro_hexapod; 0; 0; 0]);
% Perform the simulations
initializeSample('type', '0');
sim(mdl);
exp_tomo_cl_m0_1rpm = simout;
initializeSample('type', '1');
sim(mdl);
exp_tomo_cl_m1_1rpm = simout;
initializeSample('type', '2');
sim(mdl);
exp_tomo_cl_m2_1rpm = simout;
initializeSample('type', '3');
sim(mdl);
exp_tomo_cl_m3_1rpm = simout;
% Save the simulation results
save('./mat/test_id31_exp_tomo_cl_1rpm_sim.mat', 'exp_tomo_cl_m0_1rpm', 'exp_tomo_cl_m1_1rpm', 'exp_tomo_cl_m2_1rpm', 'exp_tomo_cl_m3_1rpm');
%% Positioning errors in the Y-Z plane during tomography experiments simulated using the multi-body model
figure;
tiledlayout(2, 2, 'TileSpacing', 'compact', 'Padding', 'None');
ax1 = nexttile;
hold on;
plot(1e9*exp_tomo_cl_m0_1rpm.y.y.Data(1e3:end), 1e9*exp_tomo_cl_m0_1rpm.y.z.Data(1e3:end), 'color', colors(1,:), 'DisplayName', '$m = 0$ kg')
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
plot(100*cos(theta), 50*sin(theta), 'k--', 'DisplayName', 'Beam size')
axis equal
xticks([-400:100:400]); yticks([-100:100:100]);
xlabel('$D_y$ [nm]'); ylabel('$D_z$ [nm]');
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ax2 = nexttile;
hold on;
plot(1e9*exp_tomo_cl_m1_1rpm.y.y.Data(1e3:end), 1e9*exp_tomo_cl_m1_1rpm.y.z.Data(1e3:end), 'color', colors(2,:), 'DisplayName', '$m = 13$ kg')
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
plot(100*cos(theta), 50*sin(theta), 'k--', 'HandleVisibility', 'off')
axis equal
xticks([-400:100:400]); yticks([-100:100:100]);
xlabel('$D_y$ [nm]'); ylabel('$D_z$ [nm]');
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ax3 = nexttile;
hold on;
plot(1e9*exp_tomo_cl_m2_1rpm.y.y.Data(1e3:end), 1e9*exp_tomo_cl_m2_1rpm.y.z.Data(1e3:end), 'color', colors(3,:), 'DisplayName', '$m = 26$ kg')
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
plot(100*cos(theta), 50*sin(theta), 'k--', 'HandleVisibility', 'off')
axis equal
xticks([-400:100:400]); yticks([-100:100:100]);
xlabel('$D_y$ [nm]'); ylabel('$D_z$ [nm]');
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ax4 = nexttile;
hold on;
plot(1e9*exp_tomo_cl_m3_1rpm.y.y.Data(1e3:end), 1e9*exp_tomo_cl_m3_1rpm.y.z.Data(1e3:end), 'color', colors(4,:), 'DisplayName', '$m = 39$ kg')
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
plot(100*cos(theta), 50*sin(theta), 'k--', 'HandleVisibility', 'off')
axis equal
xticks([-400:100:400]); yticks([-100:100:100]);
xlabel('$D_y$ [nm]'); ylabel('$D_z$ [nm]');
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
linkaxes([ax1,ax2,ax3, ax4],'xy');
xlim([-450, 450]); ylim([-100, 100]);

1222
matlab/test_id31_5_experiments.m Normal file
View File

File diff suppressed because it is too large Load Diff

114
test-bench-id31.org
View File

@ -302,6 +302,7 @@ Simulation: 30rpm experiment with large off-axis errors (to see if it converges
% This is done by offsetfing the micro-hexapod by 0.9um
P_micro_hexapod = [10e-6; 0; 0]; % [m]
open(mdl)
set_param(mdl, 'StopTime', '3');
initializeMicroHexapod('AP', P_micro_hexapod);
@ -2474,10 +2475,9 @@ Gm_hac_m2_Wz0 = feedback(Gm_m2_Wz0, Kiff, 'name', +1);
Gm_hac_m3_Wz0 = feedback(Gm_m3_Wz0, Kiff, 'name', +1);
% Check Stability
isstable(Gm_hac_m0_Wz0)
isstable(Gm_hac_m1_Wz0)
isstable(Gm_hac_m2_Wz0)
isstable(Gm_hac_m3_Wz0)
if not(isstable(Gm_hac_m0_Wz0) && isstable(Gm_hac_m1_Wz0) && isstable(Gm_hac_m2_Wz0) && isstable(Gm_hac_m3_Wz0))
warning("One of the damped system with decentralized IFF is not stable");
end
#+end_src
#+begin_src matlab :exports none :tangle no
@ -3094,10 +3094,10 @@ tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(f, abs(G_hac_m0_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(1,:), 'DisplayName', '$0$ kg');
plot(f, abs(G_hac_m1_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(2,:), 'DisplayName', '$13$ kg');
plot(f, abs(G_hac_m2_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(3,:), 'DisplayName', '$26$ kg');
plot(f, abs(G_hac_m3_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(4,:), 'DisplayName', '$39$ kg');
plot(f(2:end), abs(G_hac_m0_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f(2:end), 'Hz'))), 'color', colors(1,:), 'DisplayName', '$0$ kg');
plot(f(2:end), abs(G_hac_m1_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f(2:end), 'Hz'))), 'color', colors(2,:), 'DisplayName', '$13$ kg');
plot(f(2:end), abs(G_hac_m2_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f(2:end), 'Hz'))), 'color', colors(3,:), 'DisplayName', '$26$ kg');
plot(f(2:end), abs(G_hac_m3_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f(2:end), 'Hz'))), 'color', colors(4,:), 'DisplayName', '$39$ kg');
xline(5, '--', 'linewidth', 1, 'color', [0,0,0,0.2], 'HandleVisibility', 'off')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
@ -3108,10 +3108,10 @@ leg.ItemTokenSize(1) = 15;
ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(G_hac_m0_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(1,:));
plot(f, 180/pi*angle(G_hac_m1_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(2,:));
plot(f, 180/pi*angle(G_hac_m2_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(3,:));
plot(f, 180/pi*angle(G_hac_m3_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(4,:));
plot(f(2:end), 180/pi*angle(G_hac_m0_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f(2:end), 'Hz'))), 'color', colors(1,:));
plot(f(2:end), 180/pi*angle(G_hac_m1_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f(2:end), 'Hz'))), 'color', colors(2,:));
plot(f(2:end), 180/pi*angle(G_hac_m2_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f(2:end), 'Hz'))), 'color', colors(3,:));
plot(f(2:end), 180/pi*angle(G_hac_m3_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f(2:end), 'Hz'))), 'color', colors(4,:));
xline(5, '--', 'linewidth', 1, 'color', [0,0,0,0.2])
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
@ -3229,6 +3229,7 @@ The obtained closed-loop positioning accuracy was found to comply with the requi
% This is done by offsetfing the micro-hexapod by 0.9um
P_micro_hexapod = [2.5e-6; 0; -0.3e-6]; % [m]
open(mdl);
set_param(mdl, 'StopTime', '3'); % 6 turns at 180deg/s (30rpm)
initializeGround();
@ -3380,6 +3381,7 @@ Yet it was decided that this controller will be tested experimentally, and impro
#+begin_src matlab
%% Simulation of tomography experiments at 1RPM with all payloads
% Configuration
open(mdl);
set_param(mdl, 'StopTime', '2'); % 30 degrees at 1rpm
initializeLoggingConfiguration('log', 'all', 'Ts', 1e-3);
initializeController('type', 'hac-iff');
@ -4343,12 +4345,6 @@ data_dz_100ums.time = Ts*[0:length(data_dz_100ums.Dz_int)-1];
%% Performances for Dz scans - 10 um/s
% Determine when the motion starts and stops
i_dz_10ums = abs(diff(data_dz_10ums.m_hexa_dz)/Ts-10e-6) < 10*eps;
% i_dz_10ums = data_dz_10ums.m_hexa_dz>data_dz_10ums.m_hexa_dz(1) & data_dz_10ums.m_hexa_dz<data_dz_10ums.m_hexa_dz(end);
% Peak to Peak errors
dz_10ums_dy_peak = (max(detrend(data_dz_10ums.e_dy(i_dz_10ums), 0))-min(detrend(data_dz_10ums.e_dy(i_dz_10ums), 0)))/2;
dz_10ums_dz_peak = (max(detrend(data_dz_10ums.e_dz(i_dz_10ums), 0))-min(detrend(data_dz_10ums.e_dz(i_dz_10ums), 0)))/2;
dz_10ums_ry_peak = (max(detrend(data_dz_10ums.e_ry(i_dz_10ums), 0))-min(detrend(data_dz_10ums.e_ry(i_dz_10ums), 0)))/2;
% RMS error
data_dz_10ums.Dy_rms_cl = rms(detrend(data_dz_10ums.e_dy(i_dz_10ums), 0));
@ -4359,12 +4355,6 @@ data_dz_10ums.Ry_rms_cl = rms(detrend(data_dz_10ums.e_ry(i_dz_10ums), 0));
#+begin_src matlab
%% Performances for Dz scans - 100 um/s
i_dz_100ums = abs(diff(data_dz_100ums.m_hexa_dz)/Ts-100e-6) < 10*eps;
% i_dz_100ums = data_dz_100ums.m_hexa_dz>data_dz_100ums.m_hexa_dz(1) & data_dz_100ums.m_hexa_dz<data_dz_100ums.m_hexa_dz(end);
% Peak to Peak errors
dz_100ums_dy_peak = (max(detrend(data_dz_100ums.e_dy(i_dz_100ums), 0))-min(detrend(data_dz_100ums.e_dy(i_dz_100ums), 0)))/2;
dz_100ums_dz_peak = (max(detrend(data_dz_100ums.e_dz(i_dz_100ums), 0))-min(detrend(data_dz_100ums.e_dz(i_dz_100ums), 0)))/2;
dz_100ums_ry_peak = (max(detrend(data_dz_100ums.e_ry(i_dz_100ums), 0))-min(detrend(data_dz_100ums.e_ry(i_dz_100ums), 0)))/2;
% RMS error
data_dz_100ums.Dy_rms_cl = rms(detrend(data_dz_100ums.e_dy(i_dz_100ums), 0));
@ -4376,7 +4366,7 @@ data_dz_100ums.Ry_rms_cl = rms(detrend(data_dz_100ums.e_ry(i_dz_100ums), 0));
%% Dz scan at 10um/s - Lateral error
figure;
hold on;
plot(data_dz_10ums.time, 1e9data_dz_10ums.e_dy, 'DisplayName', sprintf('$\\epsilon D_y: %.0f$ nm RMS', 1e9*rms(data_dz_10ums.e_dy)))
plot(data_dz_10ums.time, 1e9*data_dz_10ums.e_dy, 'DisplayName', sprintf('$\\epsilon D_y: %.0f$ nm RMS', 1e9*rms(data_dz_10ums.e_dy)))
plot([0, 2.2], [specs_dy_peak, specs_dy_peak], '--', 'color', colors(1,:), 'HandleVisibility', 'off');
plot([0, 2.2], [-specs_dy_peak, -specs_dy_peak], '--', 'color', colors(1,:), 'HandleVisibility', 'off');
hold off;
@ -4802,23 +4792,6 @@ i_ty_cl_10ums = data_ty_cl_10ums.Ty > data_ty_cl_10ums.Ty(1) & data_ty_cl_10ums.
i_ty_ol_100ums = data_ty_ol_100ums.Ty > data_ty_ol_100ums.Ty(1) & data_ty_ol_100ums.Ty < data_ty_ol_100ums.Ty(end);
i_ty_cl_100ums = data_ty_cl_100ums.Ty > data_ty_cl_100ums.Ty(1) & data_ty_cl_100ums.Ty < data_ty_cl_100ums.Ty(end);
% Peak to Peak errors
ty_ol_10ums_dy_peak = (max(detrend(data_ty_ol_10ums.e_dy(i_ty_ol_10ums), 0))-min(detrend(data_ty_ol_10ums.e_dy(i_ty_ol_10ums), 0)))/2;
ty_ol_10ums_dz_peak = (max(detrend(data_ty_ol_10ums.e_dz(i_ty_ol_10ums), 0))-min(detrend(data_ty_ol_10ums.e_dz(i_ty_ol_10ums), 0)))/2;
ty_ol_10ums_ry_peak = (max(detrend(data_ty_ol_10ums.e_ry(i_ty_ol_10ums), 0))-min(detrend(data_ty_ol_10ums.e_ry(i_ty_ol_10ums), 0)))/2;
ty_cl_10ums_dy_peak = (max(detrend(data_ty_cl_10ums.e_dy(i_ty_cl_10ums), 0))-min(detrend(data_ty_cl_10ums.e_dy(i_ty_cl_10ums), 0)))/2;
ty_cl_10ums_dz_peak = (max(detrend(data_ty_cl_10ums.e_dz(i_ty_cl_10ums), 0))-min(detrend(data_ty_cl_10ums.e_dz(i_ty_cl_10ums), 0)))/2;
ty_cl_10ums_ry_peak = (max(detrend(data_ty_cl_10ums.e_ry(i_ty_cl_10ums), 0))-min(detrend(data_ty_cl_10ums.e_ry(i_ty_cl_10ums), 0)))/2;
ty_ol_100ums_dy_peak = (max(detrend(data_ty_ol_100ums.e_dy(i_ty_ol_100ums), 0))-min(detrend(data_ty_ol_100ums.e_dy(i_ty_ol_100ums), 0)))/2;
ty_ol_100ums_dz_peak = (max(detrend(data_ty_ol_100ums.e_dz(i_ty_ol_100ums), 0))-min(detrend(data_ty_ol_100ums.e_dz(i_ty_ol_100ums), 0)))/2;
ty_ol_100ums_ry_peak = (max(detrend(data_ty_ol_100ums.e_ry(i_ty_ol_100ums), 0))-min(detrend(data_ty_ol_100ums.e_ry(i_ty_ol_100ums), 0)))/2;
ty_cl_100ums_dy_peak = (max(detrend(data_ty_cl_100ums.e_dy(i_ty_cl_100ums), 0))-min(detrend(data_ty_cl_100ums.e_dy(i_ty_cl_100ums), 0)))/2;
ty_cl_100ums_dz_peak = (max(detrend(data_ty_cl_100ums.e_dz(i_ty_cl_100ums), 0))-min(detrend(data_ty_cl_100ums.e_dz(i_ty_cl_100ums), 0)))/2;
ty_cl_100ums_ry_peak = (max(detrend(data_ty_cl_100ums.e_ry(i_ty_cl_100ums), 0))-min(detrend(data_ty_cl_100ums.e_ry(i_ty_cl_100ums), 0)))/2;
% RMS error
data_ty_ol_10ums.Dy_rms = rms(detrend(data_ty_ol_10ums.e_dy(i_ty_ol_10ums), 0));
data_ty_ol_10ums.Dz_rms = rms(detrend(data_ty_ol_10ums.e_dz(i_ty_ol_10ums), 0));
@ -5038,19 +5011,6 @@ for i = i_dec(2:2:end)
i_dt_1000ums(i-acc_n:i) = 0;
end
% Peak to Peak errors
dt_100ums_dy_peak = (max(detrend(data_dt_100ums.Dy_int(i_dt_100ums)-data_dt_100ums.m_hexa_dy(i_dt_100ums), 0))-min(detrend(data_dt_100ums.Dy_int(i_dt_100ums)-data_dt_100ums.m_hexa_dy(i_dt_100ums), 0)))/2;
dt_100ums_dz_peak = (max(detrend(data_dt_100ums.Dz_int(i_dt_100ums), 0))-min(detrend(data_dt_100ums.Dz_int(i_dt_100ums), 0)))/2;
dt_100ums_ry_peak = (max(detrend(data_dt_100ums.Ry_int(i_dt_100ums), 0))-min(detrend(data_dt_100ums.Ry_int(i_dt_100ums), 0)))/2;
dt_500ums_dy_peak = (max(detrend(data_dt_500ums.Dy_int(i_dt_500ums)-data_dt_500ums.m_hexa_dy(i_dt_500ums), 0))-min(detrend(data_dt_500ums.Dy_int(i_dt_500ums)-data_dt_500ums.m_hexa_dy(i_dt_500ums), 0)))/2;
dt_500ums_dz_peak = (max(detrend(data_dt_500ums.Dz_int(i_dt_500ums), 0))-min(detrend(data_dt_500ums.Dz_int(i_dt_500ums), 0)))/2;
dt_500ums_ry_peak = (max(detrend(data_dt_500ums.Ry_int(i_dt_500ums), 0))-min(detrend(data_dt_500ums.Ry_int(i_dt_500ums), 0)))/2;
dt_1000ums_dy_peak = (max(detrend(data_dt_1000ums.Dy_int(i_dt_1000ums)-data_dt_1000ums.m_hexa_dy(i_dt_1000ums), 0))-min(detrend(data_dt_1000ums.Dy_int(i_dt_1000ums)-data_dt_1000ums.m_hexa_dy(i_dt_1000ums), 0)))/2;
dt_1000ums_dz_peak = (max(detrend(data_dt_1000ums.Dz_int(i_dt_1000ums), 0))-min(detrend(data_dt_1000ums.Dz_int(i_dt_1000ums), 0)))/2;
dt_1000ums_ry_peak = (max(detrend(data_dt_1000ums.Ry_int(i_dt_1000ums), 0))-min(detrend(data_dt_1000ums.Ry_int(i_dt_1000ums), 0)))/2;
% RMS error
data_dt_100ums.Dy_rms_cl = rms(detrend(data_dt_100ums.Dy_int(i_dt_100ums)-data_dt_100ums.m_hexa_dy(i_dt_100ums), 0));
data_dt_100ums.Dz_rms_cl = rms(detrend(data_dt_100ums.Dz_int(i_dt_100ums), 0));
@ -5087,27 +5047,27 @@ The identified limitations, primarily related to high-speed lateral scanning and
#+begin_src matlab
%% Summary of results
1e9*data_tomo_m0_Wz6.Dy_rms_ol, 1e9*data_tomo_m0_Wz6.Dz_rms_ol, 1e6*data_tomo_m0_Wz6.Ry_rms_ol % Tomo - OL - 6deg/s - 0kg
1e9*data_tomo_m0_Wz6.Dy_rms_cl, 1e9*data_tomo_m0_Wz6.Dz_rms_cl, 1e6*data_tomo_m0_Wz6.Ry_rms_cl % Tomo - CL - 6deg/s - 0kg
1e9*data_tomo_m1_Wz6.Dy_rms_ol, 1e9*data_tomo_m1_Wz6.Dz_rms_ol, 1e6*data_tomo_m1_Wz6.Ry_rms_ol % Tomo - OL - 6deg/s - 13kg
1e9*data_tomo_m1_Wz6.Dy_rms_cl, 1e9*data_tomo_m1_Wz6.Dz_rms_cl, 1e6*data_tomo_m1_Wz6.Ry_rms_cl % Tomo - CL - 6deg/s - 13kg
1e9*data_tomo_m2_Wz6.Dy_rms_ol, 1e9*data_tomo_m2_Wz6.Dz_rms_ol, 1e6*data_tomo_m2_Wz6.Ry_rms_ol % Tomo - OL - 6deg/s - 26kg
1e9*data_tomo_m2_Wz6.Dy_rms_cl, 1e9*data_tomo_m2_Wz6.Dz_rms_cl, 1e6*data_tomo_m2_Wz6.Ry_rms_cl % Tomo - CL - 6deg/s - 26kg
1e9*data_tomo_m3_Wz6.Dy_rms_ol, 1e9*data_tomo_m3_Wz6.Dz_rms_ol, 1e6*data_tomo_m3_Wz6.Ry_rms_ol % Tomo - OL - 6deg/s - 39kg
1e9*data_tomo_m3_Wz6.Dy_rms_cl, 1e9*data_tomo_m3_Wz6.Dz_rms_cl, 1e6*data_tomo_m3_Wz6.Ry_rms_cl % Tomo - CL - 6deg/s - 39kg
1e9*data_tomo_m0_Wz180.Dy_rms_ol, 1e9*data_tomo_m0_Wz180.Dz_rms_ol, 1e6*data_tomo_m0_Wz180.Ry_rms_ol % Tomo - OL - 180deg/s - 0kg
1e9*data_tomo_m0_Wz180.Dy_rms_cl, 1e9*data_tomo_m0_Wz180.Dz_rms_cl, 1e6*data_tomo_m0_Wz180.Ry_rms_cl % Tomo - CL - 180deg/s - 0kg
1e9*data_hac_Wz180.Dy_rms_cl, 1e9*data_hac_Wz180.Dz_rms_cl, 1e6*data_hac_Wz180.Ry_rms_cl % Tomo - CL (high performance HAC) - 180deg/s - 0kg
1e9*data_ry.Dy_rms_cl, 1e9*data_ry.Dz_rms_cl, 1e6*data_ry.Ry_rms_cl % Ry 100urad/s
1e9*data_dz_10ums.Dy_rms_cl, 1e9*data_dz_10ums.Dz_rms_cl, 1e6*data_dz_10ums.Ry_rms_cl % Dz 10um/s
1e9*data_dz_100ums.Dy_rms_cl, 1e9*data_dz_100ums.Dz_rms_cl, 1e6*data_dz_100ums.Ry_rms_cl % Dz 100um/s
1e9*data_ty_ol_10ums.Dy_rms, 1e9*data_ty_ol_10ums.Dz_rms, 1e6*data_ty_ol_10ums.Ry_rms % Ty - OL - 10um/s
1e9*data_ty_cl_10ums.Dy_rms, 1e9*data_ty_cl_10ums.Dz_rms, 1e6*data_ty_cl_10ums.Ry_rms % Ty - CL - 10um/s
1e9*data_ty_ol_100ums.Dy_rms, 1e9*data_ty_ol_100ums.Dz_rms, 1e6*data_ty_ol_100ums.Ry_rms % Ty - OL - 100um/s
1e9*data_ty_cl_100ums.Dy_rms, 1e9*data_ty_cl_100ums.Dz_rms, 1e6*data_ty_cl_100ums.Ry_rms % Ty - CL - 100um/s
1e9*data_dt_100ums.Dy_rms_cl, 1e9*data_dt_100ums.Dz_rms_cl, 1e6*data_dt_100ums.Ry_rms_cl % Diffraction Tomo - CL - 6deg/s, 100um/s
1e9*data_dt_500ums.Dy_rms_cl, 1e9*data_dt_500ums.Dz_rms_cl, 1e6*data_dt_500ums.Ry_rms_cl % Diffraction Tomo - CL - 6deg/s, 500um/s
1e9*data_dt_1000ums.Dy_rms_cl, 1e9*data_dt_1000ums.Dz_rms_cl, 1e6*data_dt_1000ums.Ry_rms_cl % Diffraction Tomo - CL - 6deg/s, 1000um/s
% 1e9*data_tomo_m0_Wz6.Dy_rms_ol, 1e9*data_tomo_m0_Wz6.Dz_rms_ol, 1e6*data_tomo_m0_Wz6.Ry_rms_ol; % Tomo - OL - 6deg/s - 0kg
% 1e9*data_tomo_m0_Wz6.Dy_rms_cl, 1e9*data_tomo_m0_Wz6.Dz_rms_cl, 1e6*data_tomo_m0_Wz6.Ry_rms_cl; % Tomo - CL - 6deg/s - 0kg
% 1e9*data_tomo_m1_Wz6.Dy_rms_ol, 1e9*data_tomo_m1_Wz6.Dz_rms_ol, 1e6*data_tomo_m1_Wz6.Ry_rms_ol; % Tomo - OL - 6deg/s - 13kg
% 1e9*data_tomo_m1_Wz6.Dy_rms_cl, 1e9*data_tomo_m1_Wz6.Dz_rms_cl, 1e6*data_tomo_m1_Wz6.Ry_rms_cl; % Tomo - CL - 6deg/s - 13kg
% 1e9*data_tomo_m2_Wz6.Dy_rms_ol, 1e9*data_tomo_m2_Wz6.Dz_rms_ol, 1e6*data_tomo_m2_Wz6.Ry_rms_ol; % Tomo - OL - 6deg/s - 26kg
% 1e9*data_tomo_m2_Wz6.Dy_rms_cl, 1e9*data_tomo_m2_Wz6.Dz_rms_cl, 1e6*data_tomo_m2_Wz6.Ry_rms_cl; % Tomo - CL - 6deg/s - 26kg
% 1e9*data_tomo_m3_Wz6.Dy_rms_ol, 1e9*data_tomo_m3_Wz6.Dz_rms_ol, 1e6*data_tomo_m3_Wz6.Ry_rms_ol; % Tomo - OL - 6deg/s - 39kg
% 1e9*data_tomo_m3_Wz6.Dy_rms_cl, 1e9*data_tomo_m3_Wz6.Dz_rms_cl, 1e6*data_tomo_m3_Wz6.Ry_rms_cl; % Tomo - CL - 6deg/s - 39kg
% 1e9*data_tomo_m0_Wz180.Dy_rms_ol, 1e9*data_tomo_m0_Wz180.Dz_rms_ol, 1e6*data_tomo_m0_Wz180.Ry_rms_ol; % Tomo - OL - 180deg/s - 0kg
% 1e9*data_tomo_m0_Wz180.Dy_rms_cl, 1e9*data_tomo_m0_Wz180.Dz_rms_cl, 1e6*data_tomo_m0_Wz180.Ry_rms_cl; % Tomo - CL - 180deg/s - 0kg
% 1e9*data_hac_Wz180.Dy_rms_cl, 1e9*data_hac_Wz180.Dz_rms_cl, 1e6*data_hac_Wz180.Ry_rms_cl; % Tomo - CL (high performance HAC) - 180deg/s - 0kg
% 1e9*data_ry.Dy_rms_cl, 1e9*data_ry.Dz_rms_cl, 1e6*data_ry.Ry_rms_cl; % Ry 100urad/s
% 1e9*data_dz_10ums.Dy_rms_cl, 1e9*data_dz_10ums.Dz_rms_cl, 1e6*data_dz_10ums.Ry_rms_cl; % Dz 10um/s
% 1e9*data_dz_100ums.Dy_rms_cl, 1e9*data_dz_100ums.Dz_rms_cl, 1e6*data_dz_100ums.Ry_rms_cl; % Dz 100um/s
% 1e9*data_ty_ol_10ums.Dy_rms, 1e9*data_ty_ol_10ums.Dz_rms, 1e6*data_ty_ol_10ums.Ry_rms; % Ty - OL - 10um/s
% 1e9*data_ty_cl_10ums.Dy_rms, 1e9*data_ty_cl_10ums.Dz_rms, 1e6*data_ty_cl_10ums.Ry_rms; % Ty - CL - 10um/s
% 1e9*data_ty_ol_100ums.Dy_rms, 1e9*data_ty_ol_100ums.Dz_rms, 1e6*data_ty_ol_100ums.Ry_rms; % Ty - OL - 100um/s
% 1e9*data_ty_cl_100ums.Dy_rms, 1e9*data_ty_cl_100ums.Dz_rms, 1e6*data_ty_cl_100ums.Ry_rms; % Ty - CL - 100um/s
% 1e9*data_dt_100ums.Dy_rms_cl, 1e9*data_dt_100ums.Dz_rms_cl, 1e6*data_dt_100ums.Ry_rms_cl; % Diffraction Tomo - CL - 6deg/s, 100um/s
% 1e9*data_dt_500ums.Dy_rms_cl, 1e9*data_dt_500ums.Dz_rms_cl, 1e6*data_dt_500ums.Ry_rms_cl; % Diffraction Tomo - CL - 6deg/s, 500um/s
% 1e9*data_dt_1000ums.Dy_rms_cl, 1e9*data_dt_1000ums.Dz_rms_cl, 1e6*data_dt_1000ums.Ry_rms_cl; % Diffraction Tomo - CL - 6deg/s, 1000um/s
#+end_src
#+name: tab:test_id31_experiments_results_summary
@ -5190,7 +5150,7 @@ addpath('./STEPS/'); % Path for STEPS
addpath('./subsystems/'); % Path for Subsystems Simulink files
%% Data directory
data_dir = './mat/'
data_dir = './mat/';
#+END_SRC
** Initialize Simscape Model