SHARE
TWEET

Untitled

a guest Jun 24th, 2019 68 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. x(:,i+1) = F*x(:,i)+Gamma*randn(size(Gamma,2),1)*sigma_w*0; % True system dynamics
  2.   %% ====================================================
  3.   %% Prediction
  4.   %% ====================================================
  5.   Y_n = inv(F*inv(Y_p)*F' + Gamma*Q*Gamma');
  6.   L = Y_n*F*inv(Y_p);
  7.   y_n = L*y_p(:,i);
  8.   x_ = inv(Y_n)*y_n;
  9.  
  10.   y_p(:,i+1) = y_n;
  11.   Y_p = Y_n;
  12.   for j = 1:ns %2 sensors
  13.     %% ====================================================
  14.     %% Measurement generation
  15.     %% ====================================================
  16.     z{j}(:,i+1) = fn_hx(xr{j},yr{j},x(:,i+1)) + diag(randn(size(sigma_v{j},1),1))*sigma_v{j};
  17.     z{j}(:,i+1) = angle_manipulation(z{j}(:,i+1));
  18.     %Linearize H at x_priori
  19.     H{j} = fn_JH(xr{j},yr{j},x_);
  20.     %% ====================================================
  21.     %% Estimation
  22.     %% ====================================================
  23.     ik = H{j}'*inv(R{j})*z{j}(:,i+1);
  24.     Ik = H{j}'*inv(R{j})*H{j};
  25.     %assimilation of information
  26.     y_p(:,i+1) = y_p(:,i+1) + ik;
  27.     Y_p = Y_p + Ik;
  28.   endfor
  29.   YStore(i) = norm(Y_p);
  30.   xp{1}(:,i+1) = inv(Y_p)*y_p(:,i+1);
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top