%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%               自適應控制理論與應用                   %%%
%%%          第四章多步預測自校正控制仿真程序            %%%
%%%                張章    212007081101007               %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear%清理工作間變量

yr=[ones(1,50),zeros(1,50),ones(1,150),zeros(1,50),ones(1,50),zeros(1,54)];

y(2)=0;y(1)=0;
u(1)=0;u(2)=0;
Yr=[0 0 0]';Y=[0 0 0]';dU=[0 0 0]';
du(1)=0; du(2)=0; 

%RLS遞推最小二乘辨識
c0=[0.1 0.1 0.001 0.0015]';%直接給出被辨識參數的初始值,即一個充分小的實向量
Q=eye(3,3);R=5;I=eye(4,4);
p0=10^10*eye(4,4);%直接給出初始狀態P0，即一個充分大的實數單位矩陣
c=[c0,zeros(4,400)];%被辨識參數矩陣的初始值及大小
v=randn(400,1);
for k=3:400; %開始求K 
    y(k)=0.8*y(k-1)+y(k-2)+u(k-1)+1.5*u(k-2)+0.02*v(k);
    h1=[-y(k-1),-y(k-2),u(k-1),u(k-2)]'; x=h1'*p0*h1+0.95; x1=1/x; %開始求K(k)
    k1=p0*h1*x1;%求出K的值
    d1=y(k)-h1'*c0; c1=c0+k1*d1;%求被辨識參數c   
    c0=c1;%新獲得的參數作為下一次遞推的舊參數
    c(:,k)=c1;%把辨識參數c 列向量加入辨識參數矩陣的最后一列 
    p1=((I-(k1*h1'))*p0)./0.95;
    p0=p1;%給下次用
    
    a1=c(1,k); a2=c(2,k); b1=c(3,k); b2=c(4,k);  
    A1=1-a1;A2=a1-a2;A3=a2;B1=b1;B2=b2-b1;B3=-b2;
    E1=[1 0 0];
    g10=A1;g11=A2;g12=A3;
    G1=[g10 g11 g12];
   % e21=g10;
   % E2=[e21 1 0];
   % g20=g11+g10*A1;g21=g12+g10*A2;g22=g10*A3;
   % G2=[g20 g21 g22];
   % e32=g20;e31=e21;
   % E3=[e32 e21 1];                                                                                                                                    
   % g30=g21+g20*A1;g31=g22+g20*A2;g32=g20*A3;
   % G3=[g30 g31 g32];
    l10=b1;
    L1=l10;
    h10=b2;                                                                   
    H1=h10 ;
   % l21=e21*b1+h10;
   % L2=l21;
   % h20=e21*b2;
   % H2=h20;
   % l32=e32*b1+h20;
   % L3=l32;                                                                                                                                                                             
   % h30=e32*b2;
   % H3=h30;
    L=[L1]';H=[H1]';G=[G1']';
    L1=[L1];
    Lm=inv(L1*Q*L+5);
    Lq=Lm*L1*Q;
    lq=Lq(1,:);
    
    Yr=[yr(k+1) yr(k+2) yr(k+3)]';  dU=du(k-1); Y=[y(k) y(k-1) y(k-2)]';
    u(k)=u(k-1)+lq*[Yr-H*dU-G*Y];
    du(k)=u(k)-u(k-1);
    
    AA1(k)=a1;AA2(k)=a2;BB1(k)=b1;BB2(k)=b2;
    
 end%大循環結束
 
h1=figure;
subplot(2,1,1)
plot(y,'-');
hold on
% k=1:1:404;
plot(k,yr,':');
subplot(2,1,2)
plot(u,'-');
hold off

figure(2);%第2個圖形
i=1:400;%橫坐標從1到15
plot(i,AA1,'r',i,AA2,':',i,BB1,'g',i,BB2,':') %畫出a1，a2，b1，b2的各次辨識結果
title('Parameter Identification with Recursive Least Squares Method')%圖形標題