
% Caso 2 - Controlador QDMC sistema 2x2
clear all
clc

% Exercicio sobre os parametros de sintonia do DMC

nu=?;% Numero de variaveis manipuladas
ny=?;% Numero de variaveis controladas
T=?;% Período de amostragem

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

% **************************************************************
% Restricoes 
umax=[? ?]';% Máximo das manipuladas
umin=[? ?]';% Mínimo das manipuladas
dumax=[? ?]';% Variacao maxima nas manipuladas
%dumax=[0. 0.]';% 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

%***************************************************************

% Definicao das funcoes de transferencia = tf([zeros(1,nk+1) b1 b2],[1 a1 a2 a3],T,'variable','z^-1')
% Note how setting the 'variable' property to 'z^-1' causes the result to be displayed as a transfer function of the variable .
%------de z^0 a z^-2----z^-3-------z^-4------z^0--z^-1-------z^-2-------z^-3
%&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
% 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 *******************************
% Criacao vetor tempo discretizado em T
t=0:T:N*T;

% Aplicacao do degrau as funcoes de transferencia em z
s11 = step(g11z,t)
s12 = step(g12z,t)
s21 = step(g21z,t)
s22 = step(g22z,t)


% Consideramos as funcoes de transferencia da planta igual ao modelo do
% exercicio (Condicao Nominal)
sp11=s11;
sp12=s12;
sp21=s21;
sp22=s22;

% Inicializacao do vetor s

% Montagem da resposta ao degrau - concatenacao
% Serao montadas varias matrizes, de 1 ate N, onde cada elemento de cada matriz
% armazena o valor da resposta da saida i com relaçao a entrada k no
% instante j
for j=1:N;
    s(:,:,j)=[s11(j) s12(j);s21(j) s22(j)]
    sp(:,:,j)=[sp11(j) sp12(j);sp21(j) sp22(j)] %resposta ao degrau da planta
end



%  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


% 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)
% Considera a açao de controle implantada na planta no instante k para a
% predicao das saidas ate o instante de estabilizacao dessa acao
Sbar=S(1:ny*N,:);
Spbar=Sp(1:ny*N,:);

               % Sbar ou Spbar
               % S1 
               % S2 
               % ...
               % Snp 

%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 com 1 de linha de nu colunas e diagonaliza o mesmo
aux=[diag(ones(1,nu))];
for i=1:m-1;
  aux=[aux diag(ones(1,nu))];
%  aux=[aux ones(1,nu)];
  Na=[Na zeros(i*nu,nu)
     aux];
               % Na para m=2 VERIFICAR
               % 1 0  0  0 
               % 0 1  0  0 
               % 1 0  1  0
               % 0 1  0  1
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];

%Including constraints on the outputs - Restricoes dispostas na forma
%exigida pela funcao quadprog nas saidas

  A3=[Smn;-Smn];
  b3=[Ymax-ypm(1:np*ny);ypm(1:ny*np)-Ymin];
  A=[A;A3];
  b=[b;b3];

  % 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','off','LargeScale','off','TolFun',1e-8);
  du=quadprog(H,ct,A,b,[],[],[],[],[],options);

%Correction of the last control move

% Atualizacao da saida  
  uat=uat+du(1:nu); % aplicamos as primeiras m acoes de controle na planta
  

% **************************************************************************
% *************************************************************************
% *************************************************************************
% Aguarda o final do intervalo para aquisicao do valor da planta
% Tudo se passa como se fosse o inicio do inter valor k+1
% *************************************************************************
% *************************************************************************
% Predicao da saida nos diversos instantes com acao de controle
% A acao de controle considerada eh apenas a implantada no sistema nessse
% instante k
  ypm=ypm+Sbar*du(1:nu); % Na predicao a acao de controle k sendo considerada
% Leitura da planta  
  ypp=ypp+Spbar*du(1:nu);
  
%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
% Matriz de deslocamento onde a predicao de k+1 no intervalo k=1 serah a
% predicao de k+2 no intervalo k
  ypp=M*ypp; % Predicao da planta para o instante k+1 antes da acao de controle
  ypm=M*ypm; % Predicao do modelo para o instante k+1 antes da acao de controle

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,2,1),plot(t,y(1,:),'r',t,yspp(1,:),'k'),ylabel('y1')
subplot(2,2,2),plot(t,y(2,:),'r',t,yspp(2,:),'k'),ylabel('y2')
subplot(2,2,3),plot(u(1,:)),ylabel('u1')
subplot(2,2,4),plot(u(2,:)),ylabel('u2')
