562 lines
24 KiB
Matlab
562 lines
24 KiB
Matlab
% Matlab Init :noexport:ignore:
|
|
|
|
%% 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]
|
|
|
|
% IFF Plant
|
|
% <<ssec:test_id31_iff_plant>>
|
|
|
|
% As the multi-body model is used to evaluate the stability of the IFF controller and to optimize the achievable damping, it is first checked whether this model accurately represents the system dynamics.
|
|
|
|
% In the previous section (Figure ref:fig:test_id31_comp_simscape_iff_diag_masses), it was shown that the model well captures the dynamics from each actuator to its collocated force sensor, and that for all considered payloads.
|
|
% Nevertheless, it is also important to well model the coupling in the system.
|
|
% To verify that, instead of comparing the 36 elements of the $6 \times 6$ frequency response matrix from $\bm{u}$ to $\bm{V_s}$, only 6 elements are compared in Figure ref:fig:test_id31_comp_simscape_Vs.
|
|
% Similar results were obtained for all other 30 elements and for the different payload conditions.
|
|
% This confirms that the multi-body model can be used to tune the IFF controller.
|
|
|
|
|
|
% 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
|
|
% <<ssec:test_id31_iff_controller>>
|
|
|
|
% A decentralized IFF controller was designed to add damping to the suspension modes of the nano-hexapod for all considered payloads.
|
|
% The frequency of the suspension modes are ranging from $\approx 30\,\text{Hz}$ to $\approx 250\,\text{Hz}$ (Figure ref:fig:test_id31_comp_simscape_iff_diag_masses), and therefore, the IFF controller should provide integral action in this frequency range.
|
|
% A second-order high-pass filter (cut-off frequency of $10\,\text{Hz}$) was added to limit the low frequency gain eqref:eq:test_id31_Kiff.
|
|
|
|
% \begin{equation}\label{eq:test_id31_Kiff}
|
|
% K_{\text{IFF}} = g_0 \cdot \underbrace{\frac{1}{s}}_{\text{int}} \cdot \underbrace{\frac{s^2/\omega_z^2}{s^2/\omega_z^2 + 2\xi_z s /\omega_z + 1}}_{\text{2nd order LPF}},\quad \left(g_0 = -100,\ \omega_z = 2\pi10\,\text{rad/s},\ \xi_z = 0.7\right)
|
|
% \end{equation}
|
|
|
|
% The bode plot of the decentralized IFF controller is shown in Figure ref:fig:test_id31_Kiff_bode_plot and the "decentralized loop-gains" for all considered payload masses are shown in Figure ref:fig:test_id31_Kiff_loop_gain.
|
|
% It can be seen that the loop-gain is larger than $1$ around the suspension modes, which indicates that some damping should be added to the suspension modes.
|
|
|
|
|
|
%% 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]);
|
|
|
|
|
|
|
|
% #+name: fig:test_id31_Kiff
|
|
% #+caption: Bode plot of the decentralized IFF controller (\subref{fig:test_id31_Kiff_bode_plot}). The decentralized controller $K_{\text{IFF}}$ multiplied by the identified dynamics from $u_1$ to $V_{s1}$ for all payloads are shown in (\subref{fig:test_id31_Kiff_loop_gain})
|
|
% #+attr_latex: :options [htbp]
|
|
% #+begin_figure
|
|
% #+attr_latex: :caption \subcaption{\label{fig:test_id31_Kiff_bode_plot}Bode plot of $K_{\text{IFF}}$}
|
|
% #+attr_latex: :options {0.49\textwidth}
|
|
% #+begin_subfigure
|
|
% #+attr_latex: :width 0.95\linewidth
|
|
% [[file:figs/test_id31_Kiff_bode_plot.png]]
|
|
% #+end_subfigure
|
|
% #+attr_latex: :caption \subcaption{\label{fig:test_id31_Kiff_loop_gain}Decentralized Loop gains}
|
|
% #+attr_latex: :options {0.49\textwidth}
|
|
% #+begin_subfigure
|
|
% #+attr_latex: :width 0.95\linewidth
|
|
% [[file:figs/test_id31_Kiff_loop_gain.png]]
|
|
% #+end_subfigure
|
|
% #+end_figure
|
|
|
|
% To estimate the added damping, a root-locus plot was computed using the multi-body model (Figure ref:fig:test_id31_iff_root_locus).
|
|
% It can be seen that for all considered payloads, the poles are bounded to the "left-half plane" indicating that the decentralized IFF is robust.
|
|
% The closed-loop poles for the chosen gain value are represented by black crosses.
|
|
% It can be seen that while damping can be added for all payloads (as compared to the open-loop case), the optimal value of the gain is different for each payload.
|
|
% For low payload masses, a higher IFF controller gain can lead to better damping.
|
|
% However, in this study, it was chosen to implement a "fixed" (i.e. non-adaptive) decentralized IFF controller.
|
|
|
|
|
|
%% 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');
|
|
|
|
% Damped Plant
|
|
% <<ssec:test_id31_iff_perf>>
|
|
|
|
% As the model accurately represents the system dynamics, it can be used to estimate the damped plant, i.e. the transfer functions from $\bm{u}^\prime$ to $\bm{\mathcal{L}}$.
|
|
% The obtained damped plants are compared to the open-loop plants in Figure ref:fig:test_id31_comp_ol_iff_plant_model.
|
|
% The peak amplitudes corresponding to the suspension modes were approximately reduced by a factor $10$ for all considered payloads, indicating the effectiveness of the decentralized IFF control strategy.
|
|
|
|
% To experimentally validate the Decentralized IFF controller, it was implemented and the damped plants (i.e. the transfer function from $\bm{u}^\prime$ to $\bm{\epsilon\mathcal{L}}$) were identified for all payload conditions.
|
|
% The obtained frequency response functions are compared with the model in Figure ref:fig:test_id31_hac_plant_effect_mass verifying the good correlation between the predicted damped plant using the multi-body model and the experimental results.
|
|
|
|
|
|
%% 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]);
|