clear?all;
close?all;
clc;
x1=0.05:0.05:10;
dt=0.05;%抽樣時(shí)間
fs=1/dt;%抽樣頻率

wn=randn（1200）;
%?x=（10*x1）.*cos（2*pi*5*x1+pi/2）+（25*x1）.*cos（2*pi*2.5*x1+pi/2）+wn;
x=（5*x1）.*cos（2*pi*4*x1+pi/2）+（10*x1）.*cos（2*pi*4.5*x1+pi/2）+wn;

N=length（x）-20;%N為了滿(mǎn)足后面r（ij）的計(jì)算，所以減少N的取值
figure（1）;
plot（x）;
grid?on;
pe=6;%生設(shè)置階數(shù)為6，記住pe和p不同，p是要算的，pe是先給出的
%書(shū)上公式有問(wèn)題，加法x維度超出，減法維度剛好不超出，所以只能將加法的維度拉回，減法的維度不管
for?i=2:pe+1
????for?j=1:pe+1
????????for?k=pe:N-1
????????????r1（ij）=?[x（k+j-2）+x（k-j+2）]*[conj（x（k+i-2））+conj（x（k-i+2））];
????????end
????end
end

Re=r1（2:pe+1:）;%構(gòu)建Re矩陣

%求解有效秩p
[U?S?V]=svd（Re）;%奇異值分解
for?k=1:pe
????b=S（kk）/S（11）;
????if（b<0.05）
????????break
????end
end?
p=k;

sp=zeros（p+1p+1）;%初始化sp矩陣p+1維
for?j=1:p
????for?i=1:（pe+1-p）
????????sp=（S（jj）.^2）*V（i:i+pj）*V（i:i+pj）‘+sp;%V（1:71）是第1行到第7行的第一列的數(shù)據(jù)，窗是6行1列???
????end
end

inv_sp=inv（sp）;?????%求矩陣sp的逆??
%根據(jù)（4.4.58），利用整體最小二乘法求出待求參數(shù)
for?i=1:p+1???????
????a_compute（1i）=inv_sp（i1）/inv_sp（11）;????%