how to program the control law of RST controller??

i have to program a script (.m)which display the control law of RST controller . I calculated the polynom R,S and T . in the litterature ,the control law : u=(1/S)*(T*yc -R*y) , yc:input s0*u(k)=-s1*u(k-1)-s2*u(k-2)...+t0*yc(k)+t1*yc(k-1)...-r0*y(k)-r1*y(k-1)-r2*y(k-2).... when i used the for loop , u diverge when i use the simulink ( make the fonctional sheme) , u converge how can i program the script (.m) to obtain the right ?

------------------------code fichier .m--------------------

t=0:0.01:1.19;
Te=0.01;
s=tf('s');
z=tf('z',Te);
for i=1:length(t)
       yc(i)=1;
end
n=[0 0.7532 -0.7012];
d=[1 -1.3196  0.36529];
M=filt(n,d,Te);
pM=[1 -0.7 -0.2 0];
 Mg=([d(1) 0 0 0;
     d(2) d(1) n(2) 0;
     d(3) d(2) n(3) n(2);
     0 d(3) 0 n(3)]);
 X=inv(Mg)* pM';
 g_s0=X(1);
 g_s1=X(2);
 g_r0=X(3);
g_r1=X(4);
%------------------polynome S------------
S= filt( [g_s0 g_s1],[1],Te);
S2=1/S;
%--------------------polynome R----------------
R= filt( [g_r0 g_r1],[1],Te);
%-----------------polynome T---------------
g_t0=(pM(1)+pM(2)+pM(3))/(n(3)+n(2)); 
u(1)=2;
u(2)=2.1;
y(1)=0;
y(2)=0.1;
 for k=3:length(t)

         y(k)=(n(2)*g_t0)*yc(k)+(n(3)*g_t0)*yc(k-1)-pM(2)*y(k-1)-pM(3)*y(k-2);
  %    
        u(k)=-g_s1*u(k-1)+g_t0*yc(k)-g_r0*y(k)-g_r1*y(k-1);
  end 
please any help!!!
bests regards
Nesrine

[raouf Hamlaoui]

CAN U HELP MEEE #nesrine