tSim = 20;
h = 0.1;
t = 0:h:tSim;
tt = numel(t);

% odchylenie standardowe zaklocenia
sigma_y = 1;

% 1.5 okresu sygnalu prostokatego
y = square(t,1/tSim * 1.5,50) * 10;
y = y + randn(1,tt) * sigma_y;

% macierze kowariancji
Q = 0.1;
R = sigma_y^2;

% FK
xHat = zeros(1,tt);
xPost = 0;
pPost = Q;
for i = 1:tt
    % predykcja
    xPrio = xPost;
    pPrio = pPost + Q;
    
    % filtracja
    K = pPrio / (pPrio + R);
    xPost = xPrio + K * (y(i) - xPrio);
    pPost = (1 - K) * pPrio;
    
    xHat(i) = xPost;
end

figure;
plot(t,y,'b'); hold on; grid on;
plot(t,xHat,'r');
legend('wyjscie','estymata');