• API
• FAQ
• Tools
• Archive
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.

Top