// P61ConReg1.sce
// Control with scalar regression model of order 2
// ------------------------------------------------
[u,t,n]=file();                         // find working directory
chdir(dirname(n(2)));                   // set working directory
clear("u","t","n")                      // clear auxiliary data
exec("ScIntro.sce",-1),mode(0)          // intro to sesion                 

nd=25;                             // number of data

// parameters of regression model
Sim.Cy.tha1=.6; 
Sim.Cy.tha2=.2; 
Sim.Cy.thb0=1; 
Sim.Cy.thb1=-.4; 
Sim.Cy.thb2=.3; 
Sim.Cy.thk=1;

a1=Sim.Cy.tha1; a2=Sim.Cy.tha2; k=Sim.Cy.thk;
b0=Sim.Cy.thb0; b1=Sim.Cy.thb1; b2=Sim.Cy.thb2;

// state-space form of regression model
Con.Cy.M=[a1 b1 a2 b2  k
           0  0  0  0  0
           1  0  0  0  0
           0  1  0  0  0
           0  0  0  0  1];
Con.Cy.N=[b0 1 0 0 0]';
nx=length(Con.Cy.N);

// penalization
om=1;             // output penalization
la=.1;            // input penalization
Om=zeros(nx,nx); 
Om(1,1)=om; Om(2,2)=la;

// setpoint
stp=sign(sin(20*(1:nd)/nd))+5;

M=Con.Cy.M; N=Con.Cy.N;

S=list();
// Time loop for optimization
R=zeros(nx,nx);
for t=nd:-1:1
  Om(1,nx)=-stp(t);
  Om(nx,1)=-stp(t);
  Om(nx,nx)=stp(t)**2;
  T=R+Om;                        // dynamic
  A=N'*T*N;                      //  programming
  B=N'*T*M;
  C=M'*T*M;
  S(t)=inv(A)*B;
  R=C-S(t)'*A*S(t);
end

// Time loop for control application
yt=zeros(1,nd); yt(1)=10; yt(2)=20;
ut=zeros(1,nd);
for t=3:nd
  x=[yt(t-1) ut(t-1) yt(t-2) ut(t-2) 1]'; // state
  ut(t)=-S(t)*x;                          // optimal input
  yt(t)=[b0 a1 b1 a2 b2 k]*[ut(t); x];    // input application
end
Con.Cy.yt=yt;
Con.Cy.ut=ut;

// Results
s=1:nd;
plot(s,yt(s),s,ut(s),'--',s,stp(s),'m.:')
title('Control with regression model')
legend('output','input','setpoint');

save _data/dataT61.dat Sim Con
