commande(Y , v , dt , theta , ind_pos , ind_vit)

function [X1 , U] = commande(Y , v , dt  , theta , ind_pos , ind_vit)

if(nargin < 5)
    
    ind_pos = [1 , 3];
    
end

if(nargin < 6)
    
    ind_vit = [2 , 4];
    
end


[d2 K , N]            = size(Y);

d1                    = 2*d2;

K1                    = size(dt , 2);

if( K1 ~= K - 1)
   
    error('Vecteur Module de vitesse doit tre de K - 1 points');
   
end


X1                     = zeros(d1 , 1 , N);

U                      = zeros(d1 , K - 1, N);

vitesse                = [ [v.*cos(theta) ; v.*sin(theta)] , zeros(d2 , 1 , N)];

U(ind_vit , : , :)     = diff(vitesse , [] , 2);


X1(ind_pos , : , :)    = Y(: , 1 , :);

X1(ind_vit , : , :)    = vitesse(: , 1 , :);



% 
% F = inline('[1 del_t 0 0 ; 0 1 0 0 ; 0 0 1 del_t ; 0 0 0 1 ]');
% 
% for k = 2 : K
%     
%     Fk      = feval(F , dt(k-1) );
%     
%     X1(:,k) = Fk*X1(: , k - 1) + U(: , k - 1);
%     
% end    
    


%Hp*X- Y