%SI3.1_RLS.m
clear all
close all
clc
miu=0.95
bata=0.95
%產生N(0,1)正態分布的隨機噪聲
randn('seed',100);
v=randn(1,60); 

%產生M序列
L=60;% M序列的周期
y1=1;y2=1;y3=1;y4=0;%四個移位積存器的輸出初始值
for i=1:L;
x1=xor(y3,y4);
x2=y1;
    x3=y2;
    x4=y3;
    y(i)=y4;
    if y(i)>0.5,u(i)=-5;%M序列的幅值為5
    else u(i)=5;
    end
    y1=x1;y2=x2;y3=x3;y4=x4;
end 
%figure(1);
%stem(u),grid on
% 遞推最小二乘辨識程序
z(2)=0;z(1)=0;
%觀測值由理想輸出值加噪聲
for k=3:60;%循環變量從3到15   
    z(k)=1.6*z(k-1)-0.7*z(k-2)+u(k-1)+5.5*u(k-2)+v(k); 
end
%RLS遞推最小二乘辨識
c0=[0.001 0.001 0.001 0.001]';
c1=[0.001 0.001 0.001 0.001]';

p0=10^3*eye(4,4);

E=0.0000000000005;%相對誤差
c=[c0,c1,zeros(4,58)];%被辨識參數矩陣的初始值及大小
e=zeros(4,60);%相對誤差的初始值及大小
lamt=1;
for k=3:60; 
    h1=[-z(k-1),-z(k-2),u(k-1),u(k-2)]';
    p1=inv(miu*eye(4)-bata*miu*eye(4)+bata*inv(p0)+h1*h1');
    
    c2=c1+bata*miu*p1*(c1-c0)+p1*h1*(z(k)-h1'*c1);
     
    %k1=p0*h1*inv(h1'*p0*h1+1*lamt);%求出K的值
    %new=z(k)-h1'*c0; 
    %c1=c0+k1*new;%求被辨識參數c
    %p1=1/lamt*(eye(4)-k1*h1')*p0;
    e1=(c2-c1)./c1;%求參數當前值與上一次的值的差值
    e(:,k)=e1; %把當前相對變化的列向量加入誤差矩陣的最后一列    
    c(:,k)=c2;%把辨識參數c 列向量加入辨識參數矩陣的最后一列 
    c0=c1;%新獲得的參數作為下一次遞推的舊參數
    c1=c2;
    p0=p1;
    if norm(e1)<=E 
        break;%若參數收斂滿足要求，終止計算
    end
end
%分離參數
a1=c(1,:);
aa1=c(1,60)
a2=c(2,:);
aa2=c(2,60)
b1=c(3,:);
bb1=c(3,60)
b2=c(4,:);
bb2=c(4,60)
ea1=e(1,:); ea2=e(2,:); eb1=e(3,:); eb2=e(4,:); 
figure(2);
i=1:60;
plot(i,a1,'k',i,a2,'b',i,b1,'r',i,b2,'g') %畫出辨識結果
grid on
legend('a1','a2','b1','b2');
title('遞推阻尼最小二乘參數辨識')
figure(3); 
i=1:60; 
plot(i,ea1,'k',i,ea2,'b',i,eb1,'r',i,eb2,'g') %畫出辨識結果的收斂情況
grid on
legend('a1','a2','b1','b2');
title('辨識精度')