%% Example of Section 6.4.1: robustness to noise
% 
% Estimation of the parameters of a switched dynamical system
% as proposed in 
%   R. Vidal, "Recursive identification of switched ARX systems", Automatica 44 (9):2274–2287, 2008. 
%
%
% You must have klinreg_matlab installed and in the PATH
% to run this test.
%
DO_OTHERS = 0; 	% if 0, use only klinreg, otherwise compare with other methods
		% this requires the code for the other methods (not included in this package)

format compact;

% true parameters
a = [-0.9, 0.7];
c = [1, -1];

% Noise levels
sigmaT = [0, 0.02, 0.1, 0.2, 0.3];

if DO_OTHERS
	MeanNMSE = zeros(5,length(sigmaT));	% these contain the results on output
	StdNMSE = zeros(5,length(sigmaT));
	Nfails = zeros(5,length(sigmaT));
else
	MeanNMSE = zeros(2,length(sigmaT));
	StdNMSE = zeros(2,length(sigmaT));
	Nfails = zeros(2,length(sigmaT));	
end

for i_sigma=1:length(sigmaT)


	% data
	N = 1000;
	sigmaNoise = sigmaT(i_sigma)

	Nmodes = 2;
	
	if sigmaNoise > 0
		Nexpe = 100;	% number of trials
	else
		Nexpe = 1;
	end
	
	w1ref = zeros(Nexpe,2);
	w2ref = zeros(Nexpe,2);


	w1 = zeros(Nexpe,2);
	w2=zeros(Nexpe,2);


	NMSEref = zeros(Nexpe,1);
	NMSE = zeros(Nexpe,1);
	
	if DO_OTHERS
		NMSEme = zeros(Nexpe,1);
		NMSEpe = zeros(Nexpe,1);
		NMSEalg = zeros(Nexpe,1);
	end
	
	u = randn(N,1); % system input
	q = (mod(1:N+1,60)>30)+1; % switching mode 
	
	for expe=1:Nexpe
	    expe

	    noise = sigmaNoise*randn(N,1);

	    y = zeros(N+1,1);

	    for t=2:N+1
		y(t) = a(q(t))*y(t-1) + c(q(t))*u(t-1) + noise(t-1);
	    end

	    X = [y(1:N), u]; 	% regression matrix
	    Y = y(2:N+1);	% target output vector
	    
	     % Reference model
	    
	    w1ref(expe,:) = (pinv(X(q(2:N+1)==1,:))*Y(q(2:N+1)==1))';
	    w2ref(expe,:) = (pinv(X(q(2:N+1)==2,:))*Y(q(2:N+1)==2))';
	    NMSEref(expe) = sum((w1ref(expe,:)-[a(1) c(1)]).^2)./sum([a(1);c(1)].^2) + sum((w2ref(expe,:)-[a(2) c(2)]).^2)./sum([a(1);c(1)].^2) ;
	   
	    
	    % k-LinReg

	    [W lambda]=klinreg(X,Y,Nmodes,1);

	    % Compute parametric error NMSE 
	    % (after reordering the parameter vectors in W)
	    if (([a(1);c(1)] - W(:,1))'*([a(1);c(1)] - W(:,1)) <([a(1);c(1)] - W(:,2))'*([a(1);c(1)] - W(:,2)))
		    w1(expe,:) = W(:,1)';
		    w2(expe,:) = W(:,2)';
		else
		    w1(expe,:) = W(:,2)';
		    w2(expe,:) = W(:,1)';
	    end
		
	    NMSE(expe) = sum((w1(expe,:)-[a(1) c(1)]).^2)./sum([a(1);c(1)].^2) + sum((w2(expe,:)-[a(2) c(2)]).^2)./sum([a(1);c(1)].^2) ;


	    if DO_OTHERS
		% ME-MCS

		[W lambda]=me_mcs(X,Y,Nmodes);

		if (([a(1);c(1)] - W(:,1))'*([a(1);c(1)] - W(:,1)) <([a(1);c(1)] - W(:,2))'*([a(1);c(1)] - W(:,2)))
		    w1(expe,:) = W(:,1)';
		    w2(expe,:) = W(:,2)';
		else
		    w1(expe,:) = W(:,2)';
		    w2(expe,:) = W(:,1)';
		end

		NMSEme(expe) = sum((w1(expe,:)-[a(1) c(1)]).^2)./sum([a(1);c(1)].^2) + sum((w2(expe,:)-[a(2) c(2)]).^2)./sum([a(1);c(1)].^2) ;

		% PE-MCS

		[W lambda]=pe_mcs(X,Y,Nmodes);

		if (([a(1);c(1)] - W(:,1))'*([a(1);c(1)] - W(:,1)) <([a(1);c(1)] - W(:,2))'*([a(1);c(1)] - W(:,2)))
		    w1(expe,:) = W(:,1)';
		    w2(expe,:) = W(:,2)';
		else
		    w1(expe,:) = W(:,2)';
		    w2(expe,:) = W(:,1)';
		end

		NMSEpe(expe) = sum((w1(expe,:)-[a(1) c(1)]).^2)./sum([a(1);c(1)].^2) + sum((w2(expe,:)-[a(2) c(2)]).^2)./sum([a(1);c(1)].^2) ;

		% ALGEBRAIC method

		b = gpca_pda([X,-Y]',Nmodes,0.02);
		W = b./(ones(3,1)*b(end,:));
		W(end,:) = [];
		[err,lambda] = min((b'*[X,-Y]').^2,[],1);

		if (([a(1);c(1)] - W(:,1))'*([a(1);c(1)] - W(:,1)) <([a(1);c(1)] - W(:,2))'*([a(1);c(1)] - W(:,2)))
		    w1(expe,:) = W(:,1)';
		    w2(expe,:) = W(:,2)';
		else
		    w1(expe,:) = W(:,2)';
		    w2(expe,:) = W(:,1)';
		end

		NMSEalg(expe) = sum((w1(expe,:)-[a(1) c(1)]).^2)./sum([a(1);c(1)].^2) + sum((w2(expe,:)-[a(2) c(2)]).^2)./sum([a(1);c(1)].^2) ;

	    end	    	    
	   
	end

	threshFailureDetect = 0.001; % Threshold on NMSE to detect a failure

	% Compute the number of failures and remove the corresponding NMSE
	NMSE(NMSE>threshFailureDetect)=[];	
	Nfails(2,i_sigma) = Nexpe - length(NMSE);

	if DO_OTHERS
		NMSEme(NMSEme>threshFailureDetect)=[];
		Nfails(3,i_sigma) = Nexpe - length(NMSEme);

		NMSEpe(NMSEpe>threshFailureDetect)=[];
		Nfails(4,i_sigma) = Nexpe - length(NMSEpe);

		NMSEalg(NMSEalg>threshFailureDetect)=[];
		Nfails(5,i_sigma) = Nexpe - length(NMSEalg);
	end

	MeanNMSE(1,i_sigma) = mean(NMSEref);	% Results of the reference model
	StdNMSE(1,i_sigma) = std(NMSEref);
	MeanNMSE(2,i_sigma) = mean(NMSE);	% Results of k-linreg
	StdNMSE(2,i_sigma) = std(NMSE);
	if DO_OTHERS
		MeanNMSE(3,i_sigma) = mean(NMSEme);	% Results of ME-MCS
		StdNMSE(3,i_sigma) = std(NMSEme);
		MeanNMSE(4,i_sigma) = mean(NMSEpe);	% Results of PE-MCS
		StdNMSE(4,i_sigma) = std(NMSEpe);
		MeanNMSE(5,i_sigma) = mean(NMSEalg);	% Results of the algebraic method
		StdNMSE(5,i_sigma) = std(NMSEalg);
	end
end

% Plot results
close all

% NMSE vs noise level
plot(sigmaT, MeanNMSE(1,:),'+r','LineWidth',2,'MarkerSize',12);
hold on;
plot(sigmaT, MeanNMSE(2,:),'-b','LineWidth',2);
if DO_OTHERS
	plot(sigmaT, MeanNMSE(3,:),'or','LineWidth',2,'MarkerSize',12);
	plot(sigmaT, MeanNMSE(4,:),'x--k','LineWidth',2,'MarkerSize',12);
	plot(sigmaT, MeanNMSE(5,:),'--k','LineWidth',2);
end
set(gca,'FontSize',16);
xlabel('Noise standard deviation');
ylabel('Average NMSE');

if DO_OTHERS
	legend('Reference','k-LinReg','ME-MCS','PE-MCS','Algebraic','Location','NorthWest');
else
	legend('Reference','k-LinReg','Location','NorthWest');
end

% Number of failures vs noise level
figure;
bar(sigmaT,Nfails(2:end,:)')
axis([0,0.35, 0, 100]) ;
set(gca,'FontSize',16);
if DO_OTHERS
	legend('k-LinReg','ME-MCS','PE-MCS','Algebraic','Location','NorthWest');
else
	legend('k-LinReg','Location','NorthWest');
end
xlabel('Noise standard deviation');
ylabel('Percentage of failures');



