tdehaeze/sensor-fusion-test-bench
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Reworked all computation / new CSS / export to PDF

Browse Source
This commit is contained in:
Thomas Dehaeze
2020-11-11 16:52:02 +01:00
parent 0bac7ef616
commit 364f138734
122 changed files with 72427 additions and 5888 deletions
Download Patch File Download Diff File Expand all files Collapse all files

423
matlab/first_identification.m Normal file
View File

@@ -0,0 +1,423 @@
%% 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]);

200
matlab/inertial_sensor_dynamics.m Normal file
View File

@@ -0,0 +1,200 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
addpath('./mat/');
% Load Data
% Both the measurement data during the identification test and during an "huddle test" are loaded.
id = load('identification_noise_opt_iff.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
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);
ht.f_meas = detrend(ht.f_meas, 0);
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);
% Compare PSD during Huddle and and during identification
% The Power Spectral Density of the measured motion during the huddle test and during the identification test are compared in Figures [[fig:comp_psd_huddle_test_identification_acc]] and [[fig:comp_psd_huddle_test_identification_geo]].
Ts = ht.t(2) - ht.t(1);
win = hann(ceil(10/Ts));
[p_id_d, f] = pwelch(id.d, win, [], [], 1/Ts);
[p_id_acc1, ~] = pwelch(id.acc_1, win, [], [], 1/Ts);
[p_id_acc2, ~] = pwelch(id.acc_2, win, [], [], 1/Ts);
[p_id_geo1, ~] = pwelch(id.geo_1, win, [], [], 1/Ts);
[p_id_geo2, ~] = pwelch(id.geo_2, win, [], [], 1/Ts);
[p_ht_d, ~] = pwelch(ht.d, win, [], [], 1/Ts);
[p_ht_acc1, ~] = pwelch(ht.acc_1, win, [], [], 1/Ts);
[p_ht_acc2, ~] = pwelch(ht.acc_2, win, [], [], 1/Ts);
[p_ht_geo1, ~] = pwelch(ht.geo_1, win, [], [], 1/Ts);
[p_ht_geo2, ~] = pwelch(ht.geo_2, win, [], [], 1/Ts);
[p_ht_fmeas, ~] = pwelch(ht.f_meas, win, [], [], 1/Ts);
figure;
hold on;
set(gca, 'ColorOrderIndex', 1);
plot(f, p_ht_acc1, 'DisplayName', 'Huddle Test');
set(gca, 'ColorOrderIndex', 1);
plot(f, p_ht_acc2, 'HandleVisibility', 'off');
set(gca, 'ColorOrderIndex', 2);
plot(f, p_id_acc1, 'DisplayName', 'Identification Test');
set(gca, 'ColorOrderIndex', 2);
plot(f, p_id_acc2, 'HandleVisibility', 'off');
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('PSD [$V^2/Hz$]'); xlabel('Frequency [Hz]');
title('Huddle Test - Accelerometers')
legend('location', 'northwest');
xlim([5e-1, 5e3]); ylim([1e-10, 1e-2])
% #+name: fig:comp_psd_huddle_test_identification_acc
% #+caption: Comparison of the PSD of the measured motion during the Huddle test and during the identification
% #+RESULTS:
% [[file:figs/comp_psd_huddle_test_identification_acc.png]]
figure;
hold on;
set(gca, 'ColorOrderIndex', 1);
plot(f, p_ht_geo1, 'DisplayName', 'Huddle Test');
set(gca, 'ColorOrderIndex', 1);
plot(f, p_ht_geo2, 'HandleVisibility', 'off');
set(gca, 'ColorOrderIndex', 2);
plot(f, p_id_geo1, 'DisplayName', 'Identification Test');
set(gca, 'ColorOrderIndex', 2);
plot(f, p_id_geo2, 'HandleVisibility', 'off');
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('PSD [$V^2/Hz$]'); xlabel('Frequency [Hz]');
title('Huddle Test - Geophones')
legend('location', 'northeast');
xlim([1e-1, 5e3]); ylim([1e-11, 1e-4]);
% Compute transfer functions
% The transfer functions from the motion as measured by the interferometer (and that should represent the absolute motion of the mass) to the inertial sensors are estimated:
[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 obtained coherence are shown in Figure [[fig:id_sensor_dynamics_coherence]].
figure;
hold on;
set(gca, 'ColorOrderIndex', 1);
plot(f, co_acc1_est, '-', 'DisplayName', 'Accelerometer')
set(gca, 'ColorOrderIndex', 1);
plot(f, co_acc2_est, '-', 'HandleVisibility', 'off')
set(gca, 'ColorOrderIndex', 2);
plot(f, co_geo1_est, '-', 'DisplayName', 'Geophone')
set(gca, 'ColorOrderIndex', 2);
plot(f, co_geo2_est, '-', 'HandleVisibility', 'off')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Coherence'); xlabel('Frequency [Hz]');
hold off;
xlim([2, 2e3]); ylim([0, 1])
legend();
% #+name: fig:id_sensor_dynamics_coherence
% #+caption: Coherence for the estimation of the sensor dynamics
% #+RESULTS:
% [[file:figs/id_sensor_dynamics_coherence.png]]
% We also make a simplified model of the inertial sensors to be compared with the identified dynamics.
G_acc = 1/(1 + s/2/pi/2500); % [V/(m/s2)]
G_geo = -1200*s^2/(s^2 + 2*0.7*2*pi*2*s + (2*pi*2)^2); % [[V/(m/s)]
% The model and identified dynamics show good agreement (Figures [[fig:id_sensor_dynamics_accelerometers]] and [[fig:id_sensor_dynamics_geophones]].)
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(f, abs(squeeze(freqresp(G_acc, f, 'Hz'))), '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(f, 180/pi*angle(squeeze(freqresp(G_acc, 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([2, 2e3]);
% #+name: fig:id_sensor_dynamics_accelerometers
% #+caption: Identified dynamics of the accelerometers
% #+RESULTS:
% [[file:figs/id_sensor_dynamics_accelerometers.png]]
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(f, abs(squeeze(freqresp(G_geo, f, 'Hz'))), '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(f, 180/pi*angle(squeeze(freqresp(G_geo, 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, 2e3]);

224
matlab/inertial_sensor_noise.m Normal file
View File

@@ -0,0 +1,224 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
addpath('./mat/');
% Load Data
% As before, the identification data is loaded and any offset if 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);
% ASD of the Measured displacement
% The Power Spectral Density of the displacement as measured by the interferometer and the inertial sensors is computed.
Ts = id.t(2) - id.t(1);
win = hann(ceil(10/Ts));
[p_id_d, f] = pwelch(id.d, win, [], [], 1/Ts);
[p_id_acc1, ~] = pwelch(id.acc_1, win, [], [], 1/Ts);
[p_id_acc2, ~] = pwelch(id.acc_2, win, [], [], 1/Ts);
[p_id_geo1, ~] = pwelch(id.geo_1, win, [], [], 1/Ts);
[p_id_geo2, ~] = pwelch(id.geo_2, win, [], [], 1/Ts);
% Let's use a model of the accelerometer and geophone to compute the motion from the measured voltage.
G_acc = 1/(1 + s/2/pi/2500); % [V/(m/s2)]
G_geo = -1200*s^2/(s^2 + 2*0.7*2*pi*2*s + (2*pi*2)^2); % [[V/(m/s)]
% The obtained ASD in $m/\sqrt{Hz}$ is shown in Figure [[fig:measure_displacement_all_sensors]].
figure;
hold on;
set(gca, 'ColorOrderIndex', 1);
plot(f, sqrt(p_id_acc1)./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))), ...
'DisplayName', 'Accelerometer');
set(gca, 'ColorOrderIndex', 1);
plot(f, sqrt(p_id_acc2)./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))), ...
'HandleVisibility', 'off');
set(gca, 'ColorOrderIndex', 2);
plot(f, sqrt(p_id_geo1)./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))), ...
'DisplayName', 'Geophone');
set(gca, 'ColorOrderIndex', 2);
plot(f, sqrt(p_id_geo2)./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))), ...
'HandleVisibility', 'off');
set(gca, 'ColorOrderIndex', 3);
plot(f, sqrt(p_id_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();
xlim([1e-1, 5e3]); ylim([1e-12, 1e-4]);
% ASD of the Sensor Noise
% The noise of a sensor can be estimated using two identical sensors by computing:
% - the Power Spectral Density of the measured motion by the two sensors
% - the Cross Spectral Density between the two sensors (coherence)
% This technique to estimate the sensor noise is described in cite:barzilai98_techn_measur_noise_sensor_presen.
% The Power Spectral Density of the sensor noise can be estimated using the following equation:
% \begin{equation}
% |S_n(\omega)| = |S_{x_1}(\omega)| \Big( 1 - \gamma_{x_1 x_2}(\omega) \Big)
% \end{equation}
% with $S_{x_1}$ the PSD of one of the sensor and $\gamma_{x_1 x_2}$ the coherence between the two sensors.
% The coherence between the two accelerometers and between the two geophones is computed.
[coh_acc, ~] = mscohere(id.acc_1, id.acc_2, win, [], [], 1/Ts);
[coh_geo, ~] = mscohere(id.geo_1, id.geo_2, win, [], [], 1/Ts);
% Finally, the Power Spectral Density of the sensors is computed and converted in $[m^2/Hz]$.
pN_acc = p_id_acc1.*(1 - coh_acc) .* ... % [V^2/Hz]
1./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))).^2; % [(m/V)^2]
pN_geo = p_id_geo1.*(1 - coh_geo) .* ... % [V^2/Hz]
1./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))).^2; % [(m/V)^2]
% The ASD of obtained noises are compared with the ASD of the measured signals in Figure [[fig:noise_inertial_sensors_comparison]].
figure;
hold on;
plot(f, sqrt(p_id_acc1)./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))), ...
'DisplayName', 'Accelerometer');
plot(f, sqrt(p_id_geo1)./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))), ...
'DisplayName', 'Geophone');
plot(f, sqrt(pN_acc), '-', 'DisplayName', 'Accelerometers - Noise');
plot(f, sqrt(pN_geo), '-', 'DisplayName', 'Geophones - Noise');
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{m}{\sqrt{Hz}}\right]$');
xlim([1, 5000]); ylim([1e-12, 1e-5]);
legend('location', 'northeast');
% Noise Model
% Transfer functions are adjusted in order to fit the ASD of the sensor noises (expressed in $[m/s/\sqrt{Hz}]$ for more easy fitting).
% These transfer functions are defined below and compared with the measured ASD in Figure [[fig:noise_models_velocity]].
N_acc = 1*(s/(2*pi*2000) + 1)^2/(s + 0.1*2*pi)/(s + 1e3*2*pi); % [m/sqrt(Hz)]
N_geo = 4e-4*(s/(2*pi*200) + 1)/(s + 1e3*2*pi); % [m/sqrt(Hz)]
freqs = logspace(0, 4, 1000);
figure;
hold on;
plot(f, sqrt(pN_acc).*(2*pi*f), '-', 'DisplayName', 'Accelerometers - Noise');
plot(f, sqrt(pN_geo).*(2*pi*f), '-', 'DisplayName', 'Geophones - Noise');
set(gca, 'ColorOrderIndex', 1);
plot(freqs, abs(squeeze(freqresp(N_acc, freqs, 'Hz'))), '--', 'DisplayName', 'Accelerometer - Noise Model');
plot(freqs, abs(squeeze(freqresp(N_geo, freqs, 'Hz'))), '--', 'DisplayName', 'Geophones - Noise Model');
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{m/s}{\sqrt{Hz}}\right]$');
xlim([1, 5000]);
legend('location', 'northeast');
% $\mathcal{H}_2$ Synthesis of the Complementary Filters
% We now wish to synthesize two complementary filters to merge the geophone and the accelerometer signal in such a way that the fused signal has the lowest possible RMS noise.
% To do so, we use the $\mathcal{H}_2$ synthesis where the transfer functions representing the noise density of both sensors are used as weights.
% The generalized plant used for the synthesis is defined below.
P = [0 N_acc 1;
N_geo -N_acc 0];
% And the $\mathcal{H}_2$ synthesis is done using the =h2syn= command.
[H_geo, ~, gamma] = h2syn(P, 1, 1);
H_acc = 1 - H_geo;
% The obtained complementary filters are shown in Figure [[fig:complementary_filters_velocity_H2]].
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(H_acc, freqs, 'Hz'))), '-', 'DisplayName', '$H_{acc}$');
plot(freqs, abs(squeeze(freqresp(H_geo, freqs, 'Hz'))), '-', 'DisplayName', '$H_{geo}$');
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Magnitude'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'northeast');
ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(H_acc, freqs, 'Hz'))), '-');
plot(freqs, 180/pi*angle(squeeze(freqresp(H_geo, freqs, '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([freqs(1), freqs(end)]);
% Results
% Finally, the signals of both sensors are merged using the complementary filters and the super sensor noise is estimated and compared with the individual sensor noises in Figure [[fig:super_sensor_noise_asd_velocity]].
freqs = logspace(0, 4, 1000);
figure;
hold on;
plot(f, pN_acc.*(2*pi*f), '-', 'DisplayName', 'Accelerometers - Noise');
plot(f, pN_geo.*(2*pi*f), '-', 'DisplayName', 'Geophones - Noise');
plot(f, sqrt((pN_acc.*(2*pi*f)).^2.*abs(squeeze(freqresp(H_acc, f, 'Hz'))).^2 + (pN_geo.*(2*pi*f)).^2.*abs(squeeze(freqresp(H_geo, f, 'Hz'))).^2), 'k-', 'DisplayName', 'Super Sensor - Noise');
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{m/s}{\sqrt{Hz}}\right]$');
xlim([1, 5000]);
legend('location', 'northeast');
% #+name: fig:super_sensor_noise_asd_velocity
% #+caption: ASD of the super sensor noise (velocity)
% #+RESULTS:
% [[file:figs/super_sensor_noise_asd_velocity.png]]
% Finally, the Cumulative Power Spectrum is computed and compared in Figure [[fig:super_sensor_noise_cas_velocity]].
[~, i_1Hz] = min(abs(f - 1));
CPS_acc = 1/pi*flip(-cumtrapz(2*pi*flip(f), flip((pN_acc.*(2*pi*f)).^2)));
CPS_geo = 1/pi*flip(-cumtrapz(2*pi*flip(f), flip((pN_geo.*(2*pi*f)).^2)));
CPS_SS = 1/pi*flip(-cumtrapz(2*pi*flip(f), flip((pN_acc.*(2*pi*f)).^2.*abs(squeeze(freqresp(H_acc, f, 'Hz'))).^2 + (pN_geo.*(2*pi*f)).^2.*abs(squeeze(freqresp(H_geo, f, 'Hz'))).^2)));
figure;
hold on;
plot(f, CPS_acc, '-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{acc}} = %.0f\\,\\mu m/s (rms)$', 1e6*sqrt(CPS_acc(i_1Hz))));
plot(f, CPS_geo, '-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{geo}} = %.0f\\,\\mu m/s (rms)$', 1e6*sqrt(CPS_geo(i_1Hz))));
plot(f, CPS_SS, 'k-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}} = %.0f\\,\\mu m/s (rms)$', 1e6*sqrt(CPS_SS(i_1Hz))));
set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum');
hold off;
xlim([1, 4e3]);
legend('location', 'northeast');

326
matlab/inertial_sensor_uncertainty.m Normal file
View File

@@ -0,0 +1,326 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
addpath('./mat/');
addpath('./src/');
% Load Data
% Data is loaded and 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);
% Compute the dynamics of both sensors
% The dynamics of inertial sensors are estimated (in $[V/m]$).
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 (nominal) models of the inertial sensors from the absolute displacement to the generated voltage are defined below:
G_acc = 1/(1 + s/2/pi/2000)
G_geo = -1200*s^2/(s^2 + 2*0.7*2*pi*2*s + (2*pi*2)^2);
% Dynamics uncertainty estimation
% Weights representing the dynamical uncertainty of the sensors are defined below.
w_acc = createWeight('n', 2, 'G0', 10, 'G1', 0.2, 'Gc', 1, 'w0', 6*2*pi) * ...
createWeight('n', 2, 'G0', 1, 'G1', 5/0.2, 'Gc', 1/0.2, 'w0', 1300*2*pi);
w_geo = createWeight('n', 2, 'G0', 0.6, 'G1', 0.2, 'Gc', 0.3, 'w0', 3*2*pi) * ...
createWeight('n', 2, 'G0', 1, 'G1', 10/0.2, 'Gc', 1/0.2, 'w0', 800*2*pi);
% The measured dynamics are compared with the modelled one as well as the modelled uncertainty in Figure [[fig:dyn_uncertainty_acc]] for the accelerometers and in Figure [[fig:dyn_uncertainty_geo]] for the geophones.
figure;
tiledlayout(2, 1, 'TileSpacing', 'None', 'Padding', 'None');
% Magnitude
ax1 = nexttile;
hold on;
plotMagUncertainty(w_acc, freqs, 'G', G_acc, 'color_i', 1, 'DisplayName', '$G_{acc}$');
set(gca,'ColorOrderIndex',1)
plot(f, abs(tf_acc1_est./(1i*2*pi*f).^2), '.', 'DisplayName', 'Meaurement')
set(gca,'ColorOrderIndex',1)
plot(f, abs(tf_acc2_est./(1i*2*pi*f).^2), '.', 'HandleVisibility', 'off')
set(gca,'ColorOrderIndex',1)
plot(freqs, abs(squeeze(freqresp(G_acc, freqs, 'Hz'))), 'DisplayName', '$\hat{G}_{acc}$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude $[\frac{V}{m}]$');
legend('location', 'southwest', 'FontSize', 8);
hold off;
ylim([1e-3, 1e1])
% Phase
ax2 = nexttile;
hold on;
plotPhaseUncertainty(w_acc, freqs, 'G', G_acc, 'color_i', 1);
set(gca,'ColorOrderIndex',1)
plot(f, 180/pi*angle(tf_acc1_est./(1i*2*pi*f).^2), '.');
set(gca,'ColorOrderIndex',1)
plot(f, 180/pi*angle(tf_acc2_est./(1i*2*pi*f).^2), '.');
set(gca,'ColorOrderIndex',1)
plot(freqs, 180/pi*angle(squeeze(freqresp(G_acc, freqs, 'Hz'))));
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
linkaxes([ax1,ax2],'x');
xlim([1, 5e3]);
% #+name: fig:dyn_uncertainty_acc
% #+caption: Modeled dynamical uncertainty and meaured dynamics of the accelerometers
% #+RESULTS:
% [[file:figs/dyn_uncertainty_acc.png]]
figure;
tiledlayout(2, 1, 'TileSpacing', 'None', 'Padding', 'None');
% Magnitude
ax1 = nexttile;
hold on;
plotMagUncertainty(w_geo, freqs, 'G', G_geo, 'color_i', 2, 'DisplayName', '$G_{geo}$');
set(gca,'ColorOrderIndex',2)
plot(f, abs(tf_geo1_est./(1i*2*pi*f)), '.', 'DisplayName', 'Measurement')
set(gca,'ColorOrderIndex',2)
plot(f, abs(tf_geo2_est./(1i*2*pi*f)), '.', 'HandleVisibility', 'off')
set(gca,'ColorOrderIndex',2)
plot(freqs, abs(squeeze(freqresp(G_geo, freqs, 'Hz'))), 'DisplayName', '$\hat{G}_{geo}$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude $[\frac{V}{m}]$');
legend('location', 'northwest', 'FontSize', 8);
hold off;
ylim([1, 1e4])
% Phase
ax2 = nexttile;
hold on;
plotPhaseUncertainty(w_geo, freqs, 'G', G_geo, 'color_i', 2);
set(gca,'ColorOrderIndex',2)
plot(f, 180/pi*unwrap(angle(tf_geo1_est./(1i*2*pi*f)))+360, '.');
set(gca,'ColorOrderIndex',2)
plot(f, 180/pi*unwrap(angle(tf_geo2_est./(1i*2*pi*f))), '.');
set(gca,'ColorOrderIndex',2)
plot(freqs, 180/pi*angle(squeeze(freqresp(G_geo, freqs, 'Hz'))));
set(gca,'xscale','log');
yticks(-270:90:180);
ylim([-270 90]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
linkaxes([ax1,ax2],'x');
xlim([1, 5e3]);
% $\mathcal{H}_\infty$ Synthesis of Complementary Filters
% A last weight is now defined that represents the maximum dynamical uncertainty that is allowed for the super sensor.
wu = inv(createWeight('n', 2, 'G0', 0.7, 'G1', 0.3, 'Gc', 0.4, 'w0', 3*2*pi) * ...
createWeight('n', 2, 'G0', 1, 'G1', 6/0.3, 'Gc', 1/0.3, 'w0', 1200*2*pi));
% This dynamical uncertainty is compared with the two sensor uncertainties in Figure [[fig:uncertainty_weight_and_sensor_uncertainties]].
Dphi_Wu = 180/pi*asin(abs(squeeze(freqresp(inv(wu), freqs, 'Hz'))));
Dphi_Wu(abs(squeeze(freqresp(inv(wu), freqs, 'Hz'))) > 1) = 360;
figure;
tiledlayout(2, 1, 'TileSpacing', 'None', 'Padding', 'None');
% Magnitude
ax1 = nexttile;
hold on;
plotMagUncertainty(w_acc, freqs, 'color_i', 1, 'DisplayName', '$1 + W_{acc} \Delta$');
plotMagUncertainty(w_geo, freqs, 'color_i', 2, 'DisplayName', '$1 + W_{geo} \Delta$');
plot(freqs, 1 + abs(squeeze(freqresp(inv(wu), freqs, 'Hz'))), 'k--', ...
'DisplayName', '$1 + W_u^{-1} \Delta$')
plot(freqs, 1 - abs(squeeze(freqresp(inv(wu), freqs, 'Hz'))), 'k--', ...
'HandleVisibility', 'off')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude');
ylim([1e-2, 1e1]);
legend('location', 'southeast', 'FontSize', 8);
hold off;
% Phase
ax2 = nexttile;
hold on;
plotPhaseUncertainty(w_acc, freqs, 'color_i', 1);
plotPhaseUncertainty(w_geo, freqs, 'color_i', 2);
plot(freqs, Dphi_Wu, 'k--');
plot(freqs, -Dphi_Wu, 'k--');
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
% #+name: fig:uncertainty_weight_and_sensor_uncertainties
% #+caption: Individual sensor uncertainty (normalized by their dynamics) and the wanted maximum super sensor noise uncertainty
% #+RESULTS:
% [[file:figs/uncertainty_weight_and_sensor_uncertainties.png]]
% The generalized plant used for the synthesis is defined:
P = [wu*w_acc -wu*w_acc;
0 wu*w_geo;
1 0];
% And the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command is performed.
[H_geo, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
% #+RESULTS:
% #+begin_example
% Test bounds: 0.8556 <= gamma <= 1.34
% gamma X>=0 Y>=0 rho(XY)<1 p/f
% 1.071e+00 0.0e+00 0.0e+00 0.000e+00 p
% 9.571e-01 0.0e+00 0.0e+00 9.436e-16 p
% 9.049e-01 0.0e+00 0.0e+00 1.084e-15 p
% 8.799e-01 0.0e+00 0.0e+00 1.191e-16 p
% 8.677e-01 0.0e+00 0.0e+00 6.905e-15 p
% 8.616e-01 0.0e+00 0.0e+00 0.000e+00 p
% 8.586e-01 1.1e-17 0.0e+00 6.917e-16 p
% 8.571e-01 0.0e+00 0.0e+00 6.991e-17 p
% 8.564e-01 0.0e+00 0.0e+00 1.492e-16 p
% Best performance (actual): 0.8563
% #+end_example
% The complementary filter is defined as follows:
H_acc = 1 - H_geo;
% The bode plot of the obtained complementary filters is shown in Figure
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
% Magnitude
ax1 = nexttile([2,1]);
hold on;
set(gca,'ColorOrderIndex',1)
plot(freqs, 1./abs(squeeze(freqresp(w_geo, freqs, 'Hz'))), '--', 'DisplayName', '$w_{geo}$');
set(gca,'ColorOrderIndex',2)
plot(freqs, 1./abs(squeeze(freqresp(w_acc, freqs, 'Hz'))), '--', 'DisplayName', '$w_{acc}$');
set(gca,'ColorOrderIndex',1)
plot(freqs, abs(squeeze(freqresp(H_geo, freqs, 'Hz'))), '-', 'DisplayName', '$H_{geo}$');
set(gca,'ColorOrderIndex',2)
plot(freqs, abs(squeeze(freqresp(H_acc, freqs, 'Hz'))), '-', 'DisplayName', '$H_{acc}$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude');
set(gca, 'XTickLabel',[]);
ylim([1e-2, 1e1]);
legend('location', 'southeast');
ax2 = nexttile;
hold on;
set(gca,'ColorOrderIndex',1)
plot(freqs, 180/pi*phase(squeeze(freqresp(H_geo, freqs, 'Hz'))), '-');
set(gca,'ColorOrderIndex',2)
plot(freqs, 180/pi*phase(squeeze(freqresp(H_acc, freqs, 'Hz'))), '-');
hold off;
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
set(gca, 'XScale', 'log');
yticks([-360:90:360]);
linkaxes([ax1,ax2],'x');
xlim([1, 1e3]);
% Obtained Super Sensor Dynamical Uncertainty
% The obtained super sensor dynamical uncertainty is shown in Figure [[fig:super_sensor_uncertainty_h_infinity]].
Dphi_Wu = 180/pi*asin(abs(squeeze(freqresp(inv(wu), freqs, 'Hz'))));
Dphi_Wu(abs(squeeze(freqresp(inv(wu), freqs, 'Hz'))) > 1) = 360;
Dphi_ss = 180/pi*asin(abs(squeeze(freqresp(w_geo*H_geo, freqs, 'Hz'))) + abs(squeeze(freqresp(w_acc*H_acc, freqs, 'Hz'))));
Dphi_ss(abs(squeeze(freqresp(w_geo*H_geo, freqs, 'Hz'))) + abs(squeeze(freqresp(w_acc*H_acc, freqs, 'Hz'))) > 1) = 360;
figure;
tiledlayout(2, 1, 'TileSpacing', 'None', 'Padding', 'None');
% Magnitude
ax1 = nexttile;
hold on;
plotMagUncertainty(w_acc, freqs, 'color_i', 1, 'DisplayName', '$1 + W_1 \Delta_1$');
plotMagUncertainty(w_geo, freqs, 'color_i', 2, 'DisplayName', '$1 + W_2 \Delta_2$');
plot(freqs, 1 + abs(squeeze(freqresp(w_geo*H_geo, freqs, 'Hz')))+abs(squeeze(freqresp(w_acc*H_acc, freqs, 'Hz'))), 'k-', ...
'DisplayName', '$1 + W_1 \Delta_1 + W_2 \Delta_2$')
plot(freqs, max(1 - abs(squeeze(freqresp(w_geo*H_geo, freqs, 'Hz')))-abs(squeeze(freqresp(w_acc*H_acc, freqs, 'Hz'))), 0.001), 'k-', ...
'HandleVisibility', 'off');
plot(freqs, 1 + abs(squeeze(freqresp(inv(wu), freqs, 'Hz'))), 'k--', ...
'DisplayName', '$1 + W_u^{-1}\Delta$')
plot(freqs, 1 - abs(squeeze(freqresp(inv(wu), freqs, 'Hz'))), 'k--', ...
'HandleVisibility', 'off')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude');
ylim([1e-2, 1e1]);
legend('location', 'southeast', 'FontSize', 8);
hold off;
% Phase
ax2 = nexttile;
hold on;
plotPhaseUncertainty(w_acc, freqs, 'color_i', 1);
plotPhaseUncertainty(w_geo, freqs, 'color_i', 2);
plot(freqs, Dphi_ss, 'k-');
plot(freqs, -Dphi_ss, 'k-');
plot(freqs, Dphi_Wu, 'k--');
plot(freqs, -Dphi_Wu, 'k--');
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);

226
matlab/integral_force_feedback.m Normal file
View File

@@ -0,0 +1,226 @@
%% 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]);

1870
matlab/mat/APA95ML.step Normal file
View File

File diff suppressed because it is too large Load Diff

BIN
matlab/mat/APA95ML_simplified_model.mat Normal file
View File

Binary file not shown.

BIN
matlab/mat/huddle_test.mat Normal file
View File

Binary file not shown.

BIN
matlab/mat/huddle_test_bis.mat Normal file
View File

Binary file not shown.

BIN
matlab/mat/identification_chirp_01_1000_iff.mat Normal file
View File

Binary file not shown.

BIN
matlab/mat/identification_chirp_02_2000_iff.mat Normal file
View File

Binary file not shown.

BIN
matlab/mat/identification_chirp_40_400.mat Normal file
View File

Binary file not shown.

BIN
matlab/mat/identification_chirp_40_400_bis.mat Normal file
View File

Binary file not shown.

BIN
matlab/mat/identification_chirp_40_400_iff.mat Normal file
View File

Binary file not shown.

BIN
matlab/mat/identification_noise.mat Normal file
View File

Binary file not shown.

BIN
matlab/mat/identification_noise_bis.mat Normal file
View File

Binary file not shown.

BIN
matlab/mat/identification_noise_iff.mat Normal file
View File

Binary file not shown.

BIN
matlab/mat/identification_noise_iff_bis.mat Normal file
View File

Binary file not shown.

BIN
matlab/mat/identification_noise_opt_iff.mat Normal file
View File

Binary file not shown.

BIN
matlab/mat/piezo_amplified_3d.mat Normal file
View File

Binary file not shown.

BIN
matlab/mat/sensor_dynamics.mat Normal file
View File

Binary file not shown.

BIN
matlab/mat/sensor_noises.mat Normal file
View File

Binary file not shown.

171
matlab/optimal_excitation.m Normal file
View File

@@ -0,0 +1,171 @@
%% 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]');

193
matlab/optimal_sensor_fusion.m Normal file
View File

@@ -0,0 +1,193 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
addpath('./mat/');
addpath('./src/');
% Noise and Dynamical uncertainty weights
N_acc = (s/(2*pi*2000) + 1)^2/(s + 0.1*2*pi)/(s + 1e3*2*pi)/(1 + s/2/pi/1e3); % [m/sqrt(Hz)]
N_geo = 4e-4*((s + 2*pi)/(2*pi*200) + 1)/(s + 1e3*2*pi)/(1 + s/2/pi/1e3); % [m/sqrt(Hz)]
w_acc = createWeight('n', 2, 'G0', 10, 'G1', 0.2, 'Gc', 1, 'w0', 6*2*pi) * ...
createWeight('n', 2, 'G0', 1, 'G1', 5/0.2, 'Gc', 1/0.2, 'w0', 1300*2*pi);
w_geo = createWeight('n', 2, 'G0', 0.6, 'G1', 0.2, 'Gc', 0.3, 'w0', 3*2*pi) * ...
createWeight('n', 2, 'G0', 1, 'G1', 10/0.2, 'Gc', 1/0.2, 'w0', 800*2*pi);
wu = inv(createWeight('n', 2, 'G0', 0.7, 'G1', 0.3, 'Gc', 0.4, 'w0', 3*2*pi) * ...
createWeight('n', 2, 'G0', 1, 'G1', 6/0.3, 'Gc', 1/0.3, 'w0', 1200*2*pi));
P = [wu*w_acc -wu*w_acc;
0 wu*w_geo;
N_acc -N_acc;
0 N_geo;
1 0];
% And the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis is performed.
[H_geo, ~] = h2hinfsyn(ss(P), 1, 1, 2, [0, 1], 'HINFMAX', 1, 'H2MAX', Inf, 'DKMAX', 100, 'TOL', 1e-3, 'DISPLAY', 'on');
H_acc = 1 - H_geo;
% Obtained Super Sensor Noise
freqs = logspace(0, 4, 1000);
PSD_Sgeo = abs(squeeze(freqresp(N_geo, freqs, 'Hz'))).^2;
PSD_Sacc = abs(squeeze(freqresp(N_acc, freqs, 'Hz'))).^2;
PSD_Hss = abs(squeeze(freqresp(N_acc*H_acc, freqs, 'Hz'))).^2 + ...
abs(squeeze(freqresp(N_geo*H_geo, freqs, 'Hz'))).^2;
figure;
hold on;
plot(freqs, sqrt(PSD_Sacc), '-', 'DisplayName', '$\Phi_{n_{acc}}$');
plot(freqs, sqrt(PSD_Sgeo), '-', 'DisplayName', '$\Phi_{n_{geo}}$');
plot(freqs, sqrt(PSD_Hss), 'k-.', 'DisplayName', '$\Phi_{n_{\mathcal{H}_2/\mathcal{H}_\infty}}$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD $\left[ \frac{m/s}{\sqrt{Hz}} \right]$');
hold off;
xlim([freqs(1), freqs(end)]);
legend('location', 'northeast', 'FontSize', 8);
% Obtained Super Sensor Dynamical Uncertainty
Dphi_wu = 180/pi*asin(abs(squeeze(freqresp(inv(wu), freqs, 'Hz'))));
Dphi_wu(abs(squeeze(freqresp(inv(wu), freqs, 'Hz'))) > 1) = 360;
Dphi_ss = 180/pi*asin(abs(squeeze(freqresp(w_geo*H_geo, freqs, 'Hz'))) + abs(squeeze(freqresp(w_acc*H_acc, freqs, 'Hz'))));
Dphi_ss(abs(squeeze(freqresp(w_geo*H_geo, freqs, 'Hz'))) + abs(squeeze(freqresp(w_acc*H_acc, freqs, 'Hz'))) > 1) = 360;
figure;
tiledlayout(2, 1, 'TileSpacing', 'None', 'Padding', 'None');
% Magnitude
ax1 = nexttile;
hold on;
plotMagUncertainty(w_acc, freqs, 'color_i', 1, 'DisplayName', '$1 + W_1 \Delta_1$');
plotMagUncertainty(w_geo, freqs, 'color_i', 2, 'DisplayName', '$1 + W_2 \Delta_2$');
plot(freqs, 1 + abs(squeeze(freqresp(w_geo*H_geo, freqs, 'Hz')))+abs(squeeze(freqresp(w_acc*H_acc, freqs, 'Hz'))), 'k-', ...
'DisplayName', '$1 + W_1 \Delta_1 + W_2 \Delta_2$')
plot(freqs, max(1 - abs(squeeze(freqresp(w_geo*H_geo, freqs, 'Hz')))-abs(squeeze(freqresp(w_acc*H_acc, freqs, 'Hz'))), 0.001), 'k-', ...
'HandleVisibility', 'off');
plot(freqs, 1 + abs(squeeze(freqresp(inv(wu), freqs, 'Hz'))), 'k--', ...
'DisplayName', '$1 + W_u^{-1}\Delta$')
plot(freqs, 1 - abs(squeeze(freqresp(inv(wu), freqs, 'Hz'))), 'k--', ...
'HandleVisibility', 'off')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude');
ylim([1e-2, 1e1]);
legend('location', 'southeast', 'FontSize', 8);
hold off;
% Phase
ax2 = nexttile;
hold on;
plotPhaseUncertainty(w_acc, freqs, 'color_i', 1);
plotPhaseUncertainty(w_geo, freqs, 'color_i', 2);
plot(freqs, Dphi_ss, 'k-');
plot(freqs, -Dphi_ss, 'k-');
plot(freqs, Dphi_wu, 'k--');
plot(freqs, -Dphi_wu, 'k--');
set(gca,'xscale','log');
ylim([-180 180]); yticks(-180:90:180);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
% Experimental Super Sensor Dynamical Uncertainty
load('./matlab/mat/sensor_dynamics.mat', 'tf_acc1_est', 'tf_acc2_est', 'tf_geo1_est', 'tf_geo2_est', 'f');
G_acc = s^2/(1 + s/2/pi/2000) % [V/m]
G_geo = -1200*s^3/(s^2 + 2*0.7*2*pi*2*s + (2*pi*2)^2); % [V/m]
% The super sensor dynamics is shown in Figure [[fig:super_sensor_optimal_uncertainty]].
figure;
tiledlayout(2, 1, 'TileSpacing', 'None', 'Padding', 'None');
% Magnitude
ax1 = nexttile;
hold on;
plotMagUncertainty(w_acc, freqs, 'color_i', 1, 'DisplayName', '$G_{acc}$');
set(gca,'ColorOrderIndex',1)
plot(f, abs(tf_acc1_est./squeeze(freqresp(G_acc, f, 'Hz'))), '.', 'DisplayName', 'Meaurement')
set(gca,'ColorOrderIndex',1)
plot(f, abs(tf_acc2_est./squeeze(freqresp(G_acc, f, 'Hz'))), '.', 'HandleVisibility', 'off')
plotMagUncertainty(w_geo, freqs, 'color_i', 2, 'DisplayName', '$G_{geo}$');
set(gca,'ColorOrderIndex',2)
plot(f, abs(tf_geo1_est./squeeze(freqresp(G_geo, f, 'Hz'))), '.', 'DisplayName', 'Meaurement')
set(gca,'ColorOrderIndex',2)
plot(f, abs(tf_geo2_est./squeeze(freqresp(G_geo, f, 'Hz'))), '.', 'HandleVisibility', 'off')
plot(f, abs(tf_acc1_est.*squeeze(freqresp(inv(G_acc)*H_acc, f, 'Hz')) + ...
tf_geo1_est.*squeeze(freqresp(inv(G_geo)*H_geo, f, 'Hz'))), 'k.', 'DisplayName', 'ss')
plot(f, abs(tf_acc2_est.*squeeze(freqresp(inv(G_acc)*H_acc, f, 'Hz')) + ...
tf_geo2_est.*squeeze(freqresp(inv(G_geo)*H_geo, f, 'Hz'))), 'k.', 'HandleVisibility', 'off')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude $[\frac{V}{m}]$');
legend('location', 'southwest', 'FontSize', 8);
hold off;
ylim([1e-3, 1e1])
% Phase
ax2 = nexttile;
hold on;
plotPhaseUncertainty(w_acc, freqs, 'color_i', 1);
set(gca,'ColorOrderIndex',1)
plot(f, 180/pi*angle(tf_acc1_est./squeeze(freqresp(G_acc, f, 'Hz'))), '.');
set(gca,'ColorOrderIndex',1)
plot(f, 180/pi*angle(tf_acc2_est./squeeze(freqresp(G_acc, f, 'Hz'))), '.');
plotPhaseUncertainty(w_geo, freqs, 'color_i', 2);
set(gca,'ColorOrderIndex',2)
plot(f, 180/pi*angle(tf_geo1_est./squeeze(freqresp(G_geo, f, 'Hz'))), '.');
set(gca,'ColorOrderIndex',2)
plot(f, 180/pi*angle(tf_geo2_est./squeeze(freqresp(G_geo, f, 'Hz'))), '.');
plot(f, 180/pi*angle(tf_acc1_est.*squeeze(freqresp(inv(G_acc)*H_acc, f, 'Hz')) + ...
tf_geo1_est.*squeeze(freqresp(inv(G_geo)*H_geo, f, 'Hz'))), 'k.')
plot(f, 180/pi*angle(tf_acc2_est.*squeeze(freqresp(inv(G_acc)*H_acc, f, 'Hz')) + ...
tf_geo2_est.*squeeze(freqresp(inv(G_geo)*H_geo, f, 'Hz'))), 'k.')
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
linkaxes([ax1,ax2],'x');
xlim([1, 5e3]);
% Experimental Super Sensor Noise
load('./matlab/mat/sensor_noises.mat', 'pN_acc', 'pN_geo', 'N_acc', 'N_geo', 'f')
% The obtained super sensor noise is shown in Figure [[fig:super_sensor_optimal_noise]].
freqs = logspace(0, 4, 1000);
figure;
hold on;
plot(f, pN_acc.*(2*pi*f), '-', 'DisplayName', 'Accelerometers - Noise');
plot(f, pN_geo.*(2*pi*f), '-', 'DisplayName', 'Geophones - Noise');
plot(f, sqrt((pN_acc.*(2*pi*f)).^2.*abs(squeeze(freqresp(H_acc, f, 'Hz'))).^2 + (pN_geo.*(2*pi*f)).^2.*abs(squeeze(freqresp(H_geo, f, 'Hz'))).^2), 'k-', 'DisplayName', 'Super Sensor - Noise');
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{m/s}{\sqrt{Hz}}\right]$');
xlim([1, 5000]);
legend('location', 'northeast');

153
matlab/runtest.m Normal file
View File

@@ -0,0 +1,153 @@
tg = slrt;
%%
f = SimulinkRealTime.openFTP(tg);
mget(f, 'apa95ml.dat', 'data');
close(f);
%% Convert the Data
data = SimulinkRealTime.utils.getFileScopeData('data/apa95ml.dat').data;
d = data(:, 1); % Interferomter [m]
acc_1 = data(:, 2);
acc_2 = data(:, 3);
geo_1 = data(:, 4);
geo_2 = data(:, 5);
u = data(:, 6); % Excitation Signal [V]
v = data(:, 7); % Input signal to the amplifier [V]
f_meas = data(:, 8); % Voltage generated by the force sensor [V]
t = data(:, 9);
save('./mat/identification_noise_opt_iff.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'v', 'u', 't');
%%
d = detrend(d, 0);
acc_1 = detrend(acc_1, 0);
acc_2 = detrend(acc_2, 0);
geo_1 = detrend(geo_1, 0);
geo_2 = detrend(geo_2, 0);
u = detrend(u, 0);
%%
run setup;
win = hann(ceil(10/Ts));
[p_d, f] = pwelch(d, win, [], [], 1/Ts);
[p_acc1, ~] = pwelch(acc_1, win, [], [], 1/Ts);
[p_acc2, ~] = pwelch(acc_2, win, [], [], 1/Ts);
[p_geo1, ~] = pwelch(geo_1, win, [], [], 1/Ts);
[p_geo2, ~] = pwelch(geo_2, win, [], [], 1/Ts);
%%
figure;
hold on;
plot(f, p_acc1);
plot(f, p_acc2);
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('PSD [$(m/s^2)^2/Hz$]'); xlabel('Frequency [Hz]');
figure;
hold on;
plot(f, p_geo1);
plot(f, p_geo2);
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('PSD [$(m/s)^2/Hz$]'); xlabel('Frequency [Hz]');
figure;
hold on;
plot(f, p_d);
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('PSD [$m^2/Hz$]'); xlabel('Frequency [Hz]');
%%
run setup;
win = hann(ceil(10/Ts));
[tf_geo1_est, f] = tfestimate(d, geo_1, win, [], [], 1/Ts);
[co_geo1_est, ~] = mscohere(d, geo_1, win, [], [], 1/Ts);
[tf_geo2_est, ~] = tfestimate(d, geo_2, win, [], [], 1/Ts);
[co_geo2_est, ~] = mscohere(d, geo_2, win, [], [], 1/Ts);
[tf_acc1_est, ~] = tfestimate(d, acc_1, win, [], [], 1/Ts);
[co_acc1_est, ~] = mscohere(d, acc_1, win, [], [], 1/Ts);
[tf_acc2_est, ~] = tfestimate(d, acc_2, win, [], [], 1/Ts);
[co_acc2_est, ~] = mscohere(d, acc_2, win, [], [], 1/Ts);
%%
figure;
hold on;
plot(f, co_geo1_est, '-')
plot(f, co_geo2_est, '-')
plot(f, co_acc1_est, '-')
plot(f, co_acc2_est, '-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Coherence'); xlabel('Frequency [Hz]');
hold off;
%%
figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(f, abs(tf_geo1_est), '-', 'DisplayName', 'Geo1')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude'); xlabel('Frequency [Hz]');
hold off;
ax2 = subplot(2, 1, 2);
hold on;
plot(f, 180/pi*unwrap(angle(tf_geo1_est)), '-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Phase'); xlabel('Frequency [Hz]');
hold off;
linkaxes([ax1,ax2], 'x');
%%
figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(f, abs(tf_acc1_est), '-', 'DisplayName', 'Acc1')
plot(f, abs(tf_acc2_est), '-', 'DisplayName', 'Acc2')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude'); xlabel('Frequency [Hz]');
hold off;
ax2 = subplot(2, 1, 2);
hold on;
plot(f, 180/pi*unwrap(angle(tf_acc1_est)), '-')
plot(f, 180/pi*unwrap(angle(tf_acc2_est)), '-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Phase'); xlabel('Frequency [Hz]');
hold off;
linkaxes([ax1,ax2], 'x');
%%
win = hann(ceil(10/Ts));
[p_acc_1, f] = pwelch(acc_1, win, [], [], 1/Ts);
[co_acc12, ~] = mscohere(acc_1, acc_2, win, [], [], 1/Ts);
[p_geo_1, ~] = pwelch(geo_1, win, [], [], 1/Ts);
[co_geo12, ~] = mscohere(geo_1, geo_2, win, [], [], 1/Ts);
p_acc_N = p_acc_1.*(1 - co_acc12);
p_geo_N = p_geo_1.*(1 - co_geo12);
figure;
hold on;
plot(f, sqrt(p_acc_N)./abs(tf_acc1_est));
plot(f, sqrt(p_geo_N)./abs(tf_geo1_est));
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('PSD'); xlabel('Frequency [Hz]');

BIN
matlab/sensor_fusion_test_bench.slx Normal file
View File

Binary file not shown.

BIN
matlab/sensor_fusion_test_bench_simscape.slx
View File

Binary file not shown.

25
matlab/setup.m Normal file
View File

@@ -0,0 +1,25 @@
%%
s = tf('s');
Ts = 1e-4; % [s]
%% Pre-Filter
% Glpf = 1/(1 + s/2/pi/2e3);
G_pf = 1/(1 + s/2/pi/50); % Used to excite with constant velocity
G_pf = c2d(G_pf, Ts, 'tustin');
%% Force Sensor Filter (HPF)
Gf_hpf = s/(s + 2*pi*2);
Gf_hpf = tf(1);
Gf_hpf = c2d(Gf_hpf, Ts, 'tustin');
%% IFF Controller
Kiff = 1/(s + 2*pi*2);
Kiff = c2d(Kiff, Ts, 'tustin');
%% Excitation Signal
Tsim = 180; % Excitation time + Measurement time [s]
t = 0:Ts:Tsim;
% u_exc = timeseries(chirp(t, 0.1, Tsim, 1e3, 'logarithmic'), t);
u_exc = timeseries(chirp(t, 40, Tsim, 400, 'logarithmic'), t);
u_exc = timeseries(y_v, t);

39
matlab/src/createWeight.m Normal file
View File

@@ -0,0 +1,39 @@
function [W] = createWeight(args)
% createWeight -
%
% Syntax: [in_data] = createWeight(in_data)
%
% Inputs:
% - n - Weight Order
% - G0 - Low frequency Gain
% - G1 - High frequency Gain
% - Gc - Gain of W at frequency w0
% - w0 - Frequency at which |W(j w0)| = Gc
%
% Outputs:
% - W - Generated Weight
arguments
args.n (1,1) double {mustBeInteger, mustBePositive} = 1
args.G0 (1,1) double {mustBeNumeric, mustBePositive} = 0.1
args.G1 (1,1) double {mustBeNumeric, mustBePositive} = 10
args.Gc (1,1) double {mustBeNumeric, mustBePositive} = 1
args.w0 (1,1) double {mustBeNumeric, mustBePositive} = 1
end
mustBeBetween(args.G0, args.Gc, args.G1);
s = tf('s');
W = (((1/args.w0)*sqrt((1-(args.G0/args.Gc)^(2/args.n))/(1-(args.Gc/args.G1)^(2/args.n)))*s + (args.G0/args.Gc)^(1/args.n))/((1/args.G1)^(1/args.n)*(1/args.w0)*sqrt((1-(args.G0/args.Gc)^(2/args.n))/(1-(args.Gc/args.G1)^(2/args.n)))*s + (1/args.Gc)^(1/args.n)))^args.n;
end
% Custom validation function
function mustBeBetween(a,b,c)
if ~((a > b && b > c) || (c > b && b > a))
eid = 'createWeight:inputError';
msg = 'Gc should be between G0 and G1.';
throwAsCaller(MException(eid,msg))
end
end

38
matlab/src/plotMagUncertainty.m Normal file
View File

@@ -0,0 +1,38 @@
function [p] = plotMagUncertainty(W, freqs, args)
% plotMagUncertainty -
%
% Syntax: [p] = plotMagUncertainty(W, freqs, args)
%
% Inputs:
% - W - Multiplicative Uncertainty Weight
% - freqs - Frequency Vector [Hz]
% - args - Optional Arguments:
% - G
% - color_i
% - opacity
%
% Outputs:
% - p - Plot Handle
arguments
W
freqs double {mustBeNumeric, mustBeNonnegative}
args.G = tf(1)
args.color_i (1,1) double {mustBeInteger, mustBePositive} = 1
args.opacity (1,1) double {mustBeNumeric, mustBeNonnegative} = 0.3
args.DisplayName char = ''
end
% Get defaults colors
colors = get(groot, 'defaultAxesColorOrder');
p = patch([freqs flip(freqs)], ...
[abs(squeeze(freqresp(args.G, freqs, 'Hz'))).*(1 + abs(squeeze(freqresp(W, freqs, 'Hz')))); ...
flip(abs(squeeze(freqresp(args.G, freqs, 'Hz'))).*max(1 - abs(squeeze(freqresp(W, freqs, 'Hz'))), 1e-6))], 'w', ...
'DisplayName', args.DisplayName);
p.FaceColor = colors(args.color_i, :);
p.EdgeColor = 'none';
p.FaceAlpha = args.opacity;
end

43
matlab/src/plotPhaseUncertainty.m Normal file
View File

@@ -0,0 +1,43 @@
function [p] = plotPhaseUncertainty(W, freqs, args)
% plotPhaseUncertainty -
%
% Syntax: [p] = plotPhaseUncertainty(W, freqs, args)
%
% Inputs:
% - W - Multiplicative Uncertainty Weight
% - freqs - Frequency Vector [Hz]
% - args - Optional Arguments:
% - G
% - color_i
% - opacity
%
% Outputs:
% - p - Plot Handle
arguments
W
freqs double {mustBeNumeric, mustBeNonnegative}
args.G = tf(1)
args.color_i (1,1) double {mustBeInteger, mustBePositive} = 1
args.opacity (1,1) double {mustBeNumeric, mustBePositive} = 0.3
args.DisplayName char = ''
end
% Get defaults colors
colors = get(groot, 'defaultAxesColorOrder');
% Compute Phase Uncertainty
Dphi = 180/pi*asin(abs(squeeze(freqresp(W, freqs, 'Hz'))));
Dphi(abs(squeeze(freqresp(W, freqs, 'Hz'))) > 1) = 360;
% Compute Plant Phase
G_ang = 180/pi*angle(squeeze(freqresp(args.G, freqs, 'Hz')));
p = patch([freqs flip(freqs)], [G_ang+Dphi; flip(G_ang-Dphi)], 'w', ...
'DisplayName', args.DisplayName);
p.FaceColor = colors(args.color_i, :);
p.EdgeColor = 'none';
p.FaceAlpha = args.opacity;
end