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

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

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

% Mixed $\mathcal{H}_2$ / $\mathcal{H}_\infty$ Synthesis
% <<sec:H2_Hinf_synthesis>>

% The synthesis architecture that is used here is shown in Figure [[fig:mixed_h2_hinf_synthesis]].

% The filter $H_2(s)$ is synthesized such that it:
% - keeps the $\mathcal{H}_\infty$ norm of the transfer function from $w$ to $z_{\mathcal{H}_\infty}$ bellow some specified value
% - minimizes the $\mathcal{H}_2$ norm of the transfer function from $w$ to $z_{\mathcal{H}_2}$

% #+name: fig:mixed_h2_hinf_synthesis
% #+caption: Mixed $\mathcal{H}_2/\mathcal{H}_\infty$ Synthesis
% [[file:figs-tikz/mixed_h2_hinf_synthesis.png]]

% Let's see that
% with $H_1(s)= 1 - H_2(s)$
% \begin{align}
%   \left\| \frac{z_\infty}{w} \right\|_\infty &= \left\| \begin{matrix}H_1(s) W_1(s) W_u(s)\\ H_2(s) W_2(s) W_u(s)\end{matrix} \right\|_\infty \\
%   \left\| \frac{z_2}{w} \right\|_2           &= \left\| \begin{matrix}H_1(s) N_1(s)       \\ H_2(s) N_2(s)\end{matrix} \right\|_2 = \sigma_n
% \end{align}

% The generalized plant $P_{\mathcal{H}_2/\mathcal{H}_\infty}$ is defined below

W1u = ss(W2*Wu); W2u = ss(W1*Wu); % Weight on the uncertainty
W1n = ss(N2); W2n = ss(N1); % Weight on the noise

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



% And the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis is performed.

[H2, ~] = h2hinfsyn(ss(P), 1, 1, 2, [0, 1], 'HINFMAX', 1, 'H2MAX', Inf, 'DKMAX', 100, 'TOL', 1e-3, 'DISPLAY', 'on');

H1 = 1 - H2;

% The obtained filters are saved for further analysis
save('./mat/H2_Hinf_filters.mat', 'H2', 'H1');



% The obtained complementary filters are shown in Figure [[fig:htwo_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',[]);
ylim([1e-3, 2]);
legend('location', 'southeast', 'FontSize', 8);

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

% Obtained Super Sensor's noise
% The Amplitude Spectral Density of the super sensor's noise is shown in Figure [[fig:psd_sensors_htwo_hinf_synthesis]].

% A time domain simulation is shown in Figure [[fig:super_sensor_time_domain_h2_hinf]].

% The RMS values of the super sensor noise for the presented three synthesis are listed in Table [[tab:rms_noise_comp]].


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

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;

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

PSD_Hinf   = abs(squeeze(freqresp(N1*Hinf_filters.H1, freqs, 'Hz'))).^2 + ...
             abs(squeeze(freqresp(N2*Hinf_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}}$');
plot(freqs, sqrt(PSD_H2Hinf), 'k-.', 'DisplayName', '$\Phi_{n_{\mathcal{H}_2/\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', 3);



% #+name: fig:psd_sensors_htwo_hinf_synthesis
% #+CAPTION: Power Spectral Density of the Super Sensor obtained with the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis
% #+RESULTS:
% [[file:figs/psd_sensors_htwo_hinf_synthesis.png]]


Fs = 1e4;  % Sampling Frequency [Hz]
Ts = 1/Fs; % Sampling Time [s]

t = 0:Ts:2; % Time Vector [s]

v = 0.1*sin((10*t).*t)'; % Velocity measured [m/s]

% Generate noises in velocity corresponding to sensor 1 and 2:
n1 = lsim(N1, sqrt(Fs/2)*randn(length(t), 1), t);
n2 = lsim(N2, sqrt(Fs/2)*randn(length(t), 1), t);

figure;
hold on;
set(gca,'ColorOrderIndex',2)
plot(t, v + n2, 'DisplayName', '$\hat{x}_2$');
set(gca,'ColorOrderIndex',1)
plot(t, v + n1, 'DisplayName', '$\hat{x}_1$');
set(gca,'ColorOrderIndex',3)
plot(t, v + (lsim(H1, n1, t) + lsim(H2, n2, t)), 'DisplayName', '$\hat{x}$');
plot(t, v, 'k--', 'DisplayName', '$x$');
hold off;
xlabel('Time [s]'); ylabel('Velocity [m/s]');
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 3);
ylim([-0.3, 0.3]);

% Obtained Super Sensor's Uncertainty
% The uncertainty on the super sensor's dynamics is shown in Figure [[fig:super_sensor_dynamical_uncertainty_Htwo_Hinf]].


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