x_dot=u(2)/3.6+0.0001;

phi=u(3)*pi/180;

phi_dot=u(4)*pi/180;

    State_Initial(1,1)=y_dot;

    State_Initial(2,1)=x_dot;

    State_Initial(3,1)=phi;

    State_Initial(4,1)=Y;

    State_Initial(5,1)=X;

    X_obstacle(1:2)=30;

    X_obstacle(3:4)=35;

    X_obstacle(5:6)=32.5;

    Y_obstacle(1)=0.5;

    Y_obstacle(2)=1;

    Y_obstacle(3)=0.5;

    Y_obstacle(4)=1;

    Y_obstacle(5)=0.5;

    Y_obstacle(6)=1;

R=5*eye(Nc,Nc); 

S=10;

cost=cost+Y_error'*Q*Y_error+ay(1:2)'*R*ay(1:2)+S*sum(J_obst(:));

            ay(i)=A(1);

            y_dot_predict(i,1)=State_Initial(1,1)+T*ay(i);

            x_dot_predict(i,1)=State_Initial(2,1);

            phi_predict(i,1)=State_Initial(3,1)+T*ay(i)/State_Initial(2,1);

            Y_predict(i,1)=State_Initial(4,1)+T*(State_Initial(2,1)*sin(State_Initial(3,1))+State_Initial(1,1)*cos(State_Initial(3,1)));

            X_predict(i,1)=State_Initial(5,1)+T*(State_Initial(2,1)*cos(State_Initial(3,1))-State_Initial(1,1)*sin(State_Initial(3,1)));

            ay(i)=A(2);

            y_dot_predict(i,1)=y_dot_predict(i-1,1)+T*ay(i);

            x_dot_predict(i,1)=State_Initial(2,1);

            phi_predict(i,1)=phi_predict(i-1,1)+T*ay(i)/x_dot_predict(i-1,1);

            Y_predict(i,1)=Y_predict(i-1)+T*(State_Initial(2,1)*sin(phi_predict(i-1))+y_dot_predict(i-1)*cos(phi_predict(i-1)));

            X_predict(i,1)=X_predict(i-1)+T*(State_Initial(2,1)*cos(phi_predict(i-1))-y_dot_predict(i-1)*sin(phi_predict(i-1)));

    Paramater_X_PHI=polyfit(X_predict,phi_predict,4);

    OutPut(1:5)=Paramater_X_Y;

    OutPut(6:10)=Paramater_X_PHI;

    sys=OutPut;