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)