% ICASSP 2011 - INCREMENTAL-COOPERATIVE STRATEGIES IN COMBINATION OF
% ADAPTIVE FILTERS
% Wilder Bezerra Lopes & Cassio Guimares Lopes
% Signal Processing Lab - Electronic Systems - University of São Paulo
% wilderlopes@gmail.com, wilder@usp.br, cassio@lps.usp.br

% The MIT License (MIT)
% 
% Copyright (c) 2014  Wilder Lopes
% 
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, including without limitation the rights
% to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
% copies of the Software, and to permit persons to whom the Software is
% furnished to do so, subject to the following conditions:
% 
% The above copyright notice and this permission notice shall be included in
% all copies or substantial portions of the Software.
% 
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
% FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
% AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
% LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
% OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
% THE SOFTWARE.

tic
clear, clc;

wo1=ones(10,1);
wo1=wo1./norm(wo1);
M=length(wo1);

N = 3000; %Iterations
L = 40;   %Realizations

var_v = 1e-3; %Noise variance
var_u = 1;    %Regressor variance
var_q = 1e-6; %Random walk variance

%### Parameters for the autoregressive Model
AR_sw = 0;   %Switch to turn on/off the Autoregressive Model: 0-Off / 1-On
b = 0.95;    %AR model transfer function pole 
%###

%### Step sizes
mu_o = 0.7;
r = 0.1;

mu1       = 1*mu_o/(M*var_u);%mu for filter 1 -> LMS_1
mu2       = r*mu_o/(M*var_u);%mu for filter 2 -> LMS_2
mu_a      = 1000;            %mu for 'a' factor adaptation
%###

%### alpha and beta
alpha=0.98;
beta=(1-alpha);%For SNR=10dB, SNR=5dB and SNR=3dB multiply it for 0.1

%### Initializing for CLMS
err1_lms=zeros(N,1); exc1_lms=zeros(N,1); dev1_lms=zeros(N,1);
err2_lms=zeros(N,1); exc2_lms=zeros(N,1); dev2_lms=zeros(N,1);
err_clms=zeros(N,1); exc_clms=zeros(N,1); dev_clms=zeros(N,1);

MSE1_lms=zeros(N,1); EMSE1_lms=zeros(N,1); MSD1_lms=zeros(N,1); %Initializing MSE1, EMSE1 and MSD1 vectors for calculation
MSE2_lms=zeros(N,1); EMSE2_lms=zeros(N,1); MSD2_lms=zeros(N,1); %Initializing MSE2, EMSE2 and MSD2 vectors for calculation
MSE_clms=zeros(N,1); EMSE_clms=zeros(N,1); MSD_clms=zeros(N,1); %Initializing MSE, EMSE and MSD vectors for calculation

LAMBDA_clms=zeros(N,1);
A_clms=zeros(N,1);
%### 

%### Initializing for INC-COOP 1
err1_inccoop1=zeros(N,1); exc1_inccoop1=zeros(N,1); dev1_inccoop1=zeros(N,1);
err2_inccoop1=zeros(N,1); exc2_inccoop1=zeros(N,1); dev2_inccoop1=zeros(N,1);
err_inccoop1=zeros(N,1); exc_inccoop1=zeros(N,1); dev_inccoop1=zeros(N,1);

MSE1_inccoop1=zeros(N,1); EMSE1_inccoop1=zeros(N,1); MSD1_inccoop1=zeros(N,1); %Initializing MSE1, EMSE1 and MSD1 vectors for calculation
MSE2_inccoop1=zeros(N,1); EMSE2_inccoop1=zeros(N,1); MSD2_inccoop1=zeros(N,1); %Initializing MSE2, EMSE2 and MSD2 vectors for calculation
MSE_inccoop1=zeros(N,1); EMSE_inccoop1=zeros(N,1); MSD_inccoop1=zeros(N,1); %Initializing MSE, EMSE and MSD vectors for calculation
%### 

%### Initializing for INC-COOP 2
err1_inccoop2=zeros(N,1); exc1_inccoop2=zeros(N,1); dev1_inccoop2=zeros(N,1);
err2_inccoop2=zeros(N,1); exc2_inccoop2=zeros(N,1); dev2_inccoop2=zeros(N,1);
err_inccoop2=zeros(N,1); exc_inccoop2=zeros(N,1); dev_inccoop2=zeros(N,1);

MSE1_inccoop2=zeros(N,1); EMSE1_inccoop2=zeros(N,1); MSD1_inccoop2=zeros(N,1); %Initializing MSE1, EMSE1 and MSD1 vectors for calculation
MSE2_inccoop2=zeros(N,1); EMSE2_inccoop2=zeros(N,1); MSD2_inccoop2=zeros(N,1); %Initializing MSE2, EMSE2 and MSD2 vectors for calculation
MSE_inccoop2=zeros(N,1); EMSE_inccoop2=zeros(N,1); MSD_inccoop2=zeros(N,1); %Initializing MSE, EMSE and MSD vectors for calculation

gamma=0.1; %constant in the scheme 2; THE HIGHER gamma, LOWER IS THE EMSE
%### 

LAMBDA_inccoop1=zeros(N,1);
A_inccoop1=zeros(N,1);

LAMBDA_inccoop2=zeros(N,1);
A_inccoop2=zeros(N,1);

Ru_i=zeros(M);
Rx_i=zeros(M);


for l=1:L %realizations
    
    fprintf('Realization %d of %d... \n', l, L);
    
    wo = wo1;
        
    wlms1=zeros(M,1); %Initializing weigth vector w1
    wlms2=zeros(M,1); %Initializing weigth vector w2
    w_clms=zeros(M,1);%Initializing weigth vector w_clms=lambda*wlms1 + (1-lambda)*wlms2;
    
    w1_convex=zeros(M,1); %Initializing weigth vector w1
    w2_convex=zeros(M,1); %Initializing weigth vector w2
    w_convex=zeros(M,1);%Initializing weigth vector w_convex=lambda*wrls1 + (1-lambda)*wrls2;
    
    w_lmmn = zeros(M,1);
    
    w1_inccoop1=zeros(M,1); %Initializing weigth vector w1
    w2_inccoop1=zeros(M,1); %Initializing weigth vector w2    
    w_inccoop1=zeros(M,1);%Initializing weigth vector w=lambda*w1 + (1-lambda)*w2;
    
    w1_inccoop2=zeros(M,1); %Initializing weigth vector w1
    w2_inccoop2=zeros(M,1); %Initializing weigth vector w2    
    w_inccoop2=zeros(M,1);%Initializing weigth vector w=lambda*w1 + (1-lambda)*w2;
           
    a_clms=0;%Initializing 'a_clms' factor;
    lambda_clms=sigmf(a_clms,[1 0]);%Initializing 'lambda_clms';
    
    a_convex=0;%Initializing 'a_convex' factor;
    lambda_convex=sigmf(a_convex,[1 0]);%Initializing 'lambda_clms';
    
    a_inccoop1=10;%Initializing 'a' factor;
    lambda_inccoop1=2*sigmf(a_inccoop1,[1 0]) - 1;%initializing lambda
    
    a_inccoop2=10;%Initializing 'a' factor;
    lambda_inccoop2=2*sigmf(a_inccoop1,[1 0]) - 1;%initializing lambda
        
    x_j=sqrt(var_u)*randn(1,M); %Input for the AR
    u_j=zeros(1,M);
    
    aa_clms=zeros(N,1);
    lamb_clms=zeros(N,1);
    
    aa_convex=zeros(N,1);
    lamb_convex=zeros(N,1);
    
    aa_inccoop1=zeros(N,1);
    lamb_inccoop1=zeros(N,1);
    
    aa_inccoop2=zeros(N,1);
    lamb_inccoop2=zeros(N,1);

    
    for i=1:N %iterations
        
%### Uncomment to implement abrupt nonstationarity at iteration i
%
%         if (i==10000)   
%             %var_v=1e-1;
%             wo=-wo;%randn(10,1);
%             %wo=wo/norm(wo);
%         end    
%###

 %#################### AR Process -> to generate colored input
 
        if AR_sw==1
            
            for j=1:(M-1) %Building up the Regressor vector 

                if j==1
                    u_j(j)=x_j(j);
                end
    
                u_j(j+1) = sqrt(1-b^2)*x_j(j+1) + b*u_j(j);

            end
            
        else            
            u_j=x_j;            
        end
%####################
        
        if l==L
            Ru_i=Ru_i + (u_j'*u_j)./N;
            Rx_i=Rx_i + (x_j'*x_j)./N;
        end
                    
        x=sqrt(var_u)*randn; %White Gaussian Noise for regressors
        v=sqrt(var_v)*randn; %White Gaussian Noise
        d=u_j*wo+v;%adding noise
        
        
%################################ Regular CLMS

        ylms1=u_j*wlms1;
        ylms2=u_j*wlms2;
                
        err1_lms(i,1)=d-ylms1;%a priori error 1
        exc1_lms(i,1)=(u_j*(wo-wlms1))^2;%instantaneous EMSE 1
        dev1_lms(i,1)=(norm(wo-wlms1))^2;%instantaneous MSD 1
        
        err2_lms(i,1)=d-ylms2;%a priori error 2
        exc2_lms(i,1)=(u_j*(wo-wlms2))^2;%instantaneous EMSE 2
        dev2_lms(i,1)=(norm(wo-wlms2))^2;%instantaneous MSD 2
        
        wlms1 = wlms1 + mu1*(u_j')*err1_lms(i,1); %Adapting coefficients of filter 1 
        wlms2 = wlms2 + mu2*(u_j')*err2_lms(i,1); %Adapting coefficients of filter 2

        y_clms=ylms1*lambda_clms + (1-lambda_clms)*ylms2;               
        err_clms(i,1)=d-y_clms;%a priori error CONVEX
        exc_clms(i,1)=(u_j*(wo-w_clms))^2;%instantaneous EMSE CONVEX
        dev_clms(i,1)=(norm(wo-w_clms))^2;%instantaneous MSD CONVEX
        w_clms = lambda_clms*wlms1 + (1-lambda_clms)*wlms2; %Weight vector of convex combination    
     

            if a_clms>4 %Limiting 'a_clms' to have always a minimum level of adaptation;
                a_clms=4;
                a_clms = a_clms + mu_a*err_clms(i,1)*(ylms1-ylms2)*lambda_clms*(1-lambda_clms);
            elseif a_clms<-4
                a_clms=-4;
                a_clms = a_clms + mu_a*err_clms(i,1)*(ylms1-ylms2)*lambda_clms*(1-lambda_clms);
            else
                a_clms = a_clms + mu_a*err_clms(i,1)*(ylms1-ylms2)*lambda_clms*(1-lambda_clms); 
            end
        
                       
            lambda_clms=sigmf(a_clms,[1 0]);%lambda defined by a sigmoidal function

            aa_clms(i,1)=a_clms;            
            lamb_clms(i,1)=lambda_clms;
            
%################################     
        

%################################ INC-COOP1       

        y_inccoop1=u_j*w_inccoop1;
        err_inccoop1(i,1)=d-y_inccoop1;%a priori error INC-COOP1
        exc_inccoop1(i,1)=(u_j*(wo-w_inccoop1))^2;%instantaneous EMSE 
        dev_inccoop1(i,1)=(norm(wo-w_inccoop1))^2;%instantaneous MSD 

        err1_inccoop1(i,1)=d-u_j*w2_inccoop1;
        exc1_inccoop1(i,1)=(u_j*(wo-w1_inccoop1))^2;%instantaneous EMSE 1
        dev1_inccoop1(i,1)=(norm(wo-w1_inccoop1))^2;%instantaneous MSD 1
        w1_inccoop1 = w2_inccoop1 + mu1*(lambda_inccoop1)*(u_j')*err1_inccoop1(i,1); %Adapting coefficients of filter 1 
     
        err2_inccoop1(i,1)=d-u_j*w1_inccoop1;
        exc2_inccoop1(i,1)=(u_j*(wo-w2_inccoop1))^2;%instantaneous EMSE 2
        dev2_inccoop1(i,1)=(norm(wo-w2_inccoop1))^2;%instantaneous MSD 2
        w2_inccoop1 = w1_inccoop1 + mu2*(1-lambda_inccoop1)*(u_j')*err2_inccoop1(i,1); %Adapting coefficients of filter 2
                  
        w_inccoop1 = w2_inccoop1; % Weight-Vector INC-COOP 1
        
        a_inccoop1 = alpha*(a_inccoop1) + (beta)*(err_inccoop1(i)^2);
        lambda_inccoop1 = 2*sigmf(a_inccoop1,[1 0]) - 1;
        
        aa_inccoop1(i,1)=a_inccoop1;
        lamb_inccoop1(i,1)=lambda_inccoop1;
        
%################################   


%################################ INC-COOP 2       
              
        y_inccoop2=u_j*w_inccoop2;
        err_inccoop2(i,1)=d-y_inccoop2;%a priori error INC-COOP2
        exc_inccoop2(i,1)=(u_j*(wo-w_inccoop2))^2;%instantaneous EMSE
        dev_inccoop2(i,1)=(norm(wo-w_inccoop2))^2;%instantaneous MSD
        
        err1_inccoop2(i,1)=d-u_j*w2_inccoop2;
        exc1_inccoop2(i,1)=(u_j*(wo-w1_inccoop2))^2;%instantaneous EMSE 1
        dev1_inccoop2(i,1)=(norm(wo-w1_inccoop2))^2;%instantaneous MSD 1
        w1_inccoop2 = w2_inccoop2 + mu1*(lambda_inccoop2)*(u_j')*err1_inccoop2(i,1); %Adapting coefficients of filter 1
 
        err2_inccoop2(i,1)=d-u_j*w1_inccoop2;
        exc2_inccoop2(i,1)=(u_j*(wo-w2_inccoop2))^2;%instantaneous EMSE 2
        dev2_inccoop2(i,1)=(norm(wo-w2_inccoop2))^2;%instantaneous MSD 2
        w2_inccoop2 = w1_inccoop2 + gamma*(lambda_inccoop2*mu1 + mu2*(1-lambda_inccoop2))*(u_j')*err2_inccoop2(i,1); %Adapting coefficients of filter 2
        
        w_inccoop2 = w2_inccoop2; % Weight-Vector INC-COOP 2  
                           
        a_inccoop2 = alpha*(a_inccoop2) + (beta)*(err_inccoop2(i)^2);
        lambda_inccoop2 = 2*sigmf(a_inccoop2,[1 0]) - 1; %a_inccoop2/(1+a_inccoop2);  
        
        aa_inccoop2(i,1)=a_inccoop2;
        lamb_inccoop2(i,1)=lambda_inccoop2;
        
%################################ 
                     
     
        x_j=[x x_j(1:M-1)];%Regressor with shift structure
      
        %wo = wo + sqrt(var_q)*randn(M,1); %random-walk model
               
    end
    
    %### Ensemble Average for REGULAR CLMS    
    MSE1_lms=MSE1_lms+(err1_lms.^2)./L;
    EMSE1_lms=EMSE1_lms+exc1_lms./L;
    MSD1_lms=MSD1_lms+dev1_lms./L;
    
    MSE2_lms=MSE2_lms+(err2_lms.^2)./L;
    EMSE2_lms=EMSE2_lms+exc2_lms./L;
    MSD2_lms=MSD2_lms+dev2_lms./L;
    
    MSE_clms=MSE_clms+(err_clms.^2)./L;
    EMSE_clms=EMSE_clms+exc_clms./L;
    MSD_clms=MSD_clms+dev_clms./L;
    
    A_clms=A_clms+aa_clms./L;
    LAMBDA_clms=LAMBDA_clms + lamb_clms./L;        
    %###    
  
    %### Ensemble Average for INC-COOP 1
    MSE1_inccoop1=MSE1_inccoop1+(err1_inccoop1.^2)./L;
    EMSE1_inccoop1=EMSE1_inccoop1+exc1_inccoop1./L;
    MSD1_inccoop1=MSD1_inccoop1+dev1_inccoop1./L;
    
    MSE2_inccoop1=MSE2_inccoop1+(err2_inccoop1.^2)./L;
    EMSE2_inccoop1=EMSE2_inccoop1+exc2_inccoop1./L;
    MSD2_inccoop1=MSD2_inccoop1+dev2_inccoop1./L;
    
    MSE_inccoop1=MSE_inccoop1+(err_inccoop1.^2)./L;
    EMSE_inccoop1=EMSE_inccoop1+exc_inccoop1./L;
    MSD_inccoop1=MSD_inccoop1+dev_inccoop1./L;
    
    A_inccoop1=A_inccoop1+aa_inccoop1./L;
    LAMBDA_inccoop1=LAMBDA_inccoop1+lamb_inccoop1./L;
    %### 
    
    %### Ensemble Average for INC-COOP 2
    MSE1_inccoop2=MSE1_inccoop2+(err1_inccoop2.^2)./L;
    EMSE1_inccoop2=EMSE1_inccoop2+exc1_inccoop2./L;
    MSD1_inccoop2=MSD1_inccoop2+dev1_inccoop2./L;
    
    MSE2_inccoop2=MSE2_inccoop2+(err2_inccoop2.^2)./L;
    EMSE2_inccoop2=EMSE2_inccoop2+exc2_inccoop2./L;
    MSD2_inccoop2=MSD2_inccoop2+dev2_inccoop2./L;
    
    MSE_inccoop2=MSE_inccoop2+(err_inccoop2.^2)./L;
    EMSE_inccoop2=EMSE_inccoop2+exc_inccoop2./L;
    MSD_inccoop2=MSD_inccoop2+dev_inccoop2./L;
         
    A_inccoop2=A_inccoop2+aa_inccoop2./L;
    LAMBDA_inccoop2=LAMBDA_inccoop2+lamb_inccoop2./L;
    %### 
       
end

% Ru_i
% Tr_Ru=trace(Ru_i)
% Rx_i
% Tr_Rx=trace(Rx_i)

%### Plot MSE
figure('Name','MSE'), title(sprintf('MSE (%d realizations) - mu_o = %g, r = %g, M = %d, var_v=%g', L, mu_o, r, M, var_v),'FontSize',12), 
xlabel('Iterations','FontSize',18), ylabel('MSE (dB)','FontSize',18), hold on,
plot((1:N),10*log10(MSE1_lms),'b',(1:N),10*log10(MSE2_lms),'r', (1:N),10*log10(MSE_clms),'magenta',(1:N),10*log10(MSE_inccoop1),'green',(1:N),10*log10(MSE_inccoop2),'black'), 
legend('LMS_1','LMS_2','CLMS','INC-COOP_1','INC-COOP_2'); 

%### Plot EMSE
figure('Name','EMSE'), title(sprintf('EMSE (%d realizations) - mu_o = %g, r = %g, M = %d, var_v=%g', L, mu_o, r, M, var_v),'FontSize',12), 
xlabel('Iterations','FontSize',18), ylabel('EMSE (dB)','FontSize',18), hold on,
plot((1:N),10*log10(EMSE1_lms),'b',(1:N),10*log10(EMSE2_lms),'r', (1:N),10*log10(EMSE_clms),'magenta',(1:N),10*log10(EMSE_inccoop1),'green',(1:N),10*log10(EMSE_inccoop2),'black'), 
legend('LMS_1','LMS_2','CLMS','INC-COOP_1','INC-COOP_2'); 

%### Plot MSD
figure('Name','MSD'), title(sprintf('MSD (%d realizations) - mu_o = %g, r = %g, M = %d, var_v=%g', L, mu_o, r, M, var_v),'FontSize',12), 
xlabel('Iterations','FontSize',18), ylabel('MSD (dB)','FontSize',18), hold on,
plot((1:N),10*log10(MSD1_lms),'b',(1:N),10*log10(MSD2_lms),'r', (1:N),10*log10(MSD_clms),'magenta',(1:N),10*log10(MSD_inccoop1),'green',(1:N),10*log10(MSD_inccoop2),'black'), 
legend('LMS_1','LMS_2','CLMS','INC-COOP_1','INC-COOP_2'); 

%### Plot 'lambda'
figure('Name','\lambda(i)'), title(sprintf('lambda(i) (%d realizations) - mu_o = %g, r = %g, M = %d, var_v=%g', L, mu_o, r, M, var_v),'FontSize',12), 
xlabel('Iterations','FontSize',12), ylabel('lambda','FontSize',12), hold on,
plot((1:N),LAMBDA_clms,'magenta', (1:N),LAMBDA_inccoop1,'green',(1:N),LAMBDA_inccoop2,'black');
legend('CLMS','INC-COOP_1','INC-COOP_2'); 

%### Plot 'a'
figure('Name','a'), title(sprintf('a(i) (%d realizations) - mu_o = %g, r = %g, M = %d, var_v=%g', L, mu_o, r, M, var_v),'FontSize',12), 
xlabel('Iterations','FontSize',12), ylabel('lambda','FontSize',12), hold on,
plot((1:N),A_clms,'magenta', (1:N),A_inccoop1,'green',(1:N),A_inccoop2,'black');
legend('CLMS','INC-COOP_1','INC-COOP_2'); 

time = toc