Add Complementary Filter Control

figs/inkscape/convert_svg.sh

@@ -0,0 +1,18 @@
+#!/bin/bash
+
+# Directory containing SVG files
+INPUT_DIR="."
+
+# Loop through all SVG files in the directory
+for svg_file in "$INPUT_DIR"/*.svg; do
+    # Check if there are SVG files in the directory
+    if [ -f "$svg_file" ]; then
+        # Output PDF file name
+        pdf_file="../${svg_file%.svg}.pdf"
+        png_file="../${svg_file%.svg}"
+        
+        # Convert SVG to PDF using Inkscape
+        inkscape "$svg_file" --export-filename="$pdf_file" && \
+            pdftocairo -png -singlefile -cropbox "$pdf_file" "$png_file"
+    fi
+done

matlab/src/h5scan.m

@@ -1,102 +1,61 @@
-function [cntrs,tp] = h5scan(pth,smp,ds,sn,varargin)
+% function [cntrs,tp] = h5scan(pth,smp,ds,sn,varargin)
+function [cntrs,tp] = h5scan(ds,sn)
+    if ~isstr(ds), ds = sprintf('%.4d',ds); end;
 
-i = cellfun(@(x) isa(x,'detector'),varargin);
-if any(i), det = varargin{i}; varargin = varargin(~i); else, det = []; end;
-if ~isstr(ds), ds = sprintf('%.4d',ds); end;
+    f = sprintf('%s.h5',ds);
+    h = h5info(f,sprintf('/%d.1/measurement',sn));
+    fid = H5F.open(f);
+    for i = 1:length(h.Links)
+        nm = h.Links(i).Name;
+        try
+            id = H5D.open(fid,h.Links(i).Value{1});
+            cntrs.(nm) = H5D.read(id);
+            H5D.close(id);
+            if ~isempty(det) & strcmp(nm,det.name), cntrs.(nm) = integrate(det,double(cntrs.(nm))); end;
+        end
+        [~,tp.(nm)] = fileparts(h.Links(i).Value{1});
+    end
+    try
+        h = h5info(f,sprintf('/%d.2/measurement',sn));
+    catch
+        h = [];
+    end
+    if ~isempty(h)
+        for i = 1:length(h.Links)
+            nm = h.Links(i).Name;
+            try
+                id = H5D.open(fid,h.Links(i).Value{1});
+                cntrs.part2.(nm) = H5D.read(id);
+                H5D.close(id);
+            end
+            [~,tp.part2.(nm)] = fileparts(h.Links(i).Value{1});
+        end
+    end
 
-f = sprintf('%s/%s/%s_%s/%s_%s.h5',pth,smp,smp,ds,smp,ds);
-h = h5info(f,sprintf('/%d.1/measurement',sn));
-fid = H5F.open(f);
-for i = 1:length(h.Links),
-  nm = h.Links(i).Name;
-  try,
-    id = H5D.open(fid,h.Links(i).Value{1});
-    cntrs.(nm) = H5D.read(id);
-    H5D.close(id);
-    if ~isempty(det) & strcmp(nm,det.name), cntrs.(nm) = integrate(det,double(cntrs.(nm))); end;
-  catch,
-    warning('solving problem with %s\n',nm);
-    cntrs.(nm) = vrtlds(sprintf('%s/%s/%s_%s/scan%.4d/',pth,smp,smp,ds,sn),nm,det);
-  end;
-  [~,tp.(nm)] = fileparts(h.Links(i).Value{1});
-end;
-try,
-  h = h5info(f,sprintf('/%d.2/measurement',sn));
-catch,
-  h = [];
-end;
-if ~isempty(h),
-  for i = 1:length(h.Links),
-    nm = h.Links(i).Name;
-    try,
-      id = H5D.open(fid,h.Links(i).Value{1});
-      cntrs.part2.(nm) = H5D.read(id);
-      H5D.close(id);
-    catch,
-      warning('solving problem with %s\n',nm);
-      cntrs.part2.(nm) = vrtlds(sprintf('%s/%s/%s_%s/scan%.4d/',pth,smp,smp,ds,sn),nm,det);
-    end;
-    [~,tp.part2.(nm)] = fileparts(h.Links(i).Value{1});
-  end;
-end;
-if length(varargin),
-  fn = sprintf('/%d.1/instrument/positioners/',sn);
-  h = h5info(f,fn);
-  [~,k,m] = intersect({h.Datasets.Name},varargin,'stable');
-  h.Datasets = h.Datasets(k);
-  for i = 1:length(h.Datasets),
-    id = H5D.open(fid,[fn h.Datasets(i).Name]);
-    cntrs.(h.Datasets(i).Name) = H5D.read(id);
-    H5D.close(id);
-  end;
-end;
+    H5F.close(fid);
 
-H5F.close(fid);
+    %%%%%%%%%%%%%%%%%%%%%%%%%%
 
-%%%%%%%%%%%%%%%%%%%%%%%%%%
+    function A = vrtlds(f,nm,det)
+        n = 0; A = [];
+        fn = sprintf('%s/%s_%.4d.h5',f,nm,n);
 
-function A = vrtlds(f,nm,det)
-%try,
-  n = 0; A = [];
-  fn = sprintf('%s/%s_%.4d.h5',f,nm,n);
-  while exist(fn) == 2,
-    fid = H5F.open(fn); n = n+1;
-    id = H5D.open(fid,sprintf('/entry_0000/ESRF-ID31/%s/data',nm));
-    if 2 < nargin & strcmp(nm,'p3') & ~isempty(det),
-      fprintf('integrating %s\n',fn);
-      if isempty(A),
-        A = integrate(det,double(H5D.read(id)),1);
-      else,
-        tmp = integrate(det,double(H5D.read(id)),1); A.y = cat(2,A.y,tmp.y); A.y0 = cat(2,A.y0,tmp.y0);
-      end;
-    else,
-      fprintf('loading %s\n',fn);
-      A = cat(3,A,H5D.read(id));
-    end;
-    H5D.close(id); H5F.close(fid);
-    fn = sprintf('%s/%s_%.4d.h5',f,nm,n);
-  end;
-%catch,
-%  A = [];
-%end;
-
-% fid = H5F.open...
-% id = H5D.open...
-% sid = H5D.get_space(id);
-% [ndims,h5_dims]=H5S.get_simple_extent_dims(sid)
-
-% Read a 2x3 hyperslab of data from a dataset, starting in the 4th row and 5th column of the example dataset.
-% Create a property list identifier, then open the HDF5 file and the dataset /g1/g1.1/dset1.1.1.
-
-% fid = H5F.open('example.h5');
-% id = H5D.open(fid,'/g1/g1.1/dset1.1.1');
-
-% dims = ([500 1679 1475];
-% msid = H5S.create_simple(3,dims,[]);
-% sid = H5D.get_space(id);
-% offset = [n*500 0 0];
-% block = dims; % d1: 500 or min(d1tot-n*500,500)
-% H5S.select_hyperslab(sid,'H5S_SELECT_SET',offset,[],[],block);
-% data = H5D.read(id,'H5ML_DEFAULT',msid,sid,'H5P_DEFAULT');
-% H5D.close(id);
-% H5F.close(fid);
+        while exist(fn) == 2
+            fid = H5F.open(fn); n = n+1;
+            id = H5D.open(fid,sprintf('/entry_0000/ESRF-ID31/%s/data',nm));
+            if 2 < nargin & strcmp(nm,'p3') & ~isempty(det)
+                fprintf('integrating %s\n',fn);
+                if isempty(A)
+                    A = integrate(det,double(H5D.read(id)),1);
+                else
+                    tmp = integrate(det,double(H5D.read(id)),1); A.y = cat(2,A.y,tmp.y); A.y0 = cat(2,A.y0,tmp.y0);
+                end
+            else
+                fprintf('loading %s\n',fn);
+                A = cat(3,A,H5D.read(id));
+            end
+            H5D.close(id); H5F.close(fid);
+            fn = sprintf('%s/%s_%.4d.h5',f,nm,n);
+        end
+    end
+end

matlab/test_id31_1_metrology.m

@@ -46,9 +46,9 @@ Hm = [ 0  1  0 -l2  0;
 
 %% 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);
+data_it0 = h5scan('alignment_h1rx_h1ry', 1);
+data_it1 = h5scan('alignment_h1rx_h1ry_0002', 3);
+data_it2 = h5scan('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);
@@ -81,8 +81,8 @@ 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);
+data_it0 = h5scan('alignment_h1rx_h1ry_0002', 5);
+data_it1 = h5scan('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);
@@ -110,7 +110,7 @@ ylim([-8, 14]);
 % This is estimated by moving the spheres using the micro-hexapod
 
 % Dx
-data_dx = h5scan(data_dir, 'metrology_acceptance_new_align', 'dx', 1);
+data_dx = h5scan('metrology_acceptance_new_align_dx', 1);
 
 dx_acceptance = zeros(5,1);
 
@@ -126,7 +126,7 @@ for i = [1:size(dx_acceptance, 1)]
 end
 
 % Dy
-data_dy = h5scan(data_dir, 'metrology_acceptance_new_align', 'dy', 1);
+data_dy = h5scan('metrology_acceptance_new_align_dy', 1);
 
 dy_acceptance = zeros(5,1);
 
@@ -142,7 +142,7 @@ for i = [1:size(dy_acceptance, 1)]
 end
 
 % Dz
-data_dz = h5scan(data_dir, 'metrology_acceptance_new_align', 'dz', 1);
+data_dz = h5scan('metrology_acceptance_new_align_dz', 1);
 
 dz_acceptance = zeros(5,1);
 
@@ -186,7 +186,7 @@ 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);
+data = h5scan('metrology_acceptance_after_int_align_meshXY', 1);
 
 x = 1e3*detrend(data.h1tx, 0); % [um]
 y = 1e3*detrend(data.h1ty, 0); % [um]

matlab/test_id31_2_open_loop_plant.m

@@ -198,10 +198,10 @@ linkaxes([ax1,ax2],'x');
 xlim([1, 1e3]);
 
 %% 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);
+data_1_dx = h5scan('align_int_enc_Rz_tx_first_scan', 2);
+data_1_dy = h5scan('align_int_enc_Rz_tx_first_scan', 3);
+data_2_dx = h5scan('align_int_enc_Rz_verif-after-correct-offset', 1);
+data_2_dy = h5scan('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);

matlab/test_id31_4_hac.m

@@ -230,10 +230,10 @@ 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');
+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');
 xline(5, '--', 'linewidth', 1, 'color', [0,0,0,0.2], 'HandleVisibility', 'off')
 hold off;
 set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
@@ -244,10 +244,10 @@ 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,:));
+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,:));
 xline(5, '--', 'linewidth', 1, 'color', [0,0,0,0.2])
 hold off;
 set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');

matlab/test_id31_5_experiments.m

@@ -934,6 +934,129 @@ data_dt_1000ums.Dy_rms_cl = rms(detrend(data_dt_1000ums.Dy_int(i_dt_1000ums)-dat
 data_dt_1000ums.Dz_rms_cl = rms(detrend(data_dt_1000ums.Dz_int(i_dt_1000ums), 0));
 data_dt_1000ums.Ry_rms_cl = rms(detrend(data_dt_1000ums.Ry_int(i_dt_1000ums), 0));
 
+%% Complementary Filter Control - Experimental Validation
+% Parameters to analyze results in the frequency domain
+Ts = 1e-4; % Sampling Time [s]
+Nfft = floor(2.0/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+
+%% Closed-Loop performance identification (S and T transfer functions)
+% Different parameters w0 in different directions
+data = load("2023-08-21_14-09_m0_cf_inv_tustin.mat");
+
+[S_dy, f] = tfestimate(data.Dy.id_cl, data.Dy.e_dy,   win, Noverlap, Nfft, 1/Ts);
+[T_dy, ~] = tfestimate(data.Dy.id_cl, data.Dy.Dy_int, win, Noverlap, Nfft, 1/Ts);
+
+[S_dz, ~] = tfestimate(data.Dz.id_cl, data.Dz.e_dz,   win, Noverlap, Nfft, 1/Ts);
+[T_dz, ~] = tfestimate(data.Dz.id_cl, data.Dz.Dz_int, win, Noverlap, Nfft, 1/Ts);
+
+[S_ry, ~] = tfestimate(data.Ry.id_cl, data.Ry.e_ry,   win, Noverlap, Nfft, 1/Ts);
+[T_ry, ~] = tfestimate(data.Ry.id_cl, data.Ry.Ry_int, win, Noverlap, Nfft, 1/Ts);
+
+%% Obtained Closed-Loop transfer functions - Different Complementary filters for Dy and Dz
+figure;
+hold on;
+plot(f, abs(S_dy), '-', 'color', colors(1,:),'DisplayName', '$S_{D_y}$');
+plot(f, abs(S_dz), '-', 'color', colors(2,:),'DisplayName', '$S_{D_z}$');
+plot(f, abs(S_ry), '-', 'color', colors(3,:),'DisplayName', '$S_{R_y}$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude');
+ylim([1e-3, 5]);
+xlim([1, 1e3]);
+leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 18;
+
+%% Effect of a change of complementary filter parameter alpha
+% w0 = 10, alpha = 2
+data_a2 = load("2023-08-21_17-56_m2_cf_10Hz_a2.mat");
+
+[S_a2, ~] = tfestimate(data_a2.Dy.id_cl, data_a2.Dy.e_dy,   win, Noverlap, Nfft, 1/Ts);
+[T_a2, ~] = tfestimate(data_a2.Dy.id_cl, data_a2.Dy.Dy_int, win, Noverlap, Nfft, 1/Ts);
+
+% Analytical formulas
+w0 = 2*pi*10;
+alpha = 2;
+Hh_a2 = (s/w0)^2*((s/w0)+1+alpha)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
+Hl_a2 = ((1+alpha)*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
+
+% w0 = 10, alpha = 0.5
+data_a05 = load("2023-08-21_17-58_m2_cf_10Hz_a05.mat");
+
+[S_a05, ~] = tfestimate(data_a05.Dz.id_cl, data_a05.Dz.e_dz,   win, Noverlap, Nfft, 1/Ts);
+[T_a05, ~] = tfestimate(data_a05.Dz.id_cl, data_a05.Dz.Dz_int, win, Noverlap, Nfft, 1/Ts);
+
+% Analytical formulas
+w0 = 2*pi*10;
+alpha = 0.5;
+Hh_a05 = (s/w0)^2*((s/w0)+1+alpha)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
+Hl_a05 = ((1+alpha)*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
+
+%% Obtained Closed-Loop transfer functions - Effect of changing alpha
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(Hh_a2, freqs, 'Hz'))), '--', 'color', colors(1,:),'DisplayName', '$\alpha = 2$, $H_H$, $H_L$');
+plot(f,     abs(S_a2), '-',  'color', [colors(1,:), 0.5], 'linewidth', 2.5,'DisplayName', '$\alpha = 2$, $S$, $T$');
+plot(freqs, abs(squeeze(freqresp(Hh_a05, freqs, 'Hz'))), '--', 'color', colors(2,:),'DisplayName', '$\alpha = 0.5$, $H_H$, $H_L$');
+plot(f,     abs(S_a05), '-',  'color', [colors(2,:), 0.5], 'linewidth', 2.5,'DisplayName', '$\alpha = 0.5$, $S$, $T$');
+plot(freqs, abs(squeeze(freqresp(Hl_a2, freqs, 'Hz'))), '--', 'color', colors(1,:), 'HandleVisibility', 'off');
+plot(f,     abs(T_a2), '-',  'color', [colors(1,:), 0.5], 'linewidth', 2.5, 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(Hl_a05, freqs, 'Hz'))), '--', 'color', colors(2,:), 'HandleVisibility', 'off');
+plot(f,     abs(T_a05), '-',  'color', [colors(2,:), 0.5], 'linewidth', 2.5, 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude');
+xlim([1, 1e3]);
+ylim([1e-3, 5]);
+leg = legend('location', 'south', 'FontSize', 8, 'NumColumns', 2);
+leg.ItemTokenSize(1) = 18;
+
+%% Trying high bandwidth controllers - 60Hz
+% Z direction
+data = load("2023-08-21_14-19_m0_cf_inv_tustin_Dz_60Hz_a_20.mat");
+[S_dz, ~] = tfestimate(data.Dz.id_cl, data.Dz.e_dz,   win, Noverlap, Nfft, 1/Ts);
+[T_dz, ~] = tfestimate(data.Dz.id_cl, data.Dz.Dz_int, win, Noverlap, Nfft, 1/Ts);
+
+% Y direction
+data = load("2023-08-21_14-20_m0_cf_inv_tustin_Dy_60Hz_a_20.mat");
+[S_dy, ~] = tfestimate(data.Dy.id_cl, data.Dy.e_dy,   win, Noverlap, Nfft, 1/Ts);
+[T_dy, ~] = tfestimate(data.Dy.id_cl, data.Dy.Dy_int, win, Noverlap, Nfft, 1/Ts);
+
+% Analytical formulas
+w0 = 2*pi*60;
+alpha = 2;
+Hh_60Hz = (s/w0)^2*((s/w0)+1+alpha)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
+Hl_60Hz = ((1+alpha)*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
+
+%% Bode plot of the Weighting filters and Obtained complementary filters
+figure;
+hold on;
+plot(f, abs(S_dy), '-', 'color', colors(1,:), 'DisplayName', '$S$, $D_y$');
+plot(f, abs(S_dz), '-', 'color', colors(2,:), 'DisplayName', '$S$, $D_z$');
+plot(freqs, abs(squeeze(freqresp(Hh_60Hz, freqs, 'Hz'))), 'k--','DisplayName', '$H_H$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude');
+xlim([1, 1e3]);
+ylim([3e-4, 5]);
+leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 18;
+
+%% Bode plot of the Weighting filters and Obtained complementary filters
+figure;
+hold on;
+plot(f, abs(T_dy), '-', 'color', colors(1,:), 'DisplayName', '$T$, $D_y$');
+plot(f, abs(T_dz), '-', 'color', colors(2,:), 'DisplayName', '$T$, $D_z$');
+plot(freqs, abs(squeeze(freqresp(Hl_60Hz, freqs, 'Hz'))), 'k--','DisplayName', '$H_L$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude');
+xlim([1, 1e3]);
+ylim([3e-4, 5]);
+leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 18;
+
 %% 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

test-bench-id31.org

@@ -685,9 +685,9 @@ The remaining errors after alignment are in the order of $\pm5\,\mu\text{rad}$ i
 #+begin_src matlab
 %% 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);
+data_it0 = h5scan('alignment_h1rx_h1ry', 1);
+data_it1 = h5scan('alignment_h1rx_h1ry_0002', 3);
+data_it2 = h5scan('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);
@@ -728,8 +728,8 @@ exportFig('figs/test_id31_metrology_align_rx_ry.pdf', 'width', 'half', 'height',
 #+begin_src matlab
 %% Eccentricity alignment
 % Load Data
-data_it0 = h5scan(data_dir, 'alignment', 'h1rx_h1ry_0002', 5);
-data_it1 = h5scan(data_dir, 'alignment', 'h1dx_h1dy', 1);
+data_it0 = h5scan('alignment_h1rx_h1ry_0002', 5);
+data_it1 = h5scan('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);
@@ -793,7 +793,7 @@ The obtained lateral acceptance for pure displacements in any direction is estim
 % This is estimated by moving the spheres using the micro-hexapod
 
 % Dx
-data_dx = h5scan(data_dir, 'metrology_acceptance_new_align', 'dx', 1);
+data_dx = h5scan('metrology_acceptance_new_align_dx', 1);
 
 dx_acceptance = zeros(5,1);
 
@@ -809,7 +809,7 @@ for i = [1:size(dx_acceptance, 1)]
 end
 
 % Dy
-data_dy = h5scan(data_dir, 'metrology_acceptance_new_align', 'dy', 1);
+data_dy = h5scan('metrology_acceptance_new_align_dy', 1);
 
 dy_acceptance = zeros(5,1);
 
@@ -825,7 +825,7 @@ for i = [1:size(dy_acceptance, 1)]
 end
 
 % Dz
-data_dz = h5scan(data_dir, 'metrology_acceptance_new_align', 'dz', 1);
+data_dz = h5scan('metrology_acceptance_new_align_dz', 1);
 
 dz_acceptance = zeros(5,1);
 
@@ -920,7 +920,7 @@ exportFig('figs/test_id31_interf_noise.pdf', 'width', 'half', 'height', 'normal'
 
 #+begin_src matlab
 %% X-Y scan with the micro-hexapod, and record of the vertical interferometer
-data = h5scan(data_dir, 'metrology_acceptance', 'after_int_align_meshXY', 1);
+data = h5scan('metrology_acceptance_after_int_align_meshXY', 1);
 
 x = 1e3*detrend(data.h1tx, 0); % [um]
 y = 1e3*detrend(data.h1ty, 0); % [um]
@@ -1305,10 +1305,10 @@ Results shown in Figure ref:fig:test_id31_Rz_align_correct are indeed indicating
 
 #+begin_src matlab
 %% 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);
+data_1_dx = h5scan('align_int_enc_Rz_tx_first_scan', 2);
+data_1_dy = h5scan('align_int_enc_Rz_tx_first_scan', 3);
+data_2_dx = h5scan('align_int_enc_Rz_verif-after-correct-offset', 1);
+data_2_dy = h5scan('align_int_enc_Rz_verif-after-correct-offset', 2);
 #+end_src
 
 #+begin_src matlab
@@ -3104,10 +3104,10 @@ 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');
+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');
 xline(5, '--', 'linewidth', 1, 'color', [0,0,0,0.2], 'HandleVisibility', 'off')
 hold off;
 set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
@@ -3118,10 +3118,10 @@ 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,:));
+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,:));
 xline(5, '--', 'linewidth', 1, 'color', [0,0,0,0.2])
 hold off;
 set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
@@ -3529,6 +3529,7 @@ Several scientific experiments were replicated, such as:
 - Diffraction Tomography:continuous $R_z$ rotation using the Spindle and lateral $D_y$ scans performed at the same time using the translation stage (Section ref:ssec:test_id31_scans_diffraction_tomo)
 
 Unless explicitly stated, all closed-loop experiments were performed using the robust (i.e. conservative) high authority controller designed in Section ref:ssec:test_id31_iff_hac_controller.
+Higher performance controllers using complementary filters are investigated in Section ref:ssec:test_id31_cf_control.
 
 For each experiment, the obtained performances are compared to the specifications for the most demanding case in which nano-focusing optics are used to focus the beam down to $200\,nm\times 100\,nm$.
 In this case, the goal is to keep the sample's point of interest in the beam, and therefore the $D_y$ and $D_z$ positioning errors should be less than $200\,nm$ and $100\,nm$ peak-to-peak, respectively.
@@ -3942,7 +3943,7 @@ data_tomo_m0_Wz180.Ry_rms_ol = rms(data_tomo_m0_Wz180.Ry_int(1:i_m0) - (y0 + R*s
 
 A comparative analysis was conducted using three tomography scans at $180\,\text{deg/s}$ to evaluate the effectiveness of the HAC-LAC strategy in reducing positioning errors.
 The scans were performed under three conditions: open-loop, with decentralized IFF control, and with the complete HAC-LAC strategy.
-For these specific measurements, an enhanced high authority controller was optimized for low payload masses to meet the performance requirements.
+For this specific measurement, an enhanced high authority controller (discussed in Section ref:ssec:test_id31_cf_control) was optimized for low payload masses to meet the performance requirements.
 
 Figure ref:fig:test_id31_hac_cas_cl presents the cumulative amplitude spectra of the position errors for all three cases.
 The results reveal two distinct control contributions: the decentralized IFF effectively attenuates vibrations near the nano-hexapod suspension modes (an achievement not possible with HAC alone), while the high authority controller suppresses low-frequency vibrations primarily arising from Spindle guiding errors.
@@ -5041,6 +5042,236 @@ data_dt_1000ums.Dz_rms_cl = rms(detrend(data_dt_1000ums.Dz_int(i_dt_1000ums), 0)
 data_dt_1000ums.Ry_rms_cl = rms(detrend(data_dt_1000ums.Ry_int(i_dt_1000ums), 0));
 #+end_src
 
+** Feedback control using Complementary Filters
+<<ssec:test_id31_cf_control>>
+
+# TODO - Add link to section
+A control architecture utilizing complementary filters to shape the closed-loop transfer functions was proposed during the detail design phase.
+Experimental validation of this architecture using the NASS is presented herein.
+
+Given that performance requirements are specified in the Cartesian frame, decoupling of the plant within this frame was achieved using Jacobian matrices.
+Consequently, the control space comprises the directions $D_x$, $D_y$, $D_z$, $R_x$, and $R_y$.
+Control performance in each of these directions can be tuned independently.
+A schematic of the proposed control architecture is illustrated in Figure\nbsp{}ref:fig:test_id31_cf_control.
+
+#+name: fig:test_id31_cf_control
+#+caption: Control architecture in the Cartesian frame. Only the controller corresponding to the $D_z$ direction is shown. $H_L$ and $H_H$ are complementary filters.
+[[file:figs/test_id31_cf_control.png]]
+
+
+# TODO - Add link to 2DoF model
+Implementation of this control architecture necessitates a plant model, which must subsequently be inverted.
+This plant model was derived from the multi-body model incorporating the previously detailed 2-DoF APA model, such that the model order stays relatively low.
+Proposed analytical formulas for complementary filters having $40\,\text{dB/dec}$ were used during this experimental validation.
+# TODO - Add link to the analytical formulas
+
+An initial experimental validation was conducted under no-payload conditions, with control applied solely to the $D_y$, $D_z$, and $R_y$ directions.
+Increased control bandwidth was achieved for the $D_z$ and $R_y$ directions through appropriate tuning of the parameter $\omega_0$.
+The experimentally measured closed-loop sensitivity transfer functions corresponding to these three controlled directions are presented in Figure ref:fig:test_id31_cf_control_dy_dz_diff.
+
+#+begin_src matlab
+%% Complementary Filter Control - Experimental Validation
+% Parameters to analyze results in the frequency domain
+Ts = 1e-4; % Sampling Time [s]
+Nfft = floor(2.0/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+
+%% Closed-Loop performance identification (S and T transfer functions)
+% Different parameters w0 in different directions
+data = load("2023-08-21_14-09_m0_cf_inv_tustin.mat");
+
+[S_dy, f] = tfestimate(data.Dy.id_cl, data.Dy.e_dy,   win, Noverlap, Nfft, 1/Ts);
+[T_dy, ~] = tfestimate(data.Dy.id_cl, data.Dy.Dy_int, win, Noverlap, Nfft, 1/Ts);
+
+[S_dz, ~] = tfestimate(data.Dz.id_cl, data.Dz.e_dz,   win, Noverlap, Nfft, 1/Ts);
+[T_dz, ~] = tfestimate(data.Dz.id_cl, data.Dz.Dz_int, win, Noverlap, Nfft, 1/Ts);
+
+[S_ry, ~] = tfestimate(data.Ry.id_cl, data.Ry.e_ry,   win, Noverlap, Nfft, 1/Ts);
+[T_ry, ~] = tfestimate(data.Ry.id_cl, data.Ry.Ry_int, win, Noverlap, Nfft, 1/Ts);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Obtained Closed-Loop transfer functions - Different Complementary filters for Dy and Dz
+figure;
+hold on;
+plot(f, abs(S_dy), '-', 'color', colors(1,:),'DisplayName', '$S_{D_y}$');
+plot(f, abs(S_dz), '-', 'color', colors(2,:),'DisplayName', '$S_{D_z}$');
+plot(f, abs(S_ry), '-', 'color', colors(3,:),'DisplayName', '$S_{R_y}$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude');
+ylim([1e-3, 5]);
+xlim([1, 1e3]);
+leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 18;
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/test_id31_cf_control_dy_dz_diff.pdf', 'width', 'half', 'height', 'normal');
+#+end_src
+
+Another test was conducted with a $26\,\text{kg}$ payload.
+For this configuration, complementary filters were implemented with $\omega_0 = 2\pi \cdot 10\,\text{rad/s}$, and parameter $\alpha$ was varied.
+The resulting experimentally obtained closed-loop transfer functions are compared against the theoretical complementary filter responses in Figure\nbsp{}ref:fig:test_id31_cf_control_alpha.
+As illustrated in the figure, a close correspondence between the measured closed-loop responses and the target complementary filter magnitude was observed.
+It also shows that the parameter $\alpha$ provides a mechanism for managing the trade-off between low-frequency disturbance rejection performance and the potential amplification of disturbances within the crossover frequency region.
+
+#+begin_src matlab
+%% Effect of a change of complementary filter parameter alpha
+% w0 = 10, alpha = 2
+data_a2 = load("2023-08-21_17-56_m2_cf_10Hz_a2.mat");
+
+[S_a2, ~] = tfestimate(data_a2.Dy.id_cl, data_a2.Dy.e_dy,   win, Noverlap, Nfft, 1/Ts);
+[T_a2, ~] = tfestimate(data_a2.Dy.id_cl, data_a2.Dy.Dy_int, win, Noverlap, Nfft, 1/Ts);
+
+% Analytical formulas
+w0 = 2*pi*10;
+alpha = 2;
+Hh_a2 = (s/w0)^2*((s/w0)+1+alpha)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
+Hl_a2 = ((1+alpha)*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
+
+% w0 = 10, alpha = 0.5
+data_a05 = load("2023-08-21_17-58_m2_cf_10Hz_a05.mat");
+
+[S_a05, ~] = tfestimate(data_a05.Dz.id_cl, data_a05.Dz.e_dz,   win, Noverlap, Nfft, 1/Ts);
+[T_a05, ~] = tfestimate(data_a05.Dz.id_cl, data_a05.Dz.Dz_int, win, Noverlap, Nfft, 1/Ts);
+
+% Analytical formulas
+w0 = 2*pi*10;
+alpha = 0.5;
+Hh_a05 = (s/w0)^2*((s/w0)+1+alpha)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
+Hl_a05 = ((1+alpha)*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Obtained Closed-Loop transfer functions - Effect of changing alpha
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(Hh_a2, freqs, 'Hz'))), '--', 'color', colors(1,:),'DisplayName', '$\alpha = 2$, $H_H$, $H_L$');
+plot(f,     abs(S_a2), '-',  'color', [colors(1,:), 0.5], 'linewidth', 2.5,'DisplayName', '$\alpha = 2$, $S$, $T$');
+plot(freqs, abs(squeeze(freqresp(Hh_a05, freqs, 'Hz'))), '--', 'color', colors(2,:),'DisplayName', '$\alpha = 0.5$, $H_H$, $H_L$');
+plot(f,     abs(S_a05), '-',  'color', [colors(2,:), 0.5], 'linewidth', 2.5,'DisplayName', '$\alpha = 0.5$, $S$, $T$');
+plot(freqs, abs(squeeze(freqresp(Hl_a2, freqs, 'Hz'))), '--', 'color', colors(1,:), 'HandleVisibility', 'off');
+plot(f,     abs(T_a2), '-',  'color', [colors(1,:), 0.5], 'linewidth', 2.5, 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(Hl_a05, freqs, 'Hz'))), '--', 'color', colors(2,:), 'HandleVisibility', 'off');
+plot(f,     abs(T_a05), '-',  'color', [colors(2,:), 0.5], 'linewidth', 2.5, 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude');
+xlim([1, 1e3]);
+ylim([1e-3, 5]);
+leg = legend('location', 'south', 'FontSize', 8, 'NumColumns', 2);
+leg.ItemTokenSize(1) = 18;
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/test_id31_cf_control_alpha.pdf', 'width', 'half', 'height', 'normal');
+#+end_src
+
+#+name: fig:test_id31_cf_control_results
+#+caption: Measured closed-loop transfer functions. Different bandwidth can be specified for different directions using $\omega_0$ (\subref{fig:test_id31_cf_control_dy_dz_diff}). The shape can be adjusted using parameter $\alpha$ (\subref{fig:test_id31_cf_control_alpha}).
+#+attr_latex: :options [htbp]
+#+begin_figure
+#+attr_latex: :caption \subcaption{\label{fig:test_id31_cf_control_dy_dz_diff}Chose of bandwidth using $\omega_0$, $m = 0\,\text{kg}$}
+#+attr_latex: :options {0.49\textwidth}
+#+begin_subfigure
+#+attr_latex: :scale 0.9
+[[file:figs/test_id31_cf_control_dy_dz_diff.png]]
+#+end_subfigure
+#+attr_latex: :caption \subcaption{\label{fig:test_id31_cf_control_alpha}Effect of a change of $\alpha$, $m = 26\,\text{kg}$}
+#+attr_latex: :options {0.49\textwidth}
+#+begin_subfigure
+#+attr_latex: :scale 0.9
+[[file:figs/test_id31_cf_control_alpha.png]]
+#+end_subfigure
+#+end_figure
+
+Finally, $\omega_0$ was gradually increased to estimate the maximum bandwidth (i.e. the best low frequency disturbance rejection) that can be achieved with this architecture.
+No payload was used for this test, and the parameter $\omega_0$ was increased for the controllers in the $D_y$ and $D_z$ directions.
+A value $\omega_0 = 2\pi \cdot 60 \,\text{rad/s}$ could be achieved.
+Measured closed-loop transfer functions are shown in Figure ref:fig:test_id31_high_bandwidth, indicating a reduction of disturbances in the considered direction of $1000$ at $1\,\text{Hz}$.
+For higher values of $\omega_0$, the system became unstable in the vertical direction, probably because of the resonance at $250\,\text{Hz}$ that is not well captured with the multi-body model (Figure ref:fig:test_id31_hac_plant_effect_mass).
+
+#+begin_src matlab
+%% Trying high bandwidth controllers - 60Hz
+% Z direction
+data = load("2023-08-21_14-19_m0_cf_inv_tustin_Dz_60Hz_a_20.mat");
+[S_dz, ~] = tfestimate(data.Dz.id_cl, data.Dz.e_dz,   win, Noverlap, Nfft, 1/Ts);
+[T_dz, ~] = tfestimate(data.Dz.id_cl, data.Dz.Dz_int, win, Noverlap, Nfft, 1/Ts);
+
+% Y direction
+data = load("2023-08-21_14-20_m0_cf_inv_tustin_Dy_60Hz_a_20.mat");
+[S_dy, ~] = tfestimate(data.Dy.id_cl, data.Dy.e_dy,   win, Noverlap, Nfft, 1/Ts);
+[T_dy, ~] = tfestimate(data.Dy.id_cl, data.Dy.Dy_int, win, Noverlap, Nfft, 1/Ts);
+
+% Analytical formulas
+w0 = 2*pi*60;
+alpha = 2;
+Hh_60Hz = (s/w0)^2*((s/w0)+1+alpha)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
+Hl_60Hz = ((1+alpha)*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Bode plot of the Weighting filters and Obtained complementary filters
+figure;
+hold on;
+plot(f, abs(S_dy), '-', 'color', colors(1,:), 'DisplayName', '$S$, $D_y$');
+plot(f, abs(S_dz), '-', 'color', colors(2,:), 'DisplayName', '$S$, $D_z$');
+plot(freqs, abs(squeeze(freqresp(Hh_60Hz, freqs, 'Hz'))), 'k--','DisplayName', '$H_H$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude');
+xlim([1, 1e3]);
+ylim([3e-4, 5]);
+leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 18;
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+yticks([1e-3, 1e-2, 1e-1, 1]);
+exportFig('figs/test_id31_high_bandwidth_S.pdf', 'width', 'half', 'height', 'normal');
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Bode plot of the Weighting filters and Obtained complementary filters
+figure;
+hold on;
+plot(f, abs(T_dy), '-', 'color', colors(1,:), 'DisplayName', '$T$, $D_y$');
+plot(f, abs(T_dz), '-', 'color', colors(2,:), 'DisplayName', '$T$, $D_z$');
+plot(freqs, abs(squeeze(freqresp(Hl_60Hz, freqs, 'Hz'))), 'k--','DisplayName', '$H_L$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude');
+xlim([1, 1e3]);
+ylim([3e-4, 5]);
+leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 18;
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+yticks([1e-3, 1e-2, 1e-1, 1]);
+exportFig('figs/test_id31_high_bandwidth_T.pdf', 'width', 'half', 'height', 'normal');
+#+end_src
+
+#+name: fig:test_id31_high_bandwidth
+#+caption: Measured Closed-Loop Sensitivity (\subref{fig:test_id31_high_bandwidth_S}) and Complementary Sensitivity (\subref{fig:test_id31_high_bandwidth_T}) transfer functions for the highest test bandwidth $\omega_0 = 2\pi\cdot 60\,\text{rad/s}$.
+#+attr_latex: :options [htbp]
+#+begin_figure
+#+attr_latex: :caption \subcaption{\label{fig:test_id31_high_bandwidth_S}Sensitivity}
+#+attr_latex: :options {0.49\textwidth}
+#+begin_subfigure
+#+attr_latex: :scale 0.9
+[[file:figs/test_id31_high_bandwidth_S.png]]
+#+end_subfigure
+#+attr_latex: :caption \subcaption{\label{fig:test_id31_high_bandwidth_T}Complementary Sensitivity}
+#+attr_latex: :options {0.49\textwidth}
+#+begin_subfigure
+#+attr_latex: :scale 0.9
+[[file:figs/test_id31_high_bandwidth_T.png]]
+#+end_subfigure
+#+end_figure
+
 ** Conclusion
 :PROPERTIES:
 :UNNUMBERED: t
@@ -5208,108 +5439,67 @@ specs_ry_rms = 0.25; % [urad RMS]
 :header-args:matlab+: :comments none :mkdirp yes :eval no
 :END:
 #+begin_src matlab
-function [cntrs,tp] = h5scan(pth,smp,ds,sn,varargin)
+% function [cntrs,tp] = h5scan(pth,smp,ds,sn,varargin)
+function [cntrs,tp] = h5scan(ds,sn)
+    if ~isstr(ds), ds = sprintf('%.4d',ds); end;
 
-i = cellfun(@(x) isa(x,'detector'),varargin);
-if any(i), det = varargin{i}; varargin = varargin(~i); else, det = []; end;
-if ~isstr(ds), ds = sprintf('%.4d',ds); end;
+    f = sprintf('%s.h5',ds);
+    h = h5info(f,sprintf('/%d.1/measurement',sn));
+    fid = H5F.open(f);
+    for i = 1:length(h.Links)
+        nm = h.Links(i).Name;
+        try
+            id = H5D.open(fid,h.Links(i).Value{1});
+            cntrs.(nm) = H5D.read(id);
+            H5D.close(id);
+            if ~isempty(det) & strcmp(nm,det.name), cntrs.(nm) = integrate(det,double(cntrs.(nm))); end;
+        end
+        [~,tp.(nm)] = fileparts(h.Links(i).Value{1});
+    end
+    try
+        h = h5info(f,sprintf('/%d.2/measurement',sn));
+    catch
+        h = [];
+    end
+    if ~isempty(h)
+        for i = 1:length(h.Links)
+            nm = h.Links(i).Name;
+            try
+                id = H5D.open(fid,h.Links(i).Value{1});
+                cntrs.part2.(nm) = H5D.read(id);
+                H5D.close(id);
+            end
+            [~,tp.part2.(nm)] = fileparts(h.Links(i).Value{1});
+        end
+    end
 
-f = sprintf('%s/%s/%s_%s/%s_%s.h5',pth,smp,smp,ds,smp,ds);
-h = h5info(f,sprintf('/%d.1/measurement',sn));
-fid = H5F.open(f);
-for i = 1:length(h.Links),
-  nm = h.Links(i).Name;
-  try,
-    id = H5D.open(fid,h.Links(i).Value{1});
-    cntrs.(nm) = H5D.read(id);
-    H5D.close(id);
-    if ~isempty(det) & strcmp(nm,det.name), cntrs.(nm) = integrate(det,double(cntrs.(nm))); end;
-  catch,
-    warning('solving problem with %s\n',nm);
-    cntrs.(nm) = vrtlds(sprintf('%s/%s/%s_%s/scan%.4d/',pth,smp,smp,ds,sn),nm,det);
-  end;
-  [~,tp.(nm)] = fileparts(h.Links(i).Value{1});
-end;
-try,
-  h = h5info(f,sprintf('/%d.2/measurement',sn));
-catch,
-  h = [];
-end;
-if ~isempty(h),
-  for i = 1:length(h.Links),
-    nm = h.Links(i).Name;
-    try,
-      id = H5D.open(fid,h.Links(i).Value{1});
-      cntrs.part2.(nm) = H5D.read(id);
-      H5D.close(id);
-    catch,
-      warning('solving problem with %s\n',nm);
-      cntrs.part2.(nm) = vrtlds(sprintf('%s/%s/%s_%s/scan%.4d/',pth,smp,smp,ds,sn),nm,det);
-    end;
-    [~,tp.part2.(nm)] = fileparts(h.Links(i).Value{1});
-  end;
-end;
-if length(varargin),
-  fn = sprintf('/%d.1/instrument/positioners/',sn);
-  h = h5info(f,fn);
-  [~,k,m] = intersect({h.Datasets.Name},varargin,'stable');
-  h.Datasets = h.Datasets(k);
-  for i = 1:length(h.Datasets),
-    id = H5D.open(fid,[fn h.Datasets(i).Name]);
-    cntrs.(h.Datasets(i).Name) = H5D.read(id);
-    H5D.close(id);
-  end;
-end;
+    H5F.close(fid);
 
-H5F.close(fid);
+    %%%%%%%%%%%%%%%%%%%%%%%%%%
 
-%%%%%%%%%%%%%%%%%%%%%%%%%%
+    function A = vrtlds(f,nm,det)
+        n = 0; A = [];
+        fn = sprintf('%s/%s_%.4d.h5',f,nm,n);
 
-function A = vrtlds(f,nm,det)
-%try,
-  n = 0; A = [];
-  fn = sprintf('%s/%s_%.4d.h5',f,nm,n);
-  while exist(fn) == 2,
-    fid = H5F.open(fn); n = n+1;
-    id = H5D.open(fid,sprintf('/entry_0000/ESRF-ID31/%s/data',nm));
-    if 2 < nargin & strcmp(nm,'p3') & ~isempty(det),
-      fprintf('integrating %s\n',fn);
-      if isempty(A),
-        A = integrate(det,double(H5D.read(id)),1);
-      else,
-        tmp = integrate(det,double(H5D.read(id)),1); A.y = cat(2,A.y,tmp.y); A.y0 = cat(2,A.y0,tmp.y0);
-      end;
-    else,
-      fprintf('loading %s\n',fn);
-      A = cat(3,A,H5D.read(id));
-    end;
-    H5D.close(id); H5F.close(fid);
-    fn = sprintf('%s/%s_%.4d.h5',f,nm,n);
-  end;
-%catch,
-%  A = [];
-%end;
-
-% fid = H5F.open...
-% id = H5D.open...
-% sid = H5D.get_space(id);
-% [ndims,h5_dims]=H5S.get_simple_extent_dims(sid)
-
-% Read a 2x3 hyperslab of data from a dataset, starting in the 4th row and 5th column of the example dataset.
-% Create a property list identifier, then open the HDF5 file and the dataset /g1/g1.1/dset1.1.1.
-
-% fid = H5F.open('example.h5');
-% id = H5D.open(fid,'/g1/g1.1/dset1.1.1');
-
-% dims = ([500 1679 1475];
-% msid = H5S.create_simple(3,dims,[]);
-% sid = H5D.get_space(id);
-% offset = [n*500 0 0];
-% block = dims; % d1: 500 or min(d1tot-n*500,500)
-% H5S.select_hyperslab(sid,'H5S_SELECT_SET',offset,[],[],block);
-% data = H5D.read(id,'H5ML_DEFAULT',msid,sid,'H5P_DEFAULT');
-% H5D.close(id);
-% H5F.close(fid);
+        while exist(fn) == 2
+            fid = H5F.open(fn); n = n+1;
+            id = H5D.open(fid,sprintf('/entry_0000/ESRF-ID31/%s/data',nm));
+            if 2 < nargin & strcmp(nm,'p3') & ~isempty(det)
+                fprintf('integrating %s\n',fn);
+                if isempty(A)
+                    A = integrate(det,double(H5D.read(id)),1);
+                else
+                    tmp = integrate(det,double(H5D.read(id)),1); A.y = cat(2,A.y,tmp.y); A.y0 = cat(2,A.y0,tmp.y0);
+                end
+            else
+                fprintf('loading %s\n',fn);
+                A = cat(3,A,H5D.read(id));
+            end
+            H5D.close(id); H5F.close(fid);
+            fn = sprintf('%s/%s_%.4d.h5',f,nm,n);
+        end
+    end
+end
 #+end_src
 
 *** =sphereFit= - Fit sphere from x,y,z points

test-bench-id31.tex

@@ -1,4 +1,4 @@
-% Created 2025-04-07 Mon 14:38
+% Created 2025-04-20 Sun 22:21
 % Intended LaTeX compiler: pdflatex
 \documentclass[a4paper, 10pt, DIV=12, parskip=full, bibliography=totoc]{scrreprt}
 
@@ -215,6 +215,7 @@ 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.
 
 \begin{table}[htbp]
+\caption{\label{tab:test_id31_metrology_acceptance}Estimated measurement range for each interferometer, and for three different directions.}
 \centering
 \begin{tabularx}{0.45\linewidth}{Xccc}
 \toprule
@@ -227,8 +228,6 @@ The obtained lateral acceptance for pure displacements in any direction is estim
 \(d_5\) (z) & \(1.33\, mm\) & \(1.06\,mm\) & \(>2\,mm\)\\
 \bottomrule
 \end{tabularx}
-\caption{\label{tab:test_id31_metrology_acceptance}Estimated measurement range for each interferometer, and for three different directions.}
-
 \end{table}
 \section{Estimated measurement errors}
 \label{ssec:test_id31_metrology_errors}
@@ -739,6 +738,7 @@ Several scientific experiments were replicated, such as:
 \end{itemize}
 
 Unless explicitly stated, all closed-loop experiments were performed using the robust (i.e. conservative) high authority controller designed in Section \ref{ssec:test_id31_iff_hac_controller}.
+Higher performance controllers using complementary filters are investigated in Section \ref{ssec:test_id31_cf_control}.
 
 For each experiment, the obtained performances are compared to the specifications for the most demanding case in which nano-focusing optics are used to focus the beam down to \(200\,nm\times 100\,nm\).
 In this case, the goal is to keep the sample's point of interest in the beam, and therefore the \(D_y\) and \(D_z\) positioning errors should be less than \(200\,nm\) and \(100\,nm\) peak-to-peak, respectively.
@@ -748,6 +748,7 @@ In terms of RMS errors, this corresponds to \(30\,nm\) in \(D_y\), \(15\,nm\) in
 Results obtained for all experiments are summarized and compared to the specifications in Section \ref{ssec:test_id31_scans_conclusion}.
 
 \begin{table}[htbp]
+\caption{\label{tab:test_id31_experiments_specifications}Specifications for the Nano-Active-Stabilization-System}
 \centering
 \begin{tabularx}{0.45\linewidth}{Xccc}
 \toprule
@@ -757,8 +758,6 @@ peak 2 peak & 200nm & 100nm & \(1.7\,\mu\text{rad}\)\\
 RMS & 30nm & 15nm & \(250\,\text{nrad}\)\\
 \bottomrule
 \end{tabularx}
-\caption{\label{tab:test_id31_experiments_specifications}Specifications for the Nano-Active-Stabilization-System}
-
 \end{table}
 \section{Tomography Scans}
 \label{ssec:test_id31_scans_tomography}
@@ -824,7 +823,7 @@ Nevertheless, even with this robust (i.e. conservative) HAC implementation, the
 
 A comparative analysis was conducted using three tomography scans at \(180\,\text{deg/s}\) to evaluate the effectiveness of the HAC-LAC strategy in reducing positioning errors.
 The scans were performed under three conditions: open-loop, with decentralized IFF control, and with the complete HAC-LAC strategy.
-For these specific measurements, an enhanced high authority controller was optimized for low payload masses to meet the performance requirements.
+For this specific measurement, an enhanced high authority controller (discussed in Section \ref{ssec:test_id31_cf_control}) was optimized for low payload masses to meet the performance requirements.
 
 Figure \ref{fig:test_id31_hac_cas_cl} presents the cumulative amplitude spectra of the position errors for all three cases.
 The results reveal two distinct control contributions: the decentralized IFF effectively attenuates vibrations near the nano-hexapod suspension modes (an achievement not possible with HAC alone), while the high authority controller suppresses low-frequency vibrations primarily arising from Spindle guiding errors.
@@ -1083,6 +1082,74 @@ Alternatively, a feedforward controller could improve the lateral positioning ac
 \end{subfigure}
 \caption{\label{fig:test_id31_diffraction_tomo}Diffraction tomography scans (combined \(R_z\) and \(D_y\) motions) at several \(D_y\) velocities (\(R_z\) rotational velocity is \(6\,\text{deg/s}\)).}
 \end{figure}
+\section{Feedback control using Complementary Filters}
+\label{ssec:test_id31_cf_control}
+
+A control architecture utilizing complementary filters to shape the closed-loop transfer functions was proposed during the detail design phase.
+Experimental validation of this architecture using the NASS is presented herein.
+
+Given that performance requirements are specified in the Cartesian frame, decoupling of the plant within this frame was achieved using Jacobian matrices.
+Consequently, the control space comprises the directions \(D_x\), \(D_y\), \(D_z\), \(R_x\), and \(R_y\).
+Control performance in each of these directions can be tuned independently.
+A schematic of the proposed control architecture is illustrated in Figure~\ref{fig:test_id31_cf_control}.
+
+\begin{figure}[htbp]
+\centering
+\includegraphics[scale=1]{figs/test_id31_cf_control.png}
+\caption{\label{fig:test_id31_cf_control}Control architecture in the Cartesian frame. Only the controller corresponding to the \(D_z\) direction is shown. \(H_L\) and \(H_H\) are complementary filters.}
+\end{figure}
+
+
+Implementation of this control architecture necessitates a plant model, which must subsequently be inverted.
+This plant model was derived from the multi-body model incorporating the previously detailed 2-DoF APA model, such that the model order stays relatively low.
+Proposed analytical formulas for complementary filters having \(40\,\text{dB/dec}\) were used during this experimental validation.
+An initial experimental validation was conducted under no-payload conditions, with control applied solely to the \(D_y\), \(D_z\), and \(R_y\) directions.
+Increased control bandwidth was achieved for the \(D_z\) and \(R_y\) directions through appropriate tuning of the parameter \(\omega_0\).
+The experimentally measured closed-loop sensitivity transfer functions corresponding to these three controlled directions are presented in Figure \ref{fig:test_id31_cf_control_dy_dz_diff}.
+
+Another test was conducted with a \(26\,\text{kg}\) payload.
+For this configuration, complementary filters were implemented with \(\omega_0 = 2\pi \cdot 10\,\text{rad/s}\), and parameter \(\alpha\) was varied.
+The resulting experimentally obtained closed-loop transfer functions are compared against the theoretical complementary filter responses in Figure~\ref{fig:test_id31_cf_control_alpha}.
+As illustrated in the figure, a close correspondence between the measured closed-loop responses and the target complementary filter magnitude was observed.
+It also shows that the parameter \(\alpha\) provides a mechanism for managing the trade-off between low-frequency disturbance rejection performance and the potential amplification of disturbances within the crossover frequency region.
+
+\begin{figure}[htbp]
+\begin{subfigure}{0.49\textwidth}
+\begin{center}
+\includegraphics[scale=1,scale=0.9]{figs/test_id31_cf_control_dy_dz_diff.png}
+\end{center}
+\subcaption{\label{fig:test_id31_cf_control_dy_dz_diff}Chose of bandwidth using $\omega_0$, $m = 0\,\text{kg}$}
+\end{subfigure}
+\begin{subfigure}{0.49\textwidth}
+\begin{center}
+\includegraphics[scale=1,scale=0.9]{figs/test_id31_cf_control_alpha.png}
+\end{center}
+\subcaption{\label{fig:test_id31_cf_control_alpha}Effect of a change of $\alpha$, $m = 26\,\text{kg}$}
+\end{subfigure}
+\caption{\label{fig:test_id31_cf_control_results}Measured closed-loop transfer functions. Different bandwidth can be specified for different directions using \(\omega_0\) (\subref{fig:test_id31_cf_control_dy_dz_diff}). The shape can be adjusted using parameter \(\alpha\) (\subref{fig:test_id31_cf_control_alpha}).}
+\end{figure}
+
+Finally, \(\omega_0\) was gradually increased to estimate the maximum bandwidth (i.e. the best low frequency disturbance rejection) that can be achieved with this architecture.
+No payload was used for this test, and the parameter \(\omega_0\) was increased for the controllers in the \(D_y\) and \(D_z\) directions.
+A value \(\omega_0 = 2\pi \cdot 60 \,\text{rad/s}\) could be achieved.
+Measured closed-loop transfer functions are shown in Figure \ref{fig:test_id31_high_bandwidth}, indicating a reduction of disturbances in the considered direction of \(1000\) at \(1\,\text{Hz}\).
+For higher values of \(\omega_0\), the system became unstable in the vertical direction, probably because of the resonance at \(250\,\text{Hz}\) that is not well captured with the multi-body model (Figure \ref{fig:test_id31_hac_plant_effect_mass}).
+
+\begin{figure}[htbp]
+\begin{subfigure}{0.49\textwidth}
+\begin{center}
+\includegraphics[scale=1,scale=0.9]{figs/test_id31_high_bandwidth_S.png}
+\end{center}
+\subcaption{\label{fig:test_id31_high_bandwidth_S}Sensitivity}
+\end{subfigure}
+\begin{subfigure}{0.49\textwidth}
+\begin{center}
+\includegraphics[scale=1,scale=0.9]{figs/test_id31_high_bandwidth_T.png}
+\end{center}
+\subcaption{\label{fig:test_id31_high_bandwidth_T}Complementary Sensitivity}
+\end{subfigure}
+\caption{\label{fig:test_id31_high_bandwidth}Measured Closed-Loop Sensitivity (\subref{fig:test_id31_high_bandwidth_S}) and Complementary Sensitivity (\subref{fig:test_id31_high_bandwidth_T}) transfer functions for the highest test bandwidth \(\omega_0 = 2\pi\cdot 60\,\text{rad/s}\).}
+\end{figure}
 \section*{Conclusion}
 \label{ssec:test_id31_scans_conclusion}
 
@@ -1104,6 +1171,7 @@ Overall, the experimental results validate the effectiveness of the developed co
 The identified limitations, primarily related to high-speed lateral scanning and heavy payload handling, provide clear directions for future improvements.
 
 \begin{table}[htbp]
+\caption{\label{tab:test_id31_experiments_results_summary}Summary of the experimental results performed using the NASS on ID31. Open-loop errors are indicated on the left of the arrows. Closed-loop errors that are outside the specifications are indicated by bold number.}
 \centering
 \begin{tabularx}{\linewidth}{Xccc}
 \toprule
@@ -1132,8 +1200,6 @@ Diffraction tomography (\(6\,\text{deg/s}\), \(1\,mm/s\)) & \(\bm{53}\) & \(10\)
 \textbf{Specifications} & \(30\) & \(15\) & \(250\)\\
 \bottomrule
 \end{tabularx}
-\caption{\label{tab:test_id31_experiments_results_summary}Summary of the experimental results performed using the NASS on ID31. Open-loop errors are indicated on the left of the arrows. Closed-loop errors that are outside the specifications are indicated by bold number.}
-
 \end{table}
 \chapter*{Conclusion}
 \label{ssec:test_id31_conclusion}