494 lines
19 KiB
Matlab
494 lines
19 KiB
Matlab
%% 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]
|
|
|
|
% 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 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]);
|
|
|
|
%% 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');
|
|
|
|
%% 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]);
|