clear all;
clc;
display('廣義最小二乘在線辨識 20081115');

y1=1;y2=0;y3=1;y4=0;
u=zeros(1,500);
for k=1:50
    if y4==0
        u(k)=1;
    else
        u(k)=-1;
    end
    x1=xor(y3,y4); %計算下一時刻的狀態(tài)，y是當前輸出
    x2=y1;         %計算下一時刻的狀態(tài)
    x3=y2;         %計算下一時刻的狀態(tài)
    x4=y3;         %計算下一時刻的狀態(tài) 
    y1=x1;         %計算下一時刻的當前輸出
    y2=x2;
    y3=x3;
    y4=x4;
end   
%生成M序列

v=0.03*randn(1,500);
%白噪聲序列

y=zeros(1,500);
y(1)=0;y(2)=0;
for k=3:500
y(k)=1.5*y(k-1)-0.7*y(k-2)+u(k-1)+0.5*u(k-2)+v(k);  %實際系統(tǒng)輸出,其中C(q-1)=1
end
%得到系統(tǒng)實際輸出序列（加了噪聲）

N=0.000000005;
p0=10^6*eye(4,4);  %計算模型theta的P矩陣初值
pe0=10^6*eye(2,2); %計算濾波器thetae的P矩陣初值
yf=zeros(1,500);
yf(1)=0;yf(2)=0;
uf=zeros(1,500);
uf(1)=0;uf(2)=0;
e=zeros(1,500);   %殘差序列
e(1)=0;e(2)=0;   
theta=zeros(4,500);  %模型參數(shù)每次辨識的結(jié)果
thetae=zeros(2,500); %濾波器參數(shù)每次辨識的結(jié)果
theta0=[0.001;0.001;0.001;0.001]; %系統(tǒng)參數(shù)初值
thetae0=[0.001;0.001]; %濾波器C(q-1)的參數(shù)初值
for k=3:500
    yf(k)=y(k)+thetae0(1)*y(k-1)+thetae0(2)*y(k-2);  %濾波后的輸入序列
    uf(k)=u(k)+thetae0(1)*u(k-1)+thetae0(2)*u(k-2);  %濾波后的輸出序列
    h1=[-yf(k-1);-yf(k-2);uf(k-1);uf(k-2)]; %濾波后的輸出輸出組成的辨識數(shù)據(jù)
    x=1+h1'*p0*h1;
    x1=inv(x);
    k1=p0*h1*x1;  %求當前時刻的K矩陣
    d1=yf(k)-h1'*theta0;
    theta1=theta0+k1*d1;
    theta(:,k)=theta1;
    e1=theta1-theta0;  
    e2=e1./theta0;   
    E(:,k)=e1;   %保存模型參數(shù)的誤差
    theta0=theta1; %為下時刻準備
    p1=p0-k1*k1'*[1+h1'*p0*h1]; %為下時刻準備
    p0=p1;  %為下時刻準備
    %%%遞推算法的實質(zhì)就是theta和P在原來的基礎上計算，因此要為下時刻的這兩個值做準備
    
    e(k)=y(k)-[-y(k-1),-y(k-2),u(k-1),u(k-2)]*theta1;  %計算殘差e(k),注意這里的h的組成
    
    he1=[-e(k-1);-e(k-2)];
    xe=1+he1'*pe0*he1;
    xe1=inv(xe);
    ke1=pe0*he1*xe1;  %求當前的K矩陣
    de1=e(k)-he1'*thetae0;
    thetae1=thetae0+ke1*de1;
    thetae(:,k)=thetae1;  %保存濾波器參數(shù)
    ee1=thetae1-thetae0;
    ee2=ee1./thetae0;      %模型參數(shù)的相對誤差
    Ee(:,k)=ee1;
    thetae0=thetae1;   %為下一時刻做準備
    pe1=pe0-ke1*ke1'*[he1'*pe0*he1+1]; %為下一時刻賦值
    pe0=pe1; %為下一時刻P矩陣賦值
    %%%遞推算法的實質(zhì)就是theta和P在原來的基礎上計算，因此要為下時刻的這兩個值做準備
end

display('->Done!');