octave_packages/m/optimization/sqp.m

   1 ## Copyright (C) 2005-2012 John W. Eaton
   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{iter}, @var{nf}, @var{lambda}] =} sqp (@var{x0}, @var{phi})
  21 ## @deftypefnx {Function File} {[@dots{}] =} sqp (@var{x0}, @var{phi}, @var{g})
  22 ## @deftypefnx {Function File} {[@dots{}] =} sqp (@var{x0}, @var{phi}, @var{g}, @var{h})
  23 ## @deftypefnx {Function File} {[@dots{}] =} sqp (@var{x0}, @var{phi}, @var{g}, @var{h}, @var{lb}, @var{ub})
  24 ## @deftypefnx {Function File} {[@dots{}] =} sqp (@var{x0}, @var{phi}, @var{g}, @var{h}, @var{lb}, @var{ub}, @var{maxiter})
  25 ## @deftypefnx {Function File} {[@dots{}] =} sqp (@var{x0}, @var{phi}, @var{g}, @var{h}, @var{lb}, @var{ub}, @var{maxiter}, @var{tol})
  26 ## Solve the nonlinear program
  27 ## @tex
  28 ## $$
  29 ## \min_x \phi (x)
  30 ## $$
  31 ## @end tex
  32 ## @ifnottex
  33 ##
  34 ## @example
  35 ## @group
  36 ## min phi (x)
  37 ##  x
  38 ## @end group
  39 ## @end example
  40 ##
  41 ## @end ifnottex
  42 ## subject to
  43 ## @tex
  44 ## $$
  45 ##  g(x) = 0 \qquad h(x) \geq 0 \qquad lb \leq x \leq ub
  46 ## $$
  47 ## @end tex
  48 ## @ifnottex
  49 ##
  50 ## @example
  51 ## @group
  52 ## g(x)  = 0
  53 ## h(x) >= 0
  54 ## lb <= x <= ub
  55 ## @end group
  56 ## @end example
  57 ##
  58 ## @end ifnottex
  59 ## @noindent
  60 ## using a sequential quadratic programming method.
  61 ##
  62 ## The first argument is the initial guess for the vector @var{x0}.
  63 ##
  64 ## The second argument is a function handle pointing to the objective
  65 ## function @var{phi}.  The objective function must accept one vector
  66 ## argument and return a scalar.
  67 ##
  68 ## The second argument may also be a 2- or 3-element cell array of
  69 ## function handles.  The first element should point to the objective
  70 ## function, the second should point to a function that computes the
  71 ## gradient of the objective function, and the third should point to a
  72 ## function that computes the Hessian of the objective function.  If the
  73 ## gradient function is not supplied, the gradient is computed by finite
  74 ## differences.  If the Hessian function is not supplied, a BFGS update
  75 ## formula is used to approximate the Hessian.
  76 ##
  77 ## When supplied, the gradient function @code{@var{phi}@{2@}} must accept
  78 ## one vector argument and return a vector.  When supplied, the Hessian
  79 ## function @code{@var{phi}@{3@}} must accept one vector argument and
  80 ## return a matrix.
  81 ##
  82 ## The third and fourth arguments @var{g} and @var{h} are function
  83 ## handles pointing to functions that compute the equality constraints
  84 ## and the inequality constraints, respectively.  If the problem does
  85 ## not have equality (or inequality) constraints, then use an empty
  86 ## matrix ([]) for @var{g} (or @var{h}).  When supplied, these equality
  87 ## and inequality constraint functions must accept one vector argument
  88 ## and return a vector.
  89 ##
  90 ## The third and fourth arguments may also be 2-element cell arrays of
  91 ## function handles.  The first element should point to the constraint
  92 ## function and the second should point to a function that computes the
  93 ## gradient of the constraint function:
  94 ## @tex
  95 ## $$
  96 ##  \Bigg( {\partial f(x) \over \partial x_1},
  97 ##         {\partial f(x) \over \partial x_2}, \ldots,
  98 ##         {\partial f(x) \over \partial x_N} \Bigg)^T
  99 ## $$
 100 ## @end tex
 101 ## @ifnottex
 102 ##
 103 ## @example
 104 ## @group
 105 ##             [ d f(x)   d f(x)        d f(x) ]
 106 ## transpose ( [ ------   -----   ...   ------ ] )
 107 ##             [  dx_1     dx_2          dx_N  ]
 108 ## @end group
 109 ## @end example
 110 ##
 111 ## @end ifnottex
 112 ## The fifth and sixth arguments, @var{lb} and @var{ub}, contain lower
 113 ## and upper bounds on @var{x}.  These must be consistent with the
 114 ## equality and inequality constraints @var{g} and @var{h}.  If the
 115 ## arguments are vectors then @var{x}(i) is bound by @var{lb}(i) and
 116 ## @var{ub}(i).  A bound can also be a scalar in which case all elements
 117 ## of @var{x} will share the same bound.  If only one bound (lb, ub) is
 118 ## specified then the other will default to (-@var{realmax},
 119 ## +@var{realmax}).
 120 ##
 121 ## The seventh argument @var{maxiter} specifies the maximum number of
 122 ## iterations.  The default value is 100.
 123 ##
 124 ## The eighth argument @var{tol} specifies the tolerance for the
 125 ## stopping criteria.  The default value is @code{sqrt(eps)}.
 126 ##
 127 ## The value returned in @var{info} may be one of the following:
 128 ##
 129 ## @table @asis
 130 ## @item 101
 131 ## The algorithm terminated normally.
 132 ## Either all constraints meet the requested tolerance, or the stepsize,
 133 ## @tex
 134 ## $\Delta x,$
 135 ## @end tex
 136 ## @ifnottex
 137 ## delta @var{x},
 138 ## @end ifnottex
 139 ## is less than @code{@var{tol} * norm (x)}.
 140 ##
 141 ## @item 102
 142 ## The BFGS update failed.
 143 ##
 144 ## @item 103
 145 ## The maximum number of iterations was reached.
 146 ## @end table
 147 ##
 148 ## An example of calling @code{sqp}:
 149 ##
 150 ## @example
 151 ## function r = g (x)
 152 ##   r = [ sumsq(x)-10;
 153 ##         x(2)*x(3)-5*x(4)*x(5);
 154 ##         x(1)^3+x(2)^3+1 ];
 155 ## endfunction
 156 ##
 157 ## function obj = phi (x)
 158 ##   obj = exp (prod (x)) - 0.5*(x(1)^3+x(2)^3+1)^2;
 159 ## endfunction
 160 ##
 161 ## x0 = [-1.8; 1.7; 1.9; -0.8; -0.8];
 162 ##
 163 ## [x, obj, info, iter, nf, lambda] = sqp (x0, @@phi, @@g, [])
 164 ##
 165 ## x =
 166 ##
 167 ##   -1.71714
 168 ##    1.59571
 169 ##    1.82725
 170 ##   -0.76364
 171 ##   -0.76364
 172 ##
 173 ## obj = 0.053950
 174 ## info = 101
 175 ## iter = 8
 176 ## nf = 10
 177 ## lambda =
 178 ##
 179 ##   -0.0401627
 180 ##    0.0379578
 181 ##   -0.0052227
 182 ## @end example
 183 ##
 184 ## @seealso{qp}
 185 ## @end deftypefn
 186
 187 function [x, obj, info, iter, nf, lambda] = sqp (x0, objf, cef, cif, lb, ub, maxiter, tolerance)
 188
 189   global __sqp_nfun__;
 190   global __sqp_obj_fun__;
 191   global __sqp_ce_fun__;
 192   global __sqp_ci_fun__;
 193   global __sqp_cif__;
 194   global __sqp_cifcn__;
 195
 196   if (nargin < 2 || nargin > 8 || nargin == 5)
 197     print_usage ();
 198   endif
 199
 200   if (!isvector (x0))
 201     error ("sqp: X0 must be a vector");
 202   endif
 203   if (rows (x0) == 1)
 204     x0 = x0';
 205   endif
 206
 207   obj_grd = @fd_obj_grd;
 208   have_hess = 0;
 209   if (iscell (objf))
 210     switch (numel (objf))
 211      case 1
 212        obj_fun = objf{1};
 213      case 2
 214        obj_fun = objf{1};
 215        obj_grd = objf{2};
 216      case 3
 217        obj_fun = objf{1};
 218        obj_grd = objf{2};
 219        obj_hess = objf{3};
 220        have_hess = 1;
 221      otherwise
 222       error ("sqp: invalid objective function specification");
 223     endswitch
 224   else
 225     obj_fun = objf;   # No cell array, only obj_fun set
 226   endif
 227   __sqp_obj_fun__ = obj_fun;
 228
 229   ce_fun = @empty_cf;
 230   ce_grd = @empty_jac;
 231   if (nargin > 2)
 232     ce_grd = @fd_ce_jac;
 233     if (iscell (cef))
 234       switch (numel (cef))
 235        case 1
 236          ce_fun = cef{1};
 237        case 2
 238          ce_fun = cef{1};
 239          ce_grd = cef{2};
 240        otherwise
 241          error ("sqp: invalid equality constraint function specification");
 242       endswitch
 243     elseif (! isempty (cef))
 244       ce_fun = cef;   # No cell array, only constraint equality function set
 245     endif
 246   endif
 247   __sqp_ce_fun__ = ce_fun;
 248
 249   ci_fun = @empty_cf;
 250   ci_grd = @empty_jac;
 251   if (nargin > 3)
 252     ## constraint function given by user with possible gradient
 253     __sqp_cif__ = cif;
 254     ## constraint function given by user without gradient
 255     __sqp_cifcn__ = @empty_cf;
 256     if (iscell (cif))
 257       if (length (cif) > 0)
 258         __sqp_cifcn__ = cif{1};
 259       endif
 260     elseif (! isempty (cif))
 261       __sqp_cifcn__ = cif;
 262     endif
 263
 264     if (nargin < 5 || (nargin > 5 && isempty (lb) && isempty (ub)))
 265       ## constraint inequality function only without any bounds
 266       ci_grd = @fd_ci_jac;
 267       if (iscell (cif))
 268         switch length (cif)
 269          case {1}
 270            ci_fun = cif{1};
 271          case {2}
 272            ci_fun = cif{1};
 273            ci_grd = cif{2};
 274         otherwise
 275           error ("sqp: invalid inequality constraint function specification");
 276         endswitch
 277       elseif (! isempty (cif))
 278         ci_fun = cif;   # No cell array, only constraint inequality function set
 279       endif
 280     else
 281       ## constraint inequality function with bounds present
 282       global __sqp_lb__;
 283       lb_idx = ub_idx = true (size (x0));
 284       ub_grad = - (lb_grad = eye (rows (x0)));
 285       if (isvector (lb))
 286         __sqp_lb__ = tmp_lb = lb(:);
 287         lb_idx(:) = tmp_idx = (lb != -Inf);
 288         __sqp_lb__ = __sqp_lb__(tmp_idx, 1);
 289         lb_grad = lb_grad(lb_idx, :);
 290       elseif (isempty (lb))
 291         if (isa (x0, "single"))
 292           __sqp_lb__ = tmp_lb = -realmax ("single");
 293         else
 294           __sqp_lb__ = tmp_lb = -realmax;
 295         endif
 296       else
 297         error ("sqp: invalid lower bound");
 298       endif
 299
 300       global __sqp_ub__;
 301       if (isvector (ub))
 302         __sqp_ub__ = tmp_ub = ub(:);
 303         ub_idx(:) = tmp_idx = (ub != Inf);
 304         __sqp_ub__ = __sqp_ub__(tmp_idx, 1);
 305         ub_grad = ub_grad(ub_idx, :);
 306       elseif (isempty (ub))
 307         if (isa (x0, "single"))
 308           __sqp_ub__ = tmp_ub = realmax ("single");
 309         else
 310           __sqp_ub__ = tmp_ub = realmax;
 311         endif
 312       else
 313         error ("sqp: invalid upper bound");
 314       endif
 315
 316       if (any (tmp_lb > tmp_ub))
 317         error ("sqp: upper bound smaller than lower bound");
 318       endif
 319       bounds_grad = [lb_grad; ub_grad];
 320       ci_fun = @ (x) cf_ub_lb (x, lb_idx, ub_idx);
 321       ci_grd = @ (x) cigrad_ub_lb (x, bounds_grad);
 322     endif
 323
 324     __sqp_ci_fun__ = ci_fun;
 325   endif   # if (nargin > 3)
 326
 327   iter_max = 100;
 328   if (nargin > 6 && ! isempty (maxiter))
 329     if (isscalar (maxiter) && maxiter > 0 && fix (maxiter) == maxiter)
 330       iter_max = maxiter;
 331     else
 332       error ("sqp: invalid number of maximum iterations");
 333     endif
 334   endif
 335
 336   tol = sqrt (eps);
 337   if (nargin > 7 && ! isempty (tolerance))
 338     if (isscalar (tolerance) && tolerance > 0)
 339       tol = tolerance;
 340     else
 341       error ("sqp: invalid value for TOLERANCE");
 342     endif
 343   endif
 344
 345   ## Initialize variables for search loop
 346   ## Seed x with initial guess and evaluate objective function, constraints,
 347   ## and gradients at initial value x0.
 348   ##
 349   ## obj_fun   -- objective function
 350   ## obj_grad  -- objective gradient
 351   ## ce_fun    -- equality constraint functions
 352   ## ci_fun    -- inequality constraint functions
 353   ## A == [grad_{x_1} cx_fun, grad_{x_2} cx_fun, ..., grad_{x_n} cx_fun]^T
 354   x = x0;
 355
 356   obj = feval (obj_fun, x0);
 357   __sqp_nfun__ = 1;
 358
 359   c = feval (obj_grd, x0);
 360
 361   ## Choose an initial NxN symmetric positive definite Hessian approximation B.
 362   n = length (x0);
 363   if (have_hess)
 364     B = feval (obj_hess, x0);
 365   else
 366     B = eye (n, n);
 367   endif
 368
 369   ce = feval (ce_fun, x0);
 370   F = feval (ce_grd, x0);
 371
 372   ci = feval (ci_fun, x0);
 373   C = feval (ci_grd, x0);
 374
 375   A = [F; C];
 376
 377   ## Choose an initial lambda (x is provided by the caller).
 378   lambda = 100 * ones (rows (A), 1);
 379
 380   qp_iter = 1;
 381   alpha = 1;
 382
 383   info = 0;
 384   iter = 0;
 385   # report ();  # Called with no arguments to initialize reporting
 386   # report (iter, qp_iter, alpha, __sqp_nfun__, obj);
 387
 388   while (++iter < iter_max)
 389
 390     ## Check convergence.  This is just a simple check on the first
 391     ## order necessary conditions.
 392     nr_f = rows (F);
 393
 394     lambda_e = lambda((1:nr_f)');
 395     lambda_i = lambda((nr_f+1:end)');
 396
 397     con = [ce; ci];
 398
 399     t0 = norm (c - A' * lambda);
 400     t1 = norm (ce);
 401     t2 = all (ci >= 0);
 402     t3 = all (lambda_i >= 0);
 403     t4 = norm (lambda .* con);
 404
 405     if (t2 && t3 && max ([t0; t1; t4]) < tol)
 406       info = 101;
 407       break;
 408     endif
 409
 410     ## Compute search direction p by solving QP.
 411     g = -ce;
 412     d = -ci;
 413
 414     [p, obj_qp, INFO, lambda] = qp (x, B, c, F, g, [], [], d, C,
 415                                     Inf (size (d)));
 416
 417     info = INFO.info;
 418
 419     ## FIXME -- check QP solution and attempt to recover if it has
 420     ## failed.  For now, just warn about possible problems.
 421
 422     id = "Octave:SQP-QP-subproblem";
 423     switch (info)
 424       case 2
 425         warning (id, "sqp: QP subproblem is non-convex and unbounded");
 426       case 3
 427         warning (id, "sqp: QP subproblem failed to converge in %d iterations",
 428                  INFO.solveiter);
 429       case 6
 430         warning (id, "sqp: QP subproblem is infeasible");
 431     endswitch
 432
 433     ## Choose mu such that p is a descent direction for the chosen
 434     ## merit function phi.
 435     [x_new, alpha, obj_new] = linesearch_L1 (x, p, obj_fun, obj_grd,
 436                                              ce_fun, ci_fun, lambda, obj);
 437
 438     ## Evaluate objective function, constraints, and gradients at x_new.
 439     c_new = feval (obj_grd, x_new);
 440
 441     ce_new = feval (ce_fun, x_new);
 442     F_new = feval (ce_grd, x_new);
 443
 444     ci_new = feval (ci_fun, x_new);
 445     C_new = feval (ci_grd, x_new);
 446
 447     A_new = [F_new; C_new];
 448
 449     ## Set
 450     ##
 451     ## s = alpha * p
 452     ## y = grad_x L (x_new, lambda) - grad_x L (x, lambda})
 453
 454     y = c_new - c;
 455
 456     if (! isempty (A))
 457       t = ((A_new - A)'*lambda);
 458       y -= t;
 459     endif
 460
 461     delx = x_new - x;
 462
 463     if (norm (delx) < tol * norm (x))
 464       info = 101;
 465       break;
 466     endif
 467
 468     if (have_hess)
 469
 470       B = feval (obj_hess, x);
 471
 472     else
 473       ## Update B using a quasi-Newton formula.
 474       delxt = delx';
 475
 476       ## Damped BFGS.  Or maybe we would actually want to use the Hessian
 477       ## of the Lagrangian, computed directly?
 478       d1 = delxt*B*delx;
 479
 480       t1 = 0.2 * d1;
 481       t2 = delxt*y;
 482
 483       if (t2 < t1)
 484         theta = 0.8*d1/(d1 - t2);
 485       else
 486         theta = 1;
 487       endif
 488
 489       r = theta*y + (1-theta)*B*delx;
 490
 491       d2 = delxt*r;
 492
 493       if (d1 == 0 || d2 == 0)
 494         info = 102;
 495         break;
 496       endif
 497
 498       B = B - B*delx*delxt*B/d1 + r*r'/d2;
 499
 500     endif
 501
 502     x = x_new;
 503
 504     obj = obj_new;
 505
 506     c = c_new;
 507
 508     ce = ce_new;
 509     F = F_new;
 510
 511     ci = ci_new;
 512     C = C_new;
 513
 514     A = A_new;
 515
 516     # report (iter, qp_iter, alpha, __sqp_nfun__, obj);
 517
 518   endwhile
 519
 520   if (iter >= iter_max)
 521     info = 103;
 522   endif
 523
 524   nf = __sqp_nfun__;
 525
 526 endfunction
 527
 528
 529 function [merit, obj] = phi_L1 (obj, obj_fun, ce_fun, ci_fun, x, mu)
 530
 531   global __sqp_nfun__;
 532
 533   ce = feval (ce_fun, x);
 534   ci = feval (ci_fun, x);
 535
 536   idx = ci < 0;
 537
 538   con = [ce; ci(idx)];
 539
 540   if (isempty (obj))
 541     obj = feval (obj_fun, x);
 542     __sqp_nfun__++;
 543   endif
 544
 545   merit = obj;
 546   t = norm (con, 1) / mu;
 547
 548   if (! isempty (t))
 549     merit += t;
 550   endif
 551
 552 endfunction
 553
 554
 555 function [x_new, alpha, obj] = linesearch_L1 (x, p, obj_fun, obj_grd,
 556                                               ce_fun, ci_fun, lambda, obj)
 557
 558   ## Choose parameters
 559   ##
 560   ## eta in the range (0, 0.5)
 561   ## tau in the range (0, 1)
 562
 563   eta = 0.25;
 564   tau = 0.5;
 565
 566   delta_bar = sqrt (eps);
 567
 568   if (isempty (lambda))
 569     mu = 1 / delta_bar;
 570   else
 571     mu = 1 / (norm (lambda, Inf) + delta_bar);
 572   endif
 573
 574   alpha = 1;
 575
 576   c = feval (obj_grd, x);
 577   ce = feval (ce_fun, x);
 578
 579   [phi_x_mu, obj] = phi_L1 (obj, obj_fun, ce_fun, ci_fun, x, mu);
 580
 581   D_phi_x_mu = c' * p;
 582   d = feval (ci_fun, x);
 583   ## only those elements of d corresponding
 584   ## to violated constraints should be included.
 585   idx = d < 0;
 586   t = - norm ([ce; d(idx)], 1) / mu;
 587   if (! isempty (t))
 588     D_phi_x_mu += t;
 589   endif
 590
 591   while (1)
 592     [p1, obj] = phi_L1 ([], obj_fun, ce_fun, ci_fun, x+alpha*p, mu);
 593     p2 = phi_x_mu+eta*alpha*D_phi_x_mu;
 594     if (p1 > p2)
 595       ## Reset alpha = tau_alpha * alpha for some tau_alpha in the
 596       ## range (0, tau).
 597       tau_alpha = 0.9 * tau;  # ??
 598       alpha = tau_alpha * alpha;
 599     else
 600       break;
 601     endif
 602   endwhile
 603
 604   x_new = x + alpha * p;
 605
 606 endfunction
 607
 608
 609 function grd = fdgrd (f, x)
 610
 611   if (! isempty (f))
 612     y0 = feval (f, x);
 613     nx = length (x);
 614     grd = zeros (nx, 1);
 615     deltax = sqrt (eps);
 616     for i = 1:nx
 617       t = x(i);
 618       x(i) += deltax;
 619       grd(i) = (feval (f, x) - y0) / deltax;
 620       x(i) = t;
 621     endfor
 622   else
 623     grd = zeros (0, 1);
 624   endif
 625
 626 endfunction
 627
 628
 629 function jac = fdjac (f, x)
 630
 631   nx = length (x);
 632   if (! isempty (f))
 633     y0 = feval (f, x);
 634     nf = length (y0);
 635     nx = length (x);
 636     jac = zeros (nf, nx);
 637     deltax = sqrt (eps);
 638     for i = 1:nx
 639       t = x(i);
 640       x(i) += deltax;
 641       jac(:,i) = (feval (f, x) - y0) / deltax;
 642       x(i) = t;
 643     endfor
 644   else
 645     jac = zeros  (0, nx);
 646   endif
 647
 648 endfunction
 649
 650
 651 function grd = fd_obj_grd (x)
 652
 653   global __sqp_obj_fun__;
 654
 655   grd = fdgrd (__sqp_obj_fun__, x);
 656
 657 endfunction
 658
 659
 660 function res = empty_cf (x)
 661
 662   res = zeros (0, 1);
 663
 664 endfunction
 665
 666
 667 function res = empty_jac (x)
 668
 669   res = zeros (0, length (x));
 670
 671 endfunction
 672
 673
 674 function jac = fd_ce_jac (x)
 675
 676   global __sqp_ce_fun__;
 677
 678   jac = fdjac (__sqp_ce_fun__, x);
 679
 680 endfunction
 681
 682
 683 function jac = fd_ci_jac (x)
 684
 685   global __sqp_cifcn__;
 686   ## __sqp_cifcn__ = constraint function without gradients and lb or ub
 687   jac = fdjac (__sqp_cifcn__, x);
 688
 689 endfunction
 690
 691
 692 function res = cf_ub_lb (x, lbidx, ubidx)
 693
 694   ## combine constraint function with ub and lb
 695   global __sqp_cifcn__ __sqp_lb__ __sqp_ub__
 696
 697   if (isempty (__sqp_cifcn__))
 698     res = [x(lbidx,1)-__sqp_lb__; __sqp_ub__-x(ubidx,1)];
 699   else
 700     res = [feval(__sqp_cifcn__,x); \
 701            x(lbidx,1)-__sqp_lb__; __sqp_ub__-x(ubidx,1)];
 702   endif
 703
 704 endfunction
 705
 706
 707 function res = cigrad_ub_lb (x, bgrad)
 708
 709   global __sqp_cif__
 710
 711   cigradfcn = @fd_ci_jac;
 712
 713   if (iscell (__sqp_cif__) && length (__sqp_cif__) > 1)
 714     cigradfcn = __sqp_cif__{2};
 715   endif
 716
 717   if (isempty (cigradfcn))
 718     res = bgrad;
 719   else
 720     res = [feval(cigradfcn,x); bgrad];
 721   endif
 722
 723 endfunction
 724
 725 # Utility function used to debug sqp
 726 function report (iter, qp_iter, alpha, nfun, obj)
 727
 728   if (nargin == 0)
 729     printf ("  Itn ItQP     Step  Nfun     Objective\n");
 730   else
 731     printf ("%5d %4d %8.1g %5d %13.6e\n", iter, qp_iter, alpha, nfun, obj);
 732   endif
 733
 734 endfunction
 735
 736 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 737 %% Test Code
 738
 739 %!function r = __g (x)
 740 %!  r = [sumsq(x)-10;
 741 %!       x(2)*x(3)-5*x(4)*x(5);
 742 %!       x(1)^3+x(2)^3+1 ];
 743 %!endfunction
 744 %!
 745 %!function obj = __phi (x)
 746 %!  obj = exp(prod(x)) - 0.5*(x(1)^3+x(2)^3+1)^2;
 747 %!endfunction
 748 %!
 749 %!test
 750 %!
 751 %! x0 = [-1.8; 1.7; 1.9; -0.8; -0.8];
 752 %!
 753 %! [x, obj, info, iter, nf, lambda] = sqp (x0, @__phi, @__g, []);
 754 %!
 755 %! x_opt = [-1.717143501952599;
 756 %!           1.595709610928535;
 757 %!           1.827245880097156;
 758 %!          -0.763643103133572;
 759 %!          -0.763643068453300];
 760 %!
 761 %! obj_opt = 0.0539498477702739;
 762 %!
 763 %! assert (all (abs (x-x_opt) < 5*sqrt (eps)) && abs (obj-obj_opt) < sqrt (eps));
 764
 765 %% Test input validation
 766 %!error sqp ()
 767 %!error sqp (1)
 768 %!error sqp (1,2,3,4,5,6,7,8,9)
 769 %!error sqp (1,2,3,4,5)
 770 %!error sqp (ones(2,2))
 771 %!error sqp (1,cell(4,1))
 772 %!error sqp (1,cell(3,1),cell(3,1))
 773 %!error sqp (1,cell(3,1),cell(2,1),cell(3,1))
 774 %!error sqp (1,cell(3,1),cell(2,1),cell(2,1),ones(2,2),[])
 775 %!error sqp (1,cell(3,1),cell(2,1),cell(2,1),[],ones(2,2))
 776 %!error sqp (1,cell(3,1),cell(2,1),cell(2,1),1,-1)
 777 %!error sqp (1,cell(3,1),cell(2,1),cell(2,1),[],[],ones(2,2))
 778 %!error sqp (1,cell(3,1),cell(2,1),cell(2,1),[],[],-1)
 779 %!error sqp (1,cell(3,1),cell(2,1),cell(2,1),[],[],1.5)
 780 %!error sqp (1,cell(3,1),cell(2,1),cell(2,1),[],[],[],ones(2,2))
 781 %!error sqp (1,cell(3,1),cell(2,1),cell(2,1),[],[],[],-1)