function [mu S]= KF(y,A,B,C,D,Q,R,mu0,S0)
% Kalman filter 
% written by Emtiyaz, CS, UBC
% last updated on Feb 25, 2008

T = length(y);
%prediction
mupre(:,1) = A*mu0;
Spre(:,:,1) = A*S0*A' + B*Q*B';
for t =1:T
  %Kalman gain and innovation
  K = C*Spre(:,:,t)*C' + D*R*D';
  ypre(:,t) = C*mupre(:,t);
  K1 = Spre(:,:,t)*C'*inv(K);
  %correction
  mu(:,t) = mupre(:,t) + K1*(y(:,t) -ypre(:,t));
  S(:,:,t) = Spre(:,:,t) - K1*C*Spre(:,:,t); 
  %prediction
  if t~=T
    mupre(:,t+1) = A*mu(:,t);
    Spre(:,:,t+1) = A*S(:,:,t)*A' + B*Q*B';
  end
end

%plot
for t = 1:T
  subplot(221)
  plot(y(1,1:t),y(2,1:t),'o');
  title('Position in 2-D plane')
  xlabel('p_x'); ylabel('p_y');
  hold on
  plot(mu(1,1:t),mu(3,1:t),'r-','linewidth',2); 
  subplot(223)
  plot(1:t,squeeze(S(1,1,1:t)));
  title('Error Covariance for x position')
  xlabel('time'); ylabel('P^{11}');
  axis([1 T 0 R(1,1)])
  subplot(224)
  plot(1:t,squeeze(S(2,2,1:t)));
  title('Error Covariance y position')
  axis([1 T 0 1.2])
  xlabel('time'); ylabel('P^{22}');
  pause(.1);
end