X-Git-Url: https://git.creatis.insa-lyon.fr/pubgit/?p=CreaPhase.git;a=blobdiff_plain;f=octave_packages%2Fm%2Foptimization%2Fqp.m;fp=octave_packages%2Fm%2Foptimization%2Fqp.m;h=4ef5db58f7e69b6bc24ed2d6afeb71ae451c26e7;hp=0000000000000000000000000000000000000000;hb=1c0469ada9531828709108a4882a751d2816994a;hpb=63de9f36673d49121015e3695f2c336ea92bc278 diff --git a/octave_packages/m/optimization/qp.m b/octave_packages/m/optimization/qp.m new file mode 100644 index 0000000..4ef5db5 --- /dev/null +++ b/octave_packages/m/optimization/qp.m @@ -0,0 +1,407 @@ +## Copyright (C) 2000-2012 Gabriele Pannocchia. +## +## This file is part of Octave. +## +## Octave is free software; you can redistribute it and/or modify it +## under the terms of the GNU General Public License as published by +## the Free Software Foundation; either version 3 of the License, or (at +## your option) any later version. +## +## Octave is distributed in the hope that it will be useful, but +## WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +## General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with Octave; see the file COPYING. If not, see +## . + +## -*- texinfo -*- +## @deftypefn {Function File} {[@var{x}, @var{obj}, @var{info}, @var{lambda}] =} qp (@var{x0}, @var{H}) +## @deftypefnx {Function File} {[@var{x}, @var{obj}, @var{info}, @var{lambda}] =} qp (@var{x0}, @var{H}, @var{q}) +## @deftypefnx {Function File} {[@var{x}, @var{obj}, @var{info}, @var{lambda}] =} qp (@var{x0}, @var{H}, @var{q}, @var{A}, @var{b}) +## @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}) +## @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}) +## @deftypefnx {Function File} {[@var{x}, @var{obj}, @var{info}, @var{lambda}] =} qp (@dots{}, @var{options}) +## Solve the quadratic program +## @tex +## $$ +## \min_x {1 \over 2} x^T H x + x^T q +## $$ +## @end tex +## @ifnottex +## +## @example +## @group +## min 0.5 x'*H*x + x'*q +## x +## @end group +## @end example +## +## @end ifnottex +## subject to +## @tex +## $$ +## Ax = b \qquad lb \leq x \leq ub \qquad A_{lb} \leq A_{in} \leq A_{ub} +## $$ +## @end tex +## @ifnottex +## +## @example +## @group +## A*x = b +## lb <= x <= ub +## A_lb <= A_in*x <= A_ub +## @end group +## @end example +## +## @end ifnottex +## @noindent +## using a null-space active-set method. +## +## Any bound (@var{A}, @var{b}, @var{lb}, @var{ub}, @var{A_lb}, +## @var{A_ub}) may be set to the empty matrix (@code{[]}) if not +## present. If the initial guess is feasible the algorithm is faster. +## +## @table @var +## @item options +## An optional structure containing the following +## parameter(s) used to define the behavior of the solver. Missing elements +## in the structure take on default values, so you only need to set the +## elements that you wish to change from the default. +## +## @table @code +## @item MaxIter (default: 200) +## Maximum number of iterations. +## @end table +## @end table +## +## @table @var +## @item info +## Structure containing run-time information about the algorithm. The +## following fields are defined: +## +## @table @code +## @item solveiter +## The number of iterations required to find the solution. +## +## @item info +## An integer indicating the status of the solution. +## +## @table @asis +## @item 0 +## The problem is feasible and convex. Global solution found. +## +## @item 1 +## The problem is not convex. Local solution found. +## +## @item 2 +## The problem is not convex and unbounded. +## +## @item 3 +## Maximum number of iterations reached. +## +## @item 6 +## The problem is infeasible. +## @end table +## @end table +## @end table +## @end deftypefn + +## PKG_ADD: ## Discard result to avoid polluting workspace with ans at startup. +## PKG_ADD: [~] = __all_opts__ ("qp"); + +function [x, obj, INFO, lambda] = qp (x0, H, varargin) + + nargs = nargin; + + if (nargin == 1 && ischar (x0) && strcmp (x0, 'defaults')) + x = optimset ("MaxIter", 200); + return; + endif + + if (nargs > 2 && isstruct (varargin{end})) + options = varargin{end}; + nargs--; + else + options = struct (); + endif + + if (nargs >= 3) + q = varargin{1}; + else + q = []; + endif + + if (nargs >= 5) + A = varargin{2}; + b = varargin{3}; + else + A = []; + b = []; + endif + + if (nargs >= 7) + lb = varargin{4}; + ub = varargin{5}; + else + lb = []; + ub = []; + endif + + if (nargs == 10) + A_lb = varargin{6}; + A_in = varargin{7}; + A_ub = varargin{8}; + else + A_lb = []; + A_in = []; + A_ub = []; + endif + + if (nargs == 2 || nargs == 3 || nargs == 5 || nargs == 7 || nargs == 10) + + maxit = optimget (options, "MaxIter", 200); + + ## Checking the quadratic penalty + if (! issquare (H)) + error ("qp: quadratic penalty matrix not square"); + elseif (! ishermitian (H)) + ## warning ("qp: quadratic penalty matrix not hermitian"); + H = (H + H')/2; + endif + n = rows (H); + + ## Checking the initial guess (if empty it is resized to the + ## right dimension and filled with 0) + if (isempty (x0)) + x0 = zeros (n, 1); + elseif (numel (x0) != n) + error ("qp: the initial guess has incorrect length"); + endif + + ## Linear penalty. + if (isempty (q)) + q = zeros (n, 1); + elseif (numel (q) != n) + error ("qp: the linear term has incorrect length"); + endif + + ## Equality constraint matrices + if (isempty (A) || isempty (b)) + A = zeros (0, n); + b = zeros (0, 1); + n_eq = 0; + else + [n_eq, n1] = size (A); + if (n1 != n) + error ("qp: equality constraint matrix has incorrect column dimension"); + endif + if (numel (b) != n_eq) + error ("qp: equality constraint matrix and vector have inconsistent dimension"); + endif + endif + + ## Bound constraints + Ain = zeros (0, n); + bin = zeros (0, 1); + n_in = 0; + if (nargs > 5) + if (! isempty (lb)) + if (numel (lb) != n) + error ("qp: lower bound has incorrect length"); + elseif (isempty (ub)) + Ain = [Ain; eye(n)]; + bin = [bin; lb]; + endif + endif + + if (! isempty (ub)) + if (numel (ub) != n) + error ("qp: upper bound has incorrect length"); + elseif (isempty (lb)) + Ain = [Ain; -eye(n)]; + bin = [bin; -ub]; + endif + endif + + if (! isempty (lb) && ! isempty (ub)) + rtol = sqrt (eps); + for i = 1:n + if (abs(lb (i) - ub(i)) < rtol*(1 + max (abs (lb(i) + ub(i))))) + ## These are actually an equality constraint + tmprow = zeros(1,n); + tmprow(i) = 1; + A = [A;tmprow]; + b = [b; 0.5*(lb(i) + ub(i))]; + n_eq = n_eq + 1; + else + tmprow = zeros(1,n); + tmprow(i) = 1; + Ain = [Ain; tmprow; -tmprow]; + bin = [bin; lb(i); -ub(i)]; + n_in = n_in + 2; + endif + endfor + endif + endif + + ## Inequality constraints + if (nargs > 7) + [dimA_in, n1] = size (A_in); + if (n1 != n) + error ("qp: inequality constraint matrix has incorrect column dimension"); + else + if (! isempty (A_lb)) + if (numel (A_lb) != dimA_in) + error ("qp: inequality constraint matrix and lower bound vector inconsistent"); + elseif (isempty (A_ub)) + Ain = [Ain; A_in]; + bin = [bin; A_lb]; + endif + endif + if (! isempty (A_ub)) + if (numel (A_ub) != dimA_in) + error ("qp: inequality constraint matrix and upper bound vector inconsistent"); + elseif (isempty (A_lb)) + Ain = [Ain; -A_in]; + bin = [bin; -A_ub]; + endif + endif + + if (! isempty (A_lb) && ! isempty (A_ub)) + rtol = sqrt (eps); + for i = 1:dimA_in + if (abs (A_lb(i) - A_ub(i)) < rtol*(1 + max (abs (A_lb(i) + A_ub(i))))) + ## These are actually an equality constraint + tmprow = A_in(i,:); + A = [A;tmprow]; + b = [b; 0.5*(A_lb(i) + A_ub(i))]; + n_eq = n_eq + 1; + else + tmprow = A_in(i,:); + Ain = [Ain; tmprow; -tmprow]; + bin = [bin; A_lb(i); -A_ub(i)]; + n_in = n_in + 2; + endif + endfor + endif + endif + endif + + ## Now we should have the following QP: + ## + ## min_x 0.5*x'*H*x + x'*q + ## s.t. A*x = b + ## Ain*x >= bin + + ## Discard inequality constraints that have -Inf bounds since those + ## will never be active. + idx = isinf (bin) & bin < 0; + + bin(idx) = []; + Ain(idx,:) = []; + + n_in = numel (bin); + + ## Check if the initial guess is feasible. + if (isa (x0, "single") || isa (H, "single") || isa (q, "single") || isa (A, "single") + || isa (b, "single")) + rtol = sqrt (eps ("single")); + else + rtol = sqrt (eps); + endif + + eq_infeasible = (n_eq > 0 && norm (A*x0-b) > rtol*(1+abs (b))); + in_infeasible = (n_in > 0 && any (Ain*x0-bin < -rtol*(1+abs (bin)))); + + info = 0; + if (eq_infeasible || in_infeasible) + ## The initial guess is not feasible. + ## First define xbar that is feasible with respect to the equality + ## constraints. + if (eq_infeasible) + if (rank (A) < n_eq) + error ("qp: equality constraint matrix must be full row rank"); + endif + xbar = pinv (A) * b; + else + xbar = x0; + endif + + ## Check if xbar is feasible with respect to the inequality + ## constraints also. + if (n_in > 0) + res = Ain * xbar - bin; + if (any (res < -rtol * (1 + abs (bin)))) + ## xbar is not feasible with respect to the inequality + ## constraints. Compute a step in the null space of the + ## equality constraints, by solving a QP. If the slack is + ## small, we have a feasible initial guess. Otherwise, the + ## problem is infeasible. + if (n_eq > 0) + Z = null (A); + if (isempty (Z)) + ## The problem is infeasible because A is square and full + ## rank, but xbar is not feasible. + info = 6; + endif + endif + + if (info != 6) + ## Solve an LP with additional slack variables to find + ## a feasible starting point. + gamma = eye (n_in); + if (n_eq > 0) + Atmp = [Ain*Z, gamma]; + btmp = -res; + else + Atmp = [Ain, gamma]; + btmp = bin; + endif + ctmp = [zeros(n-n_eq, 1); ones(n_in, 1)]; + lb = [-Inf(n-n_eq,1); zeros(n_in,1)]; + ub = []; + ctype = repmat ("L", n_in, 1); + [P, dummy, status] = glpk (ctmp, Atmp, btmp, lb, ub, ctype); + if ((status == 180 || status == 181 || status == 151) + && all (abs (P(n-n_eq+1:end)) < rtol * (1 + norm (btmp)))) + ## We found a feasible starting point + if (n_eq > 0) + x0 = xbar + Z*P(1:n-n_eq); + else + x0 = P(1:n); + endif + else + ## The problem is infeasible + info = 6; + endif + endif + else + ## xbar is feasible. We use it a starting point. + x0 = xbar; + endif + else + ## xbar is feasible. We use it a starting point. + x0 = xbar; + endif + endif + + if (info == 0) + ## The initial (or computed) guess is feasible. + ## We call the solver. + [x, lambda, info, iter] = __qp__ (x0, H, q, A, b, Ain, bin, maxit); + else + iter = 0; + x = x0; + lambda = []; + endif + obj = 0.5 * x' * H * x + q' * x; + INFO.solveiter = iter; + INFO.info = info; + + else + print_usage (); + endif + +endfunction