Real-time Numerical differentiation in MATLAB function block

I'm trying to build a matlab function block execute a real time numerical derivative of a signal using persistent variable as a form of memory to store the previous data point. I have something like this:

function dth2 = fcn(th2)
sampt = 0.1; 
persistent i
persistent dth
persistent reg_th2

if isempty(i)
    i = 1;
    dth = zeros(4,1);  %register to store output dth2
    reg_th2 = zeros(4,1); %register to store input th2
end

reg_th2(i,1) = th2;
dth(i) = (reg_th2(i+1,1) - reg_th2(i-1,1))/(2*sampt); %error keeps poping up here: Index value 5 exceeds valid range [1-4] for array 'reg_th2'
i = i + 1;
reg_th2 = [reg_th2(2:end); reg_th2(i,1)];
dth2 = dth(end);

Ps: i'm trying to build an alternative to the standard 'Discrete Derivative' simulink block

[Walter Roberson]

What are you expecting to have happen on the 4th invocation when reg_th2(i+1,1) would be outside of the range of the zeros(4,1) that you initialized reg_th2 to ?

Answer 1

[Walter Roberson]

ioff = min(3, i);
reg_th2(ioff,1) = th2;
dth(ioff) = (reg_th2(ioff+1,1) - reg_th2(ioff-1,1))/(2*sampt);
i = i + 1;
dth2 = dth(ioff);

However it then becomes unclear why you would have a memory 4 deep when you only use 3 deep (value before, value, value after) ?

[Femi Bolarinwa]

i thought since my counter i starts from 2, i'd need one more space. Thanks, this runs in simulink but dth2 doesnt appear to be the derivative of th2