97 lines
2.3 KiB
Matlab
97 lines
2.3 KiB
Matlab
%%
|
|
clear;
|
|
|
|
%% Jacobian
|
|
load('mat/jacobian.mat', 'J');
|
|
|
|
Jinv = pinv(J);
|
|
|
|
save('sim_data/J.mat', 'J', 'Jinv');
|
|
|
|
%% Saved HAC
|
|
% load('mat/Khac_iff_struts.mat', 'Khac_iff_struts');
|
|
|
|
%% Developed HAC
|
|
s = zpk('s');
|
|
|
|
a = 5; % Amount of phase lead / width of the phase lead / high frequency gain
|
|
wc = 2*pi*110; % Frequency with the maximum phase lead [rad/s]
|
|
|
|
H_lead = (1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
|
|
H_lpf = 1/(1 + s/2/pi/300);
|
|
|
|
gm = 0.02;
|
|
xi = 0.5;
|
|
wn = 2*pi*700;
|
|
|
|
H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
|
|
|
|
Khac_iff_struts = -2.2e4 * ... % Gain
|
|
H_lead * ... % Lead
|
|
H_lpf * ... % Lead
|
|
H_notch * ... % Notch
|
|
(2*pi*100/s) * ... % Integrator
|
|
eye(6); % 6x6 Diagonal
|
|
|
|
%% Save Controller
|
|
load('sim_data/data_sim.mat', 'Ts')
|
|
Khac_iff_struts = c2d(Khac_iff_struts, Ts, 'Tustin');
|
|
|
|
save('sim_data/Khac_iff_struts.mat', 'Khac_iff_struts');
|
|
|
|
%% Loop Gain
|
|
load('mat/damped_plant_enc_plates.mat', 'f', 'G_enc_iff_opt')
|
|
|
|
L_frf = zeros(size(G_enc_iff_opt));
|
|
|
|
for i = 1:size(G_enc_iff_opt, 1)
|
|
L_frf(i, :, :) = squeeze(G_enc_iff_opt(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]);
|