clear
Fs=2000;
t=0:1/Fs:1-1/Fs;
xn=2*sin(2*pi*50*t)+sin(2*pi*500*t)+randn(size(t));
%%%%%%%%%%%%%%%%%%%%%%%%%%%
N=Fs;
nfft=2048;
KK=4;
%%K=1%%%
y(1,:)=xn;
yyk=fftshift(fft(y(1,:),nfft));
[ywk(1),m]=max(abs(yyk));
w(1)=-pi+(m-1)*2*pi/nfft;
b(1)=y(1,:)*(exp(-j*w(1)*(0:N-1)'))/N;
b1=b;w1=w;
%%%%%%%%%%%%%%%%%%
for ii=1:N
   sum(ii)=b(1)*exp(-j*w(1)*(ii-1));
end
c=1;
g(2)=(y(1,:)-sum)*(y(1,:)-sum)';
g(1)=0;
c=2;
%%K>=2 時%%
for K=2:KK
   cast=10;
   c=K-1;
while (cast>=0.0001)
k=mod(c,K)+1;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for ii=1:N
   sum(ii)=0;
   for i=1:K-1
      sum(ii)=sum(ii)+b1(i)*exp(j*w1(i)*(ii-1));
   end
end
y(k,:)=xn-sum;%%求yk
yyk=fftshift(fft(y(k,:),nfft));
yyk2=(abs(yyk))/N;
[ywk(K),m]=max(yyk2);
w(k)=-pi+(m-1)*2*pi/nfft;
b(k)=y(k,:)*(exp(-j*w(k)*(0:N-1)'))/N;
f_b=b;
f_w=w;
if k==K 
   w1=f_w(2:K);
   b1=f_b(2:K);
end
if k==K-1
   w1=f_w(1:K-1);
   b1=f_b(1:K-1);
end
if (k~=K-1)&(k~=K)
   w1=[f_w(1:k) f_w(k+2:K)];
   b1=[f_b(1:k) f_b(k+2:K)];
end
%%%%%%%%%%%%%%%%%%%%%%%%%%
for ii=1:N
   be(ii)=0;
   for i=1:K
      be(ii)=b(i)*exp(j*w(i)*ii)+be(ii);
   end
   end
   g(c+1)=(xn-be)*(xn-be)';
   cast=abs(g(c+1)-g(c));
   c=c+1;
end
w1=w;
b1=b;
end
fr=w/2/pi*N;
ample=2*abs(b);
freq=fr;
%%plot%%
%f=N*(-(2*N)/2:(2*N)/2-1)/(2*N);
subplot(2,1,1);plot(xn);
title('signal picture');
xlabel('t');ylabel('xn');
subplot(2,1,2);stem(freq,ample);
title('relax method picture');
xlabel('f(hz)');
ylabel('ample');