%%%%%%%%%%%%%%%%% 維納濾波器 %%%%%%%%%%%%%%%%%%%%%%
clear all;
close all;

%%%%%%%%%%%%%%%%%%% 設置各變量初始值 %%%%%%%%%%%%%%%%%%
L=500;
N=10;
a=0.95;
row1=L-99:L;
row2=1:N;
b=sqrt((1-a^2)*3);
c=sqrt(3);
%%%%%%%%%%%%%%%%%%%%%% 產生S,X %%%%%%%%%%%%%%%%%%%%%%%%
W=unifrnd(-b,b,1,L);
S=zeros(1,L);
S(1,1)=W(1,1);
for i=2:L
    S(1,i)=a*S(1,i-1)+W(1,i);
end
clear i;

V=unifrnd(-c,c,1,L);
X=zeros(1,L);
for i=1:L
    X(1,i)=S(1,i)+V(1,i);
end
clear i;

for i=1:100
    S1(1,i)=S(1,L-100+i);
end
clear i;
for i=1:100
    X1(1,i)=X(1,L-100+i);
end
clear i;
%%%%%%%%%%%%%%%% 估計X自相關矩陣 %%%%%%%%%%%%%%%%%%%%%%%
corXX1=zeros(1,N);
for i=0:N-1
    for j=1:L-i
        corXX1(1,i+1)=X(1,j)*X(1,j+i)+corXX1(1,i+1);
    end
    corXX1(1,i+1)=corXX1(1,i+1)/(L-i);
end
clear i;
clear j;
corXX=zeros(N,N);
for i=1:N
    for j=1:N
        corXX(i,j)=corXX1(1,abs(i-j)+1);
    end
end
    clear i;
    clear j;
%%%%%%%%%%%%%%%%%%%% 估計XS相關向量 %%%%%%%%%%%%%%%%%%%%
corXS=zeros(1,N);
for i=0:N-1
    for j=1:L-i
        corXS(1,i+1)=X(1,j)*S(1,j+i)+corXS(1,i+1);
    end
    corXS(1,i+1)=corXS(1,i+1)/(L-i);
end
clear i;
clear j;
corXS=corXS';
%%%%%%%%%%%%%%%% 計算兩類h(n) 并比較差異%%%%%%%%%%%%%%%%
h1=inv(corXX)*corXS;
h2=zeros(1,N);
h2(1,1)=0.238;
for i=2:N
     h2(1,i)=h2(1,i-1)*0.724;
end
clear i;
Eh=0;
Eh2=0;
for i=1:N
    Eh=h1(i,1)-h2(1,i)+Eh;
    Eh2=(h1(i,1)-h2(1,i))^2+Eh2;
end
Eh
Eh2
%%%%%%%%%%%%%%%%%% 計算X理想濾波情況 %%%%%%%%%%%%%%%%%%%%
Sl=zeros(1,L);
Sl(1,1)=X(1,1);
for i=2:L
    Sl(1,i)=0.724*Sl(1,i-1)+0.238*X(1,i);
end
%%%%%%%%%%%%%%%%% 計算X的FIR濾波情況 %%%%%%%%%%%%%%%%%%%%%
Sr=zeros(1,L);
h1=h1';
for i=1:L
    Srr=0;
    for j=1:N
       if (i-j<=0)
           break;
       else
           Srr=h1(1,j)*X(1,i-j)+Srr;
       end
       Sr(1,i)=Srr;
   end
end
   clear i;
   clear j;
%%%%%%%%%%%%%%%%%%%% 計算Ex,El,Er %%%%%%%%%%%%%%%%%%%%%%%%%
 Ex=0;
 El=0;
 Er=0;
 for i=1:L
     Ex=(X(1,i)-S(1,i))^2+Ex;
     El=(Sl(1,i)-S(1,i))^2+El;
     Er=(Sr(1,i)-S(1,i))^2+Er;
 end
 Ex=Ex/L
 El=El/L
 Er=Er/L
%%%%%%%%%%%%%%%%%%%%%%%%% 繪圖 %%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure;
plot(row1,S1,'b',row1,X1,'r');
legend('隨機信號S','加噪后X',1);

figure;
plot(row2,h1,'*b',row2,h2,'or');
legend('估算的h(n)','理想的h(n)',1);

figure;
for i=1:100
    Sl1(1,i)=Sl(1,L-100+i);
end
clear i;
plot(row1,Sl1,'b',row1,S1,'r');
legend('理想h(n)濾波后S','原信號S',1);

figure;
for i=1:100
    Sr1(1,i)=Sr(1,L-i+1);
end
clear i;
plot(row1,Sr1,'b',row1,S1,'r');
legend('估算h(n)濾波后S','原信號S',1);