%% 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)]);