// P61ConRegAd.sce
// ADAPTIVE CONTROL WITH DYNAMIC REG. MODEL WITH CONSTANT                  
//   multivariate input and output
//   control with setpoint
//   penalization of input increments
//   receding horizon in control
// ------------------------------------------------
//chdir ...
[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                 
rand('seed',2);

// Setting the task
ni=150;             // length of prior estimation
nd=200;             // length of control
I_typU=2;           // type of input 1=noise, 2=sin, 3=jumps
I_adapt=1;          // adaptive control  1=yes, 0=no,

// Setting the simulation
Sim.Cy.ord=2;       // model order  
Sim.Cy.th(1).a=[.3 -.2; -.05 -.3]; // a1
Sim.Cy.th(2).a=[.1 -.1; .02 -.1];  // a2
Sim.Cy.thb0=[1 -.4; 1.3 -.4];      // b0
Sim.Cy.th(1).b=[.1 0;0 .2];        // b1
Sim.Cy.th(2).b=[.1 0;0 .1];        // b2
Sim.Cy.thk=[1; -1];                // k (constant)
Sim.Cy.sd=.5;                      // noise stdev

ord=Sim.Cy.ord;
ny=size(Sim.Cy.th(1).a,1);         // dimension of y
nu=size(Sim.Cy.th(1).b,2)/(1+ord); // dimension of u

th=Sim.Cy.thb0;
for i=1:ord         // parameters -> vector th
  th=[th Sim.Cy.th(i).a Sim.Cy.th(i).b];
end
th=[th Sim.Cy.thk];
k=Sim.Cy.thk;
sd=Sim.Cy.sd;

Con.Cy.nh=3;        // length of control interval
Con.Cy.om=1;        // penalty:  y(t)^2 
Con.Cy.la=.01;      // penalty: (u(t)-u(t-1))^2 
nh=Con.Cy.nh;

[XX,XX,XX,nx,ny,nu]=reg2st(th,ord);     //conversion to state model
                    
Om=zeros(nx,nx);                        // penalization matrices
Om(1:ny,1:ny)=Con.Cy.om*eye(ny,ny);     // penal. of output
nn=ny+nu;
for i=1:nu                              // penal. of input increment
  Om((ny+i),(ny+i))=Con.Cy.la;          //  (only for ord>1 !!)    
  Om((ny+i+nn),(ny+i+nn))=Con.Cy.la;
  Om((ny+i),(ny+i+nn))=-Con.Cy.la;  
  Om((ny+i+nn),(ny+i))=-Con.Cy.la;
end

stp=genstp(ny,nd,[3;5],[.95;.92],[-5;10]); // setpoint generation

// Prior estimation
yi=zeros(ny,ni);
select I_typU                 // selection of input signal
case 1, ui=rand(nu,ni,'norm');
case 2, ui=[sin(20*(1:ni)/ni);cos(20*(1:ni)/ni)];
case 3, ui=sign([sin(20*(1:ni)/ni);cos(20*(1:ni)/ni)]);
end
V=zeros(nx+ny+nu,nx+ny+nu);
for i=(ord+1):ni              // loop for inition estimation
  ps=genps(ord,i,yi,ui);
  yi(:,i)=th*ps + sd*rand(ny,1,'norm');
  Ps=[yi(:,i); ps];
  V=V+Ps*Ps';
end
theta=v2thN(V/ni,ny);         // initial point estimate of parameters

//  TIME LOOP for adaptive control ==
S=list();
y=zeros(ny,nd); y(:,1:2)=[10 10;-10 -10];
u=zeros(nu,nd);
R=.00001*eye(nx,nx);    // regularization (for first inversion)
for t=(ord+2):(nd-nh)
  // estimation
  if I_adapt==1
    ps=genps(ord,t-1,y,u);
    Ps=[y(:,t-1); ps];
    V=V+Ps*Ps';
    theta=v2thN(V/(t+ni),ny); // current point estimates
  elseif I_adapt==0
    theta=th'; // variant for known parameters
  else     
    // prior pt.estimates are used
  end
  [M,N,A,nx,ny,nu]=reg2st(theta',ord);
  // generation of control law
  for i=nh:-1:1
    st=stp(:,t+i-1);
    Om(1:ny,nx)=-st;
    Om(nx,1:ny)=-st;
    Om(nx,nx)=st'*st;
    T=R+Om;
    A=N'*T*N;
    B=N'*T*M;
    C=M'*T*M;
    S(t)=inv(A)*B;
    R=C-S(t)'*A*S(t);
  end
  // generation of optimal control
  x=genph(ord,t,y,u);         // construction of state vector
  u(:,t)=-S(t)*x;             // new control variable
  y(:,t)=th*[u(:,t); x]+sd*rand(ny,1,'norm'); // application of new u
end

// Results
s=1:(nd-nh);
subplot(211)
plot(s,y(1,s),s,u(1,s),'--',s,stp(1,s),'m.','markersize',2)
title('Control process')
xlabel('First output')
legend('output','input','setpoint',4);
subplot(212)
plot(s,y(2,s),s,u(2,s),'--',s,stp(2,s),'c.','markersize',2)
set(gcf(),'position',[500 50 600 500])
xlabel('Second output')
legend('output','input','setpoint',1);
