$title Example LQR: linear-quadratic optimal control

* here set the number of subintervals to the desired value (defined as a
* global parameter for the model)
$setglobal N 200

option limrow=0, limcol=0, solprint=off;

set allstages /0 * %N%/;
set states /1*2/;
set inputs /1/;

alias (states,n); alias (inputs,m);

parameter
        Q(states, states)       Hessian wrt states
        R(inputs, inputs)       Hessian wrt inputs
        A(states, states)       state transition matrix in continuous formulation
        B(states, inputs)       input transition matrix in continuous formulation
        Xinitial(states)        initial X
           /1  15, 2  5/
;

scalar Ubound /0.1/
       deltaT
;

* set interval width deltaT to be 1/N.
deltaT = 1.0 / %N%;

variable
         x(allstages, states)   state variables
         u(allstages, inputs)   input variables (controls)
         cost                   objective function
;

table Q
     1     2
1   2.0   0.0
2   0.0   1.0;

table R
     1
1   6.0;

table A
    1    2
1  0.0  1.0
2 -1.0  0.0

table B
   1
1  0.0
2  1.0;

equations
        state(allstages, states)        state transition equation
        objective                       cost function
;

state(allstages, states)$(ord(allstages) < card(allstages))..
    (x(allstages+1, states) - x(allstages, states) ) / deltaT
          =e= sum(n, A(states, n)*x(allstages,n))
             +sum(m, B(states, m)*u(allstages,m));

* objective includes a numerical integral over [0,1], plus a contribution
* from the final state
objective..
    cost =e= (1/2) * deltaT *
       ( sum(allstages$(ord(allstages) < card(allstages)),
           sum((states,n), x(allstages,states)*Q(states,n)*x(allstages,n))
     +     sum((inputs,m), u(allstages,inputs)*R(inputs,m)*u(allstages,m)) ))
;

* fix the initial values
x.fx('0',states) = Xinitial(states);

* fix lower and upper bounds
u.up(allstages, inputs) =  Ubound; u.lo(allstages, inputs) = -Ubound;

model lqr/all/;

solve lqr using nlp minimizing cost;

display Ubound; display cost.l; display x.l, u.l;

* write output file
file lqrout/lqr.out/;
put lqrout
put 'number of stages = ',card(allstages)/;
put 'optimal cost = ',cost.l:12:4//;
loop (allstages,
  loop(states,
    put x.l(allstages, states):10:4,'  ';
  );
  put /;
);
put //
loop (allstages$(ord(allstages)<card(allstages)),
  loop(inputs,
    put u.l(allstages, inputs):10:4,'  ';
  );
  put /;
);
put //;
putclose;