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

Top