1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.hipparchus.optim.nonlinear.vector.constrained;
18
19 import org.hipparchus.linear.Array2DRowRealMatrix;
20 import org.hipparchus.linear.ArrayRealVector;
21 import org.hipparchus.linear.EigenDecompositionSymmetric;
22 import org.hipparchus.linear.MatrixUtils;
23 import org.hipparchus.linear.RealMatrix;
24 import org.hipparchus.linear.RealVector;
25 import org.hipparchus.optim.nonlinear.scalar.ObjectiveFunction;
26 import org.hipparchus.util.FastMath;
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41 public class SQPOptimizerGM extends AbstractSQPOptimizer {
42
43
44 private RealMatrix constraintJacob;
45
46
47 private RealVector equalityEval;
48
49
50 private RealVector inequalityEval;
51
52
53 private RealVector functionGradient;
54
55
56 private RealMatrix hessian;
57
58
59 @Override
60 public LagrangeSolution doOptimize() {
61 int me = 0;
62 int mi = 0;
63
64
65 if (getEqConstraint() != null) {
66 me = getEqConstraint().dimY();
67 }
68
69 if (getIqConstraint() != null) {
70 mi = getIqConstraint().dimY();
71 }
72
73 double alpha;
74 double rho;
75 RealVector x;
76 if (getStartPoint() != null) {
77 x = new ArrayRealVector(getStartPoint());
78 } else {
79 x = new ArrayRealVector(getObj().dim());
80 }
81
82 RealVector y = new ArrayRealVector(me + mi, 0.0);
83
84 double functionEval = getObj().value(x);
85 functionGradient = getObj().gradient(x);
86 double maxGrad = functionGradient.getLInfNorm();
87
88 if (getEqConstraint() != null) {
89 equalityEval = getEqConstraint().value(x);
90 }
91 if (getIqConstraint() != null) {
92 inequalityEval = getIqConstraint().value(x);
93 }
94 constraintJacob = computeJacobianConstraint(x);
95
96 if (getEqConstraint() != null) {
97 maxGrad = FastMath.max(maxGrad, equalityEval.getLInfNorm());
98 }
99 if (getIqConstraint() != null) {
100 maxGrad = FastMath.max(maxGrad, inequalityEval.getLInfNorm());
101 }
102
103 if (!getSettings().useFunHessian()) {
104 hessian= MatrixUtils.createRealIdentityMatrix(x.getDimension()).scalarMultiply(maxGrad);
105 } else {
106 hessian = getObj().hessian(x);
107 }
108
109
110 rho = 0;
111
112 for (int i = 0; i < getMaxIterations(); i++) {
113
114 iterations.increment();
115
116 alpha = 1.0;
117
118 LagrangeSolution sol1;
119
120 sol1 = solveQP(x);
121 RealVector p = sol1.getX();
122 RealVector e = sol1.getLambda().subtract(y);
123
124 RealVector s = calculateSvector(y,rho);
125 RealVector se = null;
126 RealMatrix jacobi;
127 RealVector q = null;
128 RealVector qe = null;
129
130
131 if (getEqConstraint() != null) {
132 se = new ArrayRealVector(me);
133 jacobi = constraintJacob.getSubMatrix(0, me - 1, 0, x.getDimension() - 1);
134 qe = new ArrayRealVector(me);
135 }
136 if (getIqConstraint() != null) {
137 jacobi = constraintJacob.getSubMatrix(me, me + mi - 1, 0, x.getDimension() - 1);
138 q = jacobi.operate(p).add(inequalityEval.subtract(getIqConstraint().getLowerBound())).subtract(s);
139 }
140
141
142
143
144
145 double penaltyGradient = penaltyFunctionGrad(p, y, s, e, qe, q, rho);
146 ArrayRealVector g = new ArrayRealVector(me + mi);
147 if (me > 0) {
148 g.setSubVector(0,equalityEval.subtract(getEqConstraint().getLowerBound()));
149 }
150 if (mi > 0) {
151 g.setSubVector(me, inequalityEval.subtract(getIqConstraint().getLowerBound()).subtract(s));
152 }
153
154 double rhoSegnato = 2.0 * e.getNorm() / g.getNorm();
155
156
157
158 while (penaltyGradient > -0.5 * p.dotProduct(hessian.operate(p))) {
159
160 rho = FastMath.max(rhoSegnato,2.0 * rho);
161
162 penaltyGradient = penaltyFunctionGrad(p, y, s, e, qe, q, rho);
163 }
164
165 double alfaEval = getObj().value(x.add(p.mapMultiply(alpha)));
166
167 double alphaPenalty;
168 RealVector sineq = null;
169 RealVector seq = null;
170 if (se != null) {
171 seq = se.add(qe.mapMultiply(alpha));
172 }
173 if (s != null) {
174 sineq = s.add(q.mapMultiply(alpha));
175 }
176
177 double currentPenalty = penaltyFunction(functionEval, x, y, se, s, rho);
178 alphaPenalty = penaltyFunction(alfaEval,x.add(p.mapMultiply(alpha)),
179 y.add(e.mapMultiply(alpha)),
180 seq, sineq, rho);
181
182
183
184 int search = 0;
185 while ((alphaPenalty -currentPenalty) >= getSettings().getMu() * alpha * penaltyGradient &&
186 search < getSettings().getMaxLineSearchIteration()) {
187
188 double alfaStar = -0.5 * alpha * alpha * penaltyGradient/ (-alpha * penaltyGradient + alphaPenalty - currentPenalty);
189
190 alpha = FastMath.max(getSettings().getB() * alpha, FastMath.min(1.0,alfaStar));
191 alfaEval = getObj().value(x.add(p.mapMultiply(alpha)));
192 if (se != null) {
193 seq = se.add(qe.mapMultiply(alpha));
194 }
195 if (s != null) {
196 sineq = s.add(q.mapMultiply(alpha));
197 }
198 alphaPenalty = penaltyFunction(alfaEval,x.add(p.mapMultiply(alpha)),y.add(e.mapMultiply(alpha)),seq,sineq,rho);
199
200 search = search + 1;
201
202 }
203
204
205 if (getSettings().getConvCriteria() == 0) {
206 if (p.mapMultiply(alpha).dotProduct(hessian.operate(p.mapMultiply(alpha))) < getSettings().getEps() * getSettings().getEps()) {
207 break;
208 }
209 } else {
210 if (alpha * p.getNorm() <FastMath.sqrt(getSettings().getEps()) * (1 + x.getNorm())) {
211 break;
212 }
213
214 }
215
216
217 RealVector oldGradient = functionGradient;
218 RealMatrix oldJacob = constraintJacob;
219 RealVector old1 = lagrangianGradX(oldGradient,oldJacob,x,y.add(e.mapMultiply(alpha)));
220 functionEval = alfaEval;
221 functionGradient = getObj().gradient(x.add(p.mapMultiply(alpha)));
222 constraintJacob = computeJacobianConstraint(x.add(p.mapMultiply(alpha)));
223 RealVector new1 = lagrangianGradX(functionGradient,constraintJacob,x.add(p.mapMultiply(alpha)),y.add(e.mapMultiply(alpha)));
224 hessian = BFGSFormula(hessian, p, alpha, new1, old1);
225
226 if (getEqConstraint() != null) {
227 equalityEval = getEqConstraint().value(x.add(p.mapMultiply(alpha)));
228 }
229 if (getIqConstraint() != null) {
230 inequalityEval = getIqConstraint().value(x.add(p.mapMultiply(alpha)));
231 }
232
233 x = x.add(p.mapMultiply(alpha));
234 y = y.add(e.mapMultiply(alpha));
235 }
236
237 functionEval = getObj().value(x);
238 functionGradient = getObj().gradient(x);
239
240 constraintJacob = computeJacobianConstraint(x);
241 return new LagrangeSolution(x, y, functionEval);
242 }
243
244 private RealVector calculateSvector(RealVector y, double rho) {
245 int me = 0;
246 int mi;
247 RealVector si = null;
248 if (getEqConstraint() != null) {
249 me = getEqConstraint().dimY();
250 }
251 if (getIqConstraint() != null) {
252 mi = getIqConstraint().dimY();
253 si = new ArrayRealVector(mi);
254 RealVector yi = y.getSubVector(me, mi);
255 for (int i = 0; i < inequalityEval.getDimension(); i++) {
256 if (rho == 0) {
257 si.setEntry(i, FastMath.max(0, inequalityEval.getEntry(i)));
258 } else {
259 si.setEntry(i, FastMath.max(0, inequalityEval.getEntry(i) - yi.getEntry(i) / rho));
260 }
261 }
262 }
263 return si;
264 }
265
266 private double penaltyFunction(double currentF, RealVector x, RealVector y, RealVector se, RealVector s, double rho) {
267
268
269 int me = 0;
270 int mi;
271 double partial = currentF;
272
273 if (getEqConstraint() != null) {
274 me = getEqConstraint().dimY();
275
276 RealVector ye = y.getSubVector(0, me);
277 RealVector g = getEqConstraint().value(x).subtract(getEqConstraint().getLowerBound());
278 partial += -ye.dotProduct(g.subtract(se)) + 0.5 * rho * (g.subtract(se)).dotProduct(g.subtract(se));
279 }
280
281 if (getIqConstraint() != null) {
282 mi = getIqConstraint().dimY();
283
284 RealVector yi = y.getSubVector(me, mi);
285
286 RealVector g = getIqConstraint().value(x).subtract(getIqConstraint().getLowerBound());
287
288
289
290
291
292 partial+= -yi.dotProduct(g.subtract(s)) +0.5 * rho*(g.subtract(s)).dotProduct(g.subtract(s));
293
294 }
295
296 return partial;
297 }
298
299 private double penaltyFunctionGrad(RealVector p, RealVector y,RealVector s,RealVector e,RealVector qe, RealVector q,double rho) {
300
301 int me = 0;
302 int mi;
303 double partial = functionGradient.dotProduct(p);
304 if (getEqConstraint() != null) {
305 me = getEqConstraint().dimY();
306
307 RealVector ye = y.getSubVector(0, me);
308 RealVector ee = e.getSubVector(0, me);
309
310
311
312 RealVector ge = equalityEval.subtract(getEqConstraint().getLowerBound());
313 RealMatrix jacob = constraintJacob.getSubMatrix(0, me - 1, 0, p.getDimension() - 1);
314 double term2 = p.dotProduct(jacob.transpose().operate(ye));
315 double term3 = p.dotProduct(jacob.transpose().operate(ge))*rho;
316 double term4 = ge.dotProduct(ee);
317 double term5 = ye.dotProduct(qe);
318 double term6 = qe.dotProduct(ge)*rho;
319 partial +=-term2 + term3-term4 + term5-term6;
320 }
321
322 if (getIqConstraint() != null) {
323 mi = getIqConstraint().dimY();
324
325 RealVector yi = y.getSubVector(me, mi);
326 RealVector ei = e.getSubVector(me, mi);
327
328 RealVector gi = inequalityEval.subtract(getIqConstraint().getLowerBound());
329 RealMatrix jacob = constraintJacob.getSubMatrix(me, me + mi - 1, 0, p.getDimension() - 1);
330 double term2 = p.dotProduct(jacob.transpose().operate(yi));
331 double term3 = p.dotProduct(jacob.transpose().operate(gi.subtract(s)))*rho;
332 double term4 = (gi.subtract(s)).dotProduct(ei);
333 double term5 = yi.dotProduct(q);
334 double term6 = q.dotProduct(gi.subtract(s))*rho;
335
336 partial +=-term2 + term3-term4 + term5-term6;
337 }
338
339 return partial;
340 }
341
342 private LagrangeSolution solveQP(RealVector x) {
343
344 RealMatrix H = hessian;
345 RealVector g = functionGradient;
346
347 int me = 0;
348 int mi = 0;
349 if (getEqConstraint() != null) {
350 me = getEqConstraint().dimY();
351 }
352 if (getIqConstraint() != null) {
353
354 mi = getIqConstraint().dimY();
355
356 }
357
358 RealMatrix H1 = new Array2DRowRealMatrix(H.getRowDimension() , H.getRowDimension() );
359 H1.setSubMatrix(H.getData(), 0, 0);
360 RealVector g1 = new ArrayRealVector(g.getDimension() );
361 g1.setSubVector(0, g);
362
363 LinearEqualityConstraint eqc = null;
364 if (getEqConstraint() != null) {
365 RealMatrix eqJacob = constraintJacob.getSubMatrix(0,me-1,0,x.getDimension()-1);
366
367 RealMatrix Ae = new Array2DRowRealMatrix(me, x.getDimension() );
368 RealVector be = new ArrayRealVector(me);
369 Ae.setSubMatrix(eqJacob.getData(), 0, 0);
370
371
372
373 be.setSubVector(0, getEqConstraint().getLowerBound().subtract(equalityEval));
374 eqc = new LinearEqualityConstraint(Ae, be);
375
376 }
377 LinearInequalityConstraint iqc = null;
378
379 if (getIqConstraint() != null) {
380
381 RealMatrix iqJacob = constraintJacob.getSubMatrix(me,me + mi-1,0,x.getDimension()-1);
382
383 RealMatrix Ai = new Array2DRowRealMatrix(mi, x.getDimension());
384 RealVector bi = new ArrayRealVector(mi);
385 Ai.setSubMatrix(iqJacob.getData(), 0, 0);
386
387 bi.setSubVector(0, getIqConstraint().getLowerBound().subtract(inequalityEval));
388
389 iqc = new LinearInequalityConstraint(Ai, bi);
390
391 }
392
393 QuadraticFunction q = new QuadraticFunction(H1, g1, 0);
394 LagrangeSolution sol;
395
396 ADMMQPOptimizer solver = new ADMMQPOptimizer();
397 sol = solver.optimize(new ObjectiveFunction(q), iqc, eqc);
398
399 return new LagrangeSolution(sol.getX(), sol.getLambda(),0.0);
400
401 }
402
403 private RealMatrix computeJacobianConstraint(RealVector x) {
404 int me = 0;
405 int mi = 0;
406 RealMatrix je = null;
407 RealMatrix ji = null;
408 if (getEqConstraint() != null) {
409 me = getEqConstraint().dimY();
410 je = getEqConstraint().jacobian(x);
411 }
412
413 if (getIqConstraint() != null) {
414 mi = getIqConstraint().dimY();
415 ji = getIqConstraint().jacobian(x);
416 }
417
418 RealMatrix jacobian = new Array2DRowRealMatrix(me + mi, x.getDimension());
419 if (me > 0) {
420 jacobian.setSubMatrix(je.getData(), 0, 0);
421 }
422 if (mi > 0) {
423 jacobian.setSubMatrix(ji.getData(), me, 0);
424 }
425
426 return jacobian;
427 }
428
429
430
431
432 private RealMatrix BFGSFormula(RealMatrix oldH, RealVector dx, double alfa, RealVector newGrad, RealVector oldGrad) {
433
434 RealMatrix oldH1 = oldH;
435 RealVector y1 = newGrad.subtract(oldGrad);
436 RealVector s = dx.mapMultiply(alfa);
437
438 double theta = 1.0;
439 if (s.dotProduct(y1) <= 0.2 * s.dotProduct(oldH.operate(s))) {
440 theta = 0.8 * s.dotProduct(oldH.operate(s)) / (s.dotProduct(oldH.operate(s)) - s.dotProduct(y1));
441 }
442
443 RealVector y = y1.mapMultiply(theta).add(oldH.operate(s).mapMultiply(1.0 - theta));
444
445 RealMatrix firstTerm = y.outerProduct(y).scalarMultiply(1.0 / s.dotProduct(y));
446 RealMatrix secondTerm = oldH1.multiply(s.outerProduct(s)).multiply(oldH);
447 double thirtTerm = s.dotProduct(oldH1.operate(s));
448 RealMatrix Hnew = oldH1.add(firstTerm).subtract(secondTerm.scalarMultiply(1.0 / thirtTerm));
449
450 EigenDecompositionSymmetric dsX = new EigenDecompositionSymmetric(Hnew);
451 double min = new ArrayRealVector(dsX.getEigenvalues()).getMinValue();
452 if (new ArrayRealVector(dsX.getEigenvalues()).getMinValue() < 0) {
453
454 RealMatrix diag = dsX.getD().
455 add(MatrixUtils.createRealIdentityMatrix(dx.getDimension()).
456 scalarMultiply(getSettings().getEps() - min));
457 Hnew = dsX.getV().multiply(diag.multiply(dsX.getVT()));
458
459 }
460 return Hnew;
461
462 }
463
464 }