Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- A = [1 -0.01 ; 0 1];
- B = [0.01; 0];
- C = [1 0];
- Q = diag([0.02,0.02]);
- R = 0.2;
- N = length(ax);
- %Ppri = zeros(1,N);
- %xpost = zeros(1,N);
- %Ppost =zeros(1,N);
- %xpri(1,1) = ax(1);
- %Ppri(1) = P0;
- %xpost(1) = ax(1);
- %Ppost(1) = P0;
- x0 = zeros(1,2)';
- P0 = 1;
- xpri = x0;
- Ppri = P0;
- xpost = x0;
- Ppost = P0;
- Y = zeros(1, N);
- Y(1) = 0;
- gx_sum = zeros(1,N);
- fi = zeros(1,N);
- for i = 2:N
- fi(i) = (180/pi) * atan(ay(i)/sqrt(az(i)^2 + ax(i)^2));
- if i == 2
- gx_sum(i) = gx(i-1)*0.01;
- else
- gx_sum(i) = (gx_sum(i-1) + gx(i-1)*0.01);
- end
- %a priori
- xpri = A*xpost + B*gx(i-1);
- Ppri = A*Ppost*A' + Q;
- %a posterior
- S = C*Ppost*C' + R;
- K = Ppost*C'*S^-1;
- xpost = (1 - K*C) + K*gx_sum(i);
- Ppost = (1 - K*C)*Ppri;
- Y(i) = xpost(1,1);
- end
- X = 1:N;
- plot(X, gx_sum,X,fi,X,Y)
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement