%% 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 = subplot(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 = subplot(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');
