SHARE
TWEET

Untitled

a guest Nov 19th, 2019 76 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. A = [1 -0.01 ; 0 1];
  2. B = [0.01; 0];
  3. C = [1 0];
  4. Q = diag([0.02,0.02]);
  5. R = 0.2;
  6. N = length(ax);
  7.  
  8. %Ppri = zeros(1,N);
  9. %xpost = zeros(1,N);
  10. %Ppost =zeros(1,N);
  11. %xpri(1,1) = ax(1);
  12. %Ppri(1) = P0;
  13. %xpost(1) = ax(1);
  14. %Ppost(1) = P0;
  15.  
  16. x0 = zeros(1,2)';
  17. P0 = 1;
  18. xpri = x0;
  19. Ppri = P0;
  20. xpost = x0;
  21. Ppost = P0;
  22.  
  23. Y = zeros(1, N);
  24. Y(1) = 0;
  25.  
  26. gx_sum = zeros(1,N);
  27. fi = zeros(1,N);
  28.  
  29. for i = 2:N
  30. fi(i) = (180/pi) * atan(ay(i)/sqrt(az(i)^2 + ax(i)^2));
  31. if i == 2
  32. gx_sum(i) =  gx(i-1)*0.01;
  33. else
  34. gx_sum(i) =  (gx_sum(i-1) + gx(i-1)*0.01);
  35. end
  36. %a priori
  37. xpri = A*xpost + B*gx(i-1);
  38. Ppri = A*Ppost*A' + Q;
  39. %a posterior
  40. S = C*Ppost*C' + R;
  41. K = Ppost*C'*S^-1;
  42. xpost = (1 - K*C) + K*gx_sum(i);
  43. Ppost =  (1 - K*C)*Ppri;
  44.  
  45. Y(i) = xpost(1,1);
  46.  
  47. end
  48. X = 1:N;
  49. plot(X, gx_sum,X,fi,X,Y)
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