% Procedure GPC simulation
% The system controled has impulse TF H(q)=K(q-b)/((q-1)(q-a));
% Where q is delay factor, a=exp(-h), K=exp(-h)-1+h, b=1-h(1-exp(-h))/(exp(-h)-1+h)
% h is simple interval, h=0.25s;
% Nu=2,N0=1,N1=4,lanmuda=20,alpher=0.5;

% A(q)=1-((1+a)/a)q+(1/a)q^2;
% B(q)=-Kb/a+(K/a)q;

clear all;
h=0.25;
%h=0.75
Nu=5;
N1=5;
lanmuda=0.02;
alpher=0.5;
a=exp(-h);
K=exp(-h)-1+h;
b=1-h*(1-a)/K;
A(1)=1;
A(2)=-(1+a)/a;
A(3)=1/a;
%A(1)=1;
%A(2)=-1.5;
%A(3)=0.7;
%A(1)=1;
%A(2)=-0.7;
na=size(A,2)-1;
B(1)=-K*b/a;
B(2)=K/a;

%B(1)=0.9;
%B(2)=-0.6;
%B(3)=1.5;
%B(1)=0;
%B(2)=1;
%B(3)=1.5;
nb=size(B,2)-1;
A_(1)=A(1);
A_(2)=A(2)-A(1);
A_(3)=A(3)-A(2);
A_(4)=-A(3);
%A_(1)=A(1)-1;
%A_(2)=A(2)-A(1);
%A_(3)=-A(2);

% F is an N1*na matrix F(1,:)=1-A_
F(1,:)=-A_(2:na+2);
for j=2:N1
   for i=1:na
      F(j,i)=F(j-1,i+1)-A_(i+1)*F(j-1,1);
   end;
   F(j,na+1)=-A_(na+2)*F(j-1,1);                                                   
end;

% E is a 1*N1 matrix
E(1)=1;
E(2:N1)=F(1:N1-1,1)';

% H is an N1*nb matrix
H(1,:)=B(2:(nb+1));
for j=2:N1
   for i=1:nb-1
      H(j,i)=E(j-1)*B(i+1)+H(j-1,i+1);
   end;
   H(j,nb)=E(j)*B(nb+1);
end;

% G is an N1*Nu matrix
g(1)=E(1)*B(1);
for j=2:N1
   g(j)=E(j)*B(1)+H(j-1,1);
end;
G=zeros(N1);
for i=-1:-1:-N1
   G=G+diag(g(abs(i))*ones(1,N1+i+1),i+1);
end;
G=G(:,1:Nu);
GT=inv(G'*G+lanmuda*eye(Nu));
P=GT(1,:)*G';
alpha=zeros(1,na+1);
beita=zeros(1,nb);
for i= 1:N1
   alpha=alpha+F(i,:)*P(i);
   beita=beita+H(i,:)*P(i);
end;
n=max(na,nb+1);
y(1:n+1)=zeros(1,n+1);
%y=zeros(1,n+1);
%y(2)=0;
u(1:n)=zeros(1,n);
%y(n+1)=B*u(n:-1:n-nb)'-A(2:na+1)*y(n:-1:n-na+1)';%+(rand-0.5)/10;
%for i=3:n+1
%   y(i)=B(1:nb+1)*u(i-1:-1:i-nb-1)'-A(1:na)*y(i-1:-1:i-na)';
%end
N=1000;
du=zeros(1,n);
yr=0.5;
YR=zeros(1,N);
for i=n+1:N
   w(1)=y(i);
   if mod(i,200)==0
      yr=-yr;
   end;
   YR(i)=yr;
   %YR(i)=sin(i);
   for r=2:N1
      w(r)=alpher*w(r-1)+(1-alpher)*yr;
   end
   du(i)=P(1:N1)*w(1:N1)'-alpha*y(i:-1:i-na)'-beita*du(i-1:-1:i-nb)';
   %du(i)=P(1:N1)*YR(1:N1)'-alpha*y(i:-1:i-na)'-beita*du(i-1:-1:i-nb)'
   
   u(i)=u(i-1)+du(i);
%   if u(i)>10
%      u(i)=10;
%   end
%   if u(i)<-10
%      u(i)=-10;
%   end
   y(i+1)=B*u(i:-1:i-nb)'-A(2:na+1)*y(i:-1:i-na+1)';%+(rand-0.5)/10;
end;
figure(6);

hold on;
plot(u,'r-');
plot(y);
plot(YR,'k-');
%grid on;