I can't fix the problem of my PID simulation code(I think it's about difference of the differentiator) - matlab

Question

I'm trying to build a PID controller, its differential part multiply by a low-pass filter, witch transform func is 1/(r*s+1) (r = 4*Ts ).
I use forward difference to discrete the controller, and use state space function to handle the object, I want a step response of this system.
And I used pid() feedback() step() functions to check if I'm right.
And yet I was wrong, here is the response.
I tried only PI control.
You can see it's almost perfect. So I think the problem is about differential. But I really can't find it out. I already have checked the forward difference many times.
I tried backward difference and got same curve.
T = 0.05;
Cp = 6;
Ci = 1;
Cd = 7;
P = zeros(1, n+1);
I = zeros(1, n+1);
D = zeros(1, n+1);
r = 4*T;
for i = 1:n
if i == 1
e(i) = U(i)-0; % error of time domain, U is step function
P(i) = Cp*e(i);
I(i) = 0;
D(i) = 0;
else
e(i) = U(i)-y(i-1);
P(i) = Cp*e(i);
I(i) = Ci*T*e(i-1)+I(i-1); % forward
D(i) = (Cd*(e(i)-e(i-1))-D(i-1)*(T-r))/r; % forward
% I(i) = T*Ci*e(i)+I(i-1); % backward
% D(i) = (Cd*(e(i)-e(i-1))+r*D(i-1))/(r+T); % backward
% I(i) = (T*(e(i)+e(i-1))+2*I(i-1))/2; % Bilinear
end
u(i) = P(i) + I(i) + D(i);
x(:,i+1) = x(:,i)+T*(Ao*x(:,i)+Bo*u(i));
y(i) = Co*x(:,i);
end
% this how I get the right curve.
PIDF = pid(Cp, Ci, Cd, r);
fb = feedback(PIDF*G_sysc, 1); % G_sysc is trans func of object.
step(fb, N(1:n)*T, 'blue');
I expect should be almost same with the Step Response.
Alright... I have fingered why it's different out by myself. Because the function feedback() set the error as 0 at the first point, and next point, the error would be step function - output = 1, so the differential will be a big positive, that cause the output convergence quicker than my PID controller.
Now I only want to know how do I edit the pid() and feedback() functions to set the first error to be 1, cause this is more common sense.