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

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

%% Path for functions, data and scripts
addpath('./src/'); % Path for scripts
addpath('./mat/'); % Path for data
addpath('./STEPS/'); % Path for Simscape Model
addpath('./subsystems/'); % Path for Subsystems Simulink files

%% Colors for the figures
colors = colororder;
freqs = logspace(1,4,1000); % Frequency vector [Hz]

%% Load computed requirements
load('instrumentation_requirements.mat')

%% Sensitivity to disturbances
load('instrumentation_sensitivity.mat', 'Gd');

% Measured Noise

% The measurement of ADC noise was performed by short-circuiting its input with a $50\,\Omega$ resistor and recording the digital values at a sampling rate of $10\,\text{kHz}$.
% The amplitude spectral density of the recorded values was computed and is presented in Figure ref:fig:detail_instrumentation_adc_noise_measured.
% The ADC noise exhibits characteristics of white noise with an amplitude spectral density of $5.6\,\mu V/\sqrt{\text{Hz}}$ (equivalent to $0.4\,\text{mV RMS}$), which satisfies the established specifications.
% All ADC channels demonstrated similar performance, so only one channel's noise profile is shown.

% If necessary, oversampling can be applied to further reduce the noise cite:lab13_improv_adc.
% To gain $w$ additional bits of resolution, the oversampling frequency $f_{os}$ should be set to $f_{os} = 4^w \cdot F_s$.
% Given that the ADC can operate at 200kSPS while the real-time controller runs at 10kSPS, an oversampling factor of 16 can be employed to gain approximately two additional bits of resolution (reducing noise by a factor of 4).
% This approach is effective because the noise approximates white noise and its amplitude exceeds 1 LSB (0.3 mV) [[cite:hauser91_princ_overs_conver]].


%% ADC noise
adc = load("2023-08-23_15-42_io131_adc_noise.mat");

% Spectral Analysis parameters
Ts = 1e-4;
Nfft = floor(1/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);
% Identification of the transfer function from Va to di
[pxx, f] = pwelch(detrend(adc.adc_1, 0), win, Noverlap, Nfft, 1/Ts);

adc.pxx = pxx;
adc.f = f;

% estimated mean ASD
sprintf('Mean ASD of the ADC: %.1f uV/sqrt(Hz)', 1e6*sqrt(mean(adc.pxx)))
sprintf('Specifications: %.1f uV/sqrt(Hz)', 1e6*max_adc_asd)

% estimated RMS
sprintf('RMS of the ADC: %.2f mV RMS', 1e3*rms(detrend(adc.adc_1,0)))
sprintf('RMS specifications: %.2f mV RMS', max_adc_rms)

% Estimate quantization noise of the IO318 ADC
delta_V = 20; % +/-10 V
n = 16; % number of bits
Fs = 10e3; % [Hz]

adc.q = delta_V/2^n; % Quantization in [V]
adc.q_psd = adc.q^2/12/Fs; % Quantization noise Power Spectral Density [V^2/Hz]
adc.q_asd = sqrt(adc.q_psd); % Quantization noise Amplitude Spectral Density [V/sqrt(Hz)]

%% Measured ADC noise (IO318)
figure;
hold on;
plot(adc.f, sqrt(adc.pxx), 'color', colors(3,:), 'DisplayName', '$\Gamma_{q_{ad}}$')
plot([adc.f(2), adc.f(end)], [max_adc_asd, max_adc_asd], '--', 'color', colors(3,:), 'DisplayName', 'Specs')
plot([adc.f(2), adc.f(end)], [adc.q_asd, adc.q_asd], 'k--', 'DisplayName', 'Quantization noise (16 bits, $\pm 10\,V$)')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD [V/$\sqrt{Hz}$]');
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
ylim([1e-10, 4e-4]); xlim([1, 5e3]);
xticks([1e0, 1e1, 1e2, 1e3])

% Reading of piezoelectric force sensor

% To further validate the ADC's capability to effectively measure voltage generated by a piezoelectric stack, a test was conducted using the APA95ML.
% The setup is illustrated in Figure ref:fig:detail_instrumentation_force_sensor_adc_setup, where two stacks are used as actuators (connected in parallel) and one stack serves as a sensor.
% The voltage amplifier employed in this setup has a gain of 20.

% #+name: fig:detail_instrumentation_force_sensor_adc_setup
% #+caption: Schematic of the setup to validate the use of the ADC for reading the force sensor volage
% [[file:figs/detail_instrumentation_force_sensor_adc_setup.png]]

% Step signals with an amplitude of $1\,V$ were generated using the DAC, and the ADC signal was recorded.
% The excitation signal (steps) and the measured voltage across the sensor stack are displayed in Figure ref:fig:detail_instrumentation_step_response_force_sensor.

% Two notable observations were made: an offset voltage of $2.26\,V$ was present, and the measured voltage exhibited an exponential decay response to the step input.
% These phenomena can be explained by examining the electrical schematic shown in Figure ref:fig:detail_instrumentation_force_sensor_adc, where the ADC has an input impedance $R_i$ and an input bias current $i_n$.

% The input impedance $R_i$ of the ADC, in combination with the capacitance $C_p$ of the piezoelectric stack sensor, forms an RC circuit with a time constant $\tau = R_i C_p$.
% The charge generated by the piezoelectric effect across the stack's capacitance gradually discharges into the input resistor of the ADC.
% Consequently, the transfer function from the generated voltage $V_p$ to the measured voltage $V_{\text{ADC}}$ is a first-order high-pass filter with the time constant $\tau$.

% An exponential curve was fitted to the experimental data, yielding a time constant $\tau = 6.5\,s$.
% With the capacitance of the piezoelectric sensor stack being $C_p = 4.4\,\mu F$, the internal impedance of the Speedgoat ADC was calculated as $R_i = \tau/C_p = 1.5\,M\Omega$, which closely aligns with the specified value of $1\,M\Omega$ found in the datasheet.


%% Read force sensor voltage with the ADC
load('force_sensor_steps.mat', 't', 'encoder', 'u', 'v');

% Exponential fit to compute the time constant
% Fit function
f_exp = @(b,x) b(1).*exp(-b(2).*x) + b(3);

% Three steps are performed at the following time intervals:
t_s = [ 2.5, 23;
       23.8, 35;
       35.8, 50];

tau = zeros(size(t_s, 1),1); % Time constant [s]
V0  = zeros(size(t_s, 1),1); % Offset voltage [V]
a   = zeros(size(t_s, 1),1); %

for t_i = 1:size(t_s, 1)
    t_cur = t(t_s(t_i, 1) < t & t < t_s(t_i, 2));
    t_cur = t_cur - t_cur(1);
    y_cur = v(t_s(t_i, 1) < t & t < t_s(t_i, 2));

    nrmrsd = @(b) norm(y_cur - f_exp(b,t_cur)); % Residual Norm Cost Function
    B0 = [0.5, 0.15, 2.2];                        % Choose Appropriate Initial Estimates
    [B,rnrm] = fminsearch(nrmrsd, B0);     % Estimate Parameters ‘B’

    a(t_i)   = B(1);
    tau(t_i) = 1/B(2);
    V0(t_i)  = B(3);
end

% Data to show the exponential fit
t_fit_1 = linspace(t_s(1,1), t_s(1,2), 100);
y_fit_1 = f_exp([a(1),1/tau(1),V0(1)], t_fit_1-t_s(1,1));

t_fit_2 = linspace(t_s(2,1), t_s(2,2), 100);
y_fit_2 = f_exp([a(2),1/tau(2),V0(2)], t_fit_2-t_s(2,1));

t_fit_3 = linspace(t_s(3,1), t_s(3,2), 100);
y_fit_3 = f_exp([a(3),1/tau(3),V0(3)], t_fit_3-t_s(3,1));

% Speedgoat ADC input impedance
Cp = 4.4e-6; % [F]
Rin = abs(mean(tau))/Cp; % [Ohm]

% Estimated input bias current
in = mean(V0)/Rin; % [A]

% Resistor added in parallel to the force sensor
fc = 0.5; % Wanted corner frequency [Hz]
Ra = Rin/(2*pi*fc*Cp*Rin - 1); % [Ohm]

% New ADC offset voltage
V_offset = Ra*Rin/(Ra + Rin) * in; % [V]

%% Measured voltage accross the sensor stacks - Voltage steps are applied to the actuators
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
nexttile();
hold on;
plot(t, u, 'DisplayName', '$u$');
plot(t, v, 'DisplayName', '$V_s$');
plot(t_fit_1, y_fit_1, 'k--', 'DisplayName', 'fit');
plot(t_fit_2, y_fit_2, 'k--', 'HandleVisibility', 'off');
plot(t_fit_3, y_fit_3, 'k--', 'HandleVisibility', 'off');
hold off;
xlabel('Time [s]'); ylabel('Voltage [V]');
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xlim([0, 20]);



% #+name: fig:detail_instrumentation_force_sensor
% #+caption: Electrical schematic of the ADC measuring the piezoelectric force sensor (\subref{fig:detail_instrumentation_force_sensor_adc}), adapted from cite:reza06_piezoel_trans_vibrat_contr_dampin. Measured voltage $V_s$ while step voltages are generated for the actuator stacks (\subref{fig:detail_instrumentation_step_response_force_sensor}).
% #+attr_latex: :options [htbp]
% #+begin_figure
% #+attr_latex: :caption \subcaption{\label{fig:detail_instrumentation_force_sensor_adc}Electrical Schematic}
% #+attr_latex: :options {0.61\textwidth}
% #+begin_subfigure
% #+attr_latex: :scale 1
% [[file:figs/detail_instrumentation_force_sensor_adc.png]]
% #+end_subfigure
% #+attr_latex: :caption \subcaption{\label{fig:detail_instrumentation_step_response_force_sensor}Measured Signals}
% #+attr_latex: :options {0.35\textwidth}
% #+begin_subfigure
% #+attr_latex: :width 0.95\linewidth
% [[file:figs/detail_instrumentation_step_response_force_sensor.png]]
% #+end_subfigure
% #+end_figure

% The constant voltage offset can be explained by the input bias current $i_n$ of the ADC, represented in Figure ref:fig:detail_instrumentation_force_sensor_adc.
% At DC, the impedance of the piezoelectric stack is much larger than the input impedance of the ADC, and therefore the input bias current $i_n$ passing through the internal resistance $R_i$ produces a constant voltage offset $V_{\text{off}} = R_i \cdot i_n$.
% The input bias current $i_n$ is estimated from $i_n = V_{\text{off}}/R_i = 1.5\mu A$.

% In order to reduce the input voltage offset and to increase the corner frequency of the high pass filter, a resistor $R_p$ can be added in parallel to the force sensor, as illustrated in Figure ref:fig:detail_instrumentation_force_sensor_adc_R.
% This modification produces two beneficial effects: a reduction of input voltage offset through the relationship $V_{\text{off}} = (R_p R_i)/(R_p + R_i) i_n$, and an increase in the high pass corner frequency $f_c$ according to the equations $\tau = 1/(2\pi f_c) = (R_i R_p)/(R_i + R_p) C_p$.

% To validate this approach, a resistor $R_p \approx 82\,k\Omega$ was added in parallel with the force sensor as shown in Figure ref:fig:detail_instrumentation_force_sensor_adc_R.
% After incorporating this resistor, the same step response tests were performed, with results displayed in Figure ref:fig:detail_instrumentation_step_response_force_sensor_R.
% The measurements confirmed the expected improvements, with a substantially reduced offset voltage ($V_{\text{off}} = 0.15\,V$) and a much faster time constant ($\tau = 0.45\,s$).
% These results validate both the model of the ADC and the effectiveness of the added parallel resistor as a solution.


%% Read force sensor voltage with the ADC with added 82.7kOhm resistor
load('force_sensor_steps_R_82k7.mat', 't', 'encoder', 'u', 'v');

% Step times
t_s = [1.9,  6;
       8.5, 13;
      15.5, 21;
      22.6, 26;
      30.0, 36;
      37.5, 41;
      46.2, 49.5]; % [s]

tau = zeros(size(t_s, 1),1); % Time constant [s]
V0  = zeros(size(t_s, 1),1); % Offset voltage [V]
a   = zeros(size(t_s, 1),1); %

for t_i = 1:size(t_s, 1)
    t_cur = t(t_s(t_i, 1) < t & t < t_s(t_i, 2));
    t_cur = t_cur - t_cur(1);
    y_cur = v(t_s(t_i, 1) < t & t < t_s(t_i, 2));

    nrmrsd = @(b) norm(y_cur - f_exp(b,t_cur)); % Residual Norm Cost Function
    B0 = [0.5, 0.1, 2.2];                        % Choose Appropriate Initial Estimates
    [B,rnrm] = fminsearch(nrmrsd, B0);     % Estimate Parameters ‘B’

    a(t_i)   = B(1);
    tau(t_i) = 1/B(2);
    V0(t_i)  = B(3);
end

% Data to show the exponential fit
t_fit_1 = linspace(t_s(1,1), t_s(1,2), 100);
y_fit_1 = f_exp([a(1),1/tau(1),V0(1)], t_fit_1-t_s(1,1));

t_fit_2 = linspace(t_s(2,1), t_s(2,2), 100);
y_fit_2 = f_exp([a(2),1/tau(2),V0(2)], t_fit_2-t_s(2,1));

t_fit_3 = linspace(t_s(3,1), t_s(3,2), 100);
y_fit_3 = f_exp([a(3),1/tau(3),V0(3)], t_fit_3-t_s(3,1));

%% Measured voltage accross the sensor stacks - Voltage steps are applied to the actuators
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
nexttile();
hold on;
plot(t, u, 'DisplayName', '$u$');
plot(t, v, 'DisplayName', '$V_s$');
plot(t_fit_1, y_fit_1, 'k--', 'DisplayName', 'fit');
plot(t_fit_2, y_fit_2, 'k--', 'HandleVisibility', 'off');
plot(t_fit_3, y_fit_3, 'k--', 'HandleVisibility', 'off');
hold off;
xlabel('Time [s]'); ylabel('Voltage [V]');
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xlim([0, 20]);

%% Femto Input Voltage Noise
femto = load('noise_femto.mat', 't', 'Vout', 'notes'); % Load Data

% Compute the equivalent voltage at the input of the amplifier
femto.Vout = femto.Vout/femto.notes.pre_amp.gain;
femto.Vout = femto.Vout - mean(femto.Vout);

Ts = (femto.t(end) - femto.t(1))/(length(femto.t) - 1);
Nfft = floor(1/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);
% Power Spectral Density
[pxx, f] = pwelch(detrend(femto.Vout, 0), win, Noverlap, Nfft, 1/Ts);

% Save the results inside the struct
femto.pxx = pxx(f<=5e3);
femto.f = f(f<=5e3);

%% Measured input voltage noise of the Femto voltage pre-amplifier
figure;
hold on;
plot(femto.f, sqrt(femto.pxx), 'color', colors(5,:), 'DisplayName', '$\Gamma_{n_a}$');
plot(adc.f, sqrt(adc.pxx)./femto.notes.pre_amp.gain, 'color', colors(3,:), 'DisplayName', '$\Gamma_{q_{ad}}/|G_a|$')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD [$V/\sqrt{Hz}$]');
legend('location', 'northeast');
xlim([1, 5e3]); ylim([2e-10, 1e-7]);
xticks([1e0, 1e1, 1e2, 1e3]);
yticks([1e-9, 1e-8]);



% #+name: fig:detail_instrumentation_dac_setup
% #+caption: Measurement of the DAC output voltage noise. A pre-amplifier with a gain of 1000 is used before measuring the signal with the ADC.
% #+RESULTS:
% [[file:figs/detail_instrumentation_dac_setup.png]]


%% DAC Output Voltage Noise
dac = load('mat/noise_dac.mat', 't', 'Vn', 'notes');

% Take input acount the gain of the pre-amplifier
dac.Vn = dac.Vn/dac.notes.pre_amp.gain;
dac.Vn = dac.Vn - mean(dac.Vn);

Ts = (dac.t(end) - dac.t(1))/(length(dac.t) - 1);
Nfft = floor(1/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);
% Identification of the transfer function from Va to di
[pxx, f] = pwelch(dac.Vn, win, Noverlap, Nfft, 1/Ts);

dac.pxx = pxx(f<=5e3);
dac.f = f(f<=5e3);

% Estimated mean ASD
sprintf('Mean ASD of the DAC: %.1f uV/sqrt(Hz)', 1e6*sqrt(mean(dac.pxx)))
sprintf('Specifications: %.1f uV/sqrt(Hz)', 1e6*max_dac_asd)

% Estimated RMS
sprintf('RMS of the DAC: %.2f mV RMS', 1e3*rms(dac.Vn))
sprintf('RMS specifications: %.2f mV RMS', max_dac_rms)

figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(femto.f, sqrt(femto.pxx), 'color', colors(5,:), 'DisplayName', '$\Gamma_{n_a}$');
plot(dac.f, sqrt(dac.pxx), 'color', colors(1,:), 'DisplayName', '$\Gamma_{n_{da}}$');
plot([dac.f(2), dac.f(end)], [max_dac_asd, max_dac_asd], '--', 'color', colors(1,:), 'DisplayName', 'DAC specs')
plot(adc.f, sqrt(adc.pxx)./dac.notes.pre_amp.gain, 'color', colors(3,:), 'DisplayName', '$\Gamma_{q_{ad}}/|G_a|$')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD [$V/\sqrt{Hz}$]');
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xlim([1, 5e3]); ylim([2e-10, 4e-4]);
xticks([1e0, 1e1, 1e2, 1e3]);

% Delay from ADC to DAC
% To measure the transfer function from DAC to ADC and verify that the bandwidth and latency of both instruments is sufficient, a direct connection was established between the DAC output and the ADC input.
% A white noise signal was generated by the DAC, and the ADC response was recorded.

% The resulting frequency response function from the digital DAC signal to the digital ADC signal is presented in Figure ref:fig:detail_instrumentation_dac_adc_tf.
% The observed frequency response function corresponds to exactly one sample delay, which aligns with the specifications provided by the manufacturer.


%% Measure transfer function from DAC to ADC
data_dac_adc = load("2023-08-22_15-52_io131_dac_to_adc.mat");

% Frequency analysis parameters
Ts = 1e-4; % Sampling Time [s]
Nfft = floor(1.0/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

[G_dac_adc, f] = tfestimate(data_dac_adc.dac_1, data_dac_adc.adc_1, win, Noverlap, Nfft, 1/Ts);

%
G_delay = exp(-Ts*s);

%% Measure transfer function from DAC to ADC - It fits a pure "1-sample" delay
figure;
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(G_dac_adc), 'color', colors(2,:), 'DisplayName', 'Measurement');
plot(f, abs(squeeze(freqresp(G_delay, f, 'Hz'))), 'k--', 'DisplayName', 'Pure Delay');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-1, 1e1]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile();
hold on;
plot(f, 180/pi*unwrap(angle(G_dac_adc)), 'color', colors(2,:));
plot(f, 180/pi*unwrap(angle(squeeze(freqresp(G_delay, f, 'Hz')))), 'k--', 'DisplayName', 'Pure Delay');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-200, 20])

linkaxes([ax1,ax2],'x');
xlim([1, 5e3]);
xticks([1e0, 1e1, 1e2, 1e3]);



% #+name: fig:detail_instrumentation_pd200_setup
% #+caption: Setup used to measured the output voltage noise of the PD200 voltage amplifier. A gain $G_a = 1000$ was used for the instrumentation amplifier.
% #+RESULTS:
% [[file:figs/detail_instrumentation_pd200_setup.png]]

% The Amplitude Spectral Density $\Gamma_{n}(\omega)$ of the signal measured by the ADC was computed.
% From this, the Amplitude Spectral Density of the output voltage noise of the PD200 amplifier $n_p$ was derived, accounting for the gain of the pre-amplifier according to eqref:eq:detail_instrumentation_amp_asd.

% \begin{equation}\label{eq:detail_instrumentation_amp_asd}
%   \Gamma_{n_p}(\omega) = \frac{\Gamma_n(\omega)}{|G_p(j\omega) G_a(j\omega)|}
% \end{equation}

% The computed Amplitude Spectral Density of the PD200 output noise is presented in Figure ref:fig:detail_instrumentation_pd200_noise.
% Verification was performed to confirm that the measured noise was predominantly from the PD200, with negligible contributions from the pre-amplifier noise or ADC noise.
% The measurements from all six amplifiers are displayed in this figure.

% The noise spectrum of the PD200 amplifiers exhibits several sharp peaks.
% While the exact cause of these peaks is not fully understood, their amplitudes remain below the specified limits and should not adversely affect system performance.


%% PD200 Output Voltage Noise
% Load all the measurements
pd200 = {};
for i = 1:6
    pd200(i) = {load(['mat/noise_PD200_' num2str(i) '_10uF.mat'], 't', 'Vout', 'notes')};
end

% Take into account the pre-amplifier gain
for i = 1:6
    pd200{i}.Vout = pd200{i}.Vout/pd200{i}.notes.pre_amp.gain;
end

% Sampling time / frequency
Ts = (pd200{1}.t(end) - pd200{1}.t(1))/(length(pd200{1}.t) - 1);

% Compute the PSD of the measured noise
Nfft = floor(1/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

for i = 1:6
    % Identification of the transfer function from Va to di
    [pxx, f] = pwelch(pd200{i}.Vout, win, Noverlap, Nfft, 1/Ts);
    pd200{i}.pxx = pxx(f<=5e3);
    pd200{i}.f = f(f<=5e3);
end

% Estimated RMS
sprintf('RMS of the PD200: %.2f mV RMS', 1e3*rms(detrend(pd200{1}.Vout,0)))
sprintf('RMS specifications: %.2f mV RMS', max_amp_rms)

%% Measured output voltage noise of the PD200 amplifiers
figure;
hold on;
plot([1 Fs/2], [max_amp_asd, max_amp_asd], '--', 'color', colors(2,:), 'DisplayName', 'Specs')
plot(pd200{1}.f, sqrt(pd200{1}.pxx), 'color', [colors(2, :), 0.5], 'DisplayName', '$\Gamma_{n_p}$');
for i = 2:6
    plot(pd200{i}.f, sqrt(pd200{i}.pxx), 'color', [colors(2, :), 0.5], 'HandleVisibility', 'off');
end
plot(femto.f, sqrt(femto.pxx), 'color', [colors(5, :)], 'DisplayName', '$\Gamma_{n_a}$');
plot(adc.f, sqrt(adc.pxx)./pd200{1}.notes.pre_amp.gain, 'color', colors(3,:), 'DisplayName', '$\Gamma_{q_{ad}}/|G_a|$')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD [$V/\sqrt{Hz}$]');
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ylim([1e-10, 4e-4]); xlim([1, 5e3]);
xticks([1e0, 1e1, 1e2, 1e3])

% Small Signal Bandwidth

% The small signal dynamics of all six PD200 amplifiers were characterized through frequency response measurements.

% A logarithmic sweep sine excitation voltage was generated using the Speedgoat DAC with an amplitude of $0.1\,V$, spanning frequencies from $1\,\text{Hz}$ to $5\,\text{kHz}$.
% The output voltage of the PD200 amplifier was measured via the monitor voltage output of the amplifier, while the input voltage (generated by the DAC) was measured with a separate ADC channel of the Speedgoat system.
% This measurement approach eliminates the influence of ADC-DAC-related time delays in the results.

% All six amplifiers demonstrated consistent transfer function characteristics. The amplitude response remains constant across a wide frequency range, and the phase shift is limited to less than 1 degree up to 500Hz, well within the specified requirements.

% The identified dynamics shown in Figure ref:fig:detail_instrumentation_pd200_tf can be accurately modeled as either a first-order low-pass filter or as a simple constant gain.


%% Load all the measurements
pd200_tf = {};
for i = 1:6
    pd200_tf(i) = {load(['tf_pd200_' num2str(i) '_10uF_small_signal.mat'], 't', 'Vin', 'Vout', 'notes')};
end

% Compute sampling Frequency
Ts = (pd200_tf{1}.t(end) - pd200_tf{1}.t(1))/(length(pd200_tf{1}.t)-1);

% Compute all the transfer functions
Nfft = floor(1.0/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

for i = 1:length(pd200_tf)
    [tf_est, f] = tfestimate(pd200_tf{i}.Vin, 20*pd200_tf{i}.Vout, win, Noverlap, Nfft, 1/Ts);
    pd200_tf{i}.tf = tf_est(f<=5e3);
    pd200_tf{i}.f = f(f<=5e3);
end

% Amplified model
Gp = 20/(1 + s/2/pi/25e3);

figure;
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(pd200_tf{1}.f, abs(pd200_tf{1}.tf), '-', 'color', [colors(2,:), 0.5], 'linewidth', 2.5, 'DisplayName', 'Measurement')
plot(pd200_tf{1}.f, abs(squeeze(freqresp(Gp, pd200_tf{1}.f, 'Hz'))), '--', 'color', colors(2,:), 'DisplayName', '$1^{st}$ order LPF')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1, 1e2]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
plot(pd200_tf{1}.f, 180/pi*unwrap(angle(pd200_tf{1}.tf)), '-', 'color', [colors(2,:), 0.5], 'linewidth', 2.5)
plot(pd200_tf{1}.f, 180/pi*unwrap(angle(squeeze(freqresp(Gp, pd200_tf{1}.f, 'Hz')))), '--', 'color', colors(2,:))
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:2:360);
ylim([-13, 1]);

linkaxes([ax1,ax2],'x');
xlim([1, 5e3]);

% Linear Encoders

% To measure the noise of the encoder, the head and ruler were rigidly fixed together to ensure that no actual motion would be detected.
% Under these conditions, any measured signal would correspond solely to the encoder noise.

% The measurement setup is shown in Figure ref:fig:detail_instrumentation_vionic_bench.
% To minimize environmental disturbances, the entire bench was covered with a plastic bubble sheet during measurements.

% The amplitude spectral density of the measured displacement (which represents the measurement noise) is presented in Figure ref:fig:detail_instrumentation_vionic_asd.
% The noise profile exhibits characteristics of white noise with an amplitude of approximately $1\,\text{nm RMS}$, which complies with the system requirements.


%% Load all the measurements
enc = {};
for i = 1:6
    enc(i) = {load(['mat/noise_meas_100s_20kHz_' num2str(i) '.mat'], 't', 'x')};
end

% Compute sampling Frequency
Ts = (enc{1}.t(end) - enc{1}.t(1))/(length(enc{1}.t)-1);
Nfft = floor(1.0/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

for i = 1:length(enc)
    [pxx, f] = pwelch(detrend(enc{i}.x, 0), win, Noverlap, Nfft, 1/Ts);
    enc{i}.pxx = pxx(f<=5e3);
    enc{i}.pxx(2) = enc{i}.pxx(3); % Remove first point which corresponds to drifts
    enc{i}.f = f(f<=5e3);
end

%% Measured Amplitude Spectral Density of the encoder position noise
figure;
hold on;
plot(enc{1}.f, sqrt(enc{1}.pxx), 'color', colors(4,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD [$m/\sqrt{Hz}$]');
xlim([1, 5e3]); ylim([1e-12, 1e-8]);

% Noise budgeting from measured instrumentation noise

% After characterizing all instrumentation components individually, their combined effect on the sample's vibration was assessed using the multi-body model developed earlier.

% The vertical motion induced by the noise sources, specifically the ADC noise, DAC noise, and voltage amplifier noise, is presented in Figure ref:fig:detail_instrumentation_cl_noise_budget.

% The total motion induced by all noise sources combined is approximately $1.5\,\text{nm RMS}$, which remains well within the specified limit of $15\,\text{nm RMS}$.
% This confirms that the selected instrumentation, with its measured noise characteristics, is suitable for the intended application.


%% Estimate the resulting errors induced by noise of instruments
f = dac.f;

% Vertical direction
psd_z_dac = 6*(abs(squeeze(freqresp(Gd('z', 'nda1' ), f, 'Hz'))).^2).*dac.pxx;
psd_z_adc = 6*(abs(squeeze(freqresp(Gd('z', 'nad1' ), f, 'Hz'))).^2).*adc.pxx;
psd_z_amp = 6*(abs(squeeze(freqresp(Gd('z', 'namp1'), f, 'Hz'))).^2).*pd200{1}.pxx;
psd_z_enc = 6*(abs(squeeze(freqresp(Gd('z', 'ddL1' ), f, 'Hz'))).^2).*enc{1}.pxx;
psd_z_tot = psd_z_dac + psd_z_adc + psd_z_amp + psd_z_enc;

rms_z_dac = sqrt(trapz(f, psd_z_dac));
rms_z_adc = sqrt(trapz(f, psd_z_adc));
rms_z_amp = sqrt(trapz(f, psd_z_amp));
rms_z_enc = sqrt(trapz(f, psd_z_enc));
rms_z_tot = sqrt(trapz(f, psd_z_tot));

% Lateral direction
psd_y_dac = 6*(abs(squeeze(freqresp(Gd('y', 'nda1' ), f, 'Hz'))).^2).*dac.pxx;
psd_y_adc = 6*(abs(squeeze(freqresp(Gd('y', 'nad1' ), f, 'Hz'))).^2).*adc.pxx;
psd_y_amp = 6*(abs(squeeze(freqresp(Gd('y', 'namp1'), f, 'Hz'))).^2).*pd200{1}.pxx;
psd_y_enc = 6*(abs(squeeze(freqresp(Gd('y', 'ddL1' ), f, 'Hz'))).^2).*enc{1}.pxx;
psd_y_tot = psd_y_dac + psd_y_adc + psd_y_amp + psd_y_enc;

rms_y_tot = sqrt(trapz(f, psd_y_tot));

%% Closed-loop noise budgeting using measured noise of instrumentation
figure;
hold on;
plot(f, sqrt(psd_z_amp), 'color', [colors(2,:)], 'linewidth', 2.5, 'DisplayName', 'PD200');
plot(f, sqrt(psd_z_dac), 'color', [colors(1,:)], 'linewidth', 2.5, 'DisplayName', 'DAC')
plot(f, sqrt(psd_z_adc), 'color', [colors(3,:)], 'linewidth', 2.5, 'DisplayName', 'ADC')
plot(f, sqrt(psd_z_tot), 'k-', 'DisplayName', sprintf('Total: %.1f nm RMS', 1e9*rms_z_tot));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD [$m/\sqrt{Hz}$]');
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xlim([1, 5e3]); ylim([1e-14, 1e-9]);
xticks([1e0, 1e1, 1e2, 1e3]);