424 lines
14 KiB
Matlab
424 lines
14 KiB
Matlab
%% Clear Workspace and Close figures
|
|
clear; close all; clc;
|
|
|
|
%% Intialize Laplace variable
|
|
s = zpk('s');
|
|
|
|
addpath('./mat/');
|
|
|
|
% Load Data
|
|
% The data is loaded in the Matlab workspace.
|
|
|
|
id_ol = load('identification_noise_bis.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
|
|
|
|
|
|
|
|
% Then, any offset is removed.
|
|
|
|
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);
|
|
|
|
% Excitation Signal
|
|
% The generated voltage used to excite the system is a white noise and can be seen in Figure [[fig:excitation_signal_first_identification]].
|
|
|
|
|
|
figure;
|
|
plot(id_ol.t, id_ol.u)
|
|
xlabel('Time [s]'); ylabel('Voltage [V]');
|
|
|
|
% Identified Plant
|
|
% The transfer function from the excitation voltage to the mass displacement and to the force sensor stack voltage are identified using the =tfestimate= command.
|
|
|
|
|
|
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/V]
|
|
[tf_G_ol_est, ~] = tfestimate(id_ol.u, id_ol.d, win, [], [], 1/Ts); % [m/V]
|
|
|
|
|
|
|
|
% The bode plots of the obtained dynamics are shown in Figures [[fig:force_sensor_bode_plot]] and [[fig:displacement_sensor_bode_plot]].
|
|
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f, abs(tf_fmeas_est), '-')
|
|
hold off;
|
|
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
|
ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
|
|
|
|
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]);
|
|
|
|
|
|
|
|
% #+name: fig:force_sensor_bode_plot
|
|
% #+caption: Bode plot of the dynamics from excitation voltage to measured force sensor stack voltage
|
|
% #+RESULTS:
|
|
% [[file:figs/force_sensor_bode_plot.png]]
|
|
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f, abs(tf_G_ol_est), '-')
|
|
hold off;
|
|
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(f, 180/pi*angle(tf_G_ol_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]);
|
|
|
|
% Simscape Model - Comparison
|
|
% A simscape model representing the test-bench has been developed.
|
|
% The same transfer functions as the one identified using the test-bench can be obtained thanks to the simscape model.
|
|
|
|
% They are compared in Figure [[fig:simscape_comp_iff_plant]] and [[fig:simscape_comp_disp_plant]].
|
|
% It is shown that there is a good agreement between the model and the experiment.
|
|
|
|
|
|
load('piezo_amplified_3d.mat', 'int_xyz', 'int_i', 'n_xyz', 'n_i', 'nodes', 'M', 'K');
|
|
|
|
m = 10;
|
|
Kiff = tf(0);
|
|
|
|
%% Name of the Simulink File
|
|
mdl = 'sensor_fusion_test_bench_simscape';
|
|
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Fd'], 1, 'openinput'); io_i = io_i + 1; % External Vertical Force [N]
|
|
io(io_i) = linio([mdl, '/w'], 1, 'openinput'); io_i = io_i + 1; % Base Motion [m]
|
|
io(io_i) = linio([mdl, '/Va'], 1, 'openinput'); io_i = io_i + 1; % Actuator Voltage [V]
|
|
io(io_i) = linio([mdl, '/Interferometer'], 1, 'openoutput'); io_i = io_i + 1; % Vertical Displacement [m]
|
|
io(io_i) = linio([mdl, '/Voltage_Conditioner'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensor [V]
|
|
|
|
options = linearizeOptions('SampleTime', 1e-4);
|
|
G = linearize(mdl, io, options);
|
|
|
|
G.InputName = {'Fd', 'w', 'Va'};
|
|
G.OutputName = {'y', 'Vs'};
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f, abs(tf_fmeas_est), 'DisplayName', 'Identification')
|
|
plot(f, abs(squeeze(freqresp(G('Vs', 'Va'), f, 'Hz'))), 'DisplayName', 'Simscape Model')
|
|
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
|
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
ylim([1e-1, 1e3]);
|
|
legend('location', 'northwest');
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(f, 180/pi*angle(tf_fmeas_est))
|
|
plot(f, 180/pi*angle(squeeze(freqresp(G('Vs', 'Va'), f, 'Hz'))))
|
|
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, 5e3]);
|
|
|
|
|
|
|
|
% #+name: fig:simscape_comp_iff_plant
|
|
% #+caption: Comparison of the dynamics from excitation voltage to measured force sensor stack voltage - Identified dynamics and Simscape Model
|
|
% #+RESULTS:
|
|
% [[file:figs/simscape_comp_iff_plant.png]]
|
|
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f, abs(tf_G_ol_est), 'DisplayName', 'Identification')
|
|
plot(f, abs(squeeze(freqresp(G('y', 'Va'), f, 'Hz'))), 'DisplayName', 'Simscape Model')
|
|
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
ylim([1e-8, 1e-3]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(f, 180/pi*angle(tf_G_ol_est))
|
|
plot(f, 180/pi*angle(squeeze(freqresp(G('y', 'Va'), f, 'Hz'))))
|
|
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, 5e3]);
|
|
|
|
% Integral Force Feedback
|
|
% The force sensor stack can be used to damp the system.
|
|
% This makes the system easier to excite properly without too much amplification near resonances.
|
|
|
|
% This is done thanks to the integral force feedback control architecture.
|
|
|
|
% The force sensor stack signal is integrated (or rather low pass filtered) and fed back to the force sensor stacks.
|
|
|
|
% The low pass filter used as the controller is defined below:
|
|
|
|
Kiff = 102/(s + 2*pi*2);
|
|
|
|
|
|
|
|
% The integral force feedback control strategy is applied to the simscape model as well as to the real test bench.
|
|
|
|
|
|
%% Name of the Simulink File
|
|
mdl = 'sensor_fusion_test_bench_simscape';
|
|
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Fd'], 1, 'openinput'); io_i = io_i + 1; % External Vertical Force [N]
|
|
io(io_i) = linio([mdl, '/w'], 1, 'openinput'); io_i = io_i + 1; % Base Motion [m]
|
|
io(io_i) = linio([mdl, '/Va'], 1, 'openinput'); io_i = io_i + 1; % Actuator Voltage [V]
|
|
io(io_i) = linio([mdl, '/Interferometer'], 1, 'openoutput'); io_i = io_i + 1; % Vertical Displacement [m]
|
|
io(io_i) = linio([mdl, '/Voltage_Conditioner'], 1, 'output'); io_i = io_i + 1; % Force Sensor [V]
|
|
|
|
options = linearizeOptions('SampleTime', 1e-4);
|
|
G_cl = linearize(mdl, io, options);
|
|
|
|
G_cl.InputName = {'Fd', 'w', 'Va'};
|
|
G_cl.OutputName = {'y', 'Vs'};
|
|
|
|
|
|
|
|
% The damped system is then identified again using a noise excitation.
|
|
|
|
% The data is loaded into Matlab and any offset is 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 transfer functions are estimated using =tfestimate=.
|
|
|
|
[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 dynamics from driving voltage to the measured displacement are compared both in the open-loop and IFF case, and for the test-bench experimental identification and for the Simscape model in Figure [[fig:iff_ol_cl_identified_simscape_comp]].
|
|
% This shows that the Integral Force Feedback architecture effectively damps the first resonance of the system.
|
|
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
set(gca, 'ColorOrderIndex', 1);
|
|
plot(f, abs(tf_G_ol_est), '-', 'DisplayName', 'OL - Ident.')
|
|
set(gca, 'ColorOrderIndex', 1);
|
|
plot(f, abs(squeeze(freqresp(G('y', 'Va'), f, 'Hz'))), '--', 'DisplayName', 'OL - Simscape')
|
|
set(gca, 'ColorOrderIndex', 2);
|
|
plot(f, abs(tf_G_cl_est), '-', 'DisplayName', 'CL - Ident.')
|
|
set(gca, 'ColorOrderIndex', 2);
|
|
plot(f, abs(squeeze(freqresp(G_cl('y', 'Va'), f, 'Hz'))), '--', 'DisplayName', 'CL - Simscape')
|
|
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'northeast');
|
|
hold off;
|
|
ylim([1e-7, 1e-3]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
set(gca, 'ColorOrderIndex', 1);
|
|
plot(f, 180/pi*angle(tf_G_ol_est), '-')
|
|
set(gca, 'ColorOrderIndex', 1);
|
|
plot(f, 180/pi*angle(squeeze(freqresp(G('y', 'Va'), f, 'Hz'))), '--')
|
|
set(gca, 'ColorOrderIndex', 2);
|
|
plot(f, 180/pi*angle(tf_G_cl_est), '-')
|
|
set(gca, 'ColorOrderIndex', 2);
|
|
plot(f, 180/pi*angle(squeeze(freqresp(G_cl('y', 'Va'), f, 'Hz'))), '--')
|
|
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, 5e3]);
|
|
|
|
% Inertial Sensors
|
|
% In order to estimate the dynamics of the inertial sensor (the transfer function from the "absolute" displacement to the measured voltage), the following experiment can be performed:
|
|
% - The mass is excited such that is relative displacement as measured by the interferometer is much larger that the ground "absolute" motion.
|
|
% - The transfer function from the measured displacement by the interferometer to the measured voltage generated by the inertial sensors can be estimated.
|
|
|
|
% The first point is quite important in order to have a good coherence between the interferometer measurement and the inertial sensor measurement.
|
|
|
|
% Here, a first identification is performed were the excitation signal is a white noise.
|
|
|
|
|
|
% As usual, the data is loaded and any offset is removed.
|
|
|
|
id = load('identification_noise_opt_iff.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
|
|
|
|
id.d = detrend(id.d, 0);
|
|
id.acc_1 = detrend(id.acc_1, 0);
|
|
id.acc_2 = detrend(id.acc_2, 0);
|
|
id.geo_1 = detrend(id.geo_1, 0);
|
|
id.geo_2 = detrend(id.geo_2, 0);
|
|
id.f_meas = detrend(id.f_meas, 0);
|
|
|
|
|
|
|
|
% Then the transfer functions from the measured displacement by the interferometer to the generated voltage of the inertial sensors are computed..
|
|
|
|
Ts = id.t(2) - id.t(1);
|
|
win = hann(ceil(10/Ts));
|
|
|
|
[tf_acc1_est, f] = tfestimate(id.d, id.acc_1, win, [], [], 1/Ts);
|
|
[co_acc1_est, ~] = mscohere( id.d, id.acc_1, win, [], [], 1/Ts);
|
|
[tf_acc2_est, ~] = tfestimate(id.d, id.acc_2, win, [], [], 1/Ts);
|
|
[co_acc2_est, ~] = mscohere( id.d, id.acc_2, win, [], [], 1/Ts);
|
|
|
|
[tf_geo1_est, ~] = tfestimate(id.d, id.geo_1, win, [], [], 1/Ts);
|
|
[co_geo1_est, ~] = mscohere( id.d, id.geo_1, win, [], [], 1/Ts);
|
|
[tf_geo2_est, ~] = tfestimate(id.d, id.geo_2, win, [], [], 1/Ts);
|
|
[co_geo2_est, ~] = mscohere( id.d, id.geo_2, win, [], [], 1/Ts);
|
|
|
|
|
|
|
|
% The same transfer functions are estimated using the Simscape model.
|
|
|
|
|
|
m = 10;
|
|
Kiff = tf(0);
|
|
|
|
%% Name of the Simulink File
|
|
mdl = 'sensor_fusion_test_bench_simscape';
|
|
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Va'], 1, 'openinput'); io_i = io_i + 1; % Actuator Voltage [V]
|
|
io(io_i) = linio([mdl, '/Interferometer'], 1, 'openoutput'); io_i = io_i + 1; % Vertical Displacement [m]
|
|
io(io_i) = linio([mdl, '/Vertical_Accelerometer_1'], 1, 'openoutput'); io_i = io_i + 1; % Accelerometer [V]
|
|
io(io_i) = linio([mdl, '/Voltage_Ampl_geo_1'], 1, 'openoutput'); io_i = io_i + 1; % Geophone [V]
|
|
|
|
options = linearizeOptions('SampleTime', 1e-4);
|
|
G = linearize(mdl, io, options);
|
|
|
|
G.InputName = {'Va'};
|
|
G.OutputName = {'y', 'a', 'v'};
|
|
|
|
G_acc = G('a', 'Va')*inv(G('y', 'Va')); % [V/m]
|
|
G_geo = G('v', 'Va')*inv(G('y', 'Va')); % [V/m]
|
|
|
|
|
|
|
|
% The obtained dynamics of the accelerometer are compared in Figure [[fig:comp_dynamics_accelerometer]] while the one of the geophones are compared in Figure [[fig:comp_dynamics_geophone]].
|
|
|
|
|
|
freqs = logspace(-1, 4, 1000)';
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f, abs(tf_acc1_est./(1i*2*pi*f).^2), '.')
|
|
plot(f, abs(tf_acc2_est./(1i*2*pi*f).^2), '.')
|
|
plot(freqs, abs(squeeze(freqresp(G_acc, freqs, 'Hz'))./(1i*2*pi*freqs).^2), 'k-')
|
|
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
|
ylabel('Amplitude $\left[\frac{V}{m/s^2}\right]$'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(f, 180/pi*angle(tf_acc1_est./(1i*2*pi*f).^2), '.')
|
|
plot(f, 180/pi*angle(tf_acc2_est./(1i*2*pi*f).^2), '.')
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_acc, freqs, 'Hz'))./(1i*2*pi*freqs).^2), '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([2, 2e3]);
|
|
|
|
|
|
|
|
% #+name: fig:comp_dynamics_accelerometer
|
|
% #+caption: Comparison of the measured accelerometer dynamics
|
|
% #+RESULTS:
|
|
% [[file:figs/comp_dynamics_accelerometer.png]]
|
|
|
|
|
|
freqs = logspace(-1, 4, 1000)';
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f, abs(tf_geo1_est./(1i*2*pi*f)), '.')
|
|
plot(f, abs(tf_geo2_est./(1i*2*pi*f)), '.')
|
|
plot(freqs, abs(squeeze(freqresp(G_geo, freqs, 'Hz'))./(1i*2*pi*freqs)), 'k-')
|
|
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
|
ylabel('Amplitude $\left[\frac{V}{m/s}\right]$'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(f, 180/pi*angle(tf_geo1_est./(1i*2*pi*f)), '.')
|
|
plot(f, 180/pi*angle(tf_geo2_est./(1i*2*pi*f)), '.')
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_geo, freqs, 'Hz'))./(1i*2*pi*freqs)), '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, 2e3]);
|