x(:,i+1) = F*x(:,i)+Gamma*randn(size(Gamma,2),1)*sigma_w*0; % True system dynamics
  %% ====================================================
  %% Prediction
  %% ====================================================
  Y_n = inv(F*inv(Y_p)*F' + Gamma*Q*Gamma');
  L = Y_n*F*inv(Y_p);
  y_n = L*y_p(:,i);
  x_ = inv(Y_n)*y_n;

  y_p(:,i+1) = y_n;
  Y_p = Y_n;
  for j = 1:ns %2 sensors
    %% ====================================================
    %% Measurement generation
    %% ====================================================
    z{j}(:,i+1) = fn_hx(xr{j},yr{j},x(:,i+1)) + diag(randn(size(sigma_v{j},1),1))*sigma_v{j};
    z{j}(:,i+1) = angle_manipulation(z{j}(:,i+1));
    %Linearize H at x_priori
    H{j} = fn_JH(xr{j},yr{j},x_);
    %% ====================================================
    %% Estimation
    %% ====================================================
    ik = H{j}'*inv(R{j})*z{j}(:,i+1);
    Ik = H{j}'*inv(R{j})*H{j};
    %assimilation of information
    y_p(:,i+1) = y_p(:,i+1) + ik;
    Y_p = Y_p + Ik;
  endfor
  YStore(i) = norm(Y_p);
  xp{1}(:,i+1) = inv(Y_p)*y_p(:,i+1);