194 lines
7.2 KiB
Mathematica
194 lines
7.2 KiB
Mathematica
|
%% 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');
|