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

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

addpath('src');
freqs = logspace(0, 4, 1000);

% Sensor Dynamics
% <<sec:sensor_dynamics>>
% Let's consider two sensors measuring the velocity of an object.

% The first sensor is an accelerometer.
% Its nominal dynamics $\hat{G}_1(s)$ is defined below.

m_acc = 0.01; % Inertial Mass [kg]
c_acc = 5;    % Damping [N/(m/s)]
k_acc = 1e5;  % Stiffness [N/m]
g_acc = 1e5;  % Gain [V/m]

G1 = g_acc*m_acc*s/(m_acc*s^2 + c_acc*s + k_acc); % Accelerometer Plant [V/(m/s)]



% The second sensor is a displacement sensor, its nominal dynamics $\hat{G}_2(s)$ is defined below.

w_pos = 2*pi*2e3; % Measurement Banwdith [rad/s]
g_pos = 1e4; % Gain [V/m]

G2 = g_pos/s/(1 + s/w_pos); % Position Sensor Plant [V/(m/s)]



% These nominal dynamics are also taken as the model of the sensor dynamics.
% The true sensor dynamics has some uncertainty associated to it and described in section [[sec:sensor_uncertainty]].

% Both sensor dynamics in $[\frac{V}{m/s}]$ are shown in Figure [[fig:sensors_nominal_dynamics]].

figure;
% Magnitude
ax1 = subplot(2,1,1);
hold on;
plot(freqs, abs(squeeze(freqresp(G1, freqs, 'Hz'))), '-', 'DisplayName', '$G_1(j\omega)$');
plot(freqs, abs(squeeze(freqresp(G2, freqs, 'Hz'))), '-', 'DisplayName', '$G_2(j\omega)$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude $\left[\frac{V}{m/s}\right]$'); set(gca, 'XTickLabel',[]);
legend('location', 'northeast', 'FontSize', 8);
hold off;

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

% Sensor Model Uncertainty
% <<sec:sensor_uncertainty>>
% The uncertainty on the sensor dynamics is described by multiplicative uncertainty (Figure [[fig:sensor_model_noise_uncertainty]]).

% The true sensor dynamics $G_i(s)$ is then described by eqref:eq:sensor_dynamics_uncertainty.

% \begin{equation}
%   G_i(s) = \hat{G}_i(s) \left( 1 + W_i(s) \Delta_i(s) \right); \quad |\Delta_i(j\omega)| < 1 \forall \omega \label{eq:sensor_dynamics_uncertainty}
% \end{equation}

% The weights $W_i(s)$ representing the dynamical uncertainty are defined below and their magnitude is shown in Figure [[fig:sensors_uncertainty_weights]].

W1 = createWeight('n', 2, 'w0', 2*pi*3,   'G0', 2, 'G1', 0.1,     'Gc', 1) * ...
     createWeight('n', 2, 'w0', 2*pi*1e3, 'G0', 1, 'G1', 4/0.1, 'Gc', 1/0.1);

W2 = createWeight('n', 2, 'w0', 2*pi*1e2, 'G0', 0.05, 'G1', 4, 'Gc', 1);



% The bode plot of the sensors nominal dynamics as well as their defined dynamical spread are shown in Figure [[fig:sensors_nominal_dynamics_and_uncertainty]].


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)|$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
ylim([0, 5]);
xlim([freqs(1), freqs(end)]);
legend('location', 'northwest', 'FontSize', 8);



% #+name: fig:sensors_uncertainty_weights
% #+caption: Magnitude of the multiplicative uncertainty weights $|W_i(j\omega)|$
% #+RESULTS:
% [[file:figs/sensors_uncertainty_weights.png]]


figure;
% Magnitude
ax1 = subplot(2,1,1);
hold on;
plotMagUncertainty(W1, freqs, 'G', G1, 'color_i', 1, 'DisplayName', '$G_1$');
plotMagUncertainty(W2, freqs, 'G', G2, 'color_i', 2, 'DisplayName', '$G_2$');

set(gca,'ColorOrderIndex',1)
plot(freqs, abs(squeeze(freqresp(G1, freqs, 'Hz'))), 'DisplayName', '$\hat{G}_1$');
plot(freqs, abs(squeeze(freqresp(G2, freqs, 'Hz'))), 'DisplayName', '$\hat{G}_2$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude $[\frac{V}{m/s}]$');
ylim([1e-2, 2e3]);
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 2);
hold off;
ylim([1e-2, 1e4])

% Phase
ax2 = subplot(2,1,2);
hold on;
plotPhaseUncertainty(W1, freqs, 'G', G1, 'color_i', 1);
plotPhaseUncertainty(W2, freqs, 'G', G2, 'color_i', 2);

set(gca,'ColorOrderIndex',1)
plot(freqs, 180/pi*angle(squeeze(freqresp(G1, freqs, 'Hz'))), 'DisplayName', '$\hat{G}_1$');
plot(freqs, 180/pi*angle(squeeze(freqresp(G2, freqs, 'Hz'))), 'DisplayName', '$\hat{G}_2$');
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)]);

% Sensor Noise
% <<sec:sensor_noise>>
% The noise of the sensors $n_i$ are modelled by shaping a white noise with unitary PSD $\tilde{n}_i$ eqref:eq:unitary_noise_psd with a LTI transfer function $N_i(s)$ (Figure [[fig:sensor_model_noise_uncertainty]]).
% \begin{equation}
%   \Phi_{\tilde{n}_i}(\omega) = 1 \label{eq:unitary_noise_psd}
% \end{equation}

% The Power Spectral Density of the sensor noise $\Phi_{n_i}(\omega)$ is then computed using eqref:eq:sensor_noise_shaping and expressed in $[\frac{(m/s)^2}{Hz}]$.
% \begin{equation}
%   \Phi_{n_i}(\omega) = \left| N_i(j\omega) \right|^2 \Phi_{\tilde{n}_i}(\omega) \label{eq:sensor_noise_shaping}
% \end{equation}

% The weights $N_1$ and $N_2$ representing the amplitude spectral density of the sensor noises are defined below and shown in Figure [[fig:sensors_noise]].

omegac = 0.15*2*pi; G0 = 1e-1; Ginf = 1e-6;
N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/1e4);

omegac = 1000*2*pi; G0 = 1e-6; Ginf = 1e-3;
N2 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/1e4);

figure;
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('ASD $\left[ \frac{m/s}{\sqrt{Hz}} \right]$');
hold off;
xlim([freqs(1), freqs(end)]);
legend('location', 'northeast', 'FontSize', 8);

% Save Model
% All the dynamical systems representing the sensors are saved for further use.


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