%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%實驗三：維納濾波用于噪聲抑制
%% 作者： 李從娟 070628
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all;
N = 500;             % 數據點數
p=[3,6,9,12];      %階數
a=[0.1,0.5,1.0];  %泄露系數
n = 1:N;
n = n';
d = sin(0.05*pi*n-0.5*pi);%產生信號d(n)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%% 產生兩路觀測信號
g = randn(N,1);
v1 = filter(1,[1,-0.8],g);
v2 = filter(1,[1,0.6],g);
X = d + v1;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%% 畫d(n),x(n)和v2(n)的波形圖
figure(1);
subplot(2,1,1);
n = 1:500;
plot(n,d,'r-',n,X,'k--');
title('d(n) and x(n) Sample Sequence ');
legend('x(n)','d(n)');
grid on;
subplot(2,1,2);
plot(n,v2,'r--');
xlabel('n');
ylabel('v2(n)');
title('v2(n) Sample Sequence');
grid on;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%% 3階的維納霍夫預測

rv2=covar(v2, p(1));                %自相關,階數為3
rxv2=convm(X, p(1))'*convm(v2, p(1))/(length(X)-1); %互相關
w1=rxv2(1,:)/rv2;                   %濾波器系數
v1_estm1 = filter(w1,1,v2);
error1 = 0;
for k = 1:500
    error1 = error1 + (v1(k)-v1_estm1(k))^2;
end
error1 = error1/500;
    d21 = X - v1_estm1;
figure(2);
n = 1:200;
plot(n,d21(1:200),'k-',n,d(1:200),'r--');
title('p=3,Estimated d(n)');
legend('Estimated ','Actual')
grid on;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%% 6階的維納霍夫預測
rv22=covar(v2, p(2));                %自相關,階數為6
rxv22=convm(X, p(2))'*convm(v2, p(2))/(length(X)-1); %互相關
w2=rxv22(1,:)/rv22;                   %濾波器系數
v1_estm2 = filter(w2,1,v2);
error2 = 0;
for k = 1:500
    error2 = error2 + (v1(k)-v1_estm2(k))^2;
end
error2 = error2/500;
    d22 = X - v1_estm2;
figure(3);
n = 1:200;
plot(n,d22(1:200),'k-',n,d(1:200),'r--');
title('p=6,Estimated d(n)');
legend('Estimated ','Actual')
grid on;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%% 9階的維納霍夫預測
rv23=covar(v2, p(3));                %自相關,階數為9
rxv23=convm(X, p(3))'*convm(v2, p(3))/(length(X)-1); %互相關
w3=rxv23(1,:)/rv23;                   %濾波器系數
v1_estm3 = filter(w3,1,v2);
error3= 0;
for k = 1:500
    error3 = error3 + (v1(k)-v1_estm3(k))^2;
end
error3 = error3/500;
    d23 = X - v1_estm3;
figure(4);
n = 1:200;
plot(n,d23(1:200),'k-',n,d(1:200),'r--');
title('p=9,Estimated d(n)');
legend('Estimated ','Actual')
grid on;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%% 12階的維納霍夫預測
rv24=covar(v2, p(4));                %自相關,階數為12
rxv24=convm(X, p(4))'*convm(v2, p(4))/(length(X)-1); %互相關
w4=rxv24(1,:)/rv24;                   %濾波器參數
v1_estm4 = filter(w4,1,v2);
error4 = 0;
for k = 1:500
    error4 = error4 + (v1(k)-v1_estm4(k))^2;
end
error4 = error4/500;
    d24 = X - v1_estm4;
figure(5);
n = 1:200;
plot(n,d24(1:200),'k-',n,d(1:200),'r--');
title('p=12,Estimated d(n)');
legend('Estimated ','Actual')
grid on;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%% P=12,a=0.1時的維納霍夫預測              
v25 = v2 + a(1)*d;
rv25=covar(v25, p(4));                %自相關,階數為12
rxv25=convm(X, p(4))'*convm(v25, p(4))/(length(X)-1); %互相關
w5=rxv25(1,:)/rv25;                   %濾波器參數
v1_estm5 = filter(w5,1,v25);
error5 = 0;
for k = 1:500
    error5 = error5 + (v1(k)-v1_estm5(k))^2;
end
error5 = error5/500;
    d25 = X - v1_estm5;
figure(6);
n = 1:200;
plot(n,d25(1:200),'k-',n,d(1:200),'r--');
title('P=12,a=0.1,Estimated d(n)');
legend('Estimated ','Actual')
grid on;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%% P=12,a=0.5時的維納霍夫預測 
v26 = v2 + a(2)*d;
rv26=covar(v2, p(4));                %自相關,階數為12
rxv26=convm(X, p(4))'*convm(v26, p(4))/(length(X)-1); %互相關
w6=rxv26(1,:)/rv26;                   %濾波器參數
v1_estm6 = filter(w6,1,v26);
error6 = 0;
for k = 1:500
    error6 = error6 + (v1(k)-v1_estm6(k))^2;
end
error6 = error6/500;
    d26 = X - v1_estm6;
figure(7);
n = 1:200;
plot(n,d26(1:200),'k-',n,d(1:200),'r--');
title('P=12,a=0.5,Estimated d(n)');
legend('Estimated ','Actual');
grid on;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%% P=12,a=1.0時的維納霍夫預測 
v27 = v2 + a(3)*d;
rv27=covar(v27, p(4));                %自相關,階數為12
rxv27=convm(X, p(4))'*convm(v27, p(4))/(length(X)-1); %互相關
w7=rxv27(1,:)/rv27;                   %濾波器參數
v1_estm7 = filter(w7,1,v27);
error7 = 0;
for k = 1:500
    error7 = error7 + (v1(k)-v1_estm7(k))^2;
end
error7 = error7/500;
    d27 = X - v1_estm7;
figure(8);
n = 1:200;
plot(n,d27(1:200),'k-',n,d(1:200),'r--');
title('P=12,a=1.0,Estimated d(n)');
legend('Estimated ','Actual')
grid on;