273 lines
8.0 KiB
Matlab
273 lines
8.0 KiB
Matlab
%% 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]);
|