add all files
This commit is contained in:
272
C4-test-bench-nano-hexapod/test_nhexa_2_dynamics.m
Normal file
272
C4-test-bench-nano-hexapod/test_nhexa_2_dynamics.m
Normal file
@@ -0,0 +1,272 @@
|
||||
%% test_nhexa_2_dynamics.m
|
||||
% Identification of the nano-hexapod dynamics from u to de and to Vs
|
||||
% Encoders are fixed to the plates
|
||||
|
||||
%% 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
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Frequency Vector
|
||||
freqs = logspace(log10(10), log10(2e3), 1000);
|
||||
|
||||
%% Identification of the transfer function from u to de and from u to Vs without payload
|
||||
% Load identification data
|
||||
load('test_nhexa_identification_data_mass_0.mat', 'data');
|
||||
|
||||
% Setup useful variables
|
||||
Ts = 1e-4; % Sampling Time [s]
|
||||
Nfft = floor(1/Ts); % Number of points for the FFT computation
|
||||
win = hanning(Nfft); % Hanning window
|
||||
Noverlap = floor(Nfft/2); % Overlap between frequency analysis
|
||||
|
||||
% And we get the frequency vector
|
||||
[~, f] = tfestimate(data{1}.u, data{1}.de, win, Noverlap, Nfft, 1/Ts);
|
||||
|
||||
% Transfer function from u to de
|
||||
G_de = zeros(length(f), 6, 6);
|
||||
|
||||
for i = 1:6
|
||||
G_de(:,:,i) = tfestimate(data{i}.u, data{i}.de, win, Noverlap, Nfft, 1/Ts);
|
||||
end
|
||||
|
||||
% Transfer function from u to Vs
|
||||
G_Vs = zeros(length(f), 6, 6);
|
||||
|
||||
for i = 1:6
|
||||
G_Vs(:,:,i) = tfestimate(data{i}.u, data{i}.Vs, win, Noverlap, Nfft, 1/Ts);
|
||||
end
|
||||
|
||||
%% Bode plot for the transfer function from u to de
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
for i = 1:5
|
||||
for j = i+1:6
|
||||
plot(f, abs(G_de(:, i, j)), 'color', [0, 0, 0, 0.2], ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
end
|
||||
for i =1:6
|
||||
set(gca,'ColorOrderIndex',i)
|
||||
plot(f, abs(G_de(:,i, i)), ...
|
||||
'DisplayName', sprintf('$d_{e,%i}/u_%i$', i, i));
|
||||
end
|
||||
plot(f, abs(G_de(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
|
||||
'DisplayName', '$d_{e,i}/u_j$');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-8, 5e-4]);
|
||||
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 4);
|
||||
leg.ItemTokenSize(1) = 15;
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
for i =1:6
|
||||
set(gca,'ColorOrderIndex',i)
|
||||
plot(f, 180/pi*angle(G_de(:,i, i)));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
hold off;
|
||||
yticks(-360:90:360);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([10, 2e3]);
|
||||
|
||||
%% Bode plot of the IFF Plant (transfer function from u to Vs)
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
for i = 1:5
|
||||
for j = i+1:6
|
||||
plot(f, abs(G_Vs(:, i, j)), 'color', [0, 0, 0, 0.2], ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
end
|
||||
for i =1:6
|
||||
set(gca,'ColorOrderIndex',i)
|
||||
plot(f, abs(G_Vs(:,i , i)), ...
|
||||
'DisplayName', sprintf('$V_{s%i}/u_%i$', i, i));
|
||||
end
|
||||
plot(f, abs(G_Vs(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
|
||||
'DisplayName', '$V_{si}/u_j$');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
|
||||
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 4);
|
||||
leg.ItemTokenSize(1) = 15;
|
||||
ylim([1e-3, 6e1]);
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
for i =1:6
|
||||
set(gca,'ColorOrderIndex',i)
|
||||
plot(f, 180/pi*angle(G_Vs(:,i, i)));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
hold off;
|
||||
yticks(-360:90:360);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([10, 2e3]);
|
||||
|
||||
%% Set to true only if all the FRF matrices should again computed
|
||||
% from the experimental data
|
||||
compute_frf = false;
|
||||
if compute_frf
|
||||
|
||||
%% Identification of the FRF matrices from u to de and to Vs
|
||||
% Load identification Data
|
||||
meas_added_mass = {...
|
||||
load('test_nhexa_identification_data_mass_0.mat', 'data'), ....
|
||||
load('test_nhexa_identification_data_mass_1.mat', 'data'), ....
|
||||
load('test_nhexa_identification_data_mass_2.mat', 'data'), ....
|
||||
load('test_nhexa_identification_data_mass_3.mat', 'data')};
|
||||
|
||||
% Setup useful variables
|
||||
Ts = 1e-4; % Sampling Time [s]
|
||||
Nfft = floor(1/Ts); % Number of points for the FFT computation
|
||||
win = hanning(Nfft); % Hanning window
|
||||
Noverlap = floor(Nfft/2); % Overlap between frequency analysis
|
||||
|
||||
% And we get the frequency vector
|
||||
[~, f] = tfestimate(meas_added_mass{1}.data{1}.u, meas_added_mass{1}.data{1}.de, win, Noverlap, Nfft, 1/Ts);
|
||||
|
||||
% FRF from u to de
|
||||
G_de = {};
|
||||
|
||||
for i_mass = [0:3]
|
||||
G_de(i_mass+1) = {zeros(length(f), 6, 6)};
|
||||
for i_strut = 1:6
|
||||
G_de{i_mass+1}(:,:,i_strut) = tfestimate(meas_added_mass{i_mass+1}.data{i_strut}.u, meas_added_mass{i_mass+1}.data{i_strut}.de, win, Noverlap, Nfft, 1/Ts);
|
||||
end
|
||||
end
|
||||
|
||||
% FRF from u to Vs
|
||||
G_Vs = {};
|
||||
|
||||
for i_mass = [0:3]
|
||||
G_Vs(i_mass+1) = {zeros(length(f), 6, 6)};
|
||||
for i_strut = 1:6
|
||||
G_Vs{i_mass+1}(:,:,i_strut) = tfestimate(meas_added_mass{i_mass+1}.data{i_strut}.u, meas_added_mass{i_mass+1}.data{i_strut}.Vs, win, Noverlap, Nfft, 1/Ts);
|
||||
end
|
||||
end
|
||||
|
||||
% The identified dynamics are then saved for further use.
|
||||
save('./mat/test_nhexa_identified_frf_masses.mat', 'f', 'G_Vs', 'G_de')
|
||||
|
||||
end
|
||||
|
||||
%% Load the identified transfer functions
|
||||
frf_ol = load('test_nhexa_identified_frf_masses.mat', 'f', 'G_Vs', 'G_de');
|
||||
|
||||
%% Bode plot for the transfer function from u to de - Several payloads
|
||||
masses = [0, 13, 26, 39];
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
for i_mass = [0:3]
|
||||
% Diagonal terms
|
||||
plot(frf_ol.f, abs(frf_ol.G_de{i_mass+1}(:,1, 1)), 'color', [colors(i_mass+1,:), 0.5], ...
|
||||
'DisplayName', sprintf('$d_{ei}/u_i$ - %i kg', masses(i_mass+1)));
|
||||
for i = 2:6
|
||||
plot(frf_ol.f, abs(frf_ol.G_de{i_mass+1}(:,i, i)), 'color', [colors(i_mass+1,:), 0.5], ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
% % Off-Diagonal terms
|
||||
% for i = 1:5
|
||||
% for j = i+1:6
|
||||
% plot(frf_ol.f, abs(frf_ol.G_de{i_mass+1}(:,i,j)), 'color', [colors(i_mass+1,:), 0.2], ...
|
||||
% 'HandleVisibility', 'off');
|
||||
% end
|
||||
% end
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-8, 5e-4]);
|
||||
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
|
||||
leg.ItemTokenSize(1) = 15;
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
for i_mass = [0:3]
|
||||
for i =1:6
|
||||
plot(frf_ol.f, 180/pi*angle(frf_ol.G_de{i_mass+1}(:,i, i)), 'color', [colors(i_mass+1,:), 0.5]);
|
||||
end
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
hold off;
|
||||
yticks(-360:90:360);
|
||||
ylim([-90, 180])
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([10, 2e3]);
|
||||
|
||||
%% Bode plot for the transfer function from u to de
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
for i_mass = [0:3]
|
||||
% Diagonal terms
|
||||
plot(frf_ol.f, abs(frf_ol.G_Vs{i_mass+1}(:,1, 1)), 'color', [colors(i_mass+1,:), 0.5], ...
|
||||
'DisplayName', sprintf('$V_{si}/u_i$ - %i kg', masses(i_mass+1)));
|
||||
for i = 2:6
|
||||
plot(frf_ol.f, abs(frf_ol.G_Vs{i_mass+1}(:,i, i)), 'color', [colors(i_mass+1,:), 0.5], ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
% % Off-Diagonal terms
|
||||
% for i = 1:5
|
||||
% for j = i+1:6
|
||||
% plot(frf_ol.f, abs(frf_ol.G_Vs{i_mass+1}(:,i,j)), 'color', [colors(i_mass+1,:), 0.2], ...
|
||||
% 'HandleVisibility', 'off');
|
||||
% end
|
||||
% end
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-2, 1e2]);
|
||||
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
||||
leg.ItemTokenSize(1) = 15;
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
for i_mass = [0:3]
|
||||
for i =1:6
|
||||
plot(frf_ol.f, 180/pi*angle(frf_ol.G_Vs{i_mass+1}(:,i, i)), 'color', [colors(i_mass+1,:), 0.5]);
|
||||
end
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
hold off;
|
||||
yticks(-360:90:360);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([10, 2e3]);
|
||||
Reference in New Issue
Block a user