562 lines
24 KiB
Mathematica
562 lines
24 KiB
Mathematica
|
% 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 going to be 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 very 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 are obtained for all other 30 elements and for the different tested 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 such that it adds 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 suspension modes indicating 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 is computed using the multi-body model (Figure ref:fig:test_id31_iff_root_locus_m0).
|
||
|
|
% 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 value of the gain are displayed 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 value of the IFF controller gain could lead to better damping.
|
||
|
|
% However, in this study, it was chosen to implement a fix (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 is accurately modelling 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 are approximately reduced by a factor $10$ for all considered payloads, showing the effectiveness of the decentralized IFF control strategy.
|
||
|
|
|
||
|
|
% In order 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]);
|