// 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 single control interval with the length nd
// - following a settpoint s(t)
// Experiments
// - change penalizations of input(om) and input increments (la)
// - set new setpoint s
// -------------------------------------------------------------------
clc, clear, close, mode(0)

nd=100;                            // length of control interval
a1=.6; a2=-.2; b0=1; b1=.4; b2=-.1; k=-3; sd=.1; // regression model parameters
om=0; la=.1;                      // penalization (input, increment)
s=sign(10*sin(18*(1:nd)/nd));     // setpoint generation
// conversion to state-space model
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];           // state matrix with set-point
N=[b0 1  0  0  0]'; 
Om=diag([1 om+la 0 la 0]);                 // matrix penalization
Om(2,4)=-la; Om(4,2)=-la;

S=list();
R=list();
R(nd+1)=zeros(Om);                 // initial condition for dyn. progr.

// CONTROL
// computation of control-law
for t=nd:-1:2
  Om(1,$)=-s(t); Om($,1)=-s(t); Om($,$)=s(t)**2;
  T=R(t+1)+Om;
  A=N'*T*N;
  B=N'*T*M;
  C=M'*T*M;
  S(t)=inv(A)*B;
  R(t)=C-S(t)'*A*S(t);
end

// control-law realization
y(1)=5; y(2)=-1;
u(1)=0; u(2)=0;
for t=3:nd
  u(t)=-S(t)*[y(t-1) u(t-1) y(t-2) u(t-2) 1]';     // optimaal control
  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
end

// RESULTS
x=1:nd;
plot(x,y(x),'--',x,u(x),x,s(x),':')
legend('y','u','s');
