227 lines
6.3 KiB
Matlab
227 lines
6.3 KiB
Matlab
%% Clear Workspace and Close figures
|
|
clear; close all; clc;
|
|
|
|
%% Intialize Laplace variable
|
|
s = zpk('s');
|
|
|
|
addpath('./mat/');
|
|
|
|
% Load Data
|
|
% The experimental data is loaded and any offset is removed.
|
|
|
|
id_ol = load('identification_noise_bis.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
|
|
|
|
id_ol.d = detrend(id_ol.d, 0);
|
|
id_ol.acc_1 = detrend(id_ol.acc_1, 0);
|
|
id_ol.acc_2 = detrend(id_ol.acc_2, 0);
|
|
id_ol.geo_1 = detrend(id_ol.geo_1, 0);
|
|
id_ol.geo_2 = detrend(id_ol.geo_2, 0);
|
|
id_ol.f_meas = detrend(id_ol.f_meas, 0);
|
|
id_ol.u = detrend(id_ol.u, 0);
|
|
|
|
% Experimental Data
|
|
% The transfer function from force actuator to force sensors is estimated.
|
|
|
|
% The coherence shown in Figure [[fig:iff_identification_coh]] shows that the excitation signal is good enough.
|
|
|
|
|
|
Ts = id_ol.t(2) - id_ol.t(1);
|
|
win = hann(ceil(10/Ts));
|
|
|
|
[tf_fmeas_est, f] = tfestimate(id_ol.u, id_ol.f_meas, win, [], [], 1/Ts); % [V/m]
|
|
[co_fmeas_est, ~] = mscohere( id_ol.u, id_ol.f_meas, win, [], [], 1/Ts);
|
|
|
|
figure;
|
|
hold on;
|
|
plot(f, co_fmeas_est, '-')
|
|
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
|
ylabel('Coherence'); xlabel('Frequency [Hz]');
|
|
hold off;
|
|
xlim([1, 1e3]); ylim([0, 1])
|
|
|
|
|
|
|
|
% #+name: fig:iff_identification_coh
|
|
% #+caption: Coherence for the identification of the IFF plant
|
|
% #+RESULTS:
|
|
% [[file:figs/iff_identification_coh.png]]
|
|
|
|
% The obtained dynamics is shown in Figure [[fig:iff_identification_bode_plot]].
|
|
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f, abs(tf_fmeas_est), '-')
|
|
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
|
ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
ylim([1e-1, 1e3]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(f, 180/pi*angle(tf_fmeas_est), '-')
|
|
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
|
ylabel('Phase'); xlabel('Frequency [Hz]');
|
|
hold off;
|
|
ylim([-180, 180]);
|
|
yticks([-180, -90, 0, 90, 180]);
|
|
|
|
linkaxes([ax1,ax2], 'x');
|
|
xlim([1, 1e3]);
|
|
|
|
% Model of the IFF Plant
|
|
% In order to plot the root locus for the IFF control strategy, a model of the identified plant is developed.
|
|
|
|
% It consists of several poles and zeros are shown below.
|
|
|
|
wz = 2*pi*102;
|
|
xi_z = 0.01;
|
|
wp = 2*pi*239.4;
|
|
xi_p = 0.015;
|
|
|
|
Giff = 2.2*(s^2 + 2*xi_z*s*wz + wz^2)/(s^2 + 2*xi_p*s*wp + wp^2) * ... % Dynamics
|
|
10*(s/3/pi/(1 + s/3/pi)) * ... % Low pass filter and gain of the voltage amplifier
|
|
exp(-Ts*s); % Time delay induced by ADC/DAC
|
|
|
|
|
|
|
|
% The comparison of the identified dynamics and the developed model is done in Figure [[fig:iff_plant_model]].
|
|
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f, abs(tf_fmeas_est), '.')
|
|
plot(f, abs(squeeze(freqresp(Giff, f, 'Hz'))), 'k-')
|
|
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
|
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
ylim([1e-2, 1e3])
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(f, 180/pi*angle(tf_fmeas_est), '.')
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Giff, f, 'Hz'))), 'k-')
|
|
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
|
ylabel('Phase'); xlabel('Frequency [Hz]');
|
|
hold off;
|
|
ylim([-180, 180]);
|
|
yticks([-180, -90, 0, 90, 180]);
|
|
|
|
linkaxes([ax1,ax2], 'x');
|
|
xlim([0.5, 5e3]);
|
|
|
|
% Root Locus and optimal Controller
|
|
% Now, the root locus for the Integral Force Feedback strategy is computed and shown in Figure [[fig:iff_root_locus]].
|
|
|
|
% Note that the controller used is not a pure integrator but rather a first order low pass filter with a cut-off frequency set at 2Hz.
|
|
|
|
|
|
gains = logspace(0, 5, 1000);
|
|
|
|
figure;
|
|
hold on;
|
|
plot(real(pole(Giff)), imag(pole(Giff)), 'kx');
|
|
plot(real(tzero(Giff)), imag(tzero(Giff)), 'ko');
|
|
for i = 1:length(gains)
|
|
cl_poles = pole(feedback(Giff, gains(i)/(s + 2*pi*2)));
|
|
plot(real(cl_poles), imag(cl_poles), 'k.');
|
|
end
|
|
cl_poles = pole(feedback(Giff, 102/(s + 2*pi*2)));
|
|
plot(real(cl_poles), imag(cl_poles), 'rx');
|
|
ylim([0, 1800]);
|
|
xlim([-1600,200]);
|
|
xlabel('Real Part')
|
|
ylabel('Imaginary Part')
|
|
axis square
|
|
|
|
|
|
|
|
% #+name: fig:iff_root_locus
|
|
% #+caption: Root Locus for the IFF control
|
|
% #+RESULTS:
|
|
% [[file:figs/iff_root_locus.png]]
|
|
|
|
% The controller that yield maximum damping (shown by the red cross in Figure [[fig:iff_root_locus]]) is:
|
|
|
|
Kiff_opt = 102/(s + 2*pi*2);
|
|
|
|
% Verification of the achievable damping
|
|
% A new identification is performed with the IFF control strategy applied to the system.
|
|
|
|
% Data is loaded and offset removed.
|
|
|
|
id_cl = load('identification_noise_iff_bis.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
|
|
|
|
id_cl.d = detrend(id_cl.d, 0);
|
|
id_cl.acc_1 = detrend(id_cl.acc_1, 0);
|
|
id_cl.acc_2 = detrend(id_cl.acc_2, 0);
|
|
id_cl.geo_1 = detrend(id_cl.geo_1, 0);
|
|
id_cl.geo_2 = detrend(id_cl.geo_2, 0);
|
|
id_cl.f_meas = detrend(id_cl.f_meas, 0);
|
|
id_cl.u = detrend(id_cl.u, 0);
|
|
|
|
|
|
|
|
% The open-loop and closed-loop dynamics are estimated.
|
|
|
|
[tf_G_ol_est, f] = tfestimate(id_ol.u, id_ol.d, win, [], [], 1/Ts);
|
|
[co_G_ol_est, ~] = mscohere( id_ol.u, id_ol.d, win, [], [], 1/Ts);
|
|
[tf_G_cl_est, ~] = tfestimate(id_cl.u, id_cl.d, win, [], [], 1/Ts);
|
|
[co_G_cl_est, ~] = mscohere( id_cl.u, id_cl.d, win, [], [], 1/Ts);
|
|
|
|
|
|
|
|
% The obtained coherence is shown in Figure [[fig:Gd_identification_iff_coherence]] and the dynamics in Figure [[fig:Gd_identification_iff_bode_plot]].
|
|
|
|
|
|
figure;
|
|
hold on;
|
|
plot(f, co_G_ol_est, '-', 'DisplayName', 'OL')
|
|
plot(f, co_G_cl_est, '-', 'DisplayName', 'IFF')
|
|
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
|
ylabel('Coherence'); xlabel('Frequency [Hz]');
|
|
hold off;
|
|
xlim([1, 1e3]); ylim([0, 1])
|
|
legend('location', 'southwest');
|
|
|
|
|
|
|
|
% #+name: fig:Gd_identification_iff_coherence
|
|
% #+caption: Coherence for the transfer function from F to d, with and without IFF
|
|
% #+RESULTS:
|
|
% [[file:figs/Gd_identification_iff_coherence.png]]
|
|
|
|
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f, abs(tf_G_ol_est), '-', 'DisplayName', 'OL')
|
|
plot(f, abs(tf_G_cl_est), '-', 'DisplayName', 'IFF')
|
|
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
legend('location', 'northeast');
|
|
ylim([2e-7, 2e-4]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(f, 180/pi*angle(tf_G_ol_est), '-')
|
|
plot(f, 180/pi*angle(tf_G_cl_est), '-')
|
|
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
|
ylabel('Phase'); xlabel('Frequency [Hz]');
|
|
hold off;
|
|
ylim([-180, 180]);
|
|
yticks([-180, -90, 0, 90, 180]);
|
|
|
|
linkaxes([ax1,ax2], 'x');
|
|
xlim([1, 1e3]);
|