%% ESPRIT方法估計散射中心參數
% close all
%返回參數分別是估計得到的散射中心位置，幅度和類型
%函數參數分別是待提取的目標名稱和姿態角，該算法暫時用于訓練仿真
function [r_final,A_final,alfa_final,var]=my_esprit(target_name,azi_extract,step_array)
load(target_name)
%MAP_k_estimate=zeros(1,901);  %用于存儲每個姿態角下定的階數
% azi_extract=1;                %姿態角
azi_location=floor(azi_extract*5);%得到姿態角對應的H矩陣中頻點的起始位置數
data=H(azi_location*101+1:(azi_location+1)*101,1);
N=size(data,1);
L=50;
X=zeros(N-L+1,L);
for ii=1:L
    X(:,ii)=data(ii:N-L+ii,1);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 進行模型定階

R=X*ctranspose(X)/L;
[Us,S,Vs]=svd(R);
labna=diag(S);
labna_num=length(labna);
% for k=1:size(labna,1);                 %此處將0奇異值拋棄
%     if abs(labna(k))>1e-11
%         K=k;
%     end
% end
% labna=labna(1:K);
% labna_num=size(labna,1);
% MAP=zeros(1,labna_num);
% for k=1:labna_num-1
%     var=1/(labna_num-k)*sum(labna(k+1:labna_num));
%     MAP(k)=N*log(var)+4*k*log(N);
% end
% [MAP_estimate,k_estimate]=min(MAP);
%MAP_k_estimate(floor(azi_location)+1)=k_estimate;%用于存儲各個角度下的MAP估計階數
%step_array=[4];%階數范圍
bia=min(step_array)-1;
rr=zeros(max(step_array)-bia,max(step_array));%存儲各階散射中心的矩陣
Alfa=zeros(max(step_array)-bia,max(step_array));%存儲各階散射中心類型
AA=zeros(max(step_array)-bia,max(step_array));%存儲各階散射中心幅度


for k_estimate=step_array
%% Esprit估計DE模型參數 極點z_estimate

X0=X(:,1:L-1);
X1=X(:,2:L);
[U0,D0,V0]=svd(X0);
[U1,D1,V1]=svd(X1);
U0=U0(:,1:k_estimate);    %用主奇異矢量構成的矩陣和主奇異值構成的矩陣來近似X0,X1
V0=V0(:,1:k_estimate);
D0=D0(1:k_estimate,1:k_estimate);
U1=U1(:,1:k_estimate);
V1=V1(:,1:k_estimate);
D1=D1(1:k_estimate,1:k_estimate);
X0_appro=U0*D0*ctranspose(V0);
X1_appro=U1*D1*ctranspose(V1);
[eig_vector,eig_value]=eig(pinv(X1_appro)*X0_appro);
z_estimate=diag(eig_value(1:k_estimate,1:k_estimate));
z_estimate=z_estimate';
%% 利用估計的極點及相應關系式估計散射中心位置和類型
p_estimate=abs(z_estimate);
omiga_estimate=angle(z_estimate);
fre_num=size(fre_array,2);
fre_min=min(fre_array);
fre_max=max(fre_array);
B=fre_max-fre_min;
deltaf=B/(fre_num-1);
c=3e8;
fre_mean=mean(fre_array);
r_estimate=-c/(4*pi*deltaf)*omiga_estimate;%散射中心位置
rr(k_estimate-bia,1:size(r_estimate,2))=r_estimate; %存到大矩陣中
alfa_estimate=fre_mean/deltaf*log(p_estimate);%散射中心類型
Alfa(k_estimate-bia,1:size(alfa_estimate,2))=alfa_estimate; %存到大矩陣中
alfa_norm=alfa_estimate/(max(abs(alfa_estimate)));%歸一化了的類型
%% 估計幅度
Z=zeros(N,k_estimate);
for i=1:N
    Z(i,:)=z_estimate.^(i-1);
end
a_estimate=pinv(Z)*data;%DE模型極點留數（系數）
for i=1:k_estimate
    A(i)=a_estimate(i)*exp(j*4*pi*fre_mean*r_estimate(i)/c);%散射中心幅度
end
AA(k_estimate-bia,1:size(A,2))=A;%幅度存于大矩陣中
% hold on
% plot(r_estimate,abs(A),'bo')
%% 對參數估計進行驗證
%方法是把估計得到的參數代回GTD模型 計算回波數據
%計算和原始數據的偏差
Egtd=zeros(N,1);
windage=zeros(N,1);
for n=1:N
    summ=0;
    for l=1:k_estimate
        summ=summ+a_estimate(l)*(1+n*deltaf/fre_mean)^alfa_estimate(l)*exp(j*n*omiga_estimate(l));
    end
    Egtd(n)=summ;   %得到用GTD模型恢復的數據，和原始測量數據比較
    windage(n)=(abs(data(n)-summ))^2;%能量偏差
%     windage(n)=(abs(data(n)-summ))^2+3*k_estimate*log(N);
end
energy_lose=sum(windage,1);%總能量損失
E_lose(k_estimate-bia)=energy_lose;
end
[E_lose_min,k_location]=min(E_lose);
k_estimate=k_location+bia;
r_final=rr(k_location,:);
alfa_final=Alfa(k_location,:);
A_final=AA(k_location,:);

var=1/(labna_num-k_estimate)*sum(labna(k_estimate+1:labna_num));
% hold on
% figure(2)
% plot(windage,'r')
% figure(3)
% plot(step_array,E_lose,'g')
% figure(2)
% oo=abs(ifftshift(ifft(data,101)));
% plot([-50:50]*c/(2*B),oo)
%  hold on
% % % ee=abs(ifftshift(ifft(Egtd,101)));
% % % plot([-50:50]*c/(2*B),ee,'g')
% stem(r_final,abs(A_final),'r')