function y = kalmanFilter(gyro,accel,R, Q, C, B, A)

% Initial conditions
persistent x_est p_est
if isempty(x_est)
    x_est = zeros(2, 1);
    p_est = eye(2);


end
   Ppost = p_est;
   xpost = x_est;
% Predykcja
    xpri = A*xpost + B*gyro;
    Ppri = A*Ppost*A' + Q;
% Estymacja
    	eps = accel - C*xpri;
        S = C*Ppri*C' + R;
        K = Ppri*C'*S^(-1);
        xpost = xpri + K*eps;
        Ppos = Ppri - K*S*K';
        x1=xpost(1);
        x2=xpost(2);
        x_est=[hehe]
        p_est=Ppos;
% Wynik
y=xpost(1);

end