function dETATdt=PointContraintStateEquation(t,ETAT)

global b params dparams u_tab v_lin_tab


XPOS=ETAT(1);
YPOS=ETAT(2);
alpha=ETAT(3);
u=ETAT(4);
XR=ETAT(5);
YR=ETAT(6);
alphaR=ETAT(7);

%%% commande v�hicule de r�f�rence
cr = cr_bezier(params, u);
u_t = find(u<u_tab, 1, 'first');
if (u_t > 1)
    v_lin = v_lin_tab(u_t-1)+(v_lin_tab(u_t)-v_lin_tab(u_t-1))*(u-u_tab(u_t-1))/(u_tab(u_t)-u_tab(u_t-1));
else
    v_lin = v_lin_tab(1);
end
if (isnan(cr) || isinf(cr))
    v_ratio = 1;
else
    v_ratio = (cr+b/2) / (cr - b/2);
end
if v_lin == 0
    v_max = 0;
else
    v_max = 2/(1+min(abs(v_ratio),abs(1/v_ratio)))*v_lin;
end
if cr >= 0
    %v_l = v_max;
    %v_r = v_l / v_ratio;
    U2R = v_max * (1-1/v_ratio) / b;
else
    %v_r = v_max;
    %v_l = v_r * v_ratio;
    U2R = v_max * (v_ratio-1) / b;
end

U1R = v_lin;%(v_l + v_r) / 2.0;
%U2R = (v_l - v_r) / b;
dxu = dparams(1,:) * [1;u;u^2;u^3];
dyu = dparams(2,:) * [1;u;u^2;u^3];
du = U1R/sqrt(dxu^2+dyu^2);

% gains
%k1=0.7; k2=0.7; k3=1;
k1=0.7; k2=0.7; k3=1;

%% trajectoire du robot de r�f�rence
XPR=U1R*cos(alphaR);
YPR=U1R*sin(alphaR);
alphaPR=U2R;


z1=(XPOS-XR)*cos(alphaR)+(YPOS-YR)*sin(alphaR);
z2=-(XPOS-XR)*sin(alphaR)+(YPOS-YR)*cos(alphaR);
z3=tan(alpha-alphaR);

w1=-k1*abs(U1R)*(z1+z2*z3);
w2=-k2*U1R*z2-k3*abs(U1R)*z3;

u1=(w1+U1R)/cos(alpha-alphaR);
u2=w2*cos(alpha-alphaR)^2+U2R;

XP = u1*cos(alpha);
YP = u1*sin(alpha);
alphaP = u2;

%rajouter dans le vecteur d'�tat les d�riv�s du la trajectoire du robot de
%r�f�rence

dETATdt=[XP,YP,alphaP,du,XPR,YPR,alphaPR]';

