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

% $\mathcal{H}_2$ Synthesis
% <<sec:H2_synthesis>>

% Consider the generalized plant $P_{\mathcal{H}_2}$ shown in Figure [[fig:h_two_optimal_fusion]] and described by Equation eqref:eq:H2_generalized_plant.

% #+name: fig:h_two_optimal_fusion
% #+caption: Architecture used for $\mathcal{H}_\infty$ synthesis of complementary filters
% [[file:figs-tikz/h_two_optimal_fusion.png]]

% \begin{equation} \label{eq:H2_generalized_plant}
% \begin{pmatrix}
%   z_1 \\ z_2 \\ v
% \end{pmatrix} = \underbrace{\begin{bmatrix}
%   N_1 & -N_1 \\
%   0   &  N_2 \\
%   1   &  0
% \end{bmatrix}}_{P_{\mathcal{H}_2}} \begin{pmatrix}
%   w \\ u
% \end{pmatrix}
% \end{equation}

% Applying the $\mathcal{H}_2$ synthesis on $P_{\mathcal{H}_2}$ will generate a filter $H_2(s)$ such that the $\mathcal{H}_2$ norm from $w$ to $(z_1,z_2)$ which is actually equals to $\sigma_n$ by defining $H_1(s) = 1 - H_2(s)$:
% \begin{equation}
%    \left\| \begin{matrix} z_1/w \\ z_2/w \end{matrix} \right\|_2 = \left\| \begin{matrix} N_1 (1 - H_2) \\ N_2 H_2 \end{matrix} \right\|_2 = \sigma_n \quad \text{with} \quad H_1(s) = 1 - H_2(s)
% \end{equation}

% We then have that the $\mathcal{H}_2$ synthesis applied on $P_{\mathcal{H}_2}$ generates two complementary filters $H_1(s)$ and $H_2(s)$ such that the RMS value of super sensor noise is minimized.

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

PH2 = [N1 -N1;
       0   N2;
       1   0];



% The $\mathcal{H}_2$ synthesis using the =h2syn= command

[H2, ~, gamma] = h2syn(PH2, 1, 1);



% Finally, $H_1(s)$ is defined as follows

H1 = 1 - H2;

% Filters are saved for further use
save('./mat/H2_filters.mat', 'H2', 'H1');



% The obtained complementary filters are shown in Figure [[fig:htwo_comp_filters]].

figure;

% Magnitude
ax1 = subplot(2,1,1);
hold on;
plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), 'DisplayName', '$H_1$');
plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), 'DisplayName', '$H_2$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude');
hold off;
legend('location', 'northeast', 'FontSize', 8);

% Phase
ax2 = subplot(2,1,2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(H1, freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(H2, freqs, 'Hz'))));
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);

% Super Sensor Noise
% <<sec:H2_super_sensor_noise>>

% The Power Spectral Density of the individual sensors' noise $\Phi_{n_1}, \Phi_{n_2}$ and of the super sensor noise $\Phi_{n_{\mathcal{H}_2}}$ are computed below.

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;



% #+name: tab:rms_noise_H2
% #+caption: RMS value of the individual sensor noise and of the super sensor using the $\mathcal{H}_2$ Synthesis
% #+attr_latex: :environment tabular :align cc
% #+attr_latex: :center t :booktabs t :float t
% #+RESULTS:
% |                              | RMS value $[m/s]$ |
% |------------------------------+-------------------|
% | $\sigma_{n_1}$               |             0.015 |
% | $\sigma_{n_2}$               |             0.080 |
% | $\sigma_{n_{\mathcal{H}_2}}$ |             0.003 |


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}}$');
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);



% #+name: fig:psd_sensors_htwo_synthesis
% #+caption: Power Spectral Density of the estimated $\hat{x}$ using the two sensors alone and using the optimally fused signal
% #+RESULTS:
% [[file:figs/psd_sensors_htwo_synthesis.png]]

% A time domain simulation is now performed.
% The measured velocity $x$ is set to be a sweep sine with an amplitude of $0.1\ [m/s]$.
% The velocity estimates from the two sensors and from the super sensors are shown in Figure [[fig:super_sensor_time_domain_h2]].
% The resulting noises are displayed in Figure [[fig:sensor_noise_H2_time_domain]].


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', 2);
ylim([-0.3, 0.3]);



% #+name: fig:super_sensor_time_domain_h2
% #+caption: Noise of individual sensors and noise of the super sensor
% #+RESULTS:
% [[file:figs/super_sensor_time_domain_h2.png]]


figure;
hold on;
set(gca,'ColorOrderIndex',2)
plot(t, n2, 'DisplayName', '$n_2$');
set(gca,'ColorOrderIndex',1)
plot(t, n1, 'DisplayName', '$n_1$');
set(gca,'ColorOrderIndex',3)
plot(t, (lsim(H1, n1, t)+lsim(H2, n2, t)), '-', 'DisplayName', '$n$');
hold off;
xlabel('Time [s]'); ylabel('Sensor Noise [m/s]');
legend('FontSize', 8);
ylim([-0.2, 0.2]);

% Discrepancy between sensor dynamics and model
% If we consider sensor dynamical uncertainty as explained in Section [[sec:sensor_uncertainty]], we can compute what would be the super sensor dynamical uncertainty when using the complementary filters obtained using the $\mathcal{H}_2$ Synthesis.

% The super sensor dynamical uncertainty is shown in Figure [[fig:super_sensor_dynamical_uncertainty_H2]].

% It is shown that the phase uncertainty is not bounded between 100Hz and 200Hz.
% As a result the super sensor signal can not be used for feedback applications about 100Hz.

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