function demo_KF(plotswitch,varR,varQ);
%function demo_KF(plotswitch,varR,varQ);
%if varR is large then measurements will not matter much.


I=eye(2);
colordef black

if nargin == 3
   R=varR*randCovariance(2);
   Q=varQ*randCovariance(2);
end
if nargin < 3
  R = I;
  Q = I;
end
if nargin < 1
   plotswitch=0;
end

vt=eye(2,1);
y=zeros(2,1);
P=0.1*I;
loop=1;
switchflag=0;

figure(1); clf;
plotGauss_color(y(1),y(2),P(1,1),P(2,2),P(1,2),'c');
grid on;axis equal; hold on;


while loop>0
   loop=loop+1;
   
   [Mx,My,but]=ginput2(1);
   
   if but == 1
            
      M = [Mx;My]; 
      K = P*inv(R + P);  
      y = y + K * (M - y);
      P = (I - K)*P*(I-K)' + K*R*K'; 
      plot(Mx,My,'y+','Markersize',4,'Linewidth',4); 
      plotGauss_color(y(1),y(2),P(1,1),P(2,2),P(1,2),'r');
      axis equal; drawnow;
      grid on;
      if switchflag == 1 & plotswitch ~= 0
         hold off;
         switchflag = 0;
      else
         hold on;
      end   
       
   elseif but == 3
     
      y = y + vt;
      P = P + Q;
      plotGauss_color(y(1),y(2),P(1,1),P(2,2),P(1,2),'g');
      axis equal; grid on;
      drawnow;
      if switchflag == 1 & plotswitch ~= 0
         hold off;
         switchflag=0;
      else
         hold on;
      end  
            
   else
      
      break;
      
   end  
   
   if plotswitch  ~=0
      if rem(loop,plotswitch)==0
         switchflag = 1;
      end
   end
   
end