%---------------------------SER算法-------------------------%
%-----模型：x(k)+a1*x(k-1)+a2*x(k-2)=w(k)-------------------%
%-----為自適應一步預測器，其中w(k)為方差0.01的高斯白噪聲-------%
close all;clc;clear all;
N=500;
% 第一組參數
a1=-1.5955;
a2=0.95;
% 第二組參數
% a1=-0.1950;  
% a2=0.95;
a=2.^(-1/N);
u=0.01;%u*Rav
x=zeros(1,N);
w=1*randn(1,N);
x(1)=w(1);
x(2)=w(2)-a1*x(1);
Q_inv0=10*[1,0;0,1];%Q初值
wt=zeros(2,N);
S=zeros(2,N);
r=zeros(1,N);
e=zeros(1,N);
R0=[0,0;0,0];
wt1=zeros(2,N);
e1=zeros(1,N);
u1=0.001;
dxx=0;
pxx=0;
wt(:,3)=[3,-3];
e2=zeros(1,N);
wt1(:,3)=[3,-3];
for n=3:N
    x(n)=w(n)-a1*x(n-1)-a2*x(n-2);
    xn=[x(n-1);x(n-2)];
    R1=xn*xn';
    R0=R1+R0;
    dx=x(n)*x(n);
    dxx=dx+dxx;
    px=x(n)*[x(n-1);x(n-2)];
    pxx=px+pxx;    
    e1(:,n)=x(:,n)-xn'*wt1(:,n);
    wt1(:,n+1)=wt1(:,n)+2*u1*e1(:,n)*xn;
    ee2(:,n)=e1(:,n).^2;
end
S(:,2)=Q_inv0*[0;x(1)];
for k=3:N
    S(:,k)=Q_inv0*[x(k-1);x(k-2)];
    r(k)=a+[x(k-1),x(k-2)]*S(:,k);
    Q_inv1=(Q_inv0-(1/r(k))*S(:,k)*S(:,k)')/a;
    e(:,k)=x(k)-[x(k-1),x(k-2)]*wt(:,k);%????
    e2(:,k)=e(:,k).^2;
    wt(:,k+1)=wt(:,k)+2*u*(1-a.^(k+1))*e(:,k).*Q_inv1*...
        [x(k-1);x(k-2)]/(1-a);
    Q_inv0=Q_inv1;
end
R0=R0/(N-1);
dxx=dxx/(N-1);
pxx=pxx/(N-1);
wopt=inv(R0)*pxx
wtt=-wt';
L=length(wtt);
woptL=zeros(N,2);
for i=1:L
    woptL(i,:)=wopt;
end
% figure(1);plot(wtt);
% hold on,plot(-woptL,'r-.');
[w1,w2]=meshgrid(wopt(1)-2:.1:wopt(1)+2,wopt(2)-2:.1:wopt(2)+2);
err=dxx+w1.^2*R0(1,1)+w1.*w2.*(R0(1,2)+R0(2,1))+w2.^2*R0(2,2)-2*w1.*pxx(1)-2*w2.*pxx(2);
figure(2);
contour(w1,w2,err,40);
hold on
plot(wt1(1,3:N),wt1(2,3:N),'k',wt(1,3:N),wt(2,3:N),'r',wopt(1),wopt(2),'m-*');
title('權值收斂曲線');
legend('紅色為SER算法；黑色為LMS算法');

figure(3),subplot(2,1,1),plot(e2);title('LMS學習曲線');
subplot(2,1,2),plot(ee2);title('SER學習曲線');