% Simulacija linearnog sustava (pravo stanje)
    x = [xyz(1,1); Vgps(1)]; % početni vektor stanja
    %(za os X: 4287402.35754, os Y: 1217339.53642, os Z: 4547431.00253,
    %brzina: 3.2250)
    % Simulacija mjerenja opterećenog šumom (pravo stanje)
    Sum_mjerenja = sum_mjerenja * randn;
    z = H * x + Sum_mjerenja;
    % Procjena stanja u datom trenutku
    x_potez = x; % početni vektor procjene stanja
    % Izračun inovacije (rezidual mjerenja)
    In = z - H * x_potez;
    % Kovarijanca inovacije
    S = R + H * P * H';
    % Kalmanovo pojačanje
    W = P * H' / S;
    % Ažurirana procjena stanja
    x_potez = x_potez + W * In;
    % Ažurirana kovarijanca stanja
    P = P - W * S * W';

for i = 1 : 1 : max(size(xyz))

if i >= 2
 for t = 0 : dt : trajanje
    if xyz(i,1) == xyz(i-1,1)

      % Simulacija linearnog sustava (pravo stanje)
      Sum_procesa = sum_ubrzanja * [(dt^2/2)*randn; dt*randn];
      x = F * x + G * u(i) + Sum_procesa;
      % Simulacija mjerenja opterećenog šumom (pravo stanje)
      Sum_mjerenja = sum_mjerenja * randn;
      z = H * x + Sum_mjerenja;
      % Procjena stanja u datom trenutku
      x_potez = F * x_potez + G * u(i);

    else

      % Simulacija linearnog sustava (pravo stanje)
      x = [xyz(i,1); Vgps(i)];
      % Simulacija mjerenja opterećenog šumom (pravo stanje)
      Sum_mjerenja = sum_mjerenja * randn;
      z = H * x + Sum_mjerenja;
      % Procjena stanja u datom trenutku
      x_potez = x;

    end
  end
end

    % Izračun inovacije (rezidual mjerenja)
    In = z - H * x_potez;
    % Kovarijanca inovacije
    S = R + H * P * H';
    % Kalmanovo pojačanje
    W = P * H' / S;
    % Ažurirana procjena stanja
    x_potez = x_potez + W * In;
    % Ažurirana kovarijanca stanja
    P = P - W * S * W';

    % Spremanje vektorskih parametara za kasnije crtanje
    poz(i) = x(1);
    poz_potez(i) = x_potez(1);
    poz_mjerenja(i) = z;
    brz(i) = x(2);
    brz_potez(i) = x_potez(2);
    i = i + 1;
end