%Rotina principal
%Autor: Guilherme Goncalves
%Data: segundo semestre 2013
%Descrição: Controle NMPC do reator de VanVusse com estimação de estados
%Entrada de dados
 clc
 clear all
%--------------------------------------------------------------------------------------
%declaração de variáveis globais
global  Hc x0_integracao ii Hp ...
    w1 w2 w3 w4 desvio_x1 desvio_x2 cont Ta F1 F2 F3 F4 bb x_interior x0 Ta_int...
    u1_armazenado u2_armazenado norma_u1 norma_u2 norma_x1 norma_x2


% %Ta = 3; %periodo de amostragem da ação de controle
Ta = 0.01;
%Ta_int = 0.0001; %passo de amostragem da integração
Hct = 0:Ta:4*Ta;
Hp = 20*Ta;
w1 = 1.5;
%w2 = 1;
% w2 = 1.5;
w2 = 1.5*10;
%w3 = 0.01;
%w4 = 0.1;
w3 = 1;
w4 = 1/10;

% Ta = 0.1;
% Ta_int = 0.01; %passo de amostragem da integração
% Hct = 0:Ta:0.5;
% Hp = 2;
% w1 = 1;
% w2 = 1;
% w3 = 1;
% w4 = 1;

%---------------------------------------------------------------------------------------
%Parâmetros da otimização
% u1_0=-0.002*ones(1,(length(Hct)));   
% u2_0=-0.004*ones(1,(length(Hct)));

%u1_0=-10.9905*ones(1,(length(Hct)));   
%u2_0=-1.1819*ones(1,(length(Hct)));
% u1_0 = [-63.6669
%   -84.0377
%   -87.9677
%   -88.3421
%   -88.7623
%   -89.4842]';
% u2_0 = [-8.8469
%   -13.7325
%   -15.9872
%   -16.9819
%   -17.4726
%   -17.7619]';
lbu1=0*ones(1,(length(Hct)));      
ubu1=2000*ones(1,(length(Hct)));      
lbu2=-10000*ones(1,(length(Hct)));     
ubu2=10000*ones(1,(length(Hct)));

lb =[lbu1 lbu2];
ub =[ubu1 ubu2]; 

u1_0=ones(1,(length(Hct)))*700 ;   
u2_0=ones(1,(length(Hct)))* -4495.7;
%u1_0 = -10:-10:-50;
%u2_0 = -10:-10:-50;
norma_x1 = 1.5;
norma_x2 = 600;
norma_u1 = ubu1(1);
norma_u2 = ubu2(2);


%x0 =[u1_0 u2_0];

x0 =[u1_0/norma_u1 u2_0/norma_u2];
% lbu1=-1*ones(1,(length(Hct)));      
% ubu1=1*ones(1,(length(Hct)));      
% lbu2=-1*ones(1,(length(Hct)));     
% ubu2=1*ones(1,(length(Hct))); 


% bu1 = 0.1*ones(length(lbu1),1); %máxima variação permitida
% bu2 = 0.1*ones(length(lbu2),1); %máxima variação permitida a manipulação do vapor

bu1 = 100; %máxima variação permitida (variáveis sem normalizar)
bu2 = 100; %máxima variação permitida a manipulação do vapor

bb = [bu1/norma_u1;bu2/norma_u2];

%optionsInterior = optimset('Algorithm','interior-point','Display','iter-detailed','MaxIter',2000,'MaxFunEvals',1e5);
%optionsInterior = optimset('Algorithm','sqp','Display','iter-detailed','MaxIter',2000);
%optionsInterior = optimset('Algorithm','active-set','Display','iter-detailed','MaxIter',2000,'MaxFunEvals',1e5,'TolFun',1e-8,'TolX',1e-8);
optionsInterior = optimset('Algorithm','interior-point','Display','iter','MaxIter',1000,'MaxFunEvals',1e5,'TolFun',1e-4,'TolX',1e-5,'UseParallel','always',...
    'TolProjCG',1e-6,'MaxProjCGIter',30*length(x0),'InitTrustRegionRadius',sqrt(length(x0))/100);
%-----------------------------------------------------------------------------------------
%Fim da entrada de dados

%Inicio da otimização utilizando a estratégia de horizonte móvel
x0_integracao = [5.1 0 298 298];
%armazenamento das posições iniciais
ii = 1;
bias_fundo(ii) = 0;
bias_topo(ii)  = 0;
x1armazenado_modelo(ii) = x0_integracao(1);
x2armazenado_modelo(ii) = x0_integracao(2);
x1_planta(ii) = x0_integracao(1);
x2_planta(ii) = x0_integracao(2);
xfiltrado_armazenado_EKF(:,1) = x0_integracao;
xfiltrado_armazenado_CEKF(:,1)= x0_integracao;
xarmazenado(:,ii) = x0_integracao;

u1_armazenado(ii) = 700/norma_u1;
u2_armazenado(ii) = -4495.7/norma_u2;

u1_armazenado_desnorm(ii+1) = 700;
u2_armazenado_desnorm(ii+1) = -4495.7;

%ii corresponde ao número de otimizações que serão realizadas
cont = 0;
for ii = 1:1:50
 %variável auxiliar
tpc  = cputime; %inicio do cômputo do tempo de processamento
%Hc = (ii-1):Ta:(ii+4); %horizonte móvel;
Hc = (cont):Ta:(Hct(end)+cont); %horizonte móvel;
%Hc = (ii-1):0.5:(ii+Hct(end)-1); %horizonte móvel;
Hc = [Hc Hp + (cont)]; %horizonte de predição móvel
%Otimização do modelo da planta
[x_interior,fval_interior,exitflag_interior,output] = fmincon('funcao_objetivo_geral',x0,[],[],[],[],lb,ub,'dumax',optionsInterior);%rotina de otimizaçao
%[x_interior,fval_interior,exitflag_interior,output] = fmincon('funcao_objetivo_geral',x0,[],[],[],[],lb,ub,[],optionsInterior);%rotina de otimizaçao
%[x_interior,fval_interior,exitflag_interior,output] = fminunc('funcao_objetivo_geral',x0,optionsInterior);%rotina de otimizaçao
%[x_interior,ooo]=sqplab('destilacao_objetivo_geral',x0',[],lb,ub);
FF = (length(Hc)-1); %variável auxiliar
u1= x_interior(1:FF); 
u2= x_interior(FF+1:2*FF);
%LAST MOVE
% if ii ==1
% u1(1) = 0;
% u2(1) = 0;
% end

parametrosHC = [u1(1)*norma_u1 u2(1)*norma_u2]; %Os parâmetros da planta para a simulação no tempo de discretização são apenas as primeiras ações

%Simulação da PLANTA
if ii == 1
dxdt02 = vanvusse_dasslc_4eq(Hc(1),x0_integracao',[0 0 0 0]',parametrosHC);
[tempoHC xHc]=dasslc('vanvusse_dasslc_4eq',[Hc(1) Hc(2)],x0_integracao,dxdt02,parametrosHC);
else 
dxdt02 = vanvusse_dasslc_4eq(Hc(1),x0_integracao',[0 0 0 0]',parametrosHC);
[tempoHC xHc]=dasslc('vanvusse_dasslc_4eq',[Hc(1) Hc(2)],x0_integracao_completo,dxdt02,parametrosHC);
end

%Adição do ruído de medição a simulação da planta
medidas = xHc(end,:) + [normrnd(0,0.01*xHc(end,1)) normrnd(0,0.01*xHc(end,2)) normrnd(0,0.025*xHc(end,3)) normrnd(0,0.01*xHc(end,4))];

%Simulação do Modelo
if ii == 1
dxdt02 = vanvusse_dasslc_4eq(Hc(1),x0_integracao',[0 0 0 0]',parametrosHC);
[tempoHC x_modelo]=dasslc('vanvusse_dasslc_4eq',[Hc(1) Hc(2)],x0_integracao,dxdt02,parametrosHC);
else
dxdt02 = vanvusse_dasslc_4eq(Hc(1),x0_integracao',[0 0 0 0]',parametrosHC);
[tempoHC x_modelo]=dasslc('vanvusse_dasslc_4eq',[Hc(1) Hc(2)],x0_integracao,dxdt02,parametrosHC); %mudar aqui para x0_integracao em c. inicial
end


%Calculo do disturbio = discrepancia entre a ultima saida da planta e a
%ultima saída do modelo (pag 18 Maciejowsky (2000) eq. 1.30) 
%d(k) = y(k) - yp(k|k-1)
%desvio_x1= (xHc(end,2) - x_modelo(end,2)); %concentração de B medida
desvio_x1= (xfiltrado_armazenado_EKF(2,ii) - x_modelo(end,2)); %concentração de B estimada
desvio_x2 = (medidas(end,3) - x_modelo(end,3)); %temperatura do reator (medida)


%filtro de kalman estendido
% %Matrizes do modelo
medida = medidas(3); %temperatura do reator
x_apriori = x_modelo(end,:);
A = vanvusse_Amatrix(xfiltrado_armazenado_EKF(:,ii),parametrosHC);
%C = eye(4,4);
C = [0 0 1 0];
% %sintonia do filtro
Q = eye(4,4)*0.0001;
%R = diag([10 10 4000 4000]);
R = 4000;
if ii ==1
    Pk = eye(4,4)*100;
    %Pkcekf = [0.0748   -0.0158   -0.5056; -0.0158    0.1063    0.5943; -0.5056    0.5943   12.0106];
    Pkcekf = Pk;
end
[xmais,Pmais Kmais] = kalman_estendido(x_apriori,medida,Pk,Q,R,Ta,A,C);
%restrições
xmin = [0 0 0 0];
%ymin = xmin;
ymin = 0;

xmax = [4 4 1000 1000];
%ymax = xmax;
ymax = 1000;


    

[xmais_CEKF,Pmais_CEKF,Kmais_CEKF,exitflag_CEKF,ymais] = cekf(x_apriori,medida,Pkcekf,[],Q,R,Ta,A,C,xmin,xmax,ymin,ymax);
% %atualização
 Pk = Pmais;
 xest = xmais;
 Pkcekf = Pmais_CEKF;
 xest_cekf = ymais;


%-------------------------------------------------------------------------------------
%Condições para a realimentação
         %x0_final_modelo_real = xHc(end,:); %estado da planta
         %x0_final_modelo_real = xHc(1,:); %estado da planta atrasado 1 Ta
         x0_final_modelo_real = xest; %estado filtrado
         %x0_final_modelo_real = x_modelo(end,:);  %estado do modelo
         %x0_final_modelo_real = x_modelo(end,:) + (Kalman_gain*desvio_modelo_geral')'; %estados estimados
         
%----------------------------------------------------------------------------------------------       
x0_integracao = x0_final_modelo_real; %realimentação dos estados
%Atualização da estimativa inicial da otimização
x0 = [u1(2:end) u1(end) u2(2:end) u2(end)]; % o próximo chute é o anterior deslocado mais o valor de estado estacionário ("valor zero")
x0_integracao_completo = xHc(end,:); %nova condição inicial para o modelo completo
        
        
        
%-------------------------------------------------------------
%Armazenamento das variáveis 
bias_fundo(ii+1) = desvio_x1;
bias_topo(ii+1)  = desvio_x2;
x1armazenado_modelo(ii+1) = x_modelo(end,2);
x2armazenado_modelo(ii+1) = x_modelo(end,end);
xfiltrado_armazenado_EKF(:,ii+1) = xmais;
xfiltrado_armazenado_CEKF(:,ii+1) = xest_cekf;
exitflag_cekf(ii) = exitflag_CEKF;
x1_planta(ii+1) = xHc(end,2);
x2_planta(ii+1) = xHc(end,end);
medidas_armazenada(:,ii) = medidas;
xarmazenado(:,ii+1) = xHc(end,:);
u1_armazenado(ii+1) = u1(1);
u2_armazenado(ii+1) = u2(1);

u1_armazenado_desnorm(ii+1) = parametrosHC(1);
u2_armazenado_desnorm(ii+1) = parametrosHC(2);
%dados de desempenho armazenados
tempoproc(ii) = cputime-tpc
Nfobj(ii) = output.funcCount;
iteracoes(ii) = output.iterations;
%Exibe dados para acompanhamento do processo
disp(['ii = ' mat2str(ii)])
disp(['u1 = ' mat2str(u1(1))])
disp(['u2 = ' mat2str(u2(1))])
disp(['x1 e x2 = ' mat2str(xHc(end,2)) ' '  mat2str(xHc(end,3))]);
% disp(['x1est_ekf e x2est_ekf = ' mat2str(xmais(2,end)) ' '  mat2str(xmais(3,end))]);
% disp(['x1est_cekf e x2est_cekf = ' mat2str(ymais(2,end)) ' '  mat2str(ymais(3,end))]);
disp(['horizonte de controle = ' mat2str(Hc)])
%disp(['Razão F1/F2 = ' mat2str(Razao)])
variaveis_normalizadas(ii,:) = [u1(1) u2(1) x_modelo(end,2)/norma_x1  x_modelo(end,3)/norma_x2];
parcelas_corr(ii,:)=[w1*F1 w2*F2 w3*F3 w4*F4];
cont = cont + Ta;
end

% %-----------------------------------------------------------------------------
% %fim do processamento
% %-----------------------------------------------------------------------------
% 
% 
% % %Exibição dos Resultados
%  
% nt = ii*tam;
% 
% %referencias
% tempo = (0:1:nt-1)/(1/Ta_int);
% sp_x1 = 0.9;
% sp_x2 = 0.1;
% x1_r = sp_x1*ones(1,length(tempo)); %utilização de referência fixa;
% x2_r = sp_x2*ones(1,length(tempo)); %utilização de referência fixa;
% 
% %vetores
% 
% x1_final = reshape(x1_planta,1,size(x1_planta,1)*size(x1_planta,2));
% x2_final = reshape(x2_planta,1,size(x2_planta,1)*size(x2_planta,2));
% u1_final = reshape(u1_armazenado,1,ii*tam);
% u2_final = reshape(u2_armazenado,1,ii*tam);
% x1_modelo_final = reshape(x1armazenado_modelo,1,size(x1armazenado_modelo,1)*size(x1armazenado_modelo,2));
% x2_modelo_final = reshape(x2armazenado_modelo,1,size(x2armazenado_modelo,1)*size(x2armazenado_modelo,2));

%tempo = 0:Ta:ii*Ta;
tempo = 0:1:ii;

%sp_x1 = 1.113;
sp_x1 = 2;
sp_x2 = 273+129;
x1_r = sp_x1*ones(1,length(tempo)); %utilização de referência fixa;
x2_r = sp_x2*ones(1,length(tempo)); %utilização de referência fixa;

figure(2)
subplot(3,2,1)
stairs(tempo,u1_armazenado_desnorm)
ylabel('F'); xlabel('k');

subplot(3,2,2)
stairs(tempo,u2_armazenado_desnorm)
ylabel('Q'); xlabel('k');

subplot(3,2,3)
plot(tempo,x1_planta,'.b',tempo,x1_r,'r',tempo,x1armazenado_modelo,'.k')
ylabel('Cb'); xlabel('k'); axis([0 tempo(end) 0 2]);
legend('Cb','Referência','modelo');


subplot(3,2,4)
plot(tempo,x2_planta,'.b',tempo,x2_r,'r',tempo,x2armazenado_modelo,'.k')
ylabel('T (k)'); xlabel('k');
legend('T','Referência','modelo');

subplot(3,2,5)
plot(tempo,bias_topo,'o')
title('Desvio modelo/planta do T')
xlabel('k');

subplot(3,2,6)
plot(tempo,bias_fundo,'o')
title('Desvio modelo/planta do Cb ')
xlabel('k');

%resultado estimadores
figure(3)
%plot(tempo(2:end),x2_planta(2:end),'b.',tempo(2:end),xfiltrado_armazenado_CEKF(3,:),'k.',tempo(2:end),xfiltrado_armazenado_EKF(3,:),'r.')
%plot(tempo,x2_planta,'b.',tempo,xfiltrado_armazenado_CEKF,'k.',tempo,xfiltrado_armazenado_EKF,'r.')
plot(tempo(2:end),x2_planta(2:end),'g',tempo,xfiltrado_armazenado_CEKF(3,:),'k.',tempo,xfiltrado_armazenado_EKF(3,:),'r.',tempo(2:end),medidas_armazenada(3,:))
ylabel('T (K)'); xlabel('k');
legend('T planta','T CEKF','T EKF','T medida')
