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

172 lines
5.5 KiB
Matlab

%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
addpath('./mat/');
% Transfer function from excitation signal to displacement
% Let's first estimate the transfer function from the excitation signal in [V] to the generated displacement in [m] as measured by the inteferometer.
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);
Ts = id_cl.t(2) - id_cl.t(1);
win = hann(ceil(10/Ts));
[tf_G_cl_est, f] = tfestimate(id_cl.u, id_cl.d, win, [], [], 1/Ts);
[co_G_cl_est, ~] = mscohere( id_cl.u, id_cl.d, win, [], [], 1/Ts);
% Approximate transfer function from voltage output to generated displacement when IFF is used, in [m/V].
G_d_est = -5e-6*(2*pi*230)^2/(s^2 + 2*0.3*2*pi*240*s + (2*pi*240)^2);
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(f, abs(tf_G_cl_est), '-')
plot(f, abs(squeeze(freqresp(G_d_est, f, 'Hz'))), '--')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(tf_G_cl_est), '-')
plot(f, 180/pi*angle(squeeze(freqresp(G_d_est, 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([10, 1000]);
% Motion measured during Huddle test
% We now compute the PSD of the measured motion by the inertial sensors during the huddle test.
ht = load('huddle_test.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
ht.d = detrend(ht.d, 0);
ht.acc_1 = detrend(ht.acc_1, 0);
ht.acc_2 = detrend(ht.acc_2, 0);
ht.geo_1 = detrend(ht.geo_1, 0);
ht.geo_2 = detrend(ht.geo_2, 0);
[p_d, f] = pwelch(ht.d, win, [], [], 1/Ts);
[p_acc1, ~] = pwelch(ht.acc_1, win, [], [], 1/Ts);
[p_acc2, ~] = pwelch(ht.acc_2, win, [], [], 1/Ts);
[p_geo1, ~] = pwelch(ht.geo_1, win, [], [], 1/Ts);
[p_geo2, ~] = pwelch(ht.geo_2, win, [], [], 1/Ts);
% Using an estimated model of the sensor dynamics from the documentation of the sensors, we can compute the ASD of the motion in $m/\sqrt{Hz}$ measured by the sensors.
G_acc = 1/(1 + s/2/pi/2500); % [V/(m/s2)]
G_geo = -120*s^2/(s^2 + 2*0.7*2*pi*2*s + (2*pi*2)^2); % [V/(m/s)]
figure;
hold on;
set(gca, 'ColorOrderIndex', 1);
plot(f, sqrt(p_acc1)./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))), ...
'DisplayName', 'Accelerometer');
set(gca, 'ColorOrderIndex', 1);
plot(f, sqrt(p_acc2)./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))), ...
'HandleVisibility', 'off');
set(gca, 'ColorOrderIndex', 2);
plot(f, sqrt(p_geo1)./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))), ...
'DisplayName', 'Geophone');
set(gca, 'ColorOrderIndex', 2);
plot(f, sqrt(p_geo2)./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))), ...
'HandleVisibility', 'off');
set(gca, 'ColorOrderIndex', 3);
plot(f, sqrt(p_d), 'DisplayName', 'Interferometer');
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('ASD [$m/\sqrt{Hz}$]'); xlabel('Frequency [Hz]');
title('Huddle Test')
legend();
% #+name: fig:huddle_test_psd_motion
% #+caption: ASD of the motion measured by the sensors
% #+RESULTS:
% [[file:figs/huddle_test_psd_motion.png]]
% From the ASD of the motion measured by the sensors, we can create an excitation signal that will generate much motion motion that the motion under no excitation.
% We create =G_exc= that corresponds to the wanted generated motion.
G_exc = 0.2e-6/(1 + s/2/pi/2)/(1 + s/2/pi/50);
% And we create a time domain signal =y_d= that have the spectral density described by =G_exc=.
Fs = 1/Ts;
t = 0:Ts:180; % Time Vector [s]
u = sqrt(Fs/2)*randn(length(t), 1); % Signal with an ASD equal to one
y_d = lsim(G_exc, u, t);
[pxx, ~] = pwelch(y_d, win, 0, [], Fs);
figure;
hold on;
set(gca, 'ColorOrderIndex', 1);
plot(f, sqrt(p_acc1)./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))), ...
'DisplayName', 'Accelerometer');
set(gca, 'ColorOrderIndex', 1);
plot(f, sqrt(p_acc2)./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))), ...
'HandleVisibility', 'off');
set(gca, 'ColorOrderIndex', 2);
plot(f, sqrt(p_geo1)./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))), ...
'DisplayName', 'Geophone');
set(gca, 'ColorOrderIndex', 2);
plot(f, sqrt(p_geo2)./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))), ...
'HandleVisibility', 'off');
set(gca, 'ColorOrderIndex', 3);
plot(f, sqrt(pxx), 'k-', ...
'DisplayName', 'Excitation');
plot(f, sqrt(p_d), 'DisplayName', 'Interferometer');
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('ASD [$m/\sqrt{Hz}$]'); xlabel('Frequency [Hz]');
title('Huddle Test')
legend();
% #+name: fig:comp_huddle_test_excited_motion_psd
% #+caption: Comparison of the ASD of the motion during Huddle and the wanted generated motion
% #+RESULTS:
% [[file:figs/comp_huddle_test_excited_motion_psd.png]]
% We can now generate the voltage signal that will generate the wanted motion.
y_v = lsim(G_exc * ... % from unit PSD to shaped PSD
(1 + s/2/pi/50) * ... % Inverse of pre-filter included in the Simulink file
1/G_d_est * ... % Wanted displacement => required voltage
1/(1 + s/2/pi/5e3), ... % Add some high frequency filtering
u, t);
figure;
plot(t, y_v)
xlabel('Time [s]'); ylabel('Voltage [V]');