clear;close all;
%----------------設定計算步數和精度----------------%
step=1000;            %步數
err=0.00001;          %精度
eps1=0.001;

%----------------數據生成----------------%
k0=0:step+1;
%u=sin(2*pi.*k0/step*10);
u=rand(1,step+2)*2-1;
y=zeros(1,step+2);
y(1)=0;
y(2)=0;
for t=1:step
    y(t+2)=y(t+1)*y(t)*(y(t+1)-2.5)/(1+y(t+1)*y(t+1)+y(t)*y(t))+u(t+1);
end
%----------------繪制輸入及輸出信號波形----------------%
figure(1);
plot(y(3:step+2),'r');
hold;
plot(u(2:step+1),'g');%pause
legend('輸出y','輸入u');
%----------------初始化隸屬函數----------------%
c=[-4,-3,-2,-1,0,1];
U=zeros(6,step+2);
for t=1:step+2
    if(y(t)<=c(1))
        U(1,t)=1;
    elseif(y(t)<=c(2))
        U(1,t)=(c(2)-y(t))/(c(2)-c(1));U(2,t)=(y(t)-c(1))/(c(2)-c(1));
    elseif(y(t)<=c(3))
        U(2,t)=(c(3)-y(t))/(c(3)-c(2));U(3,t)=(y(t)-c(2))/(c(3)-c(2));
    elseif(y(t)<=c(4))
        U(3,t)=(c(4)-y(t))/(c(4)-c(3));U(4,t)=(y(t)-c(3))/(c(4)-c(3));
    elseif(y(t)<=c(5))
        U(4,t)=(c(5)-y(t))/(c(5)-c(4));U(5,t)=(y(t)-c(4))/(c(5)-c(4));
    elseif(y(t)<=c(6))
        U(5,t)=(c(6)-y(t))/(c(6)-c(5));U(6,t)=(y(t)-c(5))/(c(6)-c(5));
    else
        U(6,t)=1;
    end
end

%----------------模糊聚類----------------%
U1=U.';
U=zeros(step+2,6);
while(norm(U1-U)>err)
    U=U1;
    c=y*U./sum(U);
    d=abs(y.'*ones(1,6)-ones(step+2,1)*c);
    zflag=0;
    for t=1:step+2
        num=find(d(t,:)<=eps1*2);
        len=length(num);
        if(len~=0)
            zflag=1;
            U1(t,:)=0;
            for m=1:len
                U1(t,num(m))=1/len;
            end
        end
    end
    if (zflag==0)
        U1=(ones(6,1)*(1./(sum(1./d.')))).'./d;
    end
end
U=U1;
%----------------用同樣方法對輸入信號u進行聚類----------------%
% Uu=zeros(6,step+2);
% for t=1:step+2
%     if(u(t)<=-0.75)
%         Uu(1,t)=1;
%     elseif(u(t)<=-0.45)
%         Uu(1,t)=-u(t)+0.25;Uu(2,t)=u(t)+1.45;
%     elseif(u(t)<=-0.15)
%         Uu(2,t)=-u(t)+0.55;Uu(3,t)=u(t)+1.15;
%     elseif(u(t)<=0.15)
%         Uu(3,t)=-u(t)+0.85;Uu(4,t)=u(t)+0.85;
%     elseif(u(t)<=0.45)
%         Uu(4,t)=-u(t)+1.15;Uu(5,t)=u(t)+0.55;
%     elseif(u(t)<=0.75)
%         Uu(5,t)=-u(t)+1.45;Uu(6,t)=u(t)+0.25;
%     else
%         Uu(6,t)=1;
%     end
% end
% 
% Uu1=Uu.';
% Uu=zeros(step+2,6);
% while(norm(Uu1-Uu)>err)
%     Uu=Uu1;
%     cu=u*Uu./sum(Uu);
%     d=abs(u.'*ones(1,6)-ones(step+2,1)*cu);
%     zflag=0;
%     for t=1:step+2
%         num=find(d(t,:)<=eps1*5);
%         len=length(num);
%         if(len~=0)
%             zflag=1;
%             Uu1(t,:)=0;
%             for m=1:len
%                 Uu1(t,num(m))=1/len;
%             end
%         end
%     end
%     if (zflag==0)
%         Uu1=(ones(6,1)*(1./(sum(1./d.')))).'./d;
%     end
% end
% Uu=Uu1;

%----------------用最小二乘法辨識參數θ----------------%
for t=1:step
    temp=U(t,:).';%*U(t+1,:);
    %temp1=Uu(t+1,:).'*temp(:).';
    beta(t,:)=temp(:);
end
beta1=beta.';
beta2=beta1*diag(1./sum(beta1));
beta3=beta2.';

fy=[beta3,diag(y(2:step+1))*beta3,diag(y(1:step))*beta3,diag(u(2:step+1))*beta3];

th=inv(fy.'*fy)*fy.'*y(3:step+2).';

%----------------測試模型的擬合能力----------------%
U=zeros(6,step);
yt(1)=0;
yt(2)=0;
for t=1:step
    if(yt(t)<=c(1))
        U(1,t)=1;
    elseif(yt(t)<=c(2))
        U(1,t)=(c(2)-yt(t))/(c(2)-c(1));U(2,t)=(yt(t)-c(1))/(c(2)-c(1));
    elseif(yt(t)<=c(3))
        U(2,t)=(c(3)-yt(t))/(c(3)-c(2));U(3,t)=(yt(t)-c(2))/(c(3)-c(2));
    elseif(yt(t)<=c(4))
        U(3,t)=(c(4)-yt(t))/(c(4)-c(3));U(4,t)=(yt(t)-c(3))/(c(4)-c(3));
    elseif(yt(t)<=c(5))
        U(4,t)=(c(5)-yt(t))/(c(5)-c(4));U(5,t)=(yt(t)-c(4))/(c(5)-c(4));
    elseif(yt(t)<=c(6))
        U(5,t)=(c(6)-yt(t))/(c(6)-c(5));U(6,t)=(yt(t)-c(5))/(c(6)-c(5));
    else
        U(6,t)=1;
    end    
    yt(t+2)=th.'*[U(:,t);yt(t+1)*U(:,t);yt(t)*U(:,t);u(t+1)*U(:,t)];
end
figure(2);
plot(y(3:end),'r');
hold on;
plot(yt(3:end),'y');
legend('原模型輸出y','T-S模型輸出yt');



%----------------基于該模型的預測控制設計----------------%
steps = 500;
Y=zeros(1,steps+2).';%系統輸出量y記錄值
dUr=zeros(1,steps+1).';%△u記錄值
Ur=zeros(1, steps+1).';%控制量u的記錄值


p=4;%預測步長
L=1;%控制步長

Q=eye(p);%關于輸出偏差的權值矩陣
R=10*eye(L);%關于△u的權值矩陣
a=0.3;%柔化因子
%s=-2*ones(1,steps);
s=sin(2*pi/steps*(1:steps)*2)+1.2;%要實現的標準輸出


Y(1:2)=[0 0]';%初始化系統輸出量
Ur(1)=0;%初始化系統u的值
Uy=zeros(6,steps);
for t=1:steps
    if(Y(t)<=c(1))
        Uy(1,t)=1;
    elseif(Y(t)<=c(2))
        Uy(1,t)=(c(2)-Y(t))/(c(2)-c(1));Uy(2,t)=(Y(t)-c(1))/(c(2)-c(1));
    elseif(Y(t)<=c(3))
        Uy(2,t)=(c(3)-Y(t))/(c(3)-c(2));Uy(3,t)=(Y(t)-c(2))/(c(3)-c(2));
    elseif(Y(t)<=c(4))
        Uy(3,t)=(c(4)-Y(t))/(c(4)-c(3));Uy(4,t)=(Y(t)-c(3))/(c(4)-c(3));
    elseif(Y(t)<=c(5))
        Uy(4,t)=(c(5)-Y(t))/(c(5)-c(4));Uy(5,t)=(Y(t)-c(4))/(c(5)-c(4));
    elseif(Y(t)<=c(6))
        Uy(5,t)=(c(6)-Y(t))/(c(6)-c(5));Uy(6,t)=(Y(t)-c(5))/(c(6)-c(5));
    else
        Uy(6,t)=1;
    end

%----------------計算A(z),B(z)----------------%    
    C = Uy(:,t).'*th(1:6);
    A(1)=1;
    A(2)=-Uy(:,t).'*th(7:12);
    A(3)=-Uy(:,t).'*th(13:18);
    B=Uy(:,t).'*th(19:24);
%----------------解丟番圖方程----------------%    
    e=zeros(1,p);
    f=zeros(3,p);
    g=zeros(1,p);
    h=zeros(L,p);
    
    e(1)=1;
    e(2)=-(A(2)-A(1))/A(1)*e(1);
    e(3)=-((A(2)-A(1))*e(2)+(A(3)-A(2))*e(1))/A(1);
    for j=4:p
        e(j)=(-(A(2)-A(1))*e(j-1)-(A(3)-A(2))*e(j-2)+A(3)*e(j-3))/A(1);
        %e(j)=-((A(2)-A(1))*e(j-1)+(A(3)-A(2))*e(j-2))/A(1);
    end
    
    for j=1:p
        f(1,j)=-(A(2)-A(1))*e(j);
        f(2,j)=-(A(3)-A(2))*e(j);
        f(3,j)=A(3)*e(j);
    end
    
    for j=1:p
        g(j)=e(j)*B;
    end
    
    for m=1:L
        for j=1:p-m
            h(m,j)=-g(m+j);
        end
    end
    
    G=zeros(p,L);
    for j=1:p
        for m=1:min(j,L)
            G(j,m)=g(j-m+1);
        end
    end
    D=inv(G.'*Q*G+R)*G.'*Q;
%----------------計算柔化后的預測步長內系統的期望輸出,并將其保存在Yd中----------------%
    Yk=Y(t+1)*Y(t)*(Y(t+1)-2.5)/(1+Y(t+1)^2+Y(t)^2)+Ur(t);
    Yd=zeros(p, 1);
    Yd(1)=Yk;
    for j=1:p-1
        Yd(j+1)=a*Yd(j)+(1-a)*s(t);
    end
%----------------求出在給定權值下的系統最小偏差所對應的△u----------------%
    Y0=f.'*[Yk,Y(t+1),Y(t)].'+h.'*dUr(t);    
    dU=D*[Yd-Y0];    
%----------------依次記錄本次運行的△u,u和y值----------------%
    dUr(t+1)=dU(1);
    Ur(t+1)=dUr(t+1)+Ur(t);
    Y(t+2)=Yk;
end
%----------------繪制信號波形----------------%
figure(3);
plot(s);
hold on;
plot(Y(3:end),'r');
plot(Ur(2:end),'g');
legend('設定輸出s','實際輸出Y','實際輸入Ur');