Nc=2;

Yref=zeros(Np,1);

PHIref=zeros(Np,1);

State_Initial(1,1)=0;%x 

State_Initial(2,1)=0;%y

State_Initial(3,1)=pi/6;%phi

R=100*eye(Np,Np);

 v_actual=A(1);

 deltaf_actual=A(2);

    X00(2)=State_Initial(2,1);

    X00(3)=State_Initial(3,1);

    XOUT=dsolve('Dx-v_actual*cos(z)=0','Dy-v_actual*sin(z)=0','Dz-v_actual*tan(deltaf_actual)=0','x(0)=X00(1)','y(0)=X00(2)','z(0)=X00(3)');

    t=T;

    State_Initial(1,1)=eval(XOUT.x);

    State_Initial(2,1)=eval(XOUT.y);

    State_Initial(3,1)=eval(XOUT.z);

cost=0; 

l=1;

Y_error=zeros(Np,1);

PHI_error=zeros(Np,1);

        v(i,1)=x(1);

        delta_f(i,1)=x(2);

        X_predict(i,1)=X+T*v(i,1)*cos(PHI);

        Y_predict(i,1)=Y+T*v(i,1)*sin(PHI);

        PHI_predict(i,1)=PHI+T*v(i,1)*tan(delta_f(i,1))/l;

        v(i,1)=x(3);

        delta_f(i,1)=x(4);

        X_predict(i,1)=X_predict(i-1)+T*v(i,1)*cos(PHI_predict(i-1));

        Y_predict(i,1)=Y_predict(i-1)+T*v(i,1)*sin(PHI_predict(i-1));

        PHI_predict(i,1)=PHI_predict(i-1)+T*v(i,1)*tan(delta_f(i,1))/l;

Y_real=zeros(Np,1);

X_real(1:Np,1)=X_predict;

Y_real(1:Np,1)=Y_predict;

Y_error(i,1)=Y_real(i,1)-Yref(i,1);

PHI_error(i,1)=PHI_predict(i,1)-PHIref(i,1);