Add analysis about Mixed synthesis

matlab/matlab/comp_filter_robustness.m

@@ -4,7 +4,352 @@ clear; close all; clc;
 %% Intialize Laplace variable
 s = zpk('s');
 
-% First Basic Example with gain mismatch
+% Dynamical uncertainty of the individual sensors
+% Let say we want to merge two sensors:
+% - sensor 1 that has unknown dynamics above 10Hz: $|w_1(j\omega)| > 1$ for $\omega > 10\text{ Hz}$
+% - sensor 2 that has unknown dynamics below 1Hz and above 1kHz $|w_2(j\omega)| > 1$ for $\omega < 1\text{ Hz}$ and $\omega > 1\text{ kHz}$
+
+% We define the weights that are used to characterize the dynamic uncertainty of the sensors.
+
+
+freqs = logspace(-1, 3, 1000);
+
+omegac = 100*2*pi; G0 = 0.1; Ginf = 10;
+w1 = (Ginf*s/omegac + G0)/(s/omegac + 1);
+
+omegac = 0.2*2*pi; G0 = 5; Ginf = 0.1;
+w2 = (Ginf*s/omegac + G0)/(s/omegac + 1);
+omegac = 5000*2*pi; G0 = 1; Ginf = 50;
+w2 = w2*(Ginf*s/omegac + G0)/(s/omegac + 1);
+
+
+
+% From the weights, we define the uncertain transfer functions of the sensors. Some of the uncertain dynamics of both sensors are shown on Fig. [[fig:uncertainty_dynamics_sensors]] with the upper and lower bounds on the magnitude and on the phase.
+
+G1 = 1 + w1*ultidyn('Delta',[1 1]);
+G2 = 1 + w2*ultidyn('Delta',[1 1]);
+
+% Few random samples of the sensor dynamics are computed
+G1s = usample(G1, 10);
+G2s = usample(G2, 10);
+
+% We here compute the maximum and minimum phase of both sensors
+Dphi1 = 180/pi*asin(abs(squeeze(freqresp(w1, freqs, 'Hz'))));
+Dphi2 = 180/pi*asin(abs(squeeze(freqresp(w2, freqs, 'Hz'))));
+Dphi1(abs(squeeze(freqresp(w1, freqs, 'Hz'))) > 1) = 190;
+Dphi2(abs(squeeze(freqresp(w2, freqs, 'Hz'))) > 1) = 190;
+
+figure;
+% Magnitude
+ax1 = subaxis(2,1,1);
+hold on;
+set(gca,'ColorOrderIndex',1);
+plot(freqs, 1 + abs(squeeze(freqresp(w1, freqs, 'Hz'))), '--');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, max(1 - abs(squeeze(freqresp(w1, freqs, 'Hz'))), 0), '--');
+set(gca,'ColorOrderIndex',2);
+plot(freqs, 1 + abs(squeeze(freqresp(w2, freqs, 'Hz'))), '--');
+set(gca,'ColorOrderIndex',2);
+plot(freqs, max(1 - abs(squeeze(freqresp(w2, freqs, 'Hz'))), 0), '--');
+for i = 1:length(G1s)
+  plot(freqs, abs(squeeze(freqresp(G1s(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0.4470 0.7410 0.4]);
+  plot(freqs, abs(squeeze(freqresp(G2s(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0.8500 0.3250 0.0980 0.4]);
+end
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]);
+ylabel('Magnitude');
+ylim([1e-1, 10]);
+hold off;
+
+% Phase
+ax2 = subaxis(2,1,2);
+hold on;
+set(gca,'ColorOrderIndex',1);
+plot(freqs,  Dphi1, '--');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, -Dphi1, '--');
+set(gca,'ColorOrderIndex',2);
+plot(freqs,  Dphi2, '--');
+set(gca,'ColorOrderIndex',2);
+plot(freqs, -Dphi2, '--');
+for i = 1:length(G1s)
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G1s(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0.4470 0.7410 0.4]);
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G2s(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0.8500 0.3250 0.0980 0.4]);
+end
+set(gca,'xscale','log');
+yticks(-180:90:180);
+ylim([-180 180]);
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+linkaxes([ax1,ax2],'x');
+
+% Weighting Function used to bound the super sensor uncertainty
+% Let's define $w_\phi(s)$ in order to bound the maximum allowed phase uncertainty $\Delta\phi_\text{max}$ of the super sensor dynamics.
+% The magnitude $|w_\phi(j\omega)|$ is shown in Fig. [[fig:magnitude_wphi]] and the corresponding maximum allowed phase uncertainty of the super sensor dynamics of shown in Fig. [[fig:maximum_wanted_phase_uncertainty]].
+
+
+Dphi = 20; % [deg]
+
+n = 4; w0 = 2*pi*900; G0 = 1/sin(Dphi*pi/180); Ginf = 1/100; Gc = 1;
+wphi = (((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/Ginf)^(2/n)))*s + (G0/Gc)^(1/n))/((1/Ginf)^(1/n)*(1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/Ginf)^(2/n)))*s + (1/Gc)^(1/n)))^n;
+
+W1 = w1*wphi;
+W2 = w2*wphi;
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(wphi, freqs, 'Hz'))), '-', 'DisplayName', '$w_\phi(s)$');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude');
+hold off;
+xlim([freqs(1), freqs(end)]);
+legend('location', 'northeast');
+
+
+
+% #+NAME: fig:magnitude_wphi
+% #+CAPTION: Magnitude of the weght $w_\phi(s)$ that is used to bound the uncertainty of the super sensor ([[./figs/magnitude_wphi.png][png]], [[./figs/magnitude_wphi.pdf][pdf]])
+% [[file:figs/magnitude_wphi.png]]
+
+
+% We here compute the wanted maximum and minimum phase of the super sensor
+Dphimax = 180/pi*asin(1./abs(squeeze(freqresp(wphi, freqs, 'Hz'))));
+Dphimax(1./abs(squeeze(freqresp(wphi, freqs, 'Hz'))) > 1) = 190;
+
+figure;
+hold on;
+plot(freqs,  Dphimax, 'k--');
+plot(freqs, -Dphimax, 'k--');
+set(gca, 'XScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude');
+hold off;
+xlim([freqs(1), freqs(end)]);
+ylim([-180 180]);
+yticks(-180:45:180);
+
+
+
+% #+NAME: fig:maximum_wanted_phase_uncertainty
+% #+CAPTION: Maximum wanted phase uncertainty using this weight ([[./figs/maximum_wanted_phase_uncertainty.png][png]], [[./figs/maximum_wanted_phase_uncertainty.pdf][pdf]])
+% [[file:figs/maximum_wanted_phase_uncertainty.png]]
+
+% The obtained upper bounds on the complementary filters in order to limit the phase uncertainty of the super sensor are represented in Fig. [[fig:upper_bounds_comp_filter_max_phase_uncertainty]].
+
+
+figure;
+hold on;
+plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), '-', 'DisplayName', '$1/|w_1w_\phi|$');
+plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), '-', 'DisplayName', '$1/|w_2w_\phi|$');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude');
+hold off;
+xlim([freqs(1), freqs(end)]);
+legend('location', 'northeast');
+
+% $\mathcal{H}_\infty$ Synthesis
+% The $\mathcal{H}_\infty$ synthesis architecture used for the complementary filters is shown in Fig. [[fig:h_infinity_robust_fusion]].
+
+% #+name: fig:h_infinity_robust_fusion
+% #+caption: Architecture used for $\mathcal{H}_\infty$ synthesis of complementary filters
+% [[file:figs-tikz/h_infinity_robust_fusion.png]]
+
+% The generalized plant is defined below.
+
+P = [W1 -W1;
+     0   W2;
+     1   0];
+
+
+
+% And we do the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command.
+
+[H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
+
+
+
+% #+RESULTS:
+% #+begin_example
+% [H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
+% Resetting value of Gamma min based on D_11, D_12, D_21 terms
+
+% Test bounds:      0.0447 <  gamma  <=      1.3318
+
+%   gamma    hamx_eig  xinf_eig  hamy_eig   yinf_eig   nrho_xy   p/f
+%     1.332   1.3e+01 -1.0e-14   1.3e+00   -2.6e-18    0.0000    p
+%     0.688   1.3e-11# ********   1.3e+00   -6.7e-15  ********    f
+%     1.010   1.1e+01 -1.5e-14   1.3e+00   -2.5e-14    0.0000    p
+%     0.849   6.9e-11# ********   1.3e+00   -2.3e-14  ********    f
+%     0.930   5.2e-12# ********   1.3e+00   -6.1e-18  ********    f
+%     0.970   5.6e-11# ********   1.3e+00   -2.3e-14  ********    f
+%     0.990   5.0e-11# ********   1.3e+00   -1.7e-17  ********    f
+%     1.000   2.1e-10# ********   1.3e+00    0.0e+00  ********    f
+%     1.005   1.9e-10# ********   1.3e+00   -3.7e-14  ********    f
+%     1.008   1.1e+01 -9.1e-15   1.3e+00    0.0e+00    0.0000    p
+%     1.006   1.2e-09# ********   1.3e+00   -6.9e-16  ********    f
+%     1.007   1.1e+01 -4.6e-15   1.3e+00   -1.8e-16    0.0000    p
+
+%  Gamma value achieved:     1.0069
+% #+end_example
+
+% And $H_1(s)$ is defined as the complementary of $H_2(s)$.
+
+H1 = 1 - H2;
+
+
+
+% The obtained complementary filters are shown in Fig. [[fig:comp_filter_hinf_uncertainty]].
+
+figure;
+
+ax1 = subplot(2,1,1);
+hold on;
+set(gca,'ColorOrderIndex',1)
+plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), '--', 'DisplayName', '$W_1$');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), '--', 'DisplayName', '$W_2$');
+
+set(gca,'ColorOrderIndex',1)
+plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), '-', 'DisplayName', '$H_1$');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), '-', 'DisplayName', '$H_2$');
+
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Magnitude');
+set(gca, 'XTickLabel',[]);
+legend('location', 'northeast');
+
+ax2 = subplot(2,1,2);
+hold on;
+set(gca,'ColorOrderIndex',1)
+plot(freqs, 180/pi*phase(squeeze(freqresp(H1, freqs, 'Hz'))), '-');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, 180/pi*phase(squeeze(freqresp(H2, freqs, 'Hz'))), '-');
+hold off;
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+set(gca, 'XScale', 'log');
+yticks([-360:90:360]);
+
+linkaxes([ax1,ax2],'x');
+xlim([freqs(1), freqs(end)]);
+xticks([0.1, 1, 10, 100, 1000]);
+
+% Super sensor uncertainty
+% We can now compute the uncertainty of the super sensor. The result is shown in Fig. [[fig:super_sensor_uncertainty_bode_plot]].
+
+
+Gss = G1*H1 + G2*H2;
+
+Gsss = usample(Gss, 20);
+
+% We here compute the maximum and minimum phase of the super sensor
+Dphiss = 180/pi*asin(abs(squeeze(freqresp(w1*H1, freqs, 'Hz')))+abs(squeeze(freqresp(w2*H2, freqs, 'Hz'))));
+Dphiss(abs(squeeze(freqresp(w1*H1, freqs, 'Hz')))+abs(squeeze(freqresp(w2*H2, freqs, 'Hz'))) > 1) = 190;
+
+figure;
+% Magnitude
+ax1 = subaxis(2,1,1);
+hold on;
+set(gca,'ColorOrderIndex',1);
+plot(freqs, 1 + abs(squeeze(freqresp(w1, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S1');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, max(1 - abs(squeeze(freqresp(w1, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
+set(gca,'ColorOrderIndex',2);
+plot(freqs, 1 + abs(squeeze(freqresp(w2, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S2');
+set(gca,'ColorOrderIndex',2);
+plot(freqs, max(1 - abs(squeeze(freqresp(w2, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
+plot(freqs, 1 + abs(squeeze(freqresp(w1*H1+w2*H2, freqs, 'Hz'))), 'k--', 'DisplayName', 'Bounds - SS');
+plot(freqs, max(1 - abs(squeeze(freqresp(w1*H1+w2*H2, freqs, 'Hz'))), 0), 'k--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(Gsss(1, 1, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2], 'DisplayName', 'SS Dynamics');
+for i = 2:length(Gsss)
+  plot(freqs, abs(squeeze(freqresp(Gsss(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2], 'HandleVisibility', 'off');
+end
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]);
+legend('location', 'southwest');
+ylabel('Magnitude');
+ylim([5e-2, 10]);
+hold off;
+
+% Phase
+ax2 = subaxis(2,1,2);
+hold on;
+set(gca,'ColorOrderIndex',1);
+plot(freqs,  Dphi1, '--');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, -Dphi1, '--');
+set(gca,'ColorOrderIndex',2);
+plot(freqs,  Dphi2, '--');
+set(gca,'ColorOrderIndex',2);
+plot(freqs, -Dphi2, '--');
+plot(freqs,  Dphiss, 'k--');
+plot(freqs, -Dphiss, 'k--');
+for i = 1:length(Gsss)
+  plot(freqs, 180/pi*angle(squeeze(freqresp(Gsss(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2]);
+end
+set(gca,'xscale','log');
+yticks(-180:90:180);
+ylim([-180 180]);
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+linkaxes([ax1,ax2],'x');
+
+% Super sensor noise
+% We now compute the obtain Power Spectral Density of the super sensor's noise.
+% The noise characteristics of both individual sensor are defined below.
+
+
+omegac = 100*2*pi; G0 = 1e-5; Ginf = 1e-4;
+N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/100);
+
+omegac = 1*2*pi; G0 = 1e-3; Ginf = 1e-8;
+N2 = ((sqrt(Ginf)*s/omegac + sqrt(G0))/(s/omegac + 1))^2/(1 + s/2/pi/4000)^2;
+
+
+
+% The PSD of both sensor and of the super sensor is shown in Fig. [[fig:psd_sensors_hinf_synthesis]].
+% The CPS of both sensor and of the super sensor is shown in Fig. [[fig:cps_sensors_hinf_synthesis]].
+
+
+PSD_S1 = abs(squeeze(freqresp(N1, freqs, 'Hz'))).^2;
+PSD_S2 = abs(squeeze(freqresp(N2, freqs, 'Hz'))).^2;
+PSD_H2 = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2;
+
+figure;
+hold on;
+plot(freqs, PSD_S1, '-',  'DisplayName', '$\Phi_{\hat{x}_1}$');
+plot(freqs, PSD_S2, '-',  'DisplayName', '$\Phi_{\hat{x}_2}$');
+plot(freqs, PSD_H2, 'k-', 'DisplayName', '$\Phi_{\hat{x}_{\mathcal{H}_2}}$');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Power Spectral Density');
+hold off;
+xlim([freqs(1), freqs(end)]);
+legend('location', 'northeast');
+
+
+
+% #+NAME: fig:psd_sensors_hinf_synthesis
+% #+CAPTION: Power Spectral Density of the obtained super sensor using the $\mathcal{H}_\infty$ synthesis ([[./figs/psd_sensors_hinf_synthesis.png][png]], [[./figs/psd_sensors_hinf_synthesis.pdf][pdf]])
+% [[file:figs/psd_sensors_hinf_synthesis.png]]
+
+
+CPS_S1 = 1/pi*cumtrapz(2*pi*freqs, PSD_S1);
+CPS_S2 = 1/pi*cumtrapz(2*pi*freqs, PSD_S2);
+CPS_H2 = 1/pi*cumtrapz(2*pi*freqs, PSD_H2);
+
+figure;
+hold on;
+plot(freqs, CPS_S1, '-',  'DisplayName', sprintf('$\\sigma_{\\hat{x}_1} = %.1e$', sqrt(CPS_S1(end))));
+plot(freqs, CPS_S2, '-',  'DisplayName', sprintf('$\\sigma_{\\hat{x}_2} = %.1e$', sqrt(CPS_S2(end))));
+plot(freqs, CPS_H2, 'k-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_2}} = %.1e$', sqrt(CPS_H2(end))));
+set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum');
+hold off;
+xlim([2e-1, freqs(end)]);
+ylim([1e-10 1e-5]);
+legend('location', 'southeast');
+
+% First Basic Example with gain mismatch                         :noexport:
 % Let's consider two ideal sensors except one sensor has not an expected unity gain but a gain equal to $0.6$:
 % \begin{align*}
 %   G_1(s) &= 1 \\
@@ -96,283 +441,3 @@ xlabel('Relative Frequency $\frac{\omega}{\omega_0}$'); ylabel('Phase [deg]');
 hold off;
 linkaxes([ax1,ax2],'x');
 xlim([freqs(1), freqs(end)]);
-
-% Dynamical uncertainty of the individual sensors
-% We define the weights that are used to characterize the dynamic uncertainty of the sensors.
-
-
-freqs = logspace(-1, 3, 1000);
-
-omegac = 100*2*pi; G0 = 0.1; Ginf = 10;
-w1 = (Ginf*s/omegac + G0)/(s/omegac + 1);
-
-omegac = 0.2*2*pi; G0 = 5; Ginf = 0.1;
-w2 = (Ginf*s/omegac + G0)/(s/omegac + 1);
-omegac = 5000*2*pi; G0 = 1; Ginf = 50;
-w2 = w2*(Ginf*s/omegac + G0)/(s/omegac + 1);
-
-
-
-% From the weights, we define the uncertain transfer functions of the sensors. Some of the uncertain dynamics of both sensors are shown on Fig. [[fig:uncertainty_dynamics_sensors]] with the upper and lower bounds on the magnitude and on the phase.
-
-G1 = 1 + w1*ultidyn('Delta',[1 1]);
-G2 = 1 + w2*ultidyn('Delta',[1 1]);
-
-% Few random samples of the sensor dynamics are computed
-G1s = usample(G1, 10);
-G2s = usample(G2, 10);
-
-% We here compute the maximum and minimum phase of both sensors
-Dphi1 = 180/pi*asin(abs(squeeze(freqresp(w1, freqs, 'Hz'))));
-Dphi2 = 180/pi*asin(abs(squeeze(freqresp(w2, freqs, 'Hz'))));
-Dphi1(abs(squeeze(freqresp(w1, freqs, 'Hz'))) > 1) = 190;
-Dphi2(abs(squeeze(freqresp(w2, freqs, 'Hz'))) > 1) = 190;
-
-figure;
-% Magnitude
-ax1 = subaxis(2,1,1);
-hold on;
-set(gca,'ColorOrderIndex',1);
-plot(freqs, 1 + abs(squeeze(freqresp(w1, freqs, 'Hz'))), '--');
-set(gca,'ColorOrderIndex',1);
-plot(freqs, max(1 - abs(squeeze(freqresp(w1, freqs, 'Hz'))), 0), '--');
-set(gca,'ColorOrderIndex',2);
-plot(freqs, 1 + abs(squeeze(freqresp(w2, freqs, 'Hz'))), '--');
-set(gca,'ColorOrderIndex',2);
-plot(freqs, max(1 - abs(squeeze(freqresp(w2, freqs, 'Hz'))), 0), '--');
-for i = 1:length(G1s)
-  plot(freqs, abs(squeeze(freqresp(G1s(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0.4470 0.7410 0.4]);
-  plot(freqs, abs(squeeze(freqresp(G2s(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0.8500 0.3250 0.0980 0.4]);
-end
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]);
-ylabel('Magnitude');
-ylim([1e-1, 10]);
-hold off;
-
-% Phase
-ax2 = subaxis(2,1,2);
-hold on;
-set(gca,'ColorOrderIndex',1);
-plot(freqs,  Dphi1, '--');
-set(gca,'ColorOrderIndex',1);
-plot(freqs, -Dphi1, '--');
-set(gca,'ColorOrderIndex',2);
-plot(freqs,  Dphi2, '--');
-set(gca,'ColorOrderIndex',2);
-plot(freqs, -Dphi2, '--');
-for i = 1:length(G1s)
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G1s(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0.4470 0.7410 0.4]);
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G2s(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0.8500 0.3250 0.0980 0.4]);
-end
-set(gca,'xscale','log');
-yticks(-180:90:180);
-ylim([-180 180]);
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-linkaxes([ax1,ax2],'x');
-
-% H-Infinity Synthesis
-% Let's define $w_\phi(s)$ in order to bound the maximum allowed phase uncertainty $\Delta\phi_\text{max}$ of the super sensor dynamics.
-
-
-Dphi = 20; % [deg]
-
-n = 4; w0 = 2*pi*900; G0 = 1/sin(Dphi*pi/180); Ginf = 1/100; Gc = 1;
-wphi = (((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/Ginf)^(2/n)))*s + (G0/Gc)^(1/n))/((1/Ginf)^(1/n)*(1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/Ginf)^(2/n)))*s + (1/Gc)^(1/n)))^n;
-
-W1 = w1*wphi;
-W2 = w2*wphi;
-
-figure;
-hold on;
-plot(freqs, abs(squeeze(freqresp(wphi, freqs, 'Hz'))), '-', 'DisplayName', '$w_\phi(s)$');
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('Magnitude');
-hold off;
-xlim([freqs(1), freqs(end)]);
-legend('location', 'northeast');
-
-
-
-% #+NAME: fig:magnitude_wphi
-% #+CAPTION: Magnitude of the weght $w_\phi(s)$ that is used to bound the uncertainty of the super sensor ([[./figs/magnitude_wphi.png][png]], [[./figs/magnitude_wphi.pdf][pdf]])
-% [[file:figs/magnitude_wphi.png]]
-
-
-% We here compute the wanted maximum and minimum phase of the super sensor
-Dphimax = 180/pi*asin(1./abs(squeeze(freqresp(wphi, freqs, 'Hz'))));
-Dphimax(1./abs(squeeze(freqresp(wphi, freqs, 'Hz'))) > 1) = 190;
-
-figure;
-hold on;
-plot(freqs,  Dphimax, 'k--');
-plot(freqs, -Dphimax, 'k--');
-set(gca, 'XScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('Magnitude');
-hold off;
-xlim([freqs(1), freqs(end)]);
-ylim([-180 180]);
-yticks(-180:45:180);
-
-
-
-% #+NAME: fig:maximum_wanted_phase_uncertainty
-% #+CAPTION: Maximum wanted phase uncertainty using this weight ([[./figs/maximum_wanted_phase_uncertainty.png][png]], [[./figs/maximum_wanted_phase_uncertainty.pdf][pdf]])
-% [[file:figs/maximum_wanted_phase_uncertainty.png]]
-
-% The obtained upper bounds on the complementary filters in order to limit the phase uncertainty of the super sensor are represented in Fig. [[fig:upper_bounds_comp_filter_max_phase_uncertainty]].
-
-
-figure;
-hold on;
-plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), '-', 'DisplayName', '$1/|w_1w_\phi|$');
-plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), '-', 'DisplayName', '$1/|w_2w_\phi|$');
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('Magnitude');
-hold off;
-xlim([freqs(1), freqs(end)]);
-legend('location', 'northeast');
-
-
-
-% #+NAME: fig:upper_bounds_comp_filter_max_phase_uncertainty
-% #+CAPTION: Upper bounds on the complementary filters set in order to limit the maximum phase uncertainty of the super sensor to 30 degrees until 500Hz ([[./figs/upper_bounds_comp_filter_max_phase_uncertainty.png][png]], [[./figs/upper_bounds_comp_filter_max_phase_uncertainty.pdf][pdf]])
-% [[file:figs/upper_bounds_comp_filter_max_phase_uncertainty.png]]
-
-% The $\mathcal{H}_\infty$ synthesis is performed using the defined weights and the obtained complementary filters are shown in Fig. [[fig:comp_filter_hinf_uncertainty]].
-
-
-P = [W1 -W1;
-     0   W2;
-     1   0];
-
-
-
-% And we do the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command.
-
-[H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
-
-
-
-% #+RESULTS:
-% #+begin_example
-% [H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
-% Resetting value of Gamma min based on D_11, D_12, D_21 terms
-
-% Test bounds:      0.0447 <  gamma  <=      1.3318
-
-%   gamma    hamx_eig  xinf_eig  hamy_eig   yinf_eig   nrho_xy   p/f
-%     1.332   1.3e+01 -1.0e-14   1.3e+00   -2.6e-18    0.0000    p
-%     0.688   1.3e-11# ********   1.3e+00   -6.7e-15  ********    f
-%     1.010   1.1e+01 -1.5e-14   1.3e+00   -2.5e-14    0.0000    p
-%     0.849   6.9e-11# ********   1.3e+00   -2.3e-14  ********    f
-%     0.930   5.2e-12# ********   1.3e+00   -6.1e-18  ********    f
-%     0.970   5.6e-11# ********   1.3e+00   -2.3e-14  ********    f
-%     0.990   5.0e-11# ********   1.3e+00   -1.7e-17  ********    f
-%     1.000   2.1e-10# ********   1.3e+00    0.0e+00  ********    f
-%     1.005   1.9e-10# ********   1.3e+00   -3.7e-14  ********    f
-%     1.008   1.1e+01 -9.1e-15   1.3e+00    0.0e+00    0.0000    p
-%     1.006   1.2e-09# ********   1.3e+00   -6.9e-16  ********    f
-%     1.007   1.1e+01 -4.6e-15   1.3e+00   -1.8e-16    0.0000    p
-
-%  Gamma value achieved:     1.0069
-% #+end_example
-
-
-H1 = 1 - H2;
-
-figure;
-
-ax1 = subplot(2,1,1);
-hold on;
-set(gca,'ColorOrderIndex',1)
-plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), '--', 'DisplayName', '$W_1$');
-set(gca,'ColorOrderIndex',2)
-plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), '--', 'DisplayName', '$W_2$');
-
-set(gca,'ColorOrderIndex',1)
-plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), '-', 'DisplayName', '$H_1$');
-set(gca,'ColorOrderIndex',2)
-plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), '-', 'DisplayName', '$H_2$');
-
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Magnitude');
-set(gca, 'XTickLabel',[]);
-legend('location', 'northeast');
-
-ax2 = subplot(2,1,2);
-hold on;
-set(gca,'ColorOrderIndex',1)
-plot(freqs, 180/pi*phase(squeeze(freqresp(H1, freqs, 'Hz'))), '-');
-set(gca,'ColorOrderIndex',2)
-plot(freqs, 180/pi*phase(squeeze(freqresp(H2, freqs, 'Hz'))), '-');
-hold off;
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-set(gca, 'XScale', 'log');
-yticks([-360:90:360]);
-
-linkaxes([ax1,ax2],'x');
-xlim([freqs(1), freqs(end)]);
-xticks([0.1, 1, 10, 100, 1000]);
-
-% Super sensor uncertainty
-% We can now compute the uncertainty of the super sensor. The result is shown in Fig. [[fig:super_sensor_uncertainty_bode_plot]].
-
-
-Gss = G1*H1 + G2*H2;
-
-Gsss = usample(Gss, 20);
-
-% We here compute the maximum and minimum phase of the super sensor
-Dphiss = 180/pi*asin(abs(squeeze(freqresp(w1*H1, freqs, 'Hz')))+abs(squeeze(freqresp(w2*H2, freqs, 'Hz'))));
-Dphiss(abs(squeeze(freqresp(w1*H1, freqs, 'Hz')))+abs(squeeze(freqresp(w2*H2, freqs, 'Hz'))) > 1) = 190;
-
-figure;
-% Magnitude
-ax1 = subaxis(2,1,1);
-hold on;
-set(gca,'ColorOrderIndex',1);
-plot(freqs, 1 + abs(squeeze(freqresp(w1, freqs, 'Hz'))), '--');
-set(gca,'ColorOrderIndex',1);
-plot(freqs, max(1 - abs(squeeze(freqresp(w1, freqs, 'Hz'))), 0), '--');
-set(gca,'ColorOrderIndex',2);
-plot(freqs, 1 + abs(squeeze(freqresp(w2, freqs, 'Hz'))), '--');
-set(gca,'ColorOrderIndex',2);
-plot(freqs, max(1 - abs(squeeze(freqresp(w2, freqs, 'Hz'))), 0), '--');
-plot(freqs, 1 + abs(squeeze(freqresp(w1*H1+w2*H2, freqs, 'Hz'))), 'k--');
-plot(freqs, max(1 - abs(squeeze(freqresp(w1*H1+w2*H2, freqs, 'Hz'))), 0), 'k--');
-for i = 1:length(Gsss)
-  plot(freqs, abs(squeeze(freqresp(Gsss(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2]);
-end
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]);
-ylabel('Magnitude');
-ylim([1e-1, 10]);
-hold off;
-
-% Phase
-ax2 = subaxis(2,1,2);
-hold on;
-% plot(freqs,  Dphimax, 'r-');
-% plot(freqs, -Dphimax, 'r-');
-set(gca,'ColorOrderIndex',1);
-plot(freqs,  Dphi1, '--');
-set(gca,'ColorOrderIndex',1);
-plot(freqs, -Dphi1, '--');
-set(gca,'ColorOrderIndex',2);
-plot(freqs,  Dphi2, '--');
-set(gca,'ColorOrderIndex',2);
-plot(freqs, -Dphi2, '--');
-plot(freqs,  Dphiss, 'k--');
-plot(freqs, -Dphiss, 'k--');
-for i = 1:length(Gsss)
-  plot(freqs, 180/pi*angle(squeeze(freqresp(Gsss(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2]);
-end
-set(gca,'xscale','log');
-yticks(-180:90:180);
-ylim([-180 180]);
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-linkaxes([ax1,ax2],'x');

matlab/matlab/comp_filters_design.m

@@ -1,53 +0,0 @@
-%% Clear Workspace and Close figures
-clear; close all; clc;
-
-%% Intialize Laplace variable
-s = zpk('s');
-
-% Matlab
-
-omega0 = 1*2*pi; % [rad/s]
-tau = 1/omega0; % [s]
-
-% From cite:corke04_inert_visual_sensin_system_small_auton_helic
-HL1 = 1/(s/omega0 + 1); HH1 = s/omega0/(s/omega0 + 1);
-
-% From cite:jensen13_basic_uas
-HL2 = (2*omega0*s + omega0^2)/(s+omega0)^2; HH2 = s^2/(s+omega0)^2;
-
-% From cite:shaw90_bandw_enhan_posit_measur_using_measur_accel
-HL3 = (3*tau*s + 1)/(tau*s + 1)^3; HH3 = (tau^3*s^3 + 3*tau^2*s^2)/(tau*s + 1)^3;
-
-freqs = logspace(-1, 1, 1000);
-
-figure;
-% Magnitude
-ax1 = subaxis(2,1,1);
-hold on;
-set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(HH1, freqs, 'Hz'))));
-set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(HL1, freqs, 'Hz'))));
-set(gca,'ColorOrderIndex',2); plot(freqs, abs(squeeze(freqresp(HH2, freqs, 'Hz'))));
-set(gca,'ColorOrderIndex',2); plot(freqs, abs(squeeze(freqresp(HL2, freqs, 'Hz'))));
-set(gca,'ColorOrderIndex',3); plot(freqs, abs(squeeze(freqresp(HH3, freqs, 'Hz'))));
-set(gca,'ColorOrderIndex',3); plot(freqs, abs(squeeze(freqresp(HL3, freqs, 'Hz'))));
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]);
-ylabel('Magnitude');
-hold off;
-ylim([1e-2 2]);
-% Phase
-ax2 = subaxis(2,1,2);
-hold on;
-set(gca,'ColorOrderIndex',1); plot(freqs, 180/pi*angle(squeeze(freqresp(HH1, freqs, 'Hz'))));
-set(gca,'ColorOrderIndex',1); plot(freqs, 180/pi*angle(squeeze(freqresp(HL1, freqs, 'Hz'))));
-set(gca,'ColorOrderIndex',2); plot(freqs, 180/pi*angle(squeeze(freqresp(HH2, freqs, 'Hz'))));
-set(gca,'ColorOrderIndex',2); plot(freqs, 180/pi*angle(squeeze(freqresp(HL2, freqs, 'Hz'))));
-set(gca,'ColorOrderIndex',3); plot(freqs, 180/pi*angle(squeeze(freqresp(HH3, freqs, 'Hz'))));
-set(gca,'ColorOrderIndex',3); plot(freqs, 180/pi*angle(squeeze(freqresp(HL3, freqs, 'Hz'))));
-set(gca,'xscale','log');
-yticks(-180:90:180);
-ylim([-180 180]);
-xlabel('Relative Frequency $\frac{\omega}{\omega_0}$'); ylabel('Phase [deg]');
-hold off;
-linkaxes([ax1,ax2],'x');
-xlim([freqs(1), freqs(end)]);

matlab/matlab/hinf_syn_perf_robust.m

@@ -0,0 +1,359 @@
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+freqs = logspace(-1, 3, 1000);
+
+% Dynamical uncertainty and Noise level of the individual sensors
+% Uncertainty on the individual sensors:
+
+omegac = 100*2*pi; G0 = 0.1; Ginf = 10;
+w1 = (Ginf*s/omegac + G0)/(s/omegac + 1);
+
+omegac = 0.2*2*pi; G0 = 5; Ginf = 0.1;
+w2 = (Ginf*s/omegac + G0)/(s/omegac + 1);
+omegac = 5000*2*pi; G0 = 1; Ginf = 50;
+w2 = w2*(Ginf*s/omegac + G0)/(s/omegac + 1);
+
+
+
+% Noise level of the individual sensors:
+
+omegac = 100*2*pi; G0 = 1e-5; Ginf = 1e-4;
+N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/100);
+
+omegac = 1*2*pi; G0 = 1e-3; Ginf = 1e-8;
+N2 = ((sqrt(Ginf)*s/omegac + sqrt(G0))/(s/omegac + 1))^2/(1 + s/2/pi/4000)^2;
+
+figure;
+ax1 = subplot(2, 1, 1);
+hold on;
+plot(freqs, abs(squeeze(freqresp(N1, freqs, 'Hz'))), '-', 'DisplayName', '$|N_1(j\omega)|$');
+plot(freqs, abs(squeeze(freqresp(N2, freqs, 'Hz'))), '-', 'DisplayName', '$|N_2(j\omega)|$');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude');
+hold off;
+legend('location', 'northeast');
+
+ax2 = subplot(2, 1, 2);
+hold on;
+plot(freqs, abs(squeeze(freqresp(w1, freqs, 'Hz'))), '-', 'DisplayName', '$|w_1(j\omega)|$');
+plot(freqs, abs(squeeze(freqresp(w2, freqs, 'Hz'))), '-', 'DisplayName', '$|w_2(j\omega)|$');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude');
+hold off;
+legend('location', 'northeast');
+
+linkaxes([ax1,ax2],'x');
+xlim([freqs(1), freqs(end)]);
+
+% Weights for uncertainty and performance
+% We design weights that are used to describe the wanted upper bound on the super sensor's noise and super sensor's uncertainty.
+
+% Weight on the uncertainty:
+
+n = 4; w0 = 2*pi*500; G0 = 6; G1 = 1; Gc = 1.1;
+H = (((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (G0/Gc)^(1/n))/((1/G1)^(1/n)*(1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (1/Gc)^(1/n)))^n;
+
+Wu = 0.2*(s+3.142e04)/(s+628.3)/H;
+
+
+
+% Weight on the performance:
+
+n = 1; w0 = 2*pi*9; A = 6;
+a = sqrt(2*A^(2/n) - 1 + 2*A^(1/n)*sqrt(A^(2/n) - 1));
+G = ((1 + s/(w0/a))*(1 + s/(w0*a))/(1 + s/w0)^2)^n;
+
+n = 2; w0 = 2*pi*9; G0 = 1e-2; G1 = 1; Gc = 5e-1;
+G2 = (((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (G0/Gc)^(1/n))/((1/G1)^(1/n)*(1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (1/Gc)^(1/n)))^n;
+
+Wp = inv(G2)*inv(G)*inv(N2);
+
+
+
+% The noise and uncertainty weights of the individual sensors and the asked noise/uncertainty of the super sensor are displayed in Fig. [[fig:charac_sensors_weights]].
+
+figure;
+ax1 = subplot(2, 1, 1);
+hold on;
+plot(freqs, abs(squeeze(freqresp(N1, freqs, 'Hz'))), '-', 'DisplayName', '$|N_1(j\omega)|$');
+plot(freqs, abs(squeeze(freqresp(N2, freqs, 'Hz'))), '-', 'DisplayName', '$|N_2(j\omega)|$');
+plot(freqs, 1./abs(squeeze(freqresp(Wp, freqs, 'Hz'))), 'k--', 'DisplayName', '$|w_r(j\omega)|^{-1}$');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude');
+hold off;
+legend('location', 'northeast');
+
+ax2 = subplot(2, 1, 2);
+hold on;
+plot(freqs, abs(squeeze(freqresp(w1, freqs, 'Hz'))), '-', 'DisplayName', '$|w_1(j\omega)|$');
+plot(freqs, abs(squeeze(freqresp(w2, freqs, 'Hz'))), '-', 'DisplayName', '$|w_2(j\omega)|$');
+plot(freqs, 1./abs(squeeze(freqresp(Wu, freqs, 'Hz'))), 'k--', 'DisplayName', '$|w_u(j\omega)|^{-1}$');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude');
+hold off;
+legend('location', 'northeast');
+
+linkaxes([ax1,ax2],'x');
+xlim([freqs(1), freqs(end)]);
+
+
+
+% #+NAME: fig:charac_sensors_weights
+% #+CAPTION: Upper bounds on the super sensor's noise and super sensor's uncertainty ([[./figs/charac_sensors_weights.png][png]], [[./figs/charac_sensors_weights.pdf][pdf]])
+% [[file:figs/charac_sensors_weights.png]]
+
+
+% The corresponding maximum norms of the filters to have the perf/robust asked are shown in Fig. [[fig:upper_bound_complementary_filters_perf_robust]].
+
+figure;
+hold on;
+set(gca,'ColorOrderIndex',1)
+plot(freqs, 1./abs(squeeze(freqresp(N1*Wp, freqs, 'Hz'))), '-', 'DisplayName', '$|N_1| - perf$');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, 1./abs(squeeze(freqresp(N2*Wp, freqs, 'Hz'))), '-', 'DisplayName', '$|N_2| - perf$');
+set(gca,'ColorOrderIndex',1)
+plot(freqs, 1./abs(squeeze(freqresp(w1*Wu, freqs, 'Hz'))), '--', 'DisplayName', '$|N_1| - robu$');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, 1./abs(squeeze(freqresp(w2*Wu, freqs, 'Hz'))), '--', 'DisplayName', '$|N_2| - robu$');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude');
+hold off;
+legend('location', 'northeast');
+
+% H-infinity synthesis with 4 outputs corresponding to the 4 weights
+% We do the $\mathcal{H}_\infty$ synthesis with 4 weights and 4 outputs.
+
+% \begin{equation*}
+%   \left\| \begin{matrix}
+%     W_{1p}(s) (1 - N_2(s)) \\
+%     W_{2p}(s) N_2(s) \\
+%     W_{1u}(s) (1 - N_2(s)) \\
+%     W_{2u}(s) N_2(s)
+%   \end{matrix} \right\|_\infty < 1
+% \end{equation*}
+
+
+
+W1p = N1*Wp/(1+s/2/pi/1000); % Used to render W1p proper
+W2p = N2*Wp;
+W1u = w1*Wu;
+W2u = w2*Wu;
+
+P = [W1p -W1p;
+     0    W2p;
+     W1u -W1u;
+     0    W2u;
+     1    0];
+
+
+
+% And we do the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command.
+
+[H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
+
+
+
+% #+RESULTS:
+% #+begin_example
+% [H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
+% Resetting value of Gamma min based on D_11, D_12, D_21 terms
+
+% Test bounds:      1.4139 <  gamma  <=     65.6899
+
+%   gamma    hamx_eig  xinf_eig  hamy_eig   yinf_eig   nrho_xy   p/f
+%    65.690   1.3e+00 -6.7e-15   1.3e+00   -4.5e-13    0.0000    p
+%    33.552   1.3e+00 -9.4e-15   1.3e+00   -3.7e-14    0.0000    p
+%    17.483   1.3e+00 -5.6e-16   1.3e+00   -4.8e-13    0.0000    p
+%     9.448   1.3e+00 -3.2e-15   1.3e+00   -1.2e-13    0.0000    p
+%     5.431   1.3e+00 -2.3e-16   1.3e+00   -3.6e-13    0.0000    p
+%     3.422   1.3e+00 -7.3e-16   1.3e+00   -2.6e-15    0.0000    p
+%     2.418   1.3e+00   9.3e-17   1.3e+00   -3.0e-14    0.0000    p
+%     1.916   1.3e+00   2.4e-17   1.3e+00   -2.2e-14    0.0000    p
+%     1.665   1.3e+00 -2.5e-16   1.3e+00   -2.1e-14    0.0000    p
+%     1.539   1.3e+00 -6.9e-15   1.3e+00   -5.3e-14    0.0000    p
+%     1.477   1.3e+00 -2.1e-14   1.3e+00   -2.3e-13    0.0000    p
+%     1.445   1.3e+00 -1.3e-16   1.3e+00   -2.6e-15    0.0000    p
+%     1.430   1.3e+00 -4.9e-13   1.3e+00   -2.2e-13    0.0000    p
+%     1.422   1.3e+00 -1.2e+08#  1.3e+00   -2.6e-13    0.0000    f
+%     1.426   1.3e+00 -6.3e-13   1.3e+00   -3.3e-14    0.0000    p
+%     1.424   1.3e+00 -3.4e+08#  1.3e+00   -4.5e-14    0.0000    f
+%     1.425   1.3e+00 -1.7e+09#  1.3e+00   -5.2e-13    0.0000    f
+
+%  Gamma value achieved:     1.4256
+% #+end_example
+
+
+H1 = 1 - H2;
+
+
+
+% The obtained complementary filters with the upper bounds are shown in Fig. [[fig:hinf_result_comp_filters_4_outputs]].
+
+figure;
+hold on;
+set(gca,'ColorOrderIndex',1)
+plot(freqs, 1./abs(squeeze(freqresp(W1p, freqs, 'Hz'))), '-', 'DisplayName', '$|N_1| - perf$');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, 1./abs(squeeze(freqresp(W2p, freqs, 'Hz'))), '-', 'DisplayName', '$|N_2| - perf$');
+set(gca,'ColorOrderIndex',1)
+plot(freqs, 1./abs(squeeze(freqresp(W1u, freqs, 'Hz'))), '--', 'DisplayName', '$|N_1| - robu$');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, 1./abs(squeeze(freqresp(W2u, freqs, 'Hz'))), '--', 'DisplayName', '$|N_2| - robu$');
+plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), 'k--', 'DisplayName', '$|H_1|$');
+plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), 'k--', 'DisplayName', '$|H_2|$');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude');
+hold off;
+legend('location', 'northeast');
+
+
+
+% #+NAME: fig:hinf_result_comp_filters_4_outputs
+% #+CAPTION: caption ([[./figs/hinf_result_comp_filters_4_outputs.png][png]], [[./figs/hinf_result_comp_filters_4_outputs.pdf][pdf]])
+% [[file:figs/hinf_result_comp_filters_4_outputs.png]]
+
+
+
+
+figure;
+ax1 = subplot(2, 1, 1);
+hold on;
+plot(freqs, abs(squeeze(freqresp(N1, freqs, 'Hz'))), '-', 'DisplayName', '$|N_1(j\omega)|$');
+plot(freqs, abs(squeeze(freqresp(N2, freqs, 'Hz'))), '-', 'DisplayName', '$|N_2(j\omega)|$');
+plot(freqs, 1./abs(squeeze(freqresp(Wp, freqs, 'Hz'))), 'k--', 'DisplayName', '$|w_r(j\omega)|^{-1}$');
+plot(freqs, sqrt(abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2), 'k-', 'DisplayName', '$|N_{ss}(j\omega)|$');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude');
+hold off;
+legend('location', 'northeast');
+
+ax2 = subplot(2, 1, 2);
+hold on;
+plot(freqs, abs(squeeze(freqresp(w1, freqs, 'Hz'))), '-', 'DisplayName', '$|w_1(j\omega)|$');
+plot(freqs, abs(squeeze(freqresp(w2, freqs, 'Hz'))), '-', 'DisplayName', '$|w_2(j\omega)|$');
+plot(freqs, 1./abs(squeeze(freqresp(Wu, freqs, 'Hz'))), 'k--', 'DisplayName', '$|w_u(j\omega)|^{-1}$');
+plot(freqs, abs(squeeze(freqresp(w1*H1, freqs, 'Hz')))+abs(squeeze(freqresp(w2*H2, freqs, 'Hz'))), 'k-', 'DisplayName', '$|w_{ss}(j\omega)|$');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude');
+hold off;
+legend('location', 'northeast');
+
+linkaxes([ax1,ax2],'x');
+xlim([freqs(1), freqs(end)]);
+
+
+
+% #+NAME: fig:upper_bounds_perf_robust_result_4_outputs
+% #+CAPTION: Obtained PSD and uncertainty with the corresponding upper bounds ([[./figs/upper_bounds_perf_robust_result_4_outputs.png][png]], [[./figs/upper_bounds_perf_robust_result_4_outputs.pdf][pdf]])
+% [[file:figs/upper_bounds_perf_robust_result_4_outputs.png]]
+
+
+PSD_S1 = abs(squeeze(freqresp(N1, freqs, 'Hz'))).^2;
+PSD_S2 = abs(squeeze(freqresp(N2, freqs, 'Hz'))).^2;
+PSD_H2 = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2;
+
+CPS_S1 = 1/pi*cumtrapz(2*pi*freqs, PSD_S1);
+CPS_S2 = 1/pi*cumtrapz(2*pi*freqs, PSD_S2);
+CPS_H2 = 1/pi*cumtrapz(2*pi*freqs, PSD_H2);
+
+figure;
+
+ax1 = subplot(2, 1, 1);
+hold on;
+plot(freqs, PSD_S1, '-',  'DisplayName', '$\Phi_{\hat{x}_1}$');
+plot(freqs, PSD_S2, '-',  'DisplayName', '$\Phi_{\hat{x}_2}$');
+plot(freqs, PSD_H2, 'k-', 'DisplayName', '$\Phi_{\hat{x}}$');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Power Spectral Density');
+hold off;
+legend('location', 'northeast');
+
+ax2 = subplot(2, 1, 2);
+hold on;
+plot(freqs, CPS_S1, '-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_1} = %.1e$', sqrt(CPS_S1(end))));
+plot(freqs, CPS_S2, '-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_2} = %.1e$', sqrt(CPS_S2(end))));
+plot(freqs, CPS_H2, 'k-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}} = %.1e$', sqrt(CPS_H2(end))));
+set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum');
+hold off;
+ylim([1e-10 1e-5]);
+legend('location', 'southeast');
+
+linkaxes([ax1,ax2],'x');
+xlim([freqs(1), freqs(end)]);
+
+
+
+% #+NAME: fig:4outputs_hinf_psd_cps2svg
+% #+CAPTION: PSD and CPS ([[./figs/4outputs_hinf_psd_cps2svg.png][png]], [[./figs/4outputs_hinf_psd_cps2svg.pdf][pdf]])
+% [[file:figs/4outputs_hinf_psd_cps2svg.png]]
+
+
+
+G1 = 1 + w1*ultidyn('Delta',[1 1]);
+G2 = 1 + w2*ultidyn('Delta',[1 1]);
+
+Gss = G1*H1 + G2*H2;
+Gsss = usample(Gss, 20);
+
+% We here compute the maximum and minimum phase of the super sensor
+Dphiss = 180/pi*asin(abs(squeeze(freqresp(w1*H1, freqs, 'Hz')))+abs(squeeze(freqresp(w2*H2, freqs, 'Hz'))));
+Dphiss(abs(squeeze(freqresp(w1*H1, freqs, 'Hz')))+abs(squeeze(freqresp(w2*H2, freqs, 'Hz'))) > 1) = 190;
+
+% We here compute the maximum and minimum phase of both sensors
+Dphi1 = 180/pi*asin(abs(squeeze(freqresp(w1, freqs, 'Hz'))));
+Dphi2 = 180/pi*asin(abs(squeeze(freqresp(w2, freqs, 'Hz'))));
+Dphi1(abs(squeeze(freqresp(w1, freqs, 'Hz'))) > 1) = 190;
+Dphi2(abs(squeeze(freqresp(w2, freqs, 'Hz'))) > 1) = 190;
+
+figure;
+% Magnitude
+ax1 = subaxis(2,1,1);
+hold on;
+set(gca,'ColorOrderIndex',1);
+plot(freqs, 1 + abs(squeeze(freqresp(w1, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S1');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, max(1 - abs(squeeze(freqresp(w1, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
+set(gca,'ColorOrderIndex',2);
+plot(freqs, 1 + abs(squeeze(freqresp(w2, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S2');
+set(gca,'ColorOrderIndex',2);
+plot(freqs, max(1 - abs(squeeze(freqresp(w2, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
+plot(freqs, 1 + abs(squeeze(freqresp(w1*H1+w2*H2, freqs, 'Hz'))), 'k--', 'DisplayName', 'Bounds - SS');
+plot(freqs, max(1 - abs(squeeze(freqresp(w1*H1+w2*H2, freqs, 'Hz'))), 0), 'k--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(Gsss(1, 1, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2], 'DisplayName', 'SS Dynamics');
+for i = 2:length(Gsss)
+  plot(freqs, abs(squeeze(freqresp(Gsss(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2], 'HandleVisibility', 'off');
+end
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]);
+legend('location', 'southwest');
+ylabel('Magnitude');
+ylim([5e-2, 10]);
+hold off;
+
+% Phase
+ax2 = subaxis(2,1,2);
+hold on;
+set(gca,'ColorOrderIndex',1);
+plot(freqs,  Dphi1, '--');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, -Dphi1, '--');
+set(gca,'ColorOrderIndex',2);
+plot(freqs,  Dphi2, '--');
+set(gca,'ColorOrderIndex',2);
+plot(freqs, -Dphi2, '--');
+plot(freqs,  Dphiss, 'k--');
+plot(freqs, -Dphiss, 'k--');
+for i = 1:length(Gsss)
+  plot(freqs, 180/pi*angle(squeeze(freqresp(Gsss(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2]);
+end
+set(gca,'xscale','log');
+yticks(-180:90:180);
+ylim([-180 180]);
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+linkaxes([ax1,ax2],'x');

matlab/matlab/mixed_synthesis_sensor_fusion.m

@@ -0,0 +1,351 @@
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+freqs = logspace(-1, 3, 1000);
+
+% Noise characteristics and Uncertainty of the individual sensors
+% We define the weights that are used to characterize the dynamic uncertainty of the sensors. This will be used for the $\mathcal{H}_\infty$ part of the synthesis.
+
+omegac = 100*2*pi; G0 = 0.1; Ginf = 10;
+w1 = (Ginf*s/omegac + G0)/(s/omegac + 1);
+
+omegac = 0.2*2*pi; G0 = 5; Ginf = 0.1;
+w2 = (Ginf*s/omegac + G0)/(s/omegac + 1);
+omegac = 5000*2*pi; G0 = 1; Ginf = 50;
+w2 = w2*(Ginf*s/omegac + G0)/(s/omegac + 1);
+
+
+
+% We define the noise characteristics of the two sensors by choosing $N_1$ and $N_2$. This will be used for the $\mathcal{H}_2$ part of the synthesis.
+
+omegac = 100*2*pi; G0 = 1e-5; Ginf = 1e-4;
+N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/100);
+
+omegac = 1*2*pi; G0 = 1e-3; Ginf = 1e-8;
+N2 = ((sqrt(Ginf)*s/omegac + sqrt(G0))/(s/omegac + 1))^2/(1 + s/2/pi/4000)^2;
+
+
+
+% Both dynamical uncertainty and noise characteristics of the individual sensors are shown in Fig. [[fig:mixed_synthesis_noise_uncertainty_sensors]].
+
+
+figure;
+ax1 = subplot(2, 1, 1);
+hold on;
+plot(freqs, abs(squeeze(freqresp(N1, freqs, 'Hz'))), '-', 'DisplayName', '$|N_1(j\omega)|$');
+plot(freqs, abs(squeeze(freqresp(N2, freqs, 'Hz'))), '-', 'DisplayName', '$|N_2(j\omega)|$');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude');
+hold off;
+legend('location', 'northeast');
+
+ax2 = subplot(2, 1, 2);
+hold on;
+plot(freqs, abs(squeeze(freqresp(w1, freqs, 'Hz'))), '-', 'DisplayName', '$|w_1(j\omega)|$');
+plot(freqs, abs(squeeze(freqresp(w2, freqs, 'Hz'))), '-', 'DisplayName', '$|w_2(j\omega)|$');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude');
+hold off;
+legend('location', 'northeast');
+
+linkaxes([ax1,ax2],'x');
+xlim([freqs(1), freqs(end)]);
+
+% Weighting Functions on the uncertainty of the super sensor
+% We design weights for the $\mathcal{H}_\infty$ part of the synthesis in order to limit the dynamical uncertainty of the super sensor.
+% The maximum wanted multiplicative uncertainty is shown in Fig. [[fig:mixed_syn_hinf_weight]]. The idea here is that we don't really need low uncertainty at low frequency but only near the crossover frequency that is suppose to be around 300Hz here.
+
+
+n = 4; w0 = 2*pi*900; G0 = 9; G1 = 1; Gc = 1.1;
+H = (((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (G0/Gc)^(1/n))/((1/G1)^(1/n)*(1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (1/Gc)^(1/n)))^n;
+wphi = 0.2*(s+3.142e04)/(s+628.3)/H;
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(w1, freqs, 'Hz'))), '-', 'DisplayName', '$|w_1(j\omega)|$');
+plot(freqs, abs(squeeze(freqresp(w2, freqs, 'Hz'))), '-', 'DisplayName', '$|w_2(j\omega)|$');
+plot(freqs, 1./abs(squeeze(freqresp(wphi, freqs, 'Hz'))), 'k--', 'DisplayName', '$|w_u(j\omega)|^{-1}$');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude');
+hold off;
+legend('location', 'northeast');
+xlim([freqs(1), freqs(end)]);
+
+
+
+% #+NAME: fig:mixed_syn_hinf_weight
+% #+CAPTION: Wanted maximum module uncertainty of the super sensor ([[./figs/mixed_syn_hinf_weight.png][png]], [[./figs/mixed_syn_hinf_weight.pdf][pdf]])
+% [[file:figs/mixed_syn_hinf_weight.png]]
+
+% The equivalent Magnitude and Phase uncertainties are shown in Fig. [[fig:mixed_syn_objective_hinf]].
+
+
+G1 = 1 + w1*ultidyn('Delta',[1 1]);
+G2 = 1 + w2*ultidyn('Delta',[1 1]);
+
+% Few random samples of the sensor dynamics are computed
+G1s = usample(G1, 10);
+G2s = usample(G2, 10);
+
+% We here compute the maximum and minimum phase of both sensors
+Dphi1 = 180/pi*asin(abs(squeeze(freqresp(w1, freqs, 'Hz'))));
+Dphi2 = 180/pi*asin(abs(squeeze(freqresp(w2, freqs, 'Hz'))));
+Dphi1(abs(squeeze(freqresp(w1, freqs, 'Hz'))) > 1) = 190;
+Dphi2(abs(squeeze(freqresp(w2, freqs, 'Hz'))) > 1) = 190;
+
+% We here compute the wanted maximum and minimum phase of the super sensor
+Dphimax = 180/pi*asin(1./abs(squeeze(freqresp(wphi, freqs, 'Hz'))));
+Dphimax(1./abs(squeeze(freqresp(wphi, freqs, 'Hz'))) > 1) = 190;
+
+figure;
+% Magnitude
+ax1 = subaxis(2,1,1);
+hold on;
+set(gca,'ColorOrderIndex',1);
+plot(freqs, 1 + abs(squeeze(freqresp(w1, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S1');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, max(1 - abs(squeeze(freqresp(w1, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
+set(gca,'ColorOrderIndex',2);
+plot(freqs, 1 + abs(squeeze(freqresp(w2, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S2');
+set(gca,'ColorOrderIndex',2);
+plot(freqs, max(1 - abs(squeeze(freqresp(w2, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
+plot(freqs, 1 + 1./abs(squeeze(freqresp(wphi, freqs, 'Hz'))), 'k--', 'DisplayName', 'Synthesis Obj.');
+plot(freqs, max(1 - 1./abs(squeeze(freqresp(wphi, freqs, 'Hz'))), 0), 'k--', 'HandleVisibility', 'off');
+for i = 1:length(G1s)
+  plot(freqs, abs(squeeze(freqresp(G1s(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0.4470 0.7410 0.4], 'HandleVisibility', 'off');
+  plot(freqs, abs(squeeze(freqresp(G2s(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0.8500 0.3250 0.0980 0.4], 'HandleVisibility', 'off');
+end
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]);
+ylabel('Magnitude');
+ylim([1e-1, 10]);
+hold off;
+legend('location', 'southwest');
+
+% Phase
+ax2 = subaxis(2,1,2);
+hold on;
+set(gca,'ColorOrderIndex',1);
+plot(freqs,  Dphi1, '--');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, -Dphi1, '--');
+set(gca,'ColorOrderIndex',2);
+plot(freqs,  Dphi2, '--');
+set(gca,'ColorOrderIndex',2);
+plot(freqs, -Dphi2, '--');
+for i = 1:length(G1s)
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G1s(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0.4470 0.7410 0.4]);
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G2s(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0.8500 0.3250 0.0980 0.4]);
+end
+plot(freqs,  Dphimax, 'k--');
+plot(freqs, -Dphimax, 'k--');
+set(gca,'xscale','log');
+yticks(-180:90:180);
+ylim([-180 180]);
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+linkaxes([ax1,ax2],'x');
+
+% Mixed Synthesis Architecture
+% The synthesis architecture that is used here is shown in Fig. [[fig:mixed_h2_hinf_synthesis]].
+
+% The controller $K$ is synthesized such that it:
+% - Keeps the $\mathcal{H}_\infty$ norm $G$ of the transfer function from $w$ to $z_\infty$ bellow some specified value
+% - Keeps the $\mathcal{H}_2$ norm $H$ of the transfer function from $w$ to $z_2$ bellow some specified value
+% - Minimizes a trade-off criterion of the form $W_1 G^2 + W_2 H^2$ where $W_1$ and $W_2$ are specified values
+
+% #+name: fig:mixed_h2_hinf_synthesis
+% #+caption: Mixed H2/H-Infinity Synthesis
+% [[file:figs-tikz/mixed_h2_hinf_synthesis.png]]
+
+% Here, we define $P$ such that:
+% \begin{align*}
+%   \left\| \frac{z_\infty}{w} \right\|_\infty &= \left\| \begin{matrix}W_1(s) H_1(s) \\ W_2(s) H_2(s)\end{matrix} \right\|_\infty \\
+%   \left\| \frac{z_2}{w} \right\|_2           &= \left\| \begin{matrix}N_1(s) H_1(s) \\ N_2(s) H_2(s)\end{matrix} \right\|_2
+% \end{align*}
+
+% Then:
+% - we specify the maximum value for the $\mathcal{H}_\infty$ norm between $w$ and $z_\infty$ to be $1$
+% - we don't specify any maximum value for the $\mathcal{H}_2$ norm between $w$ and $z_2$
+% - we choose $W_1 = 0$ and $W_2 = 1$ such that the objective is to minimize the $\mathcal{H}_2$ norm between $w$ and $z_2$
+
+% The synthesis objective is to have:
+% \[ \left\| \frac{z_\infty}{w} \right\|_\infty = \left\| \begin{matrix}W_1(s) H_1(s) \\ W_2(s) H_2(s)\end{matrix} \right\|_\infty < 1 \]
+% and to minimize:
+% \[ \left\| \frac{z_2}{w} \right\|_2 = \left\| \begin{matrix}N_1(s) H_1(s) \\ N_2(s) H_2(s)\end{matrix} \right\|_2 \]
+% which is what we wanted.
+
+% We define the generalized plant that will be used for the mixed synthesis.
+
+W1u = ss(w1*wphi); W2u = ss(w2*wphi); % Weight on the uncertainty
+W1n = ss(N1); W2n = ss(N2); % Weight on the noise
+
+P = [W1u -W1u;
+     0    W2u;
+     W1n -W1n;
+     0    W2n;
+     1    0];
+
+% Mixed $\mathcal{H}_2$ / $\mathcal{H}_\infty$ Synthesis
+% The mixed $\mathcal{H}_2$/$\mathcal{H}_\infty$ synthesis is performed below.
+
+Nmeas = 1; Ncon = 1; Nz2 = 2;
+
+[H2,~,normz,~] = h2hinfsyn(P, Nmeas, Ncon, Nz2, [0, 1], 'HINFMAX', 1, 'H2MAX', Inf, 'DKMAX', 100, 'TOL', 0.01, 'DISPLAY', 'on');
+
+H1 = 1 - H2;
+
+
+
+% The obtained complementary filters are shown in Fig. [[fig:comp_filters_mixed_synthesis]].
+
+
+figure;
+
+ax1 = subplot(2,1,1);
+hold on;
+set(gca,'ColorOrderIndex',1)
+plot(freqs, 1./abs(squeeze(freqresp(W1u, freqs, 'Hz'))), '--', 'DisplayName', '$W_1$');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, 1./abs(squeeze(freqresp(W2u, freqs, 'Hz'))), '--', 'DisplayName', '$W_2$');
+
+set(gca,'ColorOrderIndex',1)
+plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), '-', 'DisplayName', '$H_1$');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), '-', 'DisplayName', '$H_2$');
+
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Magnitude');
+set(gca, 'XTickLabel',[]);
+ylim([1e-3, 2]);
+legend('location', 'southwest');
+
+ax2 = subplot(2,1,2);
+hold on;
+set(gca,'ColorOrderIndex',1)
+plot(freqs, 180/pi*phase(squeeze(freqresp(H1, freqs, 'Hz'))), '-');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, 180/pi*phase(squeeze(freqresp(H2, freqs, 'Hz'))), '-');
+hold off;
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+set(gca, 'XScale', 'log');
+yticks([-360:90:360]);
+
+linkaxes([ax1,ax2],'x');
+xlim([freqs(1), freqs(end)]);
+xticks([0.1, 1, 10, 100, 1000]);
+
+% Obtained Super Sensor's noise
+% The PSD and CPS of the super sensor's noise are shown in Fig. [[fig:psd_super_sensor_mixed_syn]] and Fig. [[fig:cps_super_sensor_mixed_syn]] respectively.
+
+
+PSD_S1 = abs(squeeze(freqresp(N1, freqs, 'Hz'))).^2;
+PSD_S2 = abs(squeeze(freqresp(N2, freqs, 'Hz'))).^2;
+PSD_H2 = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2;
+
+figure;
+hold on;
+plot(freqs, PSD_S1, '-',  'DisplayName', '$\Phi_{\hat{x}_1}$');
+plot(freqs, PSD_S2, '-',  'DisplayName', '$\Phi_{\hat{x}_2}$');
+plot(freqs, PSD_H2, 'k-', 'DisplayName', '$\Phi_{\hat{x}}$');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Power Spectral Density');
+hold off;
+xlim([freqs(1), freqs(end)]);
+legend('location', 'northeast');
+
+
+
+% #+NAME: fig:psd_super_sensor_mixed_syn
+% #+CAPTION: Power Spectral Density of the Super Sensor obtained with the mixed $\mathcal{H}_2$/$\mathcal{H}_\infty$ synthesis ([[./figs/psd_super_sensor_mixed_syn.png][png]], [[./figs/psd_super_sensor_mixed_syn.pdf][pdf]])
+% [[file:figs/psd_super_sensor_mixed_syn.png]]
+
+
+
+CPS_S1 = 1/pi*cumtrapz(2*pi*freqs, PSD_S1);
+CPS_S2 = 1/pi*cumtrapz(2*pi*freqs, PSD_S2);
+CPS_H2 = 1/pi*cumtrapz(2*pi*freqs, PSD_H2);
+
+figure;
+hold on;
+plot(freqs, CPS_S1, '-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_1} = %.1e$', sqrt(CPS_S1(end))));
+plot(freqs, CPS_S2, '-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_2} = %.1e$', sqrt(CPS_S2(end))));
+plot(freqs, CPS_H2, 'k-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_2}} = %.1e$', sqrt(CPS_H2(end))));
+set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum');
+hold off;
+xlim([2e-1, freqs(end)]);
+ylim([1e-10 1e-5]);
+legend('location', 'southeast');
+
+% Obtained Super Sensor's Uncertainty
+% The uncertainty on the super sensor's dynamics is shown in Fig. [[]].
+
+
+G1 = 1 + w1*ultidyn('Delta',[1 1]);
+G2 = 1 + w2*ultidyn('Delta',[1 1]);
+
+Gss = G1*H1 + G2*H2;
+Gsss = usample(Gss, 20);
+
+% We here compute the maximum and minimum phase of the super sensor
+Dphiss = 180/pi*asin(abs(squeeze(freqresp(w1*H1, freqs, 'Hz')))+abs(squeeze(freqresp(w2*H2, freqs, 'Hz'))));
+Dphiss(abs(squeeze(freqresp(w1*H1, freqs, 'Hz')))+abs(squeeze(freqresp(w2*H2, freqs, 'Hz'))) > 1) = 190;
+
+% We here compute the maximum and minimum phase of both sensors
+Dphi1 = 180/pi*asin(abs(squeeze(freqresp(w1, freqs, 'Hz'))));
+Dphi2 = 180/pi*asin(abs(squeeze(freqresp(w2, freqs, 'Hz'))));
+Dphi1(abs(squeeze(freqresp(w1, freqs, 'Hz'))) > 1) = 190;
+Dphi2(abs(squeeze(freqresp(w2, freqs, 'Hz'))) > 1) = 190;
+
+figure;
+% Magnitude
+ax1 = subaxis(2,1,1);
+hold on;
+set(gca,'ColorOrderIndex',1);
+plot(freqs, 1 + abs(squeeze(freqresp(w1, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S1');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, max(1 - abs(squeeze(freqresp(w1, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
+set(gca,'ColorOrderIndex',2);
+plot(freqs, 1 + abs(squeeze(freqresp(w2, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S2');
+set(gca,'ColorOrderIndex',2);
+plot(freqs, max(1 - abs(squeeze(freqresp(w2, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
+plot(freqs, 1 + abs(squeeze(freqresp(w1*H1+w2*H2, freqs, 'Hz'))), 'k--', 'DisplayName', 'Bounds - SS');
+plot(freqs, max(1 - abs(squeeze(freqresp(w1*H1+w2*H2, freqs, 'Hz'))), 0), 'k--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(Gsss(1, 1, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2], 'DisplayName', 'SS Dynamics');
+for i = 2:length(Gsss)
+  plot(freqs, abs(squeeze(freqresp(Gsss(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2], 'HandleVisibility', 'off');
+end
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]);
+legend('location', 'southwest');
+ylabel('Magnitude');
+ylim([5e-2, 10]);
+hold off;
+
+% Phase
+ax2 = subaxis(2,1,2);
+hold on;
+set(gca,'ColorOrderIndex',1);
+plot(freqs,  Dphi1, '--');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, -Dphi1, '--');
+set(gca,'ColorOrderIndex',2);
+plot(freqs,  Dphi2, '--');
+set(gca,'ColorOrderIndex',2);
+plot(freqs, -Dphi2, '--');
+plot(freqs,  Dphiss, 'k--');
+plot(freqs, -Dphiss, 'k--');
+for i = 1:length(Gsss)
+  plot(freqs, 180/pi*angle(squeeze(freqresp(Gsss(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2]);
+end
+set(gca,'xscale','log');
+yticks(-180:90:180);
+ylim([-180 180]);
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+linkaxes([ax1,ax2],'x');

matlab/matlab/optimal_comp_filters.m

@@ -41,6 +41,17 @@ legend('location', 'northeast');
 % #+caption: $\mathcal{H}_2$ Synthesis - Generalized plant used for the optimal generation of complementary filters
 % [[file:figs-tikz/h_infinity_optimal_comp_filters.png]]
 
+% \begin{equation*}
+% \begin{pmatrix}
+%   z \\ v
+% \end{pmatrix} = \begin{pmatrix}
+%   0   &  N_2 & 1 \\
+%   N_1 & -N_2 & 0
+% \end{pmatrix} \begin{pmatrix}
+%   w_1 \\ w_2 \\ u
+% \end{pmatrix}
+% \end{equation*}
+
 % The transfer function from $[n_1, n_2]$ to $\hat{x}$ is:
 % \[ \begin{bmatrix} N_1 H_1 \\ N_2 (1 - H_1) \end{bmatrix} \]
 % If we define $H_2 = 1 - H_1$, we obtain:
@@ -382,6 +393,101 @@ xticks([0.1, 1, 10, 100, 1000]);
 PSD_Hb = abs(squeeze(freqresp(N1*H1b, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2b, freqs, 'Hz'))).^2;
 CPS_Hb = 1/pi*cumtrapz(2*pi*freqs, PSD_Hb);
 
+% H-Infinity Synthesis - method C
+
+Wp = 0.56*(inv(N1)+inv(N2))/(1 + s/2/pi/1000);
+
+W1c = N1*Wp;
+W2c = N2*Wp;
+
+P = [W1c -W1c;
+     0    W2c;
+     1    0];
+
+
+
+% And we do the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command.
+
+[H2c, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
+
+
+
+% #+RESULTS:
+% #+begin_example
+% [H2c, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
+% Test bounds:      0.0000 <  gamma  <=     36.7543
+
+%   gamma    hamx_eig  xinf_eig  hamy_eig   yinf_eig   nrho_xy   p/f
+%    36.754   5.7e+00 -1.0e-13   6.3e+00   -6.2e-25    0.0000    p
+%    18.377   5.7e+00 -1.4e-12   6.3e+00   -1.8e-13    0.0000    p
+%     9.189   5.7e+00 -4.3e-13   6.3e+00   -4.7e-15    0.0000    p
+%     4.594   5.7e+00 -9.4e-13   6.3e+00   -4.7e-15    0.0000    p
+%     2.297   5.7e+00 -1.3e-16   6.3e+00   -6.8e-14    0.0000    p
+%     1.149   5.7e+00 -1.6e-17   6.3e+00   -1.5e-15    0.0000    p
+%     0.574   5.7e+00 -5.2e+02#  6.3e+00   -5.9e-14    0.0000    f
+%     0.861   5.7e+00 -3.1e+04#  6.3e+00   -3.8e-14    0.0000    f
+%     1.005   5.7e+00 -1.6e-12   6.3e+00   -1.1e-14    0.0000    p
+%     0.933   5.7e+00 -1.1e+05#  6.3e+00   -7.2e-14    0.0000    f
+%     0.969   5.7e+00 -3.3e+05#  6.3e+00   -5.6e-14    0.0000    f
+%     0.987   5.7e+00 -1.2e+06#  6.3e+00   -4.5e-15    0.0000    f
+%     0.996   5.7e+00 -6.5e-16   6.3e+00   -1.7e-15    0.0000    p
+%     0.992   5.7e+00 -2.9e+06#  6.3e+00   -6.1e-14    0.0000    f
+%     0.994   5.7e+00 -9.7e+06#  6.3e+00   -3.0e-16    0.0000    f
+%     0.995   5.7e+00 -8.0e-10   6.3e+00   -1.9e-13    0.0000    p
+%     0.994   5.7e+00 -2.3e+07#  6.3e+00   -4.3e-14    0.0000    f
+
+%  Gamma value achieved:     0.9949
+% #+end_example
+
+
+H1c = 1 - H2c;
+
+figure;
+
+ax1 = subplot(2,1,1);
+hold on;
+set(gca,'ColorOrderIndex',1)
+plot(freqs, 1./abs(squeeze(freqresp(W1c, freqs, 'Hz'))), '--', 'DisplayName', '$w_1$');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, 1./abs(squeeze(freqresp(W2c, freqs, 'Hz'))), '--', 'DisplayName', '$w_2$');
+
+set(gca,'ColorOrderIndex',1)
+plot(freqs, abs(squeeze(freqresp(H1c, freqs, 'Hz'))), '-', 'DisplayName', '$H_1$');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, abs(squeeze(freqresp(H2c, freqs, 'Hz'))), '-', 'DisplayName', '$H_2$');
+
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Magnitude');
+set(gca, 'XTickLabel',[]);
+ylim([5e-4, 20]);
+legend('location', 'northeast');
+
+ax2 = subplot(2,1,2);
+hold on;
+set(gca,'ColorOrderIndex',1)
+plot(freqs, 180/pi*phase(squeeze(freqresp(H1c, freqs, 'Hz'))), '-');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, 180/pi*phase(squeeze(freqresp(H2c, freqs, 'Hz'))), '-');
+hold off;
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+set(gca, 'XScale', 'log');
+yticks([-360:90:360]);
+
+linkaxes([ax1,ax2],'x');
+xlim([freqs(1), freqs(end)]);
+xticks([0.1, 1, 10, 100, 1000]);
+
+
+
+% #+NAME: fig:weights_comp_filters_Hinfc
+% #+CAPTION: Weights and Complementary Fitlers obtained ([[./figs/weights_comp_filters_Hinfc.png][png]], [[./figs/weights_comp_filters_Hinfc.pdf][pdf]])
+% [[file:figs/weights_comp_filters_Hinfc.png]]
+
+
+PSD_Hc = abs(squeeze(freqresp(N1*H1c, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2c, freqs, 'Hz'))).^2;
+CPS_Hc = 1/pi*cumtrapz(2*pi*freqs, PSD_Hc);
+
 
 
 % #+name: tab:rms_results
@@ -394,6 +500,7 @@ CPS_Hb = 1/pi*cumtrapz(2*pi*freqs, PSD_Hb);
 % | H2 Fusion    |   1.2e-04 |
 % | H-Infinity a |   2.4e-04 |
 % | H-Infinity b |   1.4e-04 |
+% | H-Infinity c |   2.2e-04 |
 
 
 
@@ -401,9 +508,10 @@ figure;
 hold on;
 plot(freqs, PSD_S1, '-',  'DisplayName', '$\Phi_{\hat{x}_1}$');
 plot(freqs, PSD_S2, '-',  'DisplayName', '$\Phi_{\hat{x}_2}$');
-plot(freqs, PSD_H2, 'k-', 'DisplayName', '$\Phi_{\hat{x}_{\mathcal{H}_2}}$');
+plot(freqs, PSD_H2, 'r-', 'DisplayName', '$\Phi_{\hat{x}_{\mathcal{H}_2}}$');
-plot(freqs, PSD_Ha, 'k--', 'DisplayName', '$\Phi_{\hat{x}_{\mathcal{H}_\infty},a}$');
+plot(freqs, PSD_Ha, 'k-', 'DisplayName', '$\Phi_{\hat{x}_{\mathcal{H}_\infty},a}$');
-plot(freqs, PSD_Hb, 'k-.', 'DisplayName', '$\Phi_{\hat{x}_{\mathcal{H}_\infty},b}$');
+plot(freqs, PSD_Hb, 'k--', 'DisplayName', '$\Phi_{\hat{x}_{\mathcal{H}_\infty},b}$');
+plot(freqs, PSD_Hc, 'k-.', 'DisplayName', '$\Phi_{\hat{x}_{\mathcal{H}_\infty},c}$');
 set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
 xlabel('Frequency [Hz]'); ylabel('Power Spectral Density');
 hold off;
@@ -421,12 +529,104 @@ figure;
 hold on;
 plot(freqs, CPS_S1, '-',  'DisplayName', sprintf('$\\sigma_{\\hat{x}_1} = %.1e$', sqrt(CPS_S1(end))));
 plot(freqs, CPS_S2, '-',  'DisplayName', sprintf('$\\sigma_{\\hat{x}_2} = %.1e$', sqrt(CPS_S2(end))));
-plot(freqs, CPS_H2, 'k-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_2}} = %.1e$', sqrt(CPS_H2(end))));
+plot(freqs, CPS_H2, 'r-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_2}} = %.1e$', sqrt(CPS_H2(end))));
-plot(freqs, CPS_Ha, 'k--', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_\\infty, a}} = %.1e$', sqrt(CPS_Ha(end))));
+plot(freqs, CPS_Ha, 'k-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_\\infty, a}} = %.1e$', sqrt(CPS_Ha(end))));
-plot(freqs, CPS_Hb, 'k-.', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_\\infty, b}} = %.1e$', sqrt(CPS_Hb(end))));
+plot(freqs, CPS_Hb, 'k--', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_\\infty, b}} = %.1e$', sqrt(CPS_Hb(end))));
+plot(freqs, CPS_Hc, 'k-.', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_\\infty, c}} = %.1e$', sqrt(CPS_Hc(end))));
 set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log');
 xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum');
 hold off;
 xlim([2e-1, freqs(end)]);
 ylim([1e-10 1e-5]);
 legend('location', 'southeast');
+
+% Obtained Super Sensor's noise uncertainty
+% We would like to verify if the obtained sensor fusion architecture is robust to change in the sensor dynamics.
+
+% To study the dynamical uncertainty on the super sensor, we defined some multiplicative uncertainty on both sensor dynamics.
+% Two weights $w_1(s)$ and $w_2(s)$ are used to described the amplitude of the dynamical uncertainty.
+
+
+omegac = 100*2*pi; G0 = 0.1; Ginf = 10;
+w1 = (Ginf*s/omegac + G0)/(s/omegac + 1);
+
+omegac = 0.2*2*pi; G0 = 5; Ginf = 0.1;
+w2 = (Ginf*s/omegac + G0)/(s/omegac + 1);
+omegac = 5000*2*pi; G0 = 1; Ginf = 50;
+w2 = w2*(Ginf*s/omegac + G0)/(s/omegac + 1);
+
+
+
+% The sensor uncertain models are defined below.
+
+G1 = 1 + w1*ultidyn('Delta',[1 1]);
+G2 = 1 + w2*ultidyn('Delta',[1 1]);
+
+% We here compute the maximum and minimum phase of both sensors
+Dphi1 = 180/pi*asin(abs(squeeze(freqresp(w1, freqs, 'Hz'))));
+Dphi2 = 180/pi*asin(abs(squeeze(freqresp(w2, freqs, 'Hz'))));
+Dphi1(abs(squeeze(freqresp(w1, freqs, 'Hz'))) > 1) = 190;
+Dphi2(abs(squeeze(freqresp(w2, freqs, 'Hz'))) > 1) = 190;
+
+
+
+% The super sensor uncertain model is defined below using the complementary filters obtained with the $\mathcal{H}_2$ synthesis.
+% The dynamical uncertainty bounds of the super sensor is shown in Fig. [[fig:uncertainty_super_sensor_H2_syn]].
+% Right Half Plane zero might be introduced in the super sensor dynamics which will render the feedback system unstable.
+
+
+Gss = G1*H1 + G2*H2;
+
+Gsss = usample(Gss, 20);
+
+% We here compute the maximum and minimum phase of the super sensor
+Dphiss = 180/pi*asin(abs(squeeze(freqresp(w1*H1, freqs, 'Hz')))+abs(squeeze(freqresp(w2*H2, freqs, 'Hz'))));
+Dphiss(abs(squeeze(freqresp(w1*H1, freqs, 'Hz')))+abs(squeeze(freqresp(w2*H2, freqs, 'Hz'))) > 1) = 190;
+
+figure;
+% Magnitude
+ax1 = subaxis(2,1,1);
+hold on;
+set(gca,'ColorOrderIndex',1);
+plot(freqs, 1 + abs(squeeze(freqresp(w1, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S1');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, max(1 - abs(squeeze(freqresp(w1, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
+set(gca,'ColorOrderIndex',2);
+plot(freqs, 1 + abs(squeeze(freqresp(w2, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S2');
+set(gca,'ColorOrderIndex',2);
+plot(freqs, max(1 - abs(squeeze(freqresp(w2, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
+plot(freqs, 1 + abs(squeeze(freqresp(w1*H1, freqs, 'Hz'))) + abs(squeeze(freqresp(w2*H2, freqs, 'Hz'))), 'k--', 'DisplayName', 'Bounds - SS');
+plot(freqs, max(1 - abs(squeeze(freqresp(w1*H1, freqs, 'Hz'))) - abs(squeeze(freqresp(w2*H2, freqs, 'Hz'))), 0), 'k--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(Gsss(1, 1, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2], 'DisplayName', 'SS Dynamics');
+for i = 2:length(Gsss)
+  plot(freqs, abs(squeeze(freqresp(Gsss(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2], 'HandleVisibility', 'off');
+end
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]);
+legend('location', 'southwest');
+ylabel('Magnitude');
+ylim([5e-2, 10]);
+hold off;
+
+% Phase
+ax2 = subaxis(2,1,2);
+hold on;
+set(gca,'ColorOrderIndex',1);
+plot(freqs,  Dphi1, '--');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, -Dphi1, '--');
+set(gca,'ColorOrderIndex',2);
+plot(freqs,  Dphi2, '--');
+set(gca,'ColorOrderIndex',2);
+plot(freqs, -Dphi2, '--');
+plot(freqs,  Dphiss, 'k--');
+plot(freqs, -Dphiss, 'k--');
+for i = 1:length(Gsss)
+  plot(freqs, 180/pi*angle(squeeze(freqresp(Gsss(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2]);
+end
+set(gca,'xscale','log');
+yticks(-180:90:180);
+ylim([-180 180]);
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+linkaxes([ax1,ax2],'x');