%%  Name      :  guoguo
%%  Time      :  2004.4.6
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%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;

%calculate zn
for n=1:sample-1
    zn(1)=un(1);
    zn(n+1)=-a1*zn(n)+un(n+1);
end
    
%calculate 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
    
%calculate rxx
for k=0:p
    s=0;
    for n=1:sample-k
        s=s+1/sample*(conj(xn(n))*xn(n+k));
    end
    rxx(k+1)=s;
end
rx=toeplitz(rxx(1:p));
b=reshape(rxx,p+1,1);
a=-inv(rx)*b(2:p+1);

%calculate psd of xn
for i=1:length(f)
   sum=0;
   for k=1:p
       sum=sum+a(k)*exp(-m*2*pi*f(i)*k);
   end
  Pacoef(i)=delta/(abs(1+sum))^2;
end
figure
semilogy(f,Pacoef);
title('人工計算自相關');



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%calculate cxx(j,k)
 for j=1:p
     for k=1:p
         s=0;
         for n=p+1:sample
           s=s+1/(sample-p)*conj(xn(n-j))*xn(n-k);
         end
       cxx(j,k)=s;
   end
end

%calculate cxx0(j,1)
for j=1:p
  s=0;
  for n=p+1:sample
      s=s+1/(sample-p)*conj(xn(n-j))*xn(n);
  end
cxx0(j,1)=s; 
end

%calculate a
a=-inv(cxx)*cxx0;

for i=1:length(f)
   sum=0;
   for k=1:p
       sum=sum+a(k)*exp(-m*2*pi*f(i)*k);
   end
  Pxfc(i)=delta/(abs(1+sum))^2;
end
figure
semilogy(f,Pxfc);
title('人工計算協方差');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%calculate cxx(j,k)
for j=1:p
     for k=1:p
         s=0;
         for n=p+1:sample
            s=s+1/2/(sample-p)*(conj(xn(n-j))*xn(n-k)+xn(n-p+j)*conj(xn(n-p+k)));
         end
       cxx(j,k)=s;
   end
end

%calculate cxx0(j,1)
for j=1:p
  s=0;
  for n=p+1:sample
      s=s+1/2/(sample-p)*(conj(xn(n-j))*xn(n)+xn(n-p+j)*conj(xn(n-p)));
  end
cxx0(j,1)=s; 
end
%calculate a
a=-inv(cxx)*cxx0;

for i=1:length(f)
   sum=0;
   for k=1:p
       sum=sum+a(k)*exp(-m*2*pi*f(i)*k);
   end
  Pxzxfc(i)=delta/(abs(1+sum))^2;
end
figure
semilogy(f,Pxzxfc);
title('人工計算修正協方差');


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
xn= xn(:);
N=length(xn);
% Initialization
ef = xn;
eb = xn;
a = 1;

for l=1:p
   % Calculate the next order reflection  coefficient
   efp = ef(2:end);
   ebp = eb(1:end-1);
 
   num = -2.*ebp'*efp;
   den = efp'*efp+ebp'*ebp;
   
   k(l) = num ./ den;
   
   % Update the forward and backward prediction errors
   ef = efp + k(l)*ebp;
   eb = ebp + k(l)'*efp;
   
   % Update the AR coeff.
   a=[a;0] + k(l)*[0;conj(flipud(a))];
end

a = a(:).'; 
a1=a(2:p+1);
a2=a1';

for i=1:length(f)
   sum=0;
   for k=1:p
       sum=sum+a2(k)*exp(-m*2*pi*f(i)*k);
   end
  Pbrg(i)=delta/(abs(1+sum))^2;
end
figure
semilogy(f,Pbrg);
title('人工計算伯格');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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算法');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%