Add Complementary filter Experimental validation

C5-test-bench-id31/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