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

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

addpath('./mat/');

% Excitation steps and measured generated voltage
% The measured data is loaded.

load('force_sensor_steps.mat', 't', 'encoder', 'u', 'v');



% The excitation signal (steps) and measured voltage across the sensor stack are shown in Figure [[fig:force_sen_steps_time_domain]].

figure;
tiledlayout(2, 1, 'TileSpacing', 'None', 'Padding', 'None');
nexttile;
plot(t, v);
xlabel('Time [s]'); ylabel('Measured voltage [V]');
nexttile;
plot(t, u);
xlabel('Time [s]'); ylabel('Actuator Voltage [V]');

% Estimation of the voltage offset and discharge time constant
% The measured voltage shows an exponential decay which indicates that the charge across the capacitor formed by the stack is discharging into a resistor.
% This corresponds to an RC circuit with a time constant $\tau = RC$.

% In order to estimate the time domain, we fit the data with an exponential.
% The fit function is:

f = @(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];



% We are interested by the =b(2)= term, which is the time constant of the exponential.

tau = zeros(size(t_s, 1),1);
V0  = 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(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’

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

% Estimation of the ADC input impedance

% With the capacitance being $C = 4.4 \mu F$, the internal impedance of the Speedgoat ADC can be computed as follows:

Cp = 4.4e-6; % [F]
Rin = abs(mean(tau))/Cp;

ans = Rin

% Explanation of the Voltage offset
% As shown in Figure [[fig:force_sen_steps_time_domain]], the voltage across the Piezoelectric sensor stack shows a constant voltage offset.

% We can explain this offset by looking at the electrical model shown in Figure [[fig:force_sensor_model_electronics_without_R]] (taken from cite:reza06_piezoel_trans_vibrat_contr_dampin).

% The differential amplifier in the Speedgoat has some input bias current $i_n$ that produces a voltage offset across its own internal resistance.
% Note that the impedance of the piezoelectric stack is much larger that that at DC.

% #+name: fig:force_sensor_model_electronics_without_R
% #+caption: Model of a piezoelectric transducer (left) and instrumentation amplifier (right)
% [[file:figs/force_sensor_model_electronics_without_R.png]]

% The estimated input bias current is then:

in = mean(V0)/Rin;

ans = in

% Effect of an additional Parallel Resistor
% Be looking at Figure [[fig:force_sensor_model_electronics_without_R]], we can see that an additional resistor in parallel with $R_{in}$ would have two effects:
% - reduce the input voltage offset
%   \[ V_{off} = \frac{R_a R_{in}}{R_a + R_{in}} i_n \]
% - increase the high pass corner frequency $f_c$
%   \[ C_p \frac{R_{in}R_a}{R_{in} + R_a} = \tau_c = \frac{1}{f_c} \]
%   \[ R_a = \frac{R_i}{f_c C_p R_i - 1} \]

% If we allow the high pass corner frequency to be equals to 3Hz:

fc = 3;
Ra = Rin/(fc*Cp*Rin - 1);

ans = Ra



% #+RESULTS:
% : 79804

% With this parallel resistance value, the voltage offset would be:

V_offset = Ra*Rin/(Ra + Rin) * in;

ans = V_offset

% Obtained voltage offset and time constant with the added resistor
% After the resistor is added, the same steps response is performed.


load('force_sensor_steps_R_82k7.mat', 't', 'encoder', 'u', 'v');



% The results are shown in Figure [[fig:force_sen_steps_time_domain_par_R]].


figure;
tiledlayout(2, 1, 'TileSpacing', 'None', 'Padding', 'None');
nexttile;
plot(t, v);
xlabel('Time [s]'); ylabel('Measured voltage [V]');
nexttile;
plot(t, u);
xlabel('Time [s]'); ylabel('Actuator Voltage [V]');



% #+name: fig:force_sen_steps_time_domain_par_R
% #+caption: Time domain signal during the actuator voltage steps
% #+RESULTS:
% [[file:figs/force_sen_steps_time_domain_par_R.png]]

% Three steps are performed at the following time intervals:

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



% The time constant and voltage offset are again estimated using a fit function.

f = @(b,x) b(1).*exp(b(2).*x) + b(3);

tau = zeros(size(t_s, 1),1);
V0  = 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(b,t_cur)); % Residual Norm Cost Function
    B0 = [0.5, -0.2, 0.2];                        % Choose Appropriate Initial Estimates
    [B,rnrm] = fminsearch(nrmrsd, B0);     % Estimate Parameters ‘B’

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



% #+RESULTS:
% | $tau$ [s] | $V_0$ [V] |
% |-----------+-----------|
% |      0.43 |      0.15 |
% |      0.45 |      0.16 |
% |      0.43 |      0.15 |
% |      0.43 |      0.15 |
% |      0.45 |      0.15 |
% |      0.46 |      0.16 |
% |      0.48 |      0.16 |

% Knowing the capacitance value, we can estimate the value of the added resistor (neglecting the input impedance of $\approx 1\,M\Omega$):


Cp = 4.4e-6; % [F]
Rin = abs(mean(tau))/Cp;

ans = Rin



% #+RESULTS:
% : 101200.0

% And we can verify that the bias current estimation stays the same:

in = mean(V0)/Rin;

ans = in