%
%
%  Open loop optimization of discrete-time system
%  y(k+1) = 1.1*y(k) - 0.1*u(k)
%
%
function fmincon_test ()

  close all; 

  N =  6;     % Prediction horizon
  y0 = 2;     % Initial condition of the system

  Q = 1.6;    % Weighting matrix y (control error, setpoint y = 0)
  R = 0.2;    % Weighting matrix u (control energy)
  
  u_init = 0.1*ones (N,1);  % Initial guess for input (control) variable
  y_init = [ y0; zeros(N,1) ];
  for k = 2:N+1
    % Calculate initial guess for y(k) by menas of system equation
    y_init(k) = system (y_init(k-1), u_init(k-1)); 
  end

  x_init = [ y_init; u_init ];  % Initial guess of all variables that have
                                % to be optimized
 
  lb = [];    % Unbounded
  ub = [];

  A = [];     % No inequality constraints
  b = [];

  Aeq = [1 zeros(1,2*N)];  % Initial Condition must be
  beq = y0;                 % part of the solution
  if exist ('OCTAVE_HOME')
    Aeq = Aeq';
  end

  % Optimization
  [x_opt, obj_opt] = fmincon (@(x) obj(x,Q,R,N), x_init,...
                                A, b, Aeq, beq, lb, ub,...
                                @(x) nonlcon(x,N));

  % Test solution for constraints
  [c, ceq] = nonlcon (x_opt, N);
  equality_constrainsts = ceq'
                                
  % Output of the results divided in output y and input u
  y_opt = x_opt(1:N+1,1)
  u_opt = x_opt(N+2:2*N+1,1)

  figure (1)
  clf ()
  hold on 
  grid on
  stairs (0:N,y_opt, 'linewidth',2);
  stairs (0:N-1,u_opt, 'linewidth',2);
  legend ('y_opt','u_opt');

end


% Objective function
function J = obj (x, Q, R, N)
  
  y = x(1:N+1,1);
  u = x(N+2:2*N+1);
  J = y'*Q*y + u'*R*u;

end


% Nonlinear constrains (linear in this case)
function [c, ceq] = nonlcon (x, N)

  c = [];
  ceq = [];

  y = x(1:N+1,1);
  u = x(N+2:2*N+1);

  for k=1:N
    ceq_new = y(k+1) - system(y(k),u(k));
    ceq = [ ceq  ceq_new ];
  end

end


% Discrete-time system equation
function yk1 = system (yk, uk)
  
  yk1 = 1.1*yk - 0.1*uk;
  
end
