327 lines
11 KiB
Mathematica
327 lines
11 KiB
Mathematica
|
%% 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)]);
|