// Control with scalar 1st order regression model
// - simulated data  y(t)=a*y(t-1)+b*u(t)+k+e(t);
// - state realization of the model for synthesis
// - control on a receding control interval with the length nh
// - following a settpoint s(t)
// Experiments
// - change setpoint and system parameters (slow - quick system)
// - penalization of input variable
// - initial condition for estimation (better or worse initial param.)
// - length of control interval nh
// -------------------------------------------------------------------
clc, clear, close, mode(0)

nd=100;                            // number of data to be controlled
ni=20;                             // length of pre-etimation
nh=15;                             // length of control interval
a1=.6;  a2=.2;  b0=1;   b1=-.4;  b2=.1; k=-3;  // parameters for simulation
sd=.1;                             // stdev for simulation
om=.01; la=.001;                   // penalization of input / increments

// PRE-ESTIMATION
V=1e-8*eye(7,7);                   // initial information matrix
ui(1:2)=zeros(1,2); yi(1:2)=zeros(1,2);
for t=3:ni
  ui(t)=rand(1,1,'n');
  yi(t)=a1*yi(t-1)+a2*yi(t-2)+b0*ui(t)+b1*ui(t-1)+b2*ui(t-2)+k+.01*rand(1,1,'n');
  Ps=[yi(t) yi(t-1) yi(t-2) ui(t) ui(t-1) ui(t-2) 1]';
  V=V+Ps*Ps';
end
Vyp=V(2:$,1); Vp=V(2:$,2:$); thi=inv(Vp)*Vyp; // point estimates
a1E=thi(1); a2E=thi(2); b0E=thi(3); b1E=thi(4); b2E=thi(5); kE=thi(6);     
thi=[a1E a2E b0E b1E b2E kE]; 

s=sign(100*sin(18*(1:nd+nh)/(nd+nh)));  // set-point
y(1)=1; y(2)=-1;                   // initial output
u(1)=0; u(2)=0;                    // initial control

Om=diag([1 om+la 0 la 0]);         // matrix penalization
Om(2,4)=-la; Om(4,2)=-la;
M=[a1E b1E a2E b2E kE 
   0   0   0   0   0
   1   0   0   0   0
   0   1   0   0   0
   0   0   0   0   1];             // state-space model
N=[b0E 1   0   0   0]';            // state-space model

// COMPUTATION OF CONTROL-LAW
for t=3:nd          // loop for control
  R=0;                             // initial condition for dyn.prog.
  for i=nh:-1:1     // loop for receding horizon
    Om(1,$)=-s(t+i-1); 
    Om($,1)=-s(t+i-1); 
    Om($,$)=s(t+i-1)**2;
    T=R+Om;                        // dynamic
    A=N'*T*N;                      //    programming
    B=N'*T*M;
    C=M'*T*M;
    S=inv(A)*B;
    R=C-S'*A*S;
  end

// CONTROL REALIZATION (simulation)
  u(t)=-S*[y(t-1) u(t-1) y(t-2) u(t-2) 1]';             // optimal control value
  y(t)=a1*y(t-1)+a2*y(t-2)+b0*u(t)+b1*u(t-1)+b2*u(t-2)+k+sd*rand(1,1,'n');   // simulation

// ESTIMATION
  Ps=[y(t) y(t-1) y(t-2) u(t) u(t-1) u(t-2) 1]';
  V=V+Ps*Ps';
  Vyp=V(2:$,1);
  Vp=V(2:$,2:$);
  th=inv(Vp)*Vyp;                  // point estimates
  a1E=th(1);                       // regression
  a2E=th(2);                       //    coefficients
  b0E=th(3); 
  b1E=th(4);
  b2E=th(5);
  kE=th(6); 
end            // of loop for control

// RESULTS
th=[a1 a2 b0 b1 b2 k];             // simulated parameters
thE=[a1E a2E b0E b1E b2E kE];      // estimated parameters
z=1:nd;
set(scf(),'position',[900 50 600 500])
plot(z,y(z),'--',z,u(z),z,s(z),':')
legend('y','u','s');

disp(th,'simulated parametrs')
disp(thi,'initial parametrs')
disp(thE,'estimated parametrs')
