function [m_p, S_p, m_f, S_f, m_s, S_s] = kfs(y, U, A, B, C, D, S_v, S_w, x0, S_x0)

% Obycejny Kalmanuv filtr pro stavu linearniho systemu.
% Y ... data = radky
% model je tvaru
% x(t+1) = Ax(t) + Bu(t) + v(t)
% y(t)   = Cx(t) + Du(t) + w(t)

% delka dat
n = length(y(1,:));

% pocatecni hodnoty mi(0|-1) a sigma(0|-1)
%U(:,1)   = U;
m_p(:,:,1) = x0;
S_p(:,:,1) = S_x0;

% KF
for t = 1:n
    K(:,:,t)   = S_p(:,:,t) * C' * pinv (C * S_p(:,:,t) * C' + S_w);
    m_f(:,t)   = m_p(:,t) + K(:,:,t) * (y(:,t) - C * m_p(:,t) - D*U(:,t)); % - D*U(:,t)
    S_f(:,:,t) = S_p(:,:,t) - K(:,:,t) * C * S_p(:,:,t);
    m_p(:,t+1)   = A * m_f(:,t) + B*U(:,t); % + B*U(:,t)
    S_p(:,:,t+1) = A * S_f(:,:,t) * A' + S_v;
end

% smoothing

m_s(:,n)   = m_f(:,n);
S_s(:,:,n) = S_f(:,:,n);

clear t;
for t = n-1:-1:1
    F(:,:,t)     = S_f(:,:,t) * A' * pinv( S_p(:,:,t+1) );
    m_s(:,t)   = m_f(:,t) + F(:,:,t) * (m_s(:,t+1) - m_p(:,t+1));
    S_s(:,:,t) = S_f(:,:,t) - F(:,:,t) * (S_p(:,:,t+1) - S_s(:,:,t+1) ) * (F(:,:,t))';
end
