%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%design by Hangzhou Dianzi University
%AR model parameter estimate 
clear
m=sqrt(-1);
delta=0.101043;
a1=-0.850848;
sample=32;      %number of sample spot
p=10;           %number of sample spot in coef method;
f1=0.05; f2=0.40; f3=0.42;
fstep=0.01;
fstart=-0.5;
fend=0.5;  
f=fstart:fstep:fend;
nfft=(fend-fstart)/fstep+1;

%un=urn+juin
urn= normrnd(0,delta/2,1,sample);
uin= normrnd(0,delta/2,1,sample);
un=urn+m*uin;

%計算 zn
zn(1)=un(1);
for n=1:sample-1
    zn(n+1)=-a1*zn(n)+un(n+1);
end
    
%計算 xn
    for n=1:sample
        xn(n)=2*cos(2*pi*f1*(n-1))+2*cos(2*pi*f2*(n-1))+2*cos(2*pi*f3*(n-1))+sqrt(2)*real(zn(n));
    end
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x=xn;
for k=0:1:sample-1
    s=0;
    for n=1:sample-k,
       s=s+conj(x(1,n))*x(1,n+k); %calculate the value of rxx   
    end
   rxx(1,k+1)=(1/sample)*s;
end
Rx=zeros(sample,sample);
Rx=toeplitz(rxx(1,1:32));
[U,S,V]=svd(Rx);  
Pmusicf=zeros(1,1/fstep+1);
ei=zeros(1,sample);
for i=1:length(f)
    for j=1:sample
        ei(1,j)=exp(-2*pi*(j-1)*f(i)*m);
    end;
    sum=0;
    for k=7:sample
        sum=sum+abs(ei*V(:,k))^2;
    end 
    Pmusicf(1,i)=10*log10(1/sum);
end
figure
plot(f,Pmusicf);
title('人工計算music算法');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
pp=6;
x=xn';
M=length(x);
rxx=xcorr(x,'biased');
rxx=[rxx(M:end);0];
R=toeplitz(rxx,rxx);
Rxx=R(1:M,1:M);
Rxy=R(1:M,2:end);
[U,S,V]=svd(Rxx);
U1=U(:,1:pp);
S1=S(1:pp,1:pp);
V1=V';
V1=V1(1:pp,:);
A=U1.'*Rxy*V1.';
d=eig(S1,A);
y=angle(d)/2/pi;
figure
stem(y,ones(1,length(y)));
title('人工計算esprit算法');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
f=-0.5:0.001:0.5;
j=sqrt(-1);
 for n=1:1001
     ff(n)=0;
     for k=1:length(xn)
        ff(n)=xn(k)*exp(-j*2*pi*n*k/1001)+ ff(n);
    end
     ff(n)=abs( ff(n)).^2/(length(xn));
    end
  ff=10*log10(ff);
figure
plot(f,ff)