

% Caso 1 - Controlador DMC 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 - Elas nao tem acao efetiva no DMC que nao considera
% restricoes no seu problema
umax=[? ?]';% Máximo das manipuladas
umin=[? ?]';% Mínimo das manipuladas
dumax=[50. 0.3]';% 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


% Varias matrizes nyXnu até o horizonte de estabilizacao N
%**********************************************************
%************* DMC de Cutler ******************************


%  Defining the initial conditions

% Definicao inicial para yat, ypp e ypm
yat=y0; % Valor atual de y
ypp=y0; % Prediçao do valor de y da planta
ypm=y0; % Predicao do valor de y pelo modelo

% set point eh atualizado com o valor recebido do programa principal
setp=ysp;

% Criacao de um vetor de N*ny linhas 
for i=1:N-1;
 ypp=[ypp;y0]; % A predicao inicial, sem acao de controle, eh y0
 ypm=[ypm;y0];
 setp=[setp;ysp];
end
% Ao final do for acima, Dimensao setp, ypp e ypm = [N*ny x 1]
% Inicializacao de uat - valor atual para as nu entradas
uat=u0;

% Concatenacao das N matrizes de resposta ao degrau (cada saida com cada
% entrada) na matriz do modelo S 
% - Dimensao de S = N*ny X nu

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

% Creating the Dynamic Matrix

a=S(1:ny*np,:); % varre todas as primeiras np linhas (das N) contendo ny
                % variaveis de S com todas colunas (uma coluna para cada
                % entrada) - [S1 S2 S3...Snp]'
                
% Dimensionamento da matriz Smn:ny*npXnu*m
% Smn inicia com as np*ny linhas de S
Smn=[a]; % Smn <- [S1 S2 S3...Snp]'
for iu=1:m-1;
  a=[zeros(ny,nu);a(1:(np-1)*ny,:)];
  Smn=[Smn a]; % loop 1: Smn <- [S1 S2 S3...Snp]' acrescido de [ 0 S1 S2 S3...Snp-1]'
               % loop 2: Smn <- [S1 S2 S3...Snp]'[ 0 S1 S2 S3...Snp-1]' acrescido de [ 0 0 S1 S2 S3...Snp-2]'
               % S1 0  0  0 ...
               % S2 S1 0  0 ...
               % ...
               % Snp Snp-1 ... S1
end

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

Sbar=S(1:ny*N,:); % Resposta do degrau aplicado ate o horinzonte N de estabilizacao
Spbar=Sp(1:ny*N,:);
% Sbar = S1
%        S2
%        .
%        .
%        SN

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

clear aux;,aux=[];
for i=1:m;
  aux=[aux r];
end
R=diag(aux);

% Na Funcao Objetivo: Se serivarmos e igualarmos a zero obtemos:
% du=[(S'W'W S+R)^-1]*S'W'W e
%Matrix S'W'W S+R
H=Smn'*W'*W*Smn+R;

%Shifting matrix
M=[zeros((N-1)*ny,ny) diag(ones(1,ny*(N-1))) % Cria (N-1)*ny linhas e ny colunas de zeros
                                             % ones(1,ny*(N-1)) - uma linha com ny*(N-1) colunas 
                                             % diag: uma matriz diagonal em 1
                                             % de ny*(N-1) linhas e colunas
    zeros(ny,(N-1)*ny) diag(ones(1,ny))];    % Acrescenta ny linhas e (N-1)*ny colunas com
                                             % o ultimo elemento matriz
                                             % diagonal em 1 de ny colunas
                                             % Total: (N-1)*ny +ny linhas =
                                             % N*ny linhas
                                             % Total de colunas: ny +
                                             % (N-1)*ny = N*ny colunas
                                             % 
Mp=M;

%Observer gain and output matrix
Kf=[];C=eye(ny);
for i=1:N;
    Kf=[Kf;eye(ny)]; % Assumindo condicao nominal, Kf = Identidade Dimensionamento (ny*N)X(ny*N)
end
 
for i=1:N-1;
    C=[C,zeros(ny)]; % C ajustado para recuperar apenas a primeira acao de controle
end
 
for in=1:nsim;
  ts1 = sprintf('Instante = %5.2f', in); % ts1 <- Instante = valor(in)
  disp(ts1) % Exibe ts1 na janela de comando
  ur(:,in)=uat(:);% ur recebe todas linhas do vetor uat (que na primeira passada esta com u0)
  yr(:,in)=ypp(1:ny);  % yr recebe as primeiras ny linhas do vetor ypp que corresponde
                       % a prediçao no instante k+1
  el=setp(1:ny*np)-ypm(1:ny*np); % O vetor e1 contem os desvios em relacao ao set point 
                                 % de todos valores de ypm ate o instante
                                 % np sem considerar as acoes de controle
  % No for abaixo, ub recebe m linhas com os valores de uat
  ub=uat;
  for i=1:m-1;
   ub=[ub;uat]
%    pause
  end

  % Calculo da saida Delta U
  du=inv(H)*Smn'*W'*W*el;
  
%Correction of the last control move - Atualizacao do uat pelo incremento
%de du

  uat=uat+du(1:nu);


%Correcao da predicao pela ultima acao de controle (u(k))
  ypm=ypm+Sbar*du(1:nu);
% O Sbar considera apenas a acao do instante k. As demais acoes de
% controle, dentro do horizonte de controle, nao sao consideradas para a
% predicao. Essas demais acoes de controle so sao consideradas na
% minimizacao da funcao objetivo, calculando as m acoes de controle.
% ***************************************
% Spbar simula a resposta real da planta
  ypp=ypp+Spbar*du(1:nu);
% ***************************************
  
%Correction of the last measurement
  d=ypp(1:ny)-ypm(1:ny); % d armazena os desvios de todas ny variaveis no instante k
  dex=[];% desvio estendido
  for i=1:N;
    dex=[dex;d]; % Supoe que o desvio d (erro de predicao) sera o mesmo do instante k ate N
  end
  ypm=ypm+dex; % Correcao do vetor de predicao do modelo pelo desvio ocorrido no instante k.
  

%Preparing the prediction for the next step
  ypp=M*ypp; % deslocamento da predicao ypp e ypm vetor de predicao sem a acao de controle do proximo scan implantada
  ypm=Mp*ypm; % O novo vetor de predicao do modelo sera o atual deslocado em um periodo T
            % O DESLOCAMENTO É FEITO´EM CIMA DA PREDIÇÃO CORRIGIDA
  
  if in==nsim
     %
     %Closed loop state matrix
     % du=inv(Smn'*W'*W*Smn+R)*Smn'*W'*W*e1
     KKdmc=inv(H)*Smn'*W'*W; % Dimensao: m.nu X N.ny
     Kdmc=KKdmc(1:nu,:);
     NN=[eye(np*ny) zeros(np*ny,(N-np)*ny)];
     % Modelo em variaveis de estado do MPC em malha fechada:
     Acl=[M-Kf*C-(M*Sbar-Kf*C*(Sbar-Spbar))*Kdmc*NN    Kf*C
          -Mp*Spbar*Kdmc*NN                            Mp   ];
       lamb=eig(Acl);
    end
    
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')


