sensor-fusion-test-bench/matlab/integral_force_feedback.m

%% 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]);