%
% Especialização em Controle e Otimização de Processos
% Módulo: Controle Avancado de Processos 

% Problema 3 - Controlador QDMC sistema 3x3 com perturbacao
clear all
clc
nu=?;% Numero de manipuladas
ny=?;% Numero de controladas
T=?;% Período de amostragem

% **************************************************************
% Parametros de Sintonia
N=?;% Horizonte do modelo
np=?;% Horizonte de otimização ou predição
m=?;% Horizonte de controle
nsim=500;% Tempo de simulação em periodos de amostragem
r=[? ? ?]*1e-?;% Fator de supressão das manipuladas
w=sqrt([? ? ?]);% Peso das controladas

% **************************************************************
% Restricoes
umax=[? ? ?]';% Máximo das manipuladas
umin=[? ? ?]';% Mínimo das manipuladas
dumax=[10. 0.1 0.01]';% Variacao maxima nas manipuladas
ymax=[? ? ?]';% Maximo das controladas
ymin=[? ? ?]';% Minimo das controladas

% **************************************************************
% Inicializacao
u0=[? ? ?]';% Valor inicial das manipuladas
y0=[? ? ?]';% Valor inicial das controladas

% **************************************************************
% Definicao do set point
ysp=[? ? ?]';% Set-point das controladas
ydss=[? ?]; % Valores iniciais da perturbacao

% **************************************************************
% Definicao do valor da perturbacao
dyd=[850*0. -70*0.05]; % Valor de delta da perturabacao
%***********************************************************


% Definicao das funcoes de transferencia = tf([zeros(1,nk+1) b1 b2],[1 a1 a2 a3],T,'variable','z^-1')
%&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
% ALUNO: ENTRAR AS FUNÇÕES DE TRANSFERÊNICA EM Z
%&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
g11z
g12z
g13z
g21z
g22z
g23z
g31z
g32z
g33z
% Definicao das funcoes de transferencia da perturbacao = tf([zeros(1,nk+1) b1 b2],[1 a1 a2 a3],T,'variable','z^-1')
%&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
% ALUNO: ENTRAR AS FUNÇÕES DE TRANSFERÊNICA EM Z
%&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
gd11z
gd12z
gd21z
gd22z
gd31z
gd32z

% CONTROLADOR *******************************
t=0:T:N*T;
s11 = step(g11z,t)
s12 = step(g12z,t)
s13 = step(g13z,t)
s21 = step(g21z,t)
s22 = step(g22z,t)
s23 = step(g23z,t)
s31 = step(g31z,t)
s32 = step(g32z,t)
s33 = step(g33z,t)
sd11 = step(gd11z,t)
sd12 = step(gd12z,t)
sd21 = step(gd21z,t)
sd22 = step(gd22z,t)
sd31 = step(gd31z,t)
sd32 = step(gd32z,t)


% Consideramos as funcoes de transferencia da planta igual ao modelo do
% exercicio
sp11=s11;
sp12=s12;
sp13=s13;
sp21=s21;
sp22=s22;
sp23=s23;
sp31=s31;
sp32=s32;
sp33=s33;
spd11 = sd11;
spd12 = sd12;
spd21 = sd21;
spd22 = sd22;
spd31 = sd31;
spd32 = sd32;

s=zeros(ny,nu,N);,sp=zeros(ny,nu,N);
for j=1:N;
    s(:,:,j)=[s11(j) s12(j) s13(j);s21(j) s22(j) s23(j); s31(j) s32(j) s33(j)];
    sp(:,:,j)=[sp11(j) sp12(j) sp13(j);sp21(j) sp22(j) sp23(j); sp31(j) sp32(j) sp33(j)];
    sd(:,:,j)=[sd11(j) sd12(j); sd21(j) s22(j); sd31(j) sd32(j)];
    spd(:,:,j)=[spd11(j) spd12(j); spd21(j) sp22(j); spd31(j) spd32(j)];
    end

% *************************************************************************
% *********************************** QDMC ********************************
% Problema 1 - funcao QDMC


%  Defining the initial conditions
% Definicao inicial para yat, ypp e ypm
yat=y0;% Valor atual de y (nyX1)
ypp=y0;% Valor de predicao da planta (nyX1)
ypm=y0;% Valor de predicao do modelo (nyX1)
setp=ysp;% set point (nyX1)<- ys

% Criacao de um vetor ny*N(N*nyX1)
for i=1:N-1;
 ypp=[ypp;y0];
 ypm=[ypm;y0];
 setp=[setp;ysp];
end

% Saida atualizada recebe o valor inicial de u (u0)
uat=u0;


S=[];
for in=1:N;
    S=[S;s(:,:,in)];
end

Sp=[];
for in=1:N;
    Sp=[Sp;sp(:,:,in)];
end

Sd=[];
for in=1:N;
    Sd=[Sd;sd(:,:,in)];
end

Spd=[];
for in=1:N;
    Spd=[Spd;spd(:,:,in)];
end


% Creating the Dynamic Matrix

a=S(1:ny*np,:);
               
% Dimensionamento da matriz Smn: ny*npXnu*m
Smn=[a];% Smn <- [S1 S2 S3...Snp]'
for iu=1:m-1;
  a=[zeros(ny,nu);a(1:(np-1)*ny,:)];
  Smn=[Smn a]; 
              
end

%creating matrix S (to correct the last control move)

Stil=S(1:ny*N,:);
Sptil=Sp(1:ny*N,:);
%Sdtil=Sd(1:2*N,:);
%Spdtil=Spd(1:2*N,:);

%Matrices W and Rb
aux=[];
for i=1:np;
  aux=[aux w];
end
W=diag(aux);% Dimensionamento de W: np*ny X np*ny

clear aux;,aux=[];
for i=1:m;
  aux=[aux r];
end
R=diag(aux);% Dimensionamento de R: m*nu X m*nu

%Matrix S'W'W S+R
H=Smn'*W'*W*Smn+R;
H=(H+H')/2;

%Shifting matrix
M=[zeros((N-1)*ny,ny) diag(ones(1,ny*(N-1)))
    zeros(ny,(N-1)*ny) diag(ones(1,ny))];

%Auxiliary constraint matrix

Na=[diag(ones(1,nu))];% Cria um vetor linha de nu colunas e diagonaliza o mesmo
aux=[diag(ones(1,nu))];
for i=1:m-1;
  aux=[aux diag(ones(1,nu))];
  Na=[Na zeros(i*nu,nu)
     aux];
end

Dumax=dumax;
Umax=umax;
Umin=umin;
for i=1:m-1;
 Umax=[Umax;umax];% Cria vetor de Umax com m linhas
 Umin=[Umin;umin];% Cria vetor de Umin com m linhas
 Dumax=[Dumax;dumax];% Cria vetor de Dumax com m linhas
end
Ymax=ymax;
Ymin=ymin;
for i=1:np-1;
  Ymax=[Ymax;ymax];% Cria vetor de Ymax com np linhas
  Ymin=[Ymin;ymin];% Cria vetor de Ymin com np linhas
end

for in=1:nsim;
    in
  ur(:,in)=uat(:);
  yr(:,in)=ypp(1:ny);  
  el=setp(1:ny*np)-ypm(1:ny*np);
  ct=-el'*W'*W*Smn;
  ub=uat;
  for i=1:m-1;
   ub=[ub;uat];
  end

%Including constraints on the inputs - Restricoes dispostas na forma
%exigida pela funcao quadprog nas entradas

  A1=[Na;-Na];
  b1=[Umax-ub;-Umin+ub];
  A=A1;
  b=b1;

%Including constraints on the input changes - Restricoes dispostas na forma
%exigida pela funcao quadprog nas variacoes das entradas

  A2=[diag(ones(1,nu*m));-diag(ones(1,nu*m))];
  b2=[Dumax;Dumax];
  A=[A;A2];
  b=[b;b2];

%
  % Chamada da funcao quadprog
  % A funcao quadprog resolve o seguinte problema min (x)= x'*H*x + 2*f'*x
  % s.a. Ax<=b; Aeq*x=beq; lb<=x<=ub
  options = optimset('Display','iter','LargeScale','off','TolFun',1e-8);
  du=quadprog(H,ct,A,b,[],[],-dumax,dumax,[],options);

%Correction of the last control move
% Atualizacao da saida 
  uat=uat+du(1:nu);

  
  % No instante 5 introduz uma perturbacao que eh retirado logo em seguida
  if in==5
      dyd_aux=dyd;
  else dyd_aux=[0 0];
  end
  
  % Predicao da saida nos diversos instantes com acao de controle
  ypm=ypm+Stil*du(1:nu)+Sd*dyd_aux'; % Inclui a perturbacao
  ypp=ypp+Sptil*du(1:nu)+Spd*dyd_aux';

 %Correction of the last measurement
  d=ypp(1:ny)-ypm(1:ny);
  dex=[];
  for i=1:N;
    dex=[dex;d];
  end
  ypm=ypm+dex; % O DMC assume mesmo erro para qualquer instante de predicao
  

%Preparing the prediction for the next step
  ypp=M*ypp;
  ypm=M*ypm;

end


u=ur;
y=yr;


% *************************************************************************
% *************************************************************************
% Exibicao dos resultados
figure(1)
yspp=ysp;
t=0;
for i=1:nsim-1;
yspp=[yspp ysp];
t=[t i];
end
subplot(2,3,1),plot(t,y(1,:),'r',t,yspp(1,:),'k' ),ylabel('y1')
subplot(2,3,2),plot(t,y(2,:),'r',t,yspp(2,:),'k'  ),ylabel('y2')
subplot(2,3,3),plot(t,y(3,:),'r',t,yspp(3,:),'k'  ),ylabel('y3')
subplot(2,3,4),plot([u(1,:)' ]),ylabel('u1')
subplot(2,3,5),plot([u(2,:)' ]),ylabel('u2')
subplot(2,3,6),plot([u(3,:)' ]),ylabel('u3')
