%% Clear Workspace and Close figures
clear; close all; clc;

%% Intialize Laplace variable
s = zpk('s');

addpath('src');
load('./mat/model.mat', 'freqs', 'G1', 'G2', 'N2', 'N1', 'W2', 'W1');

% Weighting Function used to bound the super sensor uncertainty
% <<sec:weight_uncertainty>>

% $W_u(s)$ is defined such that the super sensor phase uncertainty is less than 10 degrees below 100Hz eqref:eq:phase_uncertainy_bound_low_freq and is less than 180 degrees below 400Hz eqref:eq:phase_uncertainty_max.

% \begin{align}
%   \frac{1}{|W_u(j\omega)|} &< \sin\left(10 \frac{\pi}{180}\right), \quad \omega < 100\,\text{Hz} \label{eq:phase_uncertainy_bound_low_freq} \\
%   \frac{1}{|W_u(j 2 \pi 400)|} &< 1 \label{eq:phase_uncertainty_max}
% \end{align}

% The uncertainty bounds of the two individual sensor as well as the wanted maximum uncertainty bounds of the super sensor are shown in Figure [[fig:weight_uncertainty_bounds_Wu]].


Dphi = 10; % [deg]

Wu = createWeight('n', 2, 'w0', 2*pi*4e2, 'G0', 1/sin(Dphi*pi/180), 'G1', 1/4, 'Gc', 1);

% Wu is saved for further use
save('./mat/Wu.mat', 'Wu');

Dphi_Wu = 180/pi*asin(abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))));
Dphi_Wu(abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))) > 1) = 360;

figure;
% Magnitude
ax1 = subplot(2,1,1);
hold on;
plotMagUncertainty(W1, freqs, 'color_i', 1, 'DisplayName', '$1 + W_1 \Delta_1$');
plotMagUncertainty(W2, freqs, 'color_i', 2, 'DisplayName', '$1 + W_2 \Delta_2$');
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 = subplot(2,1,2);
hold on;
plotPhaseUncertainty(W1, freqs, 'color_i', 1);
plotPhaseUncertainty(W2, 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)]);

% $\mathcal{H}_\infty$ Synthesis
% <<sec:Hinfinity_synthesis>>

% The generalized plant $P_{\mathcal{H}_\infty}$ used for the $\mathcal{H}_\infty$ Synthesis of the complementary filters is shown in Figure [[fig:h_infinity_robust_fusion]] and is described by Equation eqref:eq:Hinf_generalized_plant.

% #+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]]

% \begin{equation} \label{eq:Hinf_generalized_plant}
% \begin{pmatrix}
%   z_1 \\ z_2 \\ v
% \end{pmatrix} = \underbrace{\begin{bmatrix}
%   W_u W_1 & -W_u W_1 \\
%   0       &  W_u W_2 \\
%   1       &  0
% \end{bmatrix}}_{P_{\mathcal{H}_\infty}} \begin{pmatrix}
%   w \\ u
% \end{pmatrix}
% \end{equation}

% The generalized plant is defined below.

P = [Wu*W1 -Wu*W1;
     0      Wu*W2;
     1      0];



% And the $\mathcal{H}_\infty$ synthesis is performed using the =hinfsyn= command.

H2 = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'DISPLAY', 'on');



% #+RESULTS:
% #+begin_example
%   Test bounds:  0.7071 <=  gamma  <=  1.291

%     gamma        X>=0        Y>=0       rho(XY)<1    p/f
%   9.554e-01     0.0e+00     0.0e+00     3.529e-16     p
%   8.219e-01     0.0e+00     0.0e+00     5.204e-16     p
%   7.624e-01     3.8e-17     0.0e+00     1.955e-15     p
%   7.342e-01     0.0e+00     0.0e+00     5.612e-16     p
%   7.205e-01     0.0e+00     0.0e+00     7.184e-16     p
%   7.138e-01     0.0e+00     0.0e+00     0.000e+00     p
%   7.104e-01     4.1e-16     0.0e+00     6.749e-15     p
%   7.088e-01     0.0e+00     0.0e+00     2.794e-15     p
%   7.079e-01     0.0e+00     0.0e+00     6.503e-16     p
%   7.075e-01     0.0e+00     0.0e+00     4.302e-15     p

%   Best performance (actual): 0.7071
% #+end_example

% The $\mathcal{H}_\infty$ is successful as the $\mathcal{H}_\infty$ norm of the "closed loop" transfer function from $(w)$ to $(z_1,\ z_2)$ is less than one.

% $H_1(s)$ is then defined as the complementary of $H_2(s)$.

H1 = 1 - H2;

% Complementary filters are saved for further analysis
save('./mat/Hinf_filters.mat', 'H2', 'H1');



% The obtained complementary filters as well as the wanted upper bounds are shown in Figure [[fig:hinf_comp_filters]].


figure;

ax1 = subplot(2,1,1);
hold on;
plot(freqs, 1./abs(squeeze(freqresp(Wu*W1, freqs, 'Hz'))), '--', 'DisplayName', '$1/|W_uW_1|$');
plot(freqs, 1./abs(squeeze(freqresp(Wu*W2, freqs, 'Hz'))), '--', 'DisplayName', '$1/|W_uW_2|$');
set(gca,'ColorOrderIndex',1)
plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), '-', 'DisplayName', '$|H_1|$');
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', 'FontSize', 8, 'NumColumns', 2);

ax2 = subplot(2,1,2);
hold on;
plot(freqs, 180/pi*phase(squeeze(freqresp(H1, freqs, 'Hz'))), '-');
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)]);

% Super sensor uncertainty
% The super sensor dynamical uncertainty is displayed in Figure [[fig:super_sensor_dynamical_uncertainty_Hinf]].
% It is confirmed that the super sensor dynamical uncertainty is less than the maximum allowed uncertainty defined by the norm of $W_u(s)$.

% The $\mathcal{H}_\infty$ synthesis thus allows to design filters such that the super sensor has specified bounded uncertainty.


Dphi_Wu = 180/pi*asin(abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))));
Dphi_Wu(abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))) > 1) = 360;

Dphi_ss = 180/pi*asin(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))));
Dphi_ss(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))) > 1) = 360;

figure;
% Magnitude
ax1 = subplot(2,1,1);
hold on;
plotMagUncertainty(W1, freqs, 'color_i', 1, 'DisplayName', '$1 + W_1 \Delta_1$');
plotMagUncertainty(W2, freqs, 'color_i', 2, 'DisplayName', '$1 + W_2 \Delta_2$');
plot(freqs, 1 + abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))+abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))), 'k-', ...
     'DisplayName', '$1 + W_1 \Delta_1 + W_2 \Delta_2$')
plot(freqs, max(1 - abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))-abs(squeeze(freqresp(W1*H1, 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 = subplot(2,1,2);
hold on;
plotPhaseUncertainty(W1, freqs, 'color_i', 1);
plotPhaseUncertainty(W2, 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)]);

% Super sensor noise
% We now compute the obtain Power Spectral Density of the super sensor's noise.
% The Amplitude Spectral Densities are shown in Figure [[fig:psd_sensors_hinf_synthesis]].


PSD_S2   = abs(squeeze(freqresp(N2,    freqs, 'Hz'))).^2;
PSD_S1   = abs(squeeze(freqresp(N1,    freqs, 'Hz'))).^2;
PSD_Hinf = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2 + ...
           abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2;



% The obtained RMS of the super sensor noise in the $\mathcal{H}_2$ and $\mathcal{H}_\infty$ case are shown in Table [[tab:rms_noise_comp_H2_Hinf]].
% As expected, the super sensor obtained from the $\mathcal{H}_\infty$ synthesis is much noisier than the super sensor obtained from the $\mathcal{H}_2$ synthesis.


H2_filters = load('./mat/H2_filters.mat', 'H2', 'H1');

PSD_H2   = abs(squeeze(freqresp(N1*H2_filters.H1, freqs, 'Hz'))).^2 + ...
           abs(squeeze(freqresp(N2*H2_filters.H2, freqs, 'Hz'))).^2;

figure;
hold on;
plot(freqs, sqrt(PSD_S1),   '-',   'DisplayName', '$\phi_{n_1}$');
plot(freqs, sqrt(PSD_S2),   '-',   'DisplayName', '$\phi_{n_2}$');
plot(freqs, sqrt(PSD_H2),   'k-',  'DisplayName', '$\phi_{n_{\mathcal{H}_2}}$');
plot(freqs, sqrt(PSD_Hinf), 'k--', 'DisplayName', '$\phi_{n_{\mathcal{H}_\infty}}$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD $\left[ \frac{m/s}{\sqrt{Hz}} \right]$');
hold off;
xlim([freqs(1), freqs(end)]);
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
