%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%               自適應控制理論與應用                   %%%
%%%          第四章多步預測自校正控制仿真程序            %%%
%%%                張章    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，即一個充分大的實數單位矩陣
c1=[c0,zeros(4,400)];%被辨識參數矩陣的初始值及大小
v=randn(400,1);

for k=3:129; %開始求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 L2 L3]';H=[H1 H2 H3]';G=[G1' G2' G3']';
    L1=[L1 L2 L3];
    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
    
for k=130:400; %開始求K 
    y(k)=1.0*y(k-1)+1.5*y(k-2)+0.5*u(k-1)+1.9*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 L2 L3]';H=[H1 H2 H3]';G=[G1' G2' G3']';
    L1=[L1 L2 L3];
    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')%圖形標題