%% Remove first second
Ts = t(2)-t(1);
t = t(ceil(1/Ts):end);
d = d(ceil(1/Ts):end);
dp = dp(ceil(1/Ts):end);
F = F(ceil(1/Ts):end);

t = t - t(1);
F = F - F(1);

%% LPf
s = tf('s');
G_lpf = 1/(1+s/2/pi/10);
F = lsim(G_lpf, F, t);
d = lsim(G_lpf, d, t);
dp = lsim(G_lpf, dp, t);

%%
figure;
plot(t, F);

figure;
hold on;
plot(t, d, 'DisplayName', 'Encoder');
plot(t, dp, 'DisplayName', 'Mahr')
hold off;
legend;

%%
t_start = 0.05;
t_end = 0.12;
d = d(t < t_end & t > t_start);
dp = dp(t < t_end & t > t_start);
F = F(t < t_end & t > t_start);

d = d - d(1);
dp = dp - dp(1);
F = F - F(1);

%%
figure;
plot(F, dp);
xlabel('Measured Force [N]');
ylabel('Measured Displacement [m]');

%%
sprintf('Stiffness is %.3f [N/mm]', abs(1e-3*(d\F)))

%%
figure;
hold on;
plot(F, d);
plot(F, F/(d\F));
hold off;
xlabel('Measured Force [N]');
ylabel('Measured Displacement [m]');

%%
figure;
hold on;
plot(dp, d);
hold off;
xlabel('Probe [m]');
ylabel('Encoder [m]');