%RLS Noise Cancellation
%Ryan King
%4/26/02

clear all;
close all;

%RF = load('RFrost.txt');


refGain = 1;
worder = 8;


N = 2048;
t=1:N;
signal = sin(2*pi.*t.*t/N/N*8); %.*fliplr(cos(2*pi.*t.*t/N/N*15));
%signal = 0*t;
%signal = randn(1,N);
%signal = RF(3000:7096)';
%clear RF;

N = length(signal);

wreal = randn(1,worder);


additivenoise = randn(1,N);

ref = conv(additivenoise,wreal);
primary = signal + ref(1:length(signal));
fref = additivenoise*refGain; %real reference mic


%experimental
lambda = 1; %technically, this is lambda inverse

w(1,:) = zeros(1,worder);
init = 100;
rinv = diag( ones(1,worder) * init );

%Zero pad so we can start filter at 0 and not throw of the index
frefpad = [zeros(1,worder -1) fref];
   
start = flops;
for n = 1:N;
   %offset n so we can reference the correct value in zero-padded fref
   m = n + worder -1;
   frefblock = frefpad(m-worder+1:1:m)';
      
   refP(n) = w(n,:)*(frefblock);   %ADDING EXTRA RANDOM NOISE TO REFERENCE MIC
   output(n) = primary(n) - refP(n);
   
   k = lambda * rinv * frefblock / (1 + lambda*frefblock'*rinv*frefblock);
   w(n+1,:) = w(n,:) + k'*output(n); %we are using the output as our error signal
   rinv = lambda*rinv - lambda*k*frefblock'*rinv;
end;
work = flops-start;

w(length(w),:);

WErrorNorm = norm(wreal - w(length(w),:))



%*Plot of w vs. time
%my = [max(max(w)), min(min(w))];
%mx = [worder+1, worder+1];
%plot(mx,my,'g')
%figure;hold on
for ii = 1:worder;
   %rv = realw(ii)*ones(length(w));
   %rv = realw(:,ii);
   %plot(rv,'blue');
   %plot(w(:,ii),'r');
end;

figure;
subplot(3,1,1);
plot(primary);axis([0 length(primary) min(primary) max(primary)]);
title('primary microphone signal');
subplot(3,1,2);
plot(output);axis([0 length(primary) min(signal)-.1 max(signal)+.1]);
title('filtered output');
subplot(3,1,3);
plot((ref(1:length(refP))-refP).^2);%axis([0 length(primary) min(primary)-.1 max(primary)+.1]);
title('Mean Squared Error');

%We start calculating the noise at 4 since the early values of the output
%can be VERY large, and bias our SNR measurement.  


sv = 2*worder;
sw = length(signal);

SNRpre = norm(signal(sv:sw))/norm(ref(sv:sw))
%SNRpre = 10*log10(SNRpre)

SNRpost = norm(signal(sv:sw))/norm(signal(sv:sw) - output(sv:sw))
%SNRpost = 10*log10(SNRpost)


%wavwrite(signal,10000,8,'signal.wav');
%wavwrite(primary,10000,8,'primary.wav');
%wavwrite(fref,10000,8,'reference.wav');
%wavwrite(output,10000,8,'output.wav');
%wavwrite(ref,10000,8,'reference.wav');