octave_packages/m/optimization/qp.m

   1 ## Copyright (C) 2000-2012 Gabriele Pannocchia.
   2 ##
   3 ## This file is part of Octave.
   4 ##
   5 ## Octave is free software; you can redistribute it and/or modify it
   6 ## under the terms of the GNU General Public License as published by
   7 ## the Free Software Foundation; either version 3 of the License, or (at
   8 ## your option) any later version.
   9 ##
  10 ## Octave is distributed in the hope that it will be useful, but
  11 ## WITHOUT ANY WARRANTY; without even the implied warranty of
  12 ## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13 ## General Public License for more details.
  14 ##
  15 ## You should have received a copy of the GNU General Public License
  16 ## along with Octave; see the file COPYING.  If not, see
  17 ## <http://www.gnu.org/licenses/>.
  18
  19 ## -*- texinfo -*-
  20 ## @deftypefn  {Function File} {[@var{x}, @var{obj}, @var{info}, @var{lambda}] =} qp (@var{x0}, @var{H})
  21 ## @deftypefnx {Function File} {[@var{x}, @var{obj}, @var{info}, @var{lambda}] =} qp (@var{x0}, @var{H}, @var{q})
  22 ## @deftypefnx {Function File} {[@var{x}, @var{obj}, @var{info}, @var{lambda}] =} qp (@var{x0}, @var{H}, @var{q}, @var{A}, @var{b})
  23 ## @deftypefnx {Function File} {[@var{x}, @var{obj}, @var{info}, @var{lambda}] =} qp (@var{x0}, @var{H}, @var{q}, @var{A}, @var{b}, @var{lb}, @var{ub})
  24 ## @deftypefnx {Function File} {[@var{x}, @var{obj}, @var{info}, @var{lambda}] =} qp (@var{x0}, @var{H}, @var{q}, @var{A}, @var{b}, @var{lb}, @var{ub}, @var{A_lb}, @var{A_in}, @var{A_ub})
  25 ## @deftypefnx {Function File} {[@var{x}, @var{obj}, @var{info}, @var{lambda}] =} qp (@dots{}, @var{options})
  26 ## Solve the quadratic program
  27 ## @tex
  28 ## $$
  29 ##  \min_x {1 \over 2} x^T H x + x^T q
  30 ## $$
  31 ## @end tex
  32 ## @ifnottex
  33 ##
  34 ## @example
  35 ## @group
  36 ## min 0.5 x'*H*x + x'*q
  37 ##  x
  38 ## @end group
  39 ## @end example
  40 ##
  41 ## @end ifnottex
  42 ## subject to
  43 ## @tex
  44 ## $$
  45 ##  Ax = b \qquad lb \leq x \leq ub \qquad A_{lb} \leq A_{in} \leq A_{ub}
  46 ## $$
  47 ## @end tex
  48 ## @ifnottex
  49 ##
  50 ## @example
  51 ## @group
  52 ## A*x = b
  53 ## lb <= x <= ub
  54 ## A_lb <= A_in*x <= A_ub
  55 ## @end group
  56 ## @end example
  57 ##
  58 ## @end ifnottex
  59 ## @noindent
  60 ## using a null-space active-set method.
  61 ##
  62 ## Any bound (@var{A}, @var{b}, @var{lb}, @var{ub}, @var{A_lb},
  63 ## @var{A_ub}) may be set to the empty matrix (@code{[]}) if not
  64 ## present.  If the initial guess is feasible the algorithm is faster.
  65 ##
  66 ## @table @var
  67 ## @item options
  68 ## An optional structure containing the following
  69 ## parameter(s) used to define the behavior of the solver.  Missing elements
  70 ## in the structure take on default values, so you only need to set the
  71 ## elements that you wish to change from the default.
  72 ##
  73 ## @table @code
  74 ## @item MaxIter (default: 200)
  75 ## Maximum number of iterations.
  76 ## @end table
  77 ## @end table
  78 ##
  79 ## @table @var
  80 ## @item info
  81 ## Structure containing run-time information about the algorithm.  The
  82 ## following fields are defined:
  83 ##
  84 ## @table @code
  85 ## @item solveiter
  86 ## The number of iterations required to find the solution.
  87 ##
  88 ## @item info
  89 ## An integer indicating the status of the solution.
  90 ##
  91 ## @table @asis
  92 ## @item 0
  93 ## The problem is feasible and convex.  Global solution found.
  94 ##
  95 ## @item 1
  96 ## The problem is not convex.  Local solution found.
  97 ##
  98 ## @item 2
  99 ## The problem is not convex and unbounded.
 100 ##
 101 ## @item 3
 102 ## Maximum number of iterations reached.
 103 ##
 104 ## @item 6
 105 ## The problem is infeasible.
 106 ## @end table
 107 ## @end table
 108 ## @end table
 109 ## @end deftypefn
 110
 111 ## PKG_ADD: ## Discard result to avoid polluting workspace with ans at startup.
 112 ## PKG_ADD: [~] = __all_opts__ ("qp");
 113
 114 function [x, obj, INFO, lambda] = qp (x0, H, varargin)
 115
 116   nargs = nargin;
 117
 118   if (nargin == 1 && ischar (x0) && strcmp (x0, 'defaults'))
 119     x = optimset ("MaxIter", 200);
 120     return;
 121   endif
 122
 123   if (nargs > 2 && isstruct (varargin{end}))
 124     options = varargin{end};
 125     nargs--;
 126   else
 127     options = struct ();
 128   endif
 129
 130   if (nargs >= 3)
 131     q = varargin{1};
 132   else
 133     q = [];
 134   endif
 135
 136   if (nargs >= 5)
 137     A = varargin{2};
 138     b = varargin{3};
 139   else
 140     A = [];
 141     b = [];
 142   endif
 143
 144   if (nargs >= 7)
 145     lb = varargin{4};
 146     ub = varargin{5};
 147   else
 148     lb = [];
 149     ub = [];
 150   endif
 151
 152   if (nargs == 10)
 153     A_lb = varargin{6};
 154     A_in = varargin{7};
 155     A_ub = varargin{8};
 156   else
 157     A_lb = [];
 158     A_in = [];
 159     A_ub = [];
 160   endif
 161
 162   if (nargs == 2 || nargs == 3 || nargs == 5 || nargs == 7 || nargs == 10)
 163
 164     maxit = optimget (options, "MaxIter", 200);
 165
 166     ## Checking the quadratic penalty
 167     if (! issquare (H))
 168       error ("qp: quadratic penalty matrix not square");
 169     elseif (! ishermitian (H))
 170       ## warning ("qp: quadratic penalty matrix not hermitian");
 171       H = (H + H')/2;
 172     endif
 173     n = rows (H);
 174
 175     ## Checking the initial guess (if empty it is resized to the
 176     ## right dimension and filled with 0)
 177     if (isempty (x0))
 178       x0 = zeros (n, 1);
 179     elseif (numel (x0) != n)
 180       error ("qp: the initial guess has incorrect length");
 181     endif
 182
 183     ## Linear penalty.
 184     if (isempty (q))
 185       q = zeros (n, 1);
 186     elseif (numel (q) != n)
 187       error ("qp: the linear term has incorrect length");
 188     endif
 189
 190     ## Equality constraint matrices
 191     if (isempty (A) || isempty (b))
 192       A = zeros (0, n);
 193       b = zeros (0, 1);
 194       n_eq = 0;
 195     else
 196       [n_eq, n1] = size (A);
 197       if (n1 != n)
 198         error ("qp: equality constraint matrix has incorrect column dimension");
 199       endif
 200       if (numel (b) != n_eq)
 201         error ("qp: equality constraint matrix and vector have inconsistent dimension");
 202       endif
 203     endif
 204
 205     ## Bound constraints
 206     Ain = zeros (0, n);
 207     bin = zeros (0, 1);
 208     n_in = 0;
 209     if (nargs > 5)
 210       if (! isempty (lb))
 211         if (numel (lb) != n)
 212           error ("qp: lower bound has incorrect length");
 213         elseif (isempty (ub))
 214           Ain = [Ain; eye(n)];
 215           bin = [bin; lb];
 216         endif
 217       endif
 218
 219       if (! isempty (ub))
 220         if (numel (ub) != n)
 221           error ("qp: upper bound has incorrect length");
 222         elseif (isempty (lb))
 223           Ain = [Ain; -eye(n)];
 224           bin = [bin; -ub];
 225         endif
 226       endif
 227
 228       if (! isempty (lb) && ! isempty (ub))
 229         rtol = sqrt (eps);
 230         for i = 1:n
 231           if (abs(lb (i) - ub(i)) < rtol*(1 + max (abs (lb(i) + ub(i)))))
 232             ## These are actually an equality constraint
 233             tmprow = zeros(1,n);
 234             tmprow(i) = 1;
 235             A = [A;tmprow];
 236             b = [b; 0.5*(lb(i) + ub(i))];
 237             n_eq = n_eq + 1;
 238           else
 239             tmprow = zeros(1,n);
 240             tmprow(i) = 1;
 241             Ain = [Ain; tmprow; -tmprow];
 242             bin = [bin; lb(i); -ub(i)];
 243             n_in = n_in + 2;
 244           endif
 245         endfor
 246       endif
 247     endif
 248
 249     ## Inequality constraints
 250     if (nargs > 7)
 251       [dimA_in, n1] = size (A_in);
 252       if (n1 != n)
 253         error ("qp: inequality constraint matrix has incorrect column dimension");
 254       else
 255         if (! isempty (A_lb))
 256           if (numel (A_lb) != dimA_in)
 257             error ("qp: inequality constraint matrix and lower bound vector inconsistent");
 258           elseif (isempty (A_ub))
 259             Ain = [Ain; A_in];
 260             bin = [bin; A_lb];
 261           endif
 262         endif
 263         if (! isempty (A_ub))
 264           if (numel (A_ub) != dimA_in)
 265             error ("qp: inequality constraint matrix and upper bound vector inconsistent");
 266           elseif (isempty (A_lb))
 267             Ain = [Ain; -A_in];
 268             bin = [bin; -A_ub];
 269           endif
 270         endif
 271
 272         if (! isempty (A_lb) && ! isempty (A_ub))
 273           rtol = sqrt (eps);
 274           for i = 1:dimA_in
 275             if (abs (A_lb(i) - A_ub(i)) < rtol*(1 + max (abs (A_lb(i) + A_ub(i)))))
 276               ## These are actually an equality constraint
 277               tmprow = A_in(i,:);
 278               A = [A;tmprow];
 279               b = [b; 0.5*(A_lb(i) + A_ub(i))];
 280               n_eq = n_eq + 1;
 281             else
 282               tmprow = A_in(i,:);
 283               Ain = [Ain; tmprow; -tmprow];
 284               bin = [bin; A_lb(i); -A_ub(i)];
 285               n_in = n_in + 2;
 286             endif
 287           endfor
 288         endif
 289       endif
 290     endif
 291
 292     ## Now we should have the following QP:
 293     ##
 294     ##   min_x  0.5*x'*H*x + x'*q
 295     ##   s.t.   A*x = b
 296     ##          Ain*x >= bin
 297
 298     ## Discard inequality constraints that have -Inf bounds since those
 299     ## will never be active.
 300     idx = isinf (bin) & bin < 0;
 301
 302     bin(idx) = [];
 303     Ain(idx,:) = [];
 304
 305     n_in = numel (bin);
 306
 307     ## Check if the initial guess is feasible.
 308     if (isa (x0, "single") || isa (H, "single") || isa (q, "single") || isa (A, "single")
 309         || isa (b, "single"))
 310       rtol = sqrt (eps ("single"));
 311     else
 312       rtol = sqrt (eps);
 313     endif
 314
 315     eq_infeasible = (n_eq > 0 && norm (A*x0-b) > rtol*(1+abs (b)));
 316     in_infeasible = (n_in > 0 && any (Ain*x0-bin < -rtol*(1+abs (bin))));
 317
 318     info = 0;
 319     if (eq_infeasible || in_infeasible)
 320       ## The initial guess is not feasible.
 321       ## First define xbar that is feasible with respect to the equality
 322       ## constraints.
 323       if (eq_infeasible)
 324         if (rank (A) < n_eq)
 325           error ("qp: equality constraint matrix must be full row rank");
 326         endif
 327         xbar = pinv (A) * b;
 328       else
 329         xbar = x0;
 330       endif
 331
 332       ## Check if xbar is feasible with respect to the inequality
 333       ## constraints also.
 334       if (n_in > 0)
 335         res = Ain * xbar - bin;
 336         if (any (res < -rtol * (1 + abs (bin))))
 337           ## xbar is not feasible with respect to the inequality
 338           ## constraints.  Compute a step in the null space of the
 339           ## equality constraints, by solving a QP.  If the slack is
 340           ## small, we have a feasible initial guess.  Otherwise, the
 341           ## problem is infeasible.
 342           if (n_eq > 0)
 343             Z = null (A);
 344             if (isempty (Z))
 345               ## The problem is infeasible because A is square and full
 346               ## rank, but xbar is not feasible.
 347               info = 6;
 348             endif
 349           endif
 350
 351           if (info != 6)
 352             ## Solve an LP with additional slack variables to find
 353             ## a feasible starting point.
 354             gamma = eye (n_in);
 355             if (n_eq > 0)
 356               Atmp = [Ain*Z, gamma];
 357               btmp = -res;
 358             else
 359               Atmp = [Ain, gamma];
 360               btmp = bin;
 361             endif
 362             ctmp = [zeros(n-n_eq, 1); ones(n_in, 1)];
 363             lb = [-Inf(n-n_eq,1); zeros(n_in,1)];
 364             ub = [];
 365             ctype = repmat ("L", n_in, 1);
 366             [P, dummy, status] = glpk (ctmp, Atmp, btmp, lb, ub, ctype);
 367             if ((status == 180 || status == 181 || status == 151)
 368                 && all (abs (P(n-n_eq+1:end)) < rtol * (1 + norm (btmp))))
 369               ## We found a feasible starting point
 370               if (n_eq > 0)
 371                 x0 = xbar + Z*P(1:n-n_eq);
 372               else
 373                 x0 = P(1:n);
 374               endif
 375             else
 376               ## The problem is infeasible
 377               info = 6;
 378             endif
 379           endif
 380         else
 381           ## xbar is feasible.  We use it a starting point.
 382           x0 = xbar;
 383         endif
 384       else
 385         ## xbar is feasible.  We use it a starting point.
 386         x0 = xbar;
 387       endif
 388     endif
 389
 390     if (info == 0)
 391       ## The initial (or computed) guess is feasible.
 392       ## We call the solver.
 393       [x, lambda, info, iter] = __qp__ (x0, H, q, A, b, Ain, bin, maxit);
 394     else
 395       iter = 0;
 396       x = x0;
 397       lambda = [];
 398     endif
 399     obj = 0.5 * x' * H * x + q' * x;
 400     INFO.solveiter = iter;
 401     INFO.info = info;
 402
 403   else
 404     print_usage ();
 405   endif
 406
 407 endfunction