% Jon Newman
% Test Derivative approximation algorithm

Td = 0.1;
N = 10;
K = 0.25;




fid = '2012_03_15_Ollie363_10sec-1to5hz-multi-block_01';
sim = loadsimoc([fid '.simoc']);

x = sim.error;
T = 0.01;
t = sim.time;

% T = 0.001; % sample period
% f = 30; % Hz
% t = 0:T:10;
% x = sin(2*pi*f*t);


D = [0];
for i = 1:length(x)-1
    dx = (Td/(Td + N*T))*D(end) - K*Td*N/(Td+N*T) *(x(i+1)-x(i));
    D = [D dx];
    
end

close all
figure
hold on
plot(t,x);
plot(t,D,'r');

