% RLS Adaptive Noise cancellation

%-----Filter Parameters-----;
M = 20;
delta = 1; 
lamda = 0.99;  
e_max = 500;

%-----Contants-----
pi = 3.14;
Fs = 0.01;                  %-----signal frequency
Fn = 0.05;                  %-----noise frequency

%-----Initialize-----
w = zeros(M,1);
d = zeros(M,1);
u = zeros(M,1);
P = eye(M)/delta;

%-----Generate desired signal and input(signal+noise)-----
for t=1:M-1
    d(t) = sin(2*pi*Fs*t);
    u(t) = d(t) + 0.5*sin(2*pi*Fn*t) + 0.09*randn;
end
t = M;
epoch = 0;

while epoch<e_max 
    input = sin(2*pi*Fs*t);
    for i=2:M 
        d(M-i+2) = d(M-i+1);
        u(M-i+2) = u(M-i+1);
    end
    d(1) = input;
    u(1) = input +0.5*sin(2*pi*Fn*t)+0.09*randn; 
    
    
    output = w'*u;              %-----compute filter output
    
    
    %-----RLS algorithm-----
    %k = (P*u)/(lamda + u'*P*u);       
    %k = P*u;
    %-----LMS algorithm-----
    k = 0.05*u;
    E = d(1) - w'*u;
    w = w + k*E;
    
    %P = (P/lamda) - (k*u'*P/lamda);
    %P = (P/lamda) - (k*u'*P/(lamda*(lamda + u'*P*u)));
      
    int(t-M+1) = u(1);
    out(t-M+1) = output;
    err(t-M+1) = E;
    
    t = t+1;  
    epoch = epoch + 1;
    
    %-----plot noise and filtered signal-----
    figure(1);
    subplot(211),plot(t,u(1)),axis([0 e_max -2.5 2.5]),title('LMS Filter Input(Siganal + Noise)'),drawnow,hold on
    subplot(212),plot(t,output),axis([0 e_max -2.5 2.5]),title('LMS Filtered Signal'),drawnow,hold on
end