function [] = demo_KF2(dim_s,dim_o,dim_plot,tau,scale_W,scale_V,scale_Y,ok_load,plot_N),

%%%%%%%%%%%%%%%%%%%%%%%%  kalman filter %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%							     
%%  KalmanFilter(dim_s,dim_o,dim_plot,tau,scale_W,             
%%                            scale_V,scale_Y,ok_load,plot_N)
%%								
%% input parameters:						
%% ------------------						
%% dim_s    = dimension of the state (X)	[def:3]		
%% dim_o    =  dimension of the observation (Y)	[def:3]		
%% dim_plot = dimension you may want to analyze	 [def:1]		
%% tau      = number of observations  [def:50]		  
%% scale_W  = magnitude of the noise attached to the state  [def:5] 
%% scale_V  = magnitude of the noise attached to the the obs. [def:1]
%% scale_Y  = scale parameter for B	[def:1]			   
%% ok_load = 1 (0)  -> load data from file (random data)  [def:0]         
%% plot_N  = 1 (0)  -> 3D plot  (2D plot)  [def:0]    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%    by S.Savarese - M.Welling - May 2000  
%%%%%%%%%%%%%    Vision lab - Caltech                   
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% variables:
%% ----------
%% X = state vector (dim_s x N_istant )
%% Y = observation vector  (dim_o x N_istant x N_obs )
%% W = noise attached to state  (dim_s x 1)
%% V = noise attached to observation (dim_o x 1)
%% N_obs =  number of observed sequences 
%% N_istant = max number of istant (=tau=number of obs.)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%% initialization 

close all;
colordef black;

if (nargin==0)
   dim_s= 3; 
   dim_o = 3;
   dim_plot =1;
   scale_V = 10;
   scale_W = 1;
   tau = 100;
   ok_load = 0;
   plot_N=0;
   scale_Y=1;
end;



N_count=50;
%N_istant=tau;
N_obs=10;	%% number of observed sequences 
obs_jj =1;      %% obs. seq. we deal with
N_istant =tau;	%% max number of istant
%dim_s=3;	%% dimension of the state vector (X)
%dim_o=3;	%% dimension of the observation vector (Y)
%scale_V=10; 
%scale_W=5;
%scale_V=1; 
%scale_W=1;
%scale_Y=1;


%%%%%%%%%% load data

if (ok_load==1),
   load KFdata,
   Y_copy=X;
   dim_s=size(Y_copy,1);
   dim_o=size(Y_copy,1);
   N_istant=size(Y_copy,2)
   clear X;
end;








%A=[1.2 1;0 1.3];  %% transformation from previous to current state

A=randCovariance(dim_s,1);
[U S V]=svd(A);
A=U*V';
A=eye(dim_s);

%B=scale_Y*randn(dim_o,dim_s);	  %% transformation from state to observation 
B=eye(dim_o);

mu_W = zeros(dim_s,1);   
mu_V = zeros(dim_o,1);

cov_W = randCovariance(dim_s,scale_W);
cov_V =  randCovariance(dim_o,scale_V);

X=zeros(dim_s,N_istant);
Y=zeros(dim_o,N_istant,N_obs); 



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                  generate synthetic data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('Generating data...\n');

mu_X = randMean(dim_s,1);
cov_X = randCovariance(dim_s,1);

 
X1 = randvec(1,mu_X,cov_X);
X(:,1)=X1;

%if (ok_load==1),
%   Y=Y_copy;
   
for i=1:N_istant,

    W=randvec(1,mu_W,cov_W);
    X(:,i+1) = A*X(:,i) + W;

    for j=1:N_obs,
	V=randvec(1,mu_V,cov_V);
	Y(:,i,j) = B*X(:,i) + V;
    end;

end;   

if (ok_load==1),
   Y=Y_copy;
end;

%figure(1);
%hold on;
%plot(X(1,1:N_istant),X(2,1:N_istant),'c+');
%plot(Y(1,:,1),Y(2,:,1),'r+');
%plot(X(1,1),X(2,1),'b+');
%axis([-20 20 -20 20]);
%title('Initial state (blu dot); States (cyan dots);Obs. (red dots);KF (green line); K smoother (magenta line)');

%hold off;

if (dim_s==2)&(dim_o==2),
   figure(2);
   hold on;
   plot(X(1,1:N_istant),X(2,1:N_istant),'c+');
   plot(Y(1,:,1),Y(2,:,1),'r+');
   plot(X(1,1),X(2,1),'b+');
   %axis([-20 20 -20 20]);
   title('States=cyan dots;   Observations 2 = red dots');
   drawnow;
   hold off;
end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%5

cov_w = eye(dim_s);
mu_Xo = mu_X;
cov_Xo = cov_X;
cov_Wo = cov_W;
cov_Vo =  cov_V;
Ao = A;
Bo = B;

 cov_W=randCovariance(dim_s,1);
 A=randCovariance(dim_s,1);
 B=randCovariance(dim_o,1);
 cov_V=randCovariance(dim_o,1);
 mu_X=randMean(dim_s,1);
 cov_X=randCovariance(dim_s,1);
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%              Kalman filtering  over the computed synthetic data    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% mu_X_t_tp mean of X at istant t with t-1 observations 
% mu_X_t_t mean of X at istant t with t observations 
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
count=0;

while (count<N_count),


fprintf('%d - Kalman filtering...\n',count);

%%%% init

mu_X_t_t  = zeros(dim_s,N_istant,N_obs);
mu_X_t_tp = zeros(dim_s,N_istant,N_obs);
P_t_t     = zeros(dim_s,dim_s,N_istant);
P_t_tp    = zeros(dim_s,dim_s,N_istant);
K	  = zeros(dim_s,dim_o,N_istant);  

%%% init at istant t=1
mu_X_t_to = mu_X;
P_t_to = cov_X;

K(:,:,1) = P_t_to*B'*inv(cov_V + B*P_t_to*B');
P_t_t(:,:,1)  = (eye(dim_s)-K(:,:,1)*B)*P_t_to*...
		      (eye(dim_s)-K(:,:,1)*B)' + K(:,:,1)*cov_V*K(:,:,1)';
for jj=1:N_obs,
    mu_X_t_t(:,1,jj) =  mu_X_t_to + K(:,:,1)*(Y(:,1,jj)-B*mu_X_t_to);
end;

%%%%%%% precompute Kt and Pt 

for t=2:N_istant,

    P_t_tp(:,:,t)   = A*P_t_t(:,:,t-1)*A' + cov_W;
    K(:,:,t)	    = P_t_tp(:,:,t)*B'*inv(cov_V + B*P_t_tp(:,:,t)*B');
    P_t_t(:,:,t)    = (eye(dim_s)-K(:,:,t)*B)*P_t_tp(:,:,t)*...
		      (eye(dim_s)-K(:,:,t)*B)' + K(:,:,t)*cov_V*K(:,:,t)';

end;

%%%%%%% estimate  the sequence of mu_x from  observations

for t=2:N_istant,
    for jj=1:N_obs,
      mu_X_t_tp(:,t,jj) = A*mu_X_t_t(:,t-1,jj);
     mu_X_t_t(:,t,jj) = mu_X_t_tp(:,t,jj)... 
		         + K(:,:,t)*(Y(:,t,jj)-B*mu_X_t_tp(:,t,jj));
    end;
end;


%%%%%%%%%%% show estimated mean and covariance

%figure(1);
%hold on;

%for t=2:N_istant,

%   plotGauss( mu_X_t_t(1,t),mu_X_t_t(2,t),P_t_t(1,1,t),
%                      P_t_t(2,2,t),P_t_t(1,2,t));
%   pause;

%end;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%                  Kalman smoother                          %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('E step ...\n');

%%%%% init
tau    = N_istant;   %% the variable tau is not used
t_last =1;	  % the smother will gp back until t_last

mu_X_t_tau = zeros(dim_s,N_istant,N_obs);
P_t_tau	   = zeros(dim_s,dim_s,N_istant);
J	   = zeros(dim_s,dim_s,N_istant);
LAG	   = zeros(dim_s,dim_s,N_istant); 
M_t	   = zeros(dim_s,dim_s,N_istant,N_obs); 
M_t_tp	   = zeros(dim_s,dim_s,N_istant,N_obs); 

mu_X_t_tau(:,N_istant,:) = mu_X_t_t(:,N_istant,:);
P_t_tau(:,:,N_istant) = P_t_t(:,:,N_istant);

list=(-N_istant):(-t_last-1);
list=list*(-1);

for t=list,
   J(:,:,t-1)= P_t_t(:,:,t-1)*A'*inv(P_t_tp(:,:,t));
   P_t_tau(:,:,t-1) = P_t_t(:,:,t-1) + J(:,:,t-1)*(P_t_tau(:,:,t)...
  		      - P_t_tp(:,:,t))*J(:,:,t-1)';
end;

LAG(:,:,N_istant) = A*P_t_t(:,:,N_istant-1)...
		    - K(:,:,N_istant)*B*A*P_t_t(:,:,N_istant-1);



for t=list(1:N_istant-2),  
     LAG(:,:,t-1) = P_t_t(:,:,t-1)*J(:,:,t-2)' ...
		   + J(:,:,t-1)'*(LAG(:,:,t)-A*P_t_t(:,:,t-1))*J(:,:,t-2)';
end;

for t=list,
  for jj=1:N_obs,
     
      mu_X_t_tau(:,t-1,jj) = mu_X_t_t(:,t-1,jj)...
		       + J(:,:,t-1)*( mu_X_t_tau(:,t,jj) - mu_X_t_tp(:,t,jj)); 
   end;
end;

for t=1:N_istant,
  for jj=1:N_obs,
      M_t(:,:,t,jj) =  mu_X_t_tau(:,t,jj)* mu_X_t_tau(:,t,jj)'...
		       + P_t_tau(:,:,t); 
  end;
end;


for t=2:N_istant,
  for jj=1:N_obs,
      M_t_tp(:,:,t,jj) =  mu_X_t_tau(:,t,jj)* mu_X_t_tau(:,t-1,jj)'...
		       + LAG(:,:,t);    
  end;
end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%              M-step                               %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('M step ...\n');
%%%%%%%%%%%%%%% mu_X_new %%%%%%%%%%%%%%%%%%%%%

mu_X_new = sum(mu_X_t_tau(:,1,:),3)/N_obs;


%%%%%%%%%%%%%% cov_new %%%%%%%%%%%%%%%%%%%%%%%%%%

cov_X_new = sum(M_t(:,:,1,:),4)/N_obs - mu_X_new*mu_X_new';


%%%%%%%%%%%%% A_new %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

M_t_tp2 = M_t_tp(:,:,2:N_istant,:);
M_t2    = M_t(:,:,1:N_istant-1,:);

sum_Mp = sum(M_t_tp2(:,:,:,:),3);
sum_M = sum(M_t2(:,:,:,:),3);

A_new = sum(sum_Mp,4)*inv(sum(sum_M,4));


%%%%%%%% Q_new %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Q1=zeros(dim_s,dim_s,N_istant,N_obs);

for t=1:N_istant,
    for jj=1:N_obs,
	Q1(:,:,t,jj) = M_t(:,:,t,jj) - A_new*M_t_tp(:,:,t,jj)';
    end;
end;

Q2 =  Q1(:,:,2:N_istant,:);
sum_Q = sum(Q2,3);
Q_new = 1/(N_obs*(N_istant-1))*sum(sum_Q,4);


%%%%%%%% B_new %%%%%%%%%%%%%%%%%%%%%%%%%%%%%

B1=zeros(dim_o,dim_s,N_istant,N_obs);

for t=1:N_istant,
    for jj=1:N_obs,
	B1(:,:,t,jj)=Y(:,t,jj)*mu_X_t_tau(:,t,jj)';
    end;
end;

sum_B1 = sum(B1(:,:,:,:),3);
sum_M = sum(M_t(:,:,:,:),3);

B_new = sum(sum_B1,4)*inv(sum(sum_M,4));



%%%%%%%%%  R_new  %%%%%%%%%%%%%%%%%%%%%%%%%%%%

R1=zeros(dim_o,dim_o,N_istant,N_obs);

for t=1:N_istant,
    for jj=1:N_obs,
	R1(:,:,t,jj) = Y(:,t,jj)*Y(:,t,jj)' - B_new*mu_X_t_tau(:,t,jj)*Y(:,t,jj)';
    end;
end;

sum_R = sum(R1(:,:,:,:),3);
R_new = 1/(N_istant*N_obs)*sum(sum_R,4);


%%%%%%%% update parametrs %%%%%%%%%%%%

mu_X  = mu_X_new;
cov_X = cov_X_new;
cov_W = Q_new;
cov_V = R_new;
A     =A_new;
B     =Bo;       %;B_new;

%fprintf('\n mu_X = %f',mu_X);
fprintf('\n');

%%%%%%%%%%%%%%%%%% estimate errors  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
E_K=zeros(N_obs,N_istant);
E_KS=zeros(N_obs,N_istant);
tot_E_K=zeros(1,N_obs);
tot_E_KS=zeros(1,N_obs);

for jj=1:N_obs,
    E_K(jj,:) = sum(( mu_X_t_t(:,:,jj) - X(:,1:N_istant) ).^2);
    E_KS(jj,:) = sum((mu_X_t_tau(:,:,jj) - X(:,1:N_istant) ).^2);
end;

tot_E_K = sum(E_K,2)'/N_istant;
tot_E_KS = sum(E_KS,2)'/N_istant;

mean_tot_E_K =sum(tot_E_K)/N_obs;
mean_tot_E_KS =sum(tot_E_KS)/N_obs;
%fprintf('Average KF Error = %f\n',mean_tot_E_K);
%fprintf('Average KFS Error = %f\n',mean_tot_E_KS);





%%%%%%%%%%% show smooth mean and covariance

%figure(1);
%hold on;

%for t=2:N_istant,

 %plotGauss( mu_X_t_tau(1,t),mu_X_t_tau(2,t),P_t_tau(1,1,t),P_t_tau(2,2,t),P_t_tau(1,2,t));
   %pause;
   %keyboard;
   %end;



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if (plot_N==0),
   %jj=1;
   figure(3);
   zoom on;
   
   plot(Y(dim_plot,:,1),'y.-','linewidth',2);
   hold on;
   plot(X(dim_plot,1),'c+');
   %plot(mu_X_t_t(dim_plot,:,1),'g-','linewidth',2);
   plot(mu_X_t_tau(dim_plot,:,1),'m-','linewidth',2);
   %axis([-20 20 -20 20]);
   title('Initial state = cyan dot;  Observations = yellow line;  Kalman smoother = magenta line');
   hold off;drawnow;
end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if (plot_N==1),
   flag1=ok_load;
   show_data(Y,mu_X_t_tau(:,:,1),flag1);
end;


hold off;


count = count +1;
%pause;

%return;
%%%%%%%%%%%%% log likelihood  %%%%%%%%%%%%%%%%%%%%%%%%%%5

%mu_Y_t_tp  = B*mu_X_t_tp; 
%cov_Y_t_tp = cov_V + B*cov_X*


end;   %% end while loop



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5
%jj=1;
%figure(5);
%zoom on;
%hold on;
%plot(E_K(1,:),'g-','linewidth',2);
%plot(E_KS(1,:),'m-','linewidth',2);
%axis([-20 20 -20 20]);
%title('States (cyan line); KF (green line); K smoother (magenta line)');


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%