
    punti_prova;
    load('movimento_punti_3D_100frames');
    misure=points;
    %pairs initialization:
    T2Mpair = points;
    
    Ts = 1; 
    sigmaW = 0.02;
    sigmaV = 0.01;
    P0 = 1*ones(6,6); %P0 e X0 andranno inizializzate con le precedenti nel caso della sliding window
    
    A = [1 Ts 0 0  0 0;
         0  1 0 0  0 0;
         0  0 1 Ts 0 0;
         0  0 0 1  0 0;
         0  0 0 0  1 Ts;
         0  0 0 0  0 1];
    
    C = [1 0 0 0 0 0;
         0 0 1 0 0 0;
         0 0 0 0 1 0];
    
    Q = sigmaW * eye(6);
    R = sigmaV * eye(3);
     
    [n,~]=size(y);
    xhat = zeros(6,n);
    
    P = P0;
    Pout=P0;
    
    ultima_misura = 0;
    cont = 0;
    
    
%    
%      for cont = 1:1:(end_i -start_i + 1)
         while ~(ultima_misura)
            cont = cont +1;

    %         nuova_misura=


            % update
            % measure = Y(cont).data(Tau(cont) , :); %estraggo da tutte le misure la misura al tempo T relativo            

            K = P * ((C')/(C*P*C' + R));
            xhat(:,cont) = xhat(:,cont) + K * (y(cont,:)' - C*xhat(:,cont));
            P = P - K*C*P;

            % prediction
            xhat(:,cont+1) = A*xhat(:,cont);
            P = A*P*A' + Q;

            Pout(:,:,cont)=P;
        end
% end

 