71 lines
1.7 KiB
Matlab
71 lines
1.7 KiB
Matlab
%%
|
|
clear; close all; clc;
|
|
|
|
%% Saved HAC
|
|
% load('mat/Khac_iff_struts.mat', 'Khac_iff_struts');
|
|
% load('mat/Khac_iff_struts_rob.mat', 'Kd');
|
|
load('mat/Khac_iff_struts_svd.mat', 'Kd');
|
|
% load('mat/Khac_iff_struts_jacobian_cok.mat', 'Kd');
|
|
|
|
%% Save Controller
|
|
load('sim_data/data_sim.mat', 'Ts')
|
|
Khac_iff_struts = c2d(Kd, Ts, 'Tustin');
|
|
|
|
save('sim_data/Khac_iff_struts.mat', 'Khac_iff_struts');
|
|
|
|
%% Loop Gain
|
|
load('mat/frf_iff_vib_table_m.mat', 'f', 'G_dL');
|
|
|
|
L_frf = zeros(size(G_dL{3}));
|
|
|
|
for i = 1:length(f)
|
|
L_frf(i, :, :) = squeeze(G_dL{3}(i,:,:))*freqresp(Khac_iff_struts, f(i), 'Hz');
|
|
end
|
|
|
|
colors = colororder;
|
|
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
% Diagonal Elements FRF
|
|
plot(f, abs(L_frf(:,1,1)), 'color', colors(1,:), ...
|
|
'DisplayName', 'Diagonal');
|
|
for i = 2:6
|
|
plot(f, abs(L_frf(:,i,i)), 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
plot(f, abs(L_frf(:,1,2)), 'color', [colors(2,:), 0.2], ...
|
|
'DisplayName', 'Off-Diag');
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, abs(L_frf(:,i,j)), 'color', [colors(2,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Loop Gain [-]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-3, 1e2]);
|
|
legend('location', 'northeast');
|
|
grid;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
plot(f, 180/pi*angle(L_frf(:,i,i)), 'color', colors(1,:));
|
|
end
|
|
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]);
|
|
grid;
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([1, 2e3]);
|