clc
clear
%信号频率
f1 = 0.2;
f2 = 0.3;
f3 = 0.1;
T = 1000;
Ns = 6;
N = 15;
beta = 0.99;
A1 = sqrt(10^1.5);
A2 = A1;
A3 = sqrt(10);
f = 0.001;
wr = 0:f:2*pi;
[m n] = size(wr);
w = i*wr;
a = ones(N,n);
for i = 1:n
a(:,i)=exp(-w(i)*(0:N-1));
end
Pw = zeros(1,n);
% P = eye(Ns,Ns);
% W = [eye(Ns);zeros(N-Ns,Ns)];
P = rand(Ns,Ns);
W = rand(N,Ns);
phi1 = 2*pi*rand(1);
phi2 = 2*pi*rand(1);
phi3 = 2*pi*rand(1);
v = randn(T,1);
s = zeros(T,1);
freq1 = zeros(T-N+1,1);
freq2 = zeros(T-N+1,1);
freq3 = zeros(T-N+1,1);
for t = 1:T
s(t) = sqrt(2)*A1*sin(2*pi*f1*(t-1)+phi1)...
+sqrt(2)*A2*sin(2*pi*f2*(t-1)+phi2)...
+sqrt(2)*A3*sin(2*pi*f3*(t-1)+phi3) + v(t);
end
figure
for t = 1:T-N+1
y = W'*s(t:t+N-1);
h = P*y;
g = h/(beta+y'*h);
tmpu = triu(P-g*h');
tmpd = diag(P-g*h');
P = (tmpu+tmpu.'-diag(tmpd))/beta;
e = s(t:t+N-1)-W*y;
W = W+e*g';
% % syms z
% % i= 0:N-1;
% % pz = z.^i;
% % pt = z.^(-i);
% % %pf = (z^(N-1))*pt*W*W'*pz.';
% % pf = (z^(N-1))*pt*(eye(N)-W*W')*(pz.');
% % %pp = sym2poly(pf);
% % %result1 = roots(pp);
% % result = solve(pf,z);
% % result = double(vpa(result,32));
% % [yy ins]=sort(abs(result)-1,'descend');
% % ff = atan(imag(result(ins))./real(result(ins)))/(2*pi);
% % freq2(t) = ff(1);
% % freq1(t) = ff(2);
% % freq3(t) = ff(3);
freq = rootmusic(W*W',6);
[Y Index] = sort(freq,1,'descend');
freq1(t) = Y(1)/(2*pi);
freq2(t) = Y(2)/(2*pi);
freq3(t) = Y(3)/(2*pi);
end
% syms z
% i= 0:N-1;
% pz = z.^i;
% pt = z.^(-i);
% %pf = (z^(N-1))*pt*W*W'*pz.';
% pf = (z^(N-1))*pt*(eye(N)-W*W')*(pz.');
% %pp = sym2poly(pf);
% %result1 = roots(pp);
% result = solve(pf,z);
% result = double(vpa(result,32));
% [yy ins]=sort(abs(result)-1,'descend');
% ff = atan(imag(result(ins))./real(result(ins)))/(2*pi);
% freq2(t) = ff(1);
% freq1(t) = ff(2);
% freq3(t) = ff(3);
nn = n/2;
for i = 1:n
Pw(i) = 1/(a(:,i)'*(eye(N)-W*W')*a(:,i));
end
plot(wr(1:nn)./(2*pi),abs(Pw(1:nn)),'r-','linewidth',2);
hold on
plot([f1 f2 f3],[0 0 0],'bo')
axis('tight');
grid on
xlabel('Frequence t')
ylabel('Response')
figure
plot(1:T-N+1,freq1,'r-','linewidth',2);
hold on
plot(1:T-N+1,freq2,'b-','linewidth',2);
plot(1:T-N+1,freq3,'k-','linewidth',2);
axis('tight')
grid on
xlabel('Time')
ylabel('Estimated Frequence')
err1 = sum((freq3 - f3).^2,1)/(T-N+1);
err2 = sum((freq2 - f1).^2,1)/(T-N+1);
err3 = sum((freq1 - f2).^2,1)/(T-N+1);
