Mercurial > octave-nkf
annotate src/DLD-FUNCTIONS/__qp__.cc @ 7578:91f8446ce4ae
handle possible error from EIG
author | John W. Eaton <jwe@octave.org> |
---|---|
date | Tue, 11 Mar 2008 10:49:33 -0400 |
parents | a2870fd8ac58 |
children | 6f2b2cc4b957 |
rev | line source |
---|---|
5290 | 1 /* |
2 | |
7361 | 3 Copyright (C) 2000, 2001, 2004, 2005, 2006, 2007, 2008 Gabriele Pannocchia |
5290 | 4 |
5 This file is part of Octave. | |
6 | |
7 Octave is free software; you can redistribute it and/or modify it | |
8 under the terms of the GNU General Public License as published by the | |
7016 | 9 Free Software Foundation; either version 3 of the License, or (at your |
10 option) any later version. | |
5290 | 11 |
12 Octave is distributed in the hope that it will be useful, but WITHOUT | |
13 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or | |
14 FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License | |
15 for more details. | |
16 | |
17 You should have received a copy of the GNU General Public License | |
7016 | 18 along with Octave; see the file COPYING. If not, see |
19 <http://www.gnu.org/licenses/>. | |
5290 | 20 |
21 */ | |
22 | |
5293 | 23 #ifdef HAVE_CONFIG_H |
24 #include <config.h> | |
25 #endif | |
26 | |
5290 | 27 #include <cfloat> |
28 | |
5293 | 29 #include "dbleCHOL.h" |
30 #include "dbleSVD.h" | |
31 #include "mx-m-dm.h" | |
32 #include "EIG.h" | |
5290 | 33 |
5293 | 34 #include "defun-dld.h" |
35 #include "error.h" | |
36 #include "gripes.h" | |
37 #include "oct-obj.h" | |
38 #include "pr-output.h" | |
39 #include "utils.h" | |
5290 | 40 |
41 static Matrix | |
5295 | 42 null (const Matrix& A, octave_idx_type& rank) |
5290 | 43 { |
44 Matrix retval; | |
45 | |
46 rank = 0; | |
47 | |
48 if (! A.is_empty ()) | |
49 { | |
50 SVD A_svd (A); | |
51 | |
52 DiagMatrix S = A_svd.singular_values (); | |
53 | |
54 ColumnVector s = S.diag (); | |
55 | |
56 Matrix V = A_svd.right_singular_matrix (); | |
57 | |
5295 | 58 octave_idx_type A_nr = A.rows (); |
59 octave_idx_type A_nc = A.cols (); | |
5290 | 60 |
5295 | 61 octave_idx_type tmp = A_nr > A_nc ? A_nr : A_nc; |
5290 | 62 |
63 double tol = tmp * s(0) * DBL_EPSILON; | |
64 | |
5295 | 65 octave_idx_type n = s.length (); |
5290 | 66 |
5295 | 67 for (octave_idx_type i = 0; i < n; i++) |
5290 | 68 { |
69 if (s(i) > tol) | |
70 rank++; | |
71 } | |
72 | |
73 if (rank < A_nc) | |
74 retval = V.extract (0, rank, A_nc-1, A_nc-1); | |
75 else | |
76 retval.resize (A_nc, 0); | |
6431 | 77 |
78 for (octave_idx_type i = 0; i < retval.numel (); i++) | |
79 if (std::abs (retval(i)) < DBL_EPSILON) | |
80 retval(i) = 0; | |
5290 | 81 } |
82 | |
83 return retval; | |
84 } | |
85 | |
86 static int | |
87 qp (const Matrix& H, const ColumnVector& q, | |
88 const Matrix& Aeq, const ColumnVector& beq, | |
89 const Matrix& Ain, const ColumnVector& bin, | |
90 int maxit, | |
91 ColumnVector& x, ColumnVector& lambda, int& iter) | |
92 { | |
93 int info = 0; | |
94 | |
95 iter = 0; | |
96 | |
97 double rtol = sqrt (DBL_EPSILON); | |
98 | |
99 // Problem dimension. | |
5295 | 100 octave_idx_type n = x.length (); |
5290 | 101 |
102 // Dimension of constraints. | |
5295 | 103 octave_idx_type n_eq = beq.length (); |
104 octave_idx_type n_in = bin.length (); | |
5290 | 105 |
106 // Filling the current active set. | |
107 | |
5295 | 108 octave_idx_type n_act = n_eq; |
5290 | 109 |
5295 | 110 octave_idx_type n_tot = n_eq + n_in; |
5290 | 111 |
112 // Equality constraints come first. We won't check the sign of the | |
113 // Lagrange multiplier for those. | |
114 | |
115 Matrix Aact = Aeq; | |
116 ColumnVector bact = beq; | |
117 ColumnVector Wact; | |
118 | |
119 if (n_in > 0) | |
120 { | |
121 ColumnVector res = Ain*x - bin; | |
122 | |
5295 | 123 for (octave_idx_type i = 0; i < n_in; i++) |
5290 | 124 { |
6431 | 125 res(i) /= (1.0 + std::abs (bin(i))); |
5290 | 126 |
127 if (res(i) < rtol) | |
128 { | |
129 n_act++; | |
130 Aact = Aact.stack (Ain.row (i)); | |
131 bact.resize (n_act, bin(i)); | |
6848 | 132 Wact.resize (n_act-n_eq, i); |
5290 | 133 } |
134 } | |
135 } | |
136 | |
137 // Computing the ??? | |
138 | |
139 EIG eigH (H); | |
7578
91f8446ce4ae
handle possible error from EIG
John W. Eaton <jwe@octave.org>
parents:
7361
diff
changeset
|
140 |
91f8446ce4ae
handle possible error from EIG
John W. Eaton <jwe@octave.org>
parents:
7361
diff
changeset
|
141 if (error_state) |
91f8446ce4ae
handle possible error from EIG
John W. Eaton <jwe@octave.org>
parents:
7361
diff
changeset
|
142 { |
91f8446ce4ae
handle possible error from EIG
John W. Eaton <jwe@octave.org>
parents:
7361
diff
changeset
|
143 error ("qp: failed to compute eigenvalues of H"); |
91f8446ce4ae
handle possible error from EIG
John W. Eaton <jwe@octave.org>
parents:
7361
diff
changeset
|
144 return -1; |
91f8446ce4ae
handle possible error from EIG
John W. Eaton <jwe@octave.org>
parents:
7361
diff
changeset
|
145 } |
91f8446ce4ae
handle possible error from EIG
John W. Eaton <jwe@octave.org>
parents:
7361
diff
changeset
|
146 |
5290 | 147 ColumnVector eigenvalH = real (eigH.eigenvalues ()); |
148 Matrix eigenvecH = real (eigH.eigenvectors ()); | |
149 double minReal = eigenvalH.min (); | |
5295 | 150 octave_idx_type indminR = 0; |
151 for (octave_idx_type i = 0; i < n; i++) | |
5290 | 152 { |
153 if (minReal == eigenvalH(i)) | |
154 { | |
155 indminR = i; | |
156 break; | |
157 } | |
158 } | |
159 | |
160 bool done = false; | |
161 | |
162 double alpha = 0.0; | |
163 | |
164 Matrix R; | |
165 Matrix Y (n, 0, 0.0); | |
166 | |
167 ColumnVector g (n, 0.0); | |
168 ColumnVector p (n, 0.0); | |
169 | |
170 ColumnVector lambda_tmp (n_in, 0.0); | |
171 | |
172 while (! done) | |
173 { | |
174 iter++; | |
175 | |
176 // Current Gradient | |
177 // g = q + H * x; | |
178 | |
179 g = q + H * x; | |
180 | |
181 if (n_act == 0) | |
182 { | |
183 // There are no active constraints. | |
184 | |
185 if (minReal > 0.0) | |
186 { | |
187 // Inverting the Hessian. Using the Cholesky | |
188 // factorization since the Hessian is positive | |
189 // definite. | |
190 | |
191 CHOL cholH (H); | |
192 | |
193 R = cholH.chol_matrix (); | |
194 | |
5341 | 195 Matrix Hinv = chol2inv (R); |
5290 | 196 |
197 // Computing the unconstrained step. | |
198 // p = -Hinv * g; | |
199 | |
200 p = -Hinv * g; | |
201 | |
202 info = 0; | |
203 } | |
204 else | |
205 { | |
206 // Finding the negative curvature of H. | |
207 | |
208 p = eigenvecH.column (indminR); | |
209 | |
210 // Following the negative curvature of H. | |
211 | |
212 if (p.transpose () * g > DBL_EPSILON) | |
213 p = -p; | |
214 | |
215 info = 1; | |
216 } | |
217 | |
218 // Multipliers are zero. | |
219 lambda_tmp.fill (0.0); | |
220 } | |
221 else | |
222 { | |
223 // There are active constraints. | |
224 | |
225 // Computing the null space. | |
226 | |
5295 | 227 octave_idx_type rank; |
5290 | 228 |
229 Matrix Z = null (Aact, rank); | |
230 | |
5295 | 231 octave_idx_type dimZ = n - rank; |
5290 | 232 |
5775 | 233 // FIXME -- still remain to handle the case of |
5290 | 234 // non-full rank active set matrix. |
235 | |
7361 | 236 // Computing the Y matrix (orthogonal to Z) |
237 Y = Aact.pseudo_inverse (); | |
238 | |
5290 | 239 // Reduced Hessian |
240 Matrix Zt = Z.transpose (); | |
241 Matrix rH = Zt * H * Z; | |
242 | |
5295 | 243 octave_idx_type pR = 0; |
5290 | 244 |
245 if (dimZ > 0) | |
246 { | |
247 // Computing the Cholesky factorization (pR = 0 means | |
248 // that the reduced Hessian was positive definite). | |
249 | |
250 CHOL cholrH (rH, pR); | |
251 Matrix tR = cholrH.chol_matrix (); | |
252 if (pR == 0) | |
253 R = tR; | |
254 } | |
255 | |
256 if (pR == 0) | |
257 { | |
258 info = 0; | |
259 | |
260 // Computing the step pz. | |
261 if (dimZ > 0) | |
262 { | |
263 // Using the Cholesky factorization to invert rH | |
264 | |
5341 | 265 Matrix rHinv = chol2inv (R); |
5290 | 266 |
267 ColumnVector pz = -rHinv * Zt * g; | |
268 | |
269 // Global step. | |
270 p = Z * pz; | |
271 } | |
272 else | |
273 { | |
274 // Global step. | |
275 p.fill (0.0); | |
276 } | |
277 } | |
278 else | |
279 { | |
280 info = 1; | |
281 | |
282 // Searching for the most negative curvature. | |
283 | |
284 EIG eigrH (rH); | |
7578
91f8446ce4ae
handle possible error from EIG
John W. Eaton <jwe@octave.org>
parents:
7361
diff
changeset
|
285 |
91f8446ce4ae
handle possible error from EIG
John W. Eaton <jwe@octave.org>
parents:
7361
diff
changeset
|
286 if (error_state) |
91f8446ce4ae
handle possible error from EIG
John W. Eaton <jwe@octave.org>
parents:
7361
diff
changeset
|
287 { |
91f8446ce4ae
handle possible error from EIG
John W. Eaton <jwe@octave.org>
parents:
7361
diff
changeset
|
288 error ("qp: failed to compute eigenvalues of rH"); |
91f8446ce4ae
handle possible error from EIG
John W. Eaton <jwe@octave.org>
parents:
7361
diff
changeset
|
289 return -1; |
91f8446ce4ae
handle possible error from EIG
John W. Eaton <jwe@octave.org>
parents:
7361
diff
changeset
|
290 } |
91f8446ce4ae
handle possible error from EIG
John W. Eaton <jwe@octave.org>
parents:
7361
diff
changeset
|
291 |
5290 | 292 ColumnVector eigenvalrH = real (eigrH.eigenvalues ()); |
293 Matrix eigenvecrH = real (eigrH.eigenvectors ()); | |
294 double mRrH = eigenvalrH.min (); | |
295 indminR = 0; | |
5295 | 296 for (octave_idx_type i = 0; i < n; i++) |
5290 | 297 { |
298 if (mRrH == eigenvalH(i)) | |
299 { | |
300 indminR = i; | |
301 break; | |
302 } | |
303 } | |
304 | |
305 ColumnVector eVrH = eigenvecrH.column (indminR); | |
306 | |
307 // Computing the step pz. | |
308 p = Z * eVrH; | |
309 | |
310 if (p.transpose () * g > DBL_EPSILON) | |
311 p = -p; | |
312 } | |
313 } | |
314 | |
315 // Checking the step-size. | |
316 ColumnVector abs_p (n); | |
5295 | 317 for (octave_idx_type i = 0; i < n; i++) |
6431 | 318 abs_p(i) = std::abs (p(i)); |
5290 | 319 double max_p = abs_p.max (); |
320 | |
321 if (max_p < rtol) | |
322 { | |
323 // The step is null. Checking constraints. | |
324 if (n_act - n_eq == 0) | |
325 // Solution is found because no inequality | |
326 // constraints are active. | |
327 done = true; | |
328 else | |
329 { | |
330 // Computing the multipliers only for the inequality | |
331 // constraints that are active. We do NOT compute | |
332 // multipliers for the equality constraints. | |
333 Matrix Yt = Y.transpose (); | |
334 Yt = Yt.extract_n (n_eq, 0, n_act-n_eq, n); | |
335 lambda_tmp = Yt * (g + H * p); | |
336 | |
337 // Checking the multipliers. We remove the most | |
338 // negative from the set (if any). | |
339 double min_lambda = lambda_tmp.min (); | |
340 if (min_lambda >= 0) | |
341 { | |
342 // Solution is found. | |
343 done = true; | |
344 } | |
345 else | |
346 { | |
5295 | 347 octave_idx_type which_eig = 0; |
348 for (octave_idx_type i = 0; i < n_act; i++) | |
5290 | 349 { |
350 if (lambda_tmp(i) == min_lambda) | |
351 { | |
352 which_eig = i; | |
353 break; | |
354 } | |
355 } | |
356 | |
357 // At least one multiplier is negative, we | |
358 // remove it from the set. | |
359 | |
5295 | 360 for (octave_idx_type i = which_eig; i < n_act - n_eq; i++) |
5290 | 361 { |
362 Wact(i) = Wact(i+1); | |
5295 | 363 for (octave_idx_type j = 0; j < n; j++) |
5290 | 364 Aact(n_eq+i,j) = Aact(n_eq+i+1,j); |
365 bact(n_eq+i) = bact(n_eq+i+1); | |
366 } | |
367 n_act--; | |
368 | |
369 // Resizing the active set. | |
370 Wact.resize (n_act-n_eq); | |
371 bact.resize (n_act); | |
372 Aact.resize (n_act, n); | |
373 } | |
374 } | |
375 } | |
376 else | |
377 { | |
378 // The step is not null. | |
379 if (n_act - n_eq == n_in) | |
380 { | |
381 // All inequality constraints were active. We can | |
382 // add the whole step. | |
383 x += p; | |
384 } | |
385 else | |
386 { | |
387 // Some constraints were not active. Checking if | |
388 // there is a blocking constraint. | |
389 alpha = 1.0; | |
5295 | 390 octave_idx_type is_block = -1; |
5290 | 391 |
5295 | 392 for (octave_idx_type i = 0; i < n_in; i++) |
5290 | 393 { |
394 bool found = false; | |
395 | |
5295 | 396 for (octave_idx_type j = 0; j < n_act-n_eq; j++) |
5290 | 397 { |
7035 | 398 if (Wact(j) == i) |
5290 | 399 { |
400 found = true; | |
401 break; | |
402 } | |
403 } | |
404 | |
405 if (! found) | |
406 { | |
407 // The i-th constraint was not in the set. Is it a | |
408 // blocking constraint? | |
409 | |
410 RowVector tmp_row = Ain.row (i); | |
411 double tmp = tmp_row * p; | |
412 double res = tmp_row * x; | |
413 | |
414 if (tmp < 0.0) | |
415 { | |
416 double alpha_tmp = (bin(i) - res) / tmp; | |
417 | |
418 if (alpha_tmp < alpha) | |
419 { | |
420 alpha = alpha_tmp; | |
421 is_block = i; | |
422 } | |
423 } | |
424 } | |
425 } | |
426 | |
427 // In is_block there is the index of the blocking | |
428 // constraint (if any). | |
429 if (is_block >= 0) | |
430 { | |
431 // There is a blocking constraint (index in | |
432 // is_block) which is added to the active set. | |
433 n_act++; | |
434 Aact = Aact.stack (Ain.row (is_block)); | |
435 bact.resize (n_act, bin(is_block)); | |
6848 | 436 Wact.resize (n_act-n_eq, is_block); |
5290 | 437 |
438 // Adding the reduced step | |
439 x += alpha * p; | |
440 } | |
441 else | |
442 { | |
443 // There are no blocking constraints. Adding the | |
444 // whole step. | |
445 x += alpha * p; | |
446 } | |
447 } | |
448 } | |
449 | |
450 if (iter == maxit) | |
451 { | |
452 done = true; | |
453 // warning ("qp_main: maximum number of iteration reached"); | |
454 info = 3; | |
455 } | |
456 } | |
457 | |
458 lambda_tmp = Y.transpose () * (g + H * p); | |
459 | |
460 // Reordering the Lagrange multipliers. | |
461 | |
462 lambda.resize (n_tot); | |
463 lambda.fill (0.0); | |
5295 | 464 for (octave_idx_type i = 0; i < n_eq; i++) |
5290 | 465 lambda(i) = lambda_tmp(i); |
466 | |
5295 | 467 for (octave_idx_type i = n_eq; i < n_tot; i++) |
5290 | 468 { |
5295 | 469 for (octave_idx_type j = 0; j < n_act-n_eq; j++) |
5290 | 470 { |
7035 | 471 if (Wact(j) == i - n_eq) |
5290 | 472 { |
473 lambda(i) = lambda_tmp(n_eq+j); | |
474 break; | |
475 } | |
476 } | |
477 } | |
478 | |
479 return info; | |
480 } | |
481 | |
482 DEFUN_DLD (__qp__, args, , | |
6945 | 483 "-*- texinfo -*-\n\ |
484 @deftypefn {Loadable Function} {[@var{x}, @var{lambda}, @var{info}, @var{iter}] =} __qp__ (@var{x0}, @var{H}, @var{q}, @var{Aeq}, @var{beq}, @var{Ain}, @var{bin}, @var{maxit})\n\ | |
485 Undocumented internal function.\n\ | |
486 @end deftypefn") | |
5290 | 487 { |
488 octave_value_list retval; | |
489 | |
490 if (args.length () == 8) | |
491 { | |
492 const ColumnVector x0 (args(0) . vector_value ()); | |
493 const Matrix H (args(1) . matrix_value ()); | |
494 const ColumnVector q (args(2) . vector_value ()); | |
495 const Matrix Aeq (args(3) . matrix_value ()); | |
496 const ColumnVector beq (args(4) . vector_value ()); | |
497 const Matrix Ain (args(5) . matrix_value ()); | |
498 const ColumnVector bin (args(6) . vector_value ()); | |
499 const int maxit (args(7) . int_value ()); | |
500 | |
501 if (! error_state) | |
502 { | |
503 int iter = 0; | |
504 | |
505 // Copying the initial guess in the working variable | |
506 ColumnVector x = x0; | |
507 | |
508 // Reordering the Lagrange multipliers | |
509 ColumnVector lambda; | |
510 | |
511 int info = qp (H, q, Aeq, beq, Ain, bin, maxit, x, lambda, iter); | |
512 | |
7578
91f8446ce4ae
handle possible error from EIG
John W. Eaton <jwe@octave.org>
parents:
7361
diff
changeset
|
513 if (! error_state) |
91f8446ce4ae
handle possible error from EIG
John W. Eaton <jwe@octave.org>
parents:
7361
diff
changeset
|
514 { |
91f8446ce4ae
handle possible error from EIG
John W. Eaton <jwe@octave.org>
parents:
7361
diff
changeset
|
515 retval(3) = iter; |
91f8446ce4ae
handle possible error from EIG
John W. Eaton <jwe@octave.org>
parents:
7361
diff
changeset
|
516 retval(2) = info; |
91f8446ce4ae
handle possible error from EIG
John W. Eaton <jwe@octave.org>
parents:
7361
diff
changeset
|
517 retval(1) = lambda; |
91f8446ce4ae
handle possible error from EIG
John W. Eaton <jwe@octave.org>
parents:
7361
diff
changeset
|
518 retval(0) = x; |
91f8446ce4ae
handle possible error from EIG
John W. Eaton <jwe@octave.org>
parents:
7361
diff
changeset
|
519 } |
91f8446ce4ae
handle possible error from EIG
John W. Eaton <jwe@octave.org>
parents:
7361
diff
changeset
|
520 else |
91f8446ce4ae
handle possible error from EIG
John W. Eaton <jwe@octave.org>
parents:
7361
diff
changeset
|
521 error ("qp: internal error"); |
5290 | 522 } |
523 else | |
524 error ("__qp__: invalid arguments"); | |
525 } | |
526 else | |
5823 | 527 print_usage (); |
5290 | 528 |
529 return retval; | |
530 } |