function [X,P] = KalmanFil(Y,A,Phi,Q,R,mu,S)
%function [X,P] = KalmanFil(Y,A,Phi,Q,R,mu,S)
%Kalman filter

[q,n] = size(Y);
p = size(Phi,1);
X = zeros(p,n);
P = zeros(p,p,n);

x1 = Phi*mu;
P1 = Phi*S*Phi' + Q;
K = P1*A'*inv(A*P1*A'+R);
X(:,1) = x1 + K*(Y(:,1)-A*x1);
P(:,:,1) = (eye(p)-K*A)*P1;
for t = 2:n
    x1 = Phi*X(:,t-1);    
    P1 = Phi*P(:,:,t-1)*Phi' + Q;
    K = P1*A'*inv(A*P1*A'+R);
    X(:,t) = x1 + K*(Y(:,t)-A*x1);
    P(:,:,t) = (eye(p)-K*A)*P1;
end