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
% Predykcja
 x_prd = A*(x_est)' + B + gyro;
 p_prd = A*p_est*A' + Q;

% Estymacja
K = p_est*C'/(C*p_est*C' + R);
x_est = x_prd - K*C*x_prd + K*accel;
p_est = p_prd - K*C*p_prd;

% Wynik
y=C*x_est;

end