SQPOptimizerS.java

  1. /*
  2.  * Licensed to the Hipparchus project under one or more
  3.  * contributor license agreements.  See the NOTICE file distributed with
  4.  * this work for additional information regarding copyright ownership.
  5.  * The Hipparchus project licenses this file to You under the Apache License, Version 2.0
  6.  * (the "License"); you may not use this file except in compliance with
  7.  * the License.  You may obtain a copy of the License at
  8.  *
  9.  *      https://www.apache.org/licenses/LICENSE-2.0
  10.  *
  11.  * Unless required by applicable law or agreed to in writing, software
  12.  * distributed under the License is distributed on an "AS IS" BASIS,
  13.  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14.  * See the License for the specific language governing permissions and
  15.  * limitations under the License.
  16.  */
  17. package org.hipparchus.optim.nonlinear.vector.constrained;

  19. import java.util.ArrayList;
  20. import java.util.Collections;

  22. import org.hipparchus.linear.Array2DRowRealMatrix;
  23. import org.hipparchus.linear.ArrayRealVector;
  24. import org.hipparchus.linear.EigenDecompositionSymmetric;
  25. import org.hipparchus.linear.MatrixUtils;
  26. import org.hipparchus.linear.RealMatrix;
  27. import org.hipparchus.linear.RealVector;
  28. import org.hipparchus.optim.nonlinear.scalar.ObjectiveFunction;
  29. import org.hipparchus.util.FastMath;

  31. /**
  32.  * Sequential Quadratic Programming Optimizer.
  33.  * <br/>
  34.  * min f(x)
  35.  * <br/>
  36.  * q(x)=b1
  37.  * <br/>
  38.  * h(x)>=b2
  39.  * <br/>
  40.  * Algorithm based on paper:"On the convergence of a sequential quadratic
  41.  * programming method(Klaus Shittkowki,January 1982)"
  42.  * @since 3.1
  43.  */
  44. public class SQPOptimizerS extends AbstractSQPOptimizer {

  46.     /** Forgetting factor. */
  47.     private static final int FORGETTING_FACTOR = 10;

  49.     /** Jacobian constraint. */
  50.     private RealMatrix constraintJacob;

  52.     /** Value of the equality constraint. */
  53.     private RealVector equalityEval;

  55.     /** Value of the inequality constraint. */
  56.     private RealVector inequalityEval;

  58.     /** Gradient of the objective function. */
  59.     private RealVector functionGradient;

  61.     /** Hessian of the objective function. */
  62.     private RealMatrix hessian;

  64.     /** {@inheritDoc} */
  65.     @Override
  66.     public LagrangeSolution doOptimize() {
  67.         int me = 0;
  68.         int mi = 0;

  70.         //EQUALITY CONSTRAINT
  71.         if (this.getEqConstraint() != null) {
  72.             me = getEqConstraint().dimY();
  73.         }
  74.         //INEQUALITY CONSTRAINT
  75.         if (this.getIqConstraint() != null) {
  76.             mi = getIqConstraint().dimY();
  77.         }

  79.         double alpha;
  80.         double rho = 100.0;
  81.         int failedSearch = 0;
  82.         RealVector x;
  83.         if (this.getStartPoint() != null) {
  84.             x = new ArrayRealVector(this.getStartPoint());
  85.         } else {
  86.             x = new ArrayRealVector(this.getObj().dim());
  87.         }

  89.         RealVector y = new ArrayRealVector(me + mi, 0.0);
  90.         RealVector r = new ArrayRealVector(me + mi, 1.0);
  91.         ArrayList<Double> oldPenalty = new ArrayList<>();
  92.         //INITIAL VALUES
  93.         double functionEval = this.getObj().value(x);
  94.         functionGradient = this.getObj().gradient(x);
  95.         double maxGrad = functionGradient.getLInfNorm();


  98.         if (this.getEqConstraint() != null) {

  100.             equalityEval = this.getEqConstraint().value(x);
  101.         }
  102.         if (this.getIqConstraint() != null) {

  104.             inequalityEval = this.getIqConstraint().value(x);
  105.         }
  106.         constraintJacob = computeJacobianConstraint(x);

  108.         if (this.getEqConstraint() != null) {
  109.             maxGrad = FastMath.max(maxGrad, equalityEval.getLInfNorm());
  110.         }
  111.         if (this.getIqConstraint() != null) {
  112.             maxGrad = FastMath.max(maxGrad, inequalityEval.getLInfNorm());
  113.         }

  115.         if (!getSettings().useFunHessian()) {
  116.             hessian = MatrixUtils.createRealIdentityMatrix(x.getDimension());
  117.         } else {
  118.             hessian = this.getObj().hessian(x);
  119.         }

  121.         for (int i = 0; i < this.getMaxIterations(); i++) {
  122.             iterations.increment();

  124.             alpha = 1.0;
  125.             LagrangeSolution sol1 = null;

  127.             int qpLoop = 0;
  128.             double sigma = maxGrad;
  129.             double currentPenaltyGrad;

  131.             //LOOP TO FIND SOLUTION WITH SIGMA<SIGMA THRESHOLD
  132.             while (sigma > getSettings().getSigmaMax() && qpLoop < getSettings().getQpMaxLoop()) {
  133.                 sol1 = solveQP(x, y, rho);
  134.                 sigma = sol1.getValue();

  136.                 if (sigma > getSettings().getSigmaMax()) {
  137.                     rho = getSettings().getRhoCons() * rho;
  138.                     qpLoop += 1;

  140.                 }

  142.             }
  143.             //IF SIGMA>SIGMA THRESHOLD ASSIGN DIRECTION FROM PENALTY GRADIENT
  144.             final RealVector dx;
  145.             final RealVector dy;
  146.             if (qpLoop == getSettings().getQpMaxLoop()) {

  148.                 dx = (MatrixUtils.inverse(hessian).operate(penaltyFunctionGradX(functionGradient, x, y, r))).mapMultiply(-1.0);
  149.                 dy = y.subtract(penaltyFunctionGradY(x, y, r));
  150.                 sigma = 0;

  152.             } else {
  153.                 dx = sol1.getX();
  154.                 sigma = sol1.getValue();
  155.                 if (sigma < 0) {
  156.                     sigma = 0;
  157.                 }
  158.                 dy = sol1.getLambda();
  159.                 r = updateRj(hessian, y, dx, dy, r, sigma);
  160.             }

  162.             currentPenaltyGrad = penaltyFunctionGrad(functionGradient, dx, dy, x, y, r);
  163.             int search = 0;

  165.             rho = updateRho(dx, dy, hessian, constraintJacob, sigma);

  167.             double currentPenalty = penaltyFunction(functionEval, 0, x, y, dx, dy.subtract(y), r);

  169.             double alphaF = this.getObj().value(x.add(dx.mapMultiply(alpha)));
  170.             double alfaPenalty = penaltyFunction(alphaF, alpha, x, y, dx, dy.subtract(y), r);


  173.             //LINE SEARCH

  175.             while ((alfaPenalty - currentPenalty) >= getSettings().getMu() * alpha * currentPenaltyGrad &&
  176.                     search < getSettings().getMaxLineSearchIteration()) {
  177.                 double alfaStar = -0.5 * alpha * alpha * currentPenaltyGrad / (-alpha * currentPenaltyGrad + alfaPenalty - currentPenalty);


  180.                 alpha = FastMath.max(getSettings().getB() * alpha, FastMath.min(1.0, alfaStar));
  181.                 alphaF = this.getObj().value(x.add(dx.mapMultiply(alpha)));
  182.                 alfaPenalty = penaltyFunction(alphaF, alpha, x, y, dx, dy.subtract(y), r);
  183.                 search = search + 1;

  185.             }



  189.             if (getSettings().getConvCriteria() == 0) {
  190.                 if (dx.mapMultiply(alpha).dotProduct(hessian.operate(dx.mapMultiply(alpha))) < getSettings().getEps() * getSettings().getEps()) {
  191. //                    x = x.add(dx.mapMultiply(alfa));
  192. //                    y = y.add((dy.subtract(y)).mapMultiply(alfa));
  193.                     break;
  194.                 }
  195.             } else {
  196.                 if (alpha * dx.getNorm() < getSettings().getEps() * (1 + x.getNorm())) {
  197. //                    x = x.add(dx.mapMultiply(alfa));
  198. //                    y = y.add((dy.subtract(y)).mapMultiply(alfa));
  199.                     break;
  200.                 }

  202.             }

  204.             if (search == getSettings().getMaxLineSearchIteration()) {
  205.                 failedSearch += 1;
  206.             }

  208.             boolean notMonotone = false;
  209.             if (search == getSettings().getMaxLineSearchIteration()) {

  211.                 alpha = 1.0;
  212.                 search = 0;
  213.                 Double max = Collections.max(oldPenalty);
  214.                 if (oldPenalty.size() == 1) {
  215.                     max = max * 1.3;
  216.                 }
  217.                 while ((alfaPenalty - max) >= getSettings().getMu() * alpha * currentPenaltyGrad &&
  218.                        search < getSettings().getMaxLineSearchIteration()) {

  220.                     double alfaStar = -0.5 * alpha * alpha * currentPenaltyGrad / (-alpha * currentPenaltyGrad + alfaPenalty - currentPenalty);


  223.                     alpha = FastMath.max(getSettings().getB() * alpha, FastMath.min(1.0, alfaStar));
  224.                     // alfa = FastMath.min(1.0, FastMath.max(this.b * alfa, alfaStar));
  225.                     // alfa = FastMath.max(this.b * alfa, alfaStar);
  226.                     alphaF = this.getObj().value(x.add(dx.mapMultiply(alpha)));
  227.                     alfaPenalty = penaltyFunction(alphaF, alpha, x, y, dx, dy.subtract(y), r);
  228.                     search = search + 1;

  230.                 }
  231.                 notMonotone = true;
  232.                 // if (search < maxLineSearchIteration) failedSearch = 0;
  233.             }

  235.             //UPDATE ALL FUNCTION
  236.             RealVector oldGradient = functionGradient;
  237.             RealMatrix oldJacob = constraintJacob;
  238.             // RealVector old1 = penaltyFunctionGradX(oldGradient,x, y.add((dy.subtract(y)).mapMultiply(alfa)),r);
  239.             RealVector old1 = lagrangianGradX(oldGradient, oldJacob, x, y.add((dy.subtract(y)).mapMultiply(alpha)));
  240.             functionEval = alphaF;
  241.             functionGradient = this.getObj().gradient(x.add(dx.mapMultiply(alpha)));
  242.             if (this.getEqConstraint() != null) {

  244.                 equalityEval = this.getEqConstraint().value(x.add(dx.mapMultiply(alpha)));
  245.             }
  246.             if (this.getIqConstraint() != null) {

  248.                 inequalityEval = this.getIqConstraint().value(x.add(dx.mapMultiply(alpha)));
  249.             }

  251.             constraintJacob = computeJacobianConstraint(x.add(dx.mapMultiply(alpha)));
  252.             // RealVector new1 = penaltyFunctionGradX(functionGradient,x.add(dx.mapMultiply(alfa)), y.add((dy.subtract(y)).mapMultiply(alfa)),r);
  253.             RealVector new1 = lagrangianGradX(functionGradient, constraintJacob, x.add(dx.mapMultiply(alpha)), y.add((dy.subtract(y)).mapMultiply(alpha)));

  255.             if (failedSearch == 2) {
  256.                 hessian = MatrixUtils.createRealIdentityMatrix(x.getDimension());
  257.                 failedSearch = 0;
  258.             }

  260.             if (!notMonotone) {
  261. //                if (iterations.getCount()==1)
  262. //                {
  263. //                    RealVector yfirst = new1.subtract(old1);
  264. //                    RealVector sfirst = dx.mapMultiply(alfa);
  265. //                    double scaleFactor = yfirst.dotProduct(sfirst)/yfirst.dotProduct(yfirst);
  266. //                    hessian = hessian.scalarMultiply(scaleFactor);
  267. //                }
  268.                 hessian = BFGSFormula(hessian, dx, alpha, new1, old1);
  269.             }

  271.             //STORE PENALTY IN QUEQUE TO PERFORM NOT MONOTONE LINE SEARCH IF NECESSARY
  272.             // oldPenalty.add(alfaPenalty);
  273.             oldPenalty.add(currentPenalty);
  274.             if (oldPenalty.size() > FORGETTING_FACTOR) {
  275.                 oldPenalty.remove(0);
  276.             }

  278.             x = x.add(dx.mapMultiply(alpha));
  279.             y = y.add((dy.subtract(y)).mapMultiply(alpha));
  280.         }

  282.         return new LagrangeSolution(x, y, functionEval);
  283.     }

  285.     private double penaltyFunction(double currentF, double alfa, RealVector x, RealVector y, RealVector dx, RealVector uv, RealVector r) {
  286.         // the set of constraints is the same as the previous one but they must be evaluated with the increment

  288.         int me = 0;
  289.         int mi;
  290.         double partial = currentF;
  291.         RealVector yalfa = y.add(uv.mapMultiply(alfa));
  292.         if (getEqConstraint() != null) {
  293.             me = getEqConstraint().dimY();
  294.             RealVector re = r.getSubVector(0, me);
  295.             RealVector ye = yalfa.getSubVector(0, me);
  296.             RealVector g = this.getEqConstraint().value(x.add(dx.mapMultiply(alfa))).subtract(getEqConstraint().getLowerBound());
  297.             RealVector g2 = g.ebeMultiply(g);
  298.             partial -= ye.dotProduct(g) - 0.5 * re.dotProduct(g2);
  299.         }

  301.         if (getIqConstraint() != null) {
  302.             mi = getIqConstraint().dimY();
  303.             RealVector ri = r.getSubVector(me, mi);
  304.             RealVector yi = yalfa.getSubVector(me, mi);
  305.             RealVector yk = y.getSubVector(me, mi);
  306.             RealVector gk = this.inequalityEval.subtract(getIqConstraint().getLowerBound());
  307.             RealVector g = this.getIqConstraint().value(x.add(dx.mapMultiply(alfa))).subtract(getIqConstraint().getLowerBound());
  308.             RealVector mask = new ArrayRealVector(g.getDimension(), 1.0);

  310.             for (int i = 0; i < gk.getDimension(); i++) {

  312.                 if (gk.getEntry(i) > (yk.getEntry(i) / ri.getEntry(i))) {

  314.                     mask.setEntry(i, 0.0);

  316.                     partial -= 0.5 * yi.getEntry(i) * yi.getEntry(i) / ri.getEntry(i);
  317.                 }

  319.             }

  321.             RealVector g2 = g.ebeMultiply(g.ebeMultiply(mask));
  322.             partial -= yi.dotProduct(g.ebeMultiply(mask)) - 0.5 * ri.dotProduct(g2);

  324.         }

  326.         return partial;
  327.     }

  329.     private double penaltyFunctionGrad(RealVector currentGrad, RealVector dx, RealVector dy, RealVector x, RealVector y, RealVector r) {

  331.         int me = 0;
  332.         int mi;
  333.         double partial = currentGrad.dotProduct(dx);
  334.         if (getEqConstraint() != null) {
  335.             me = getEqConstraint().dimY();
  336.             RealVector re = r.getSubVector(0, me);
  337.             RealVector ye = y.getSubVector(0, me);
  338.             RealVector dye = dy.getSubVector(0, me);
  339.             RealVector ge = this.getEqConstraint().value(x).subtract(getEqConstraint().getLowerBound());
  340.             RealMatrix jacob = this.getEqConstraint().jacobian(x);
  341.             RealVector firstTerm = jacob.transpose().operate(ye);
  342.             RealVector secondTerm = jacob.transpose().operate(ge.ebeMultiply(re));
  343. //partial -= firstTerm.dotProduct(dx) - secondTerm.dotProduct(dx) + g.dotProduct(dye);
  344.             partial += -firstTerm.dotProduct(dx) + secondTerm.dotProduct(dx) - ge.dotProduct(dye.subtract(ye));
  345.         }

  347.         if (getIqConstraint() != null) {
  348.             mi = getIqConstraint().dimY();

  350.             RealVector ri = r.getSubVector(me, mi);
  351.             RealVector dyi = dy.getSubVector(me, mi);
  352.             RealVector yi = y.getSubVector(me, mi);

  354.             RealVector gi = this.getIqConstraint().value(x).subtract(getIqConstraint().getLowerBound());
  355.             RealMatrix jacob = this.getIqConstraint().jacobian(x);

  357.             RealVector mask = new ArrayRealVector(mi, 1.0);
  358.             RealVector viri = new ArrayRealVector(mi, 0.0);

  360.             for (int i = 0; i < gi.getDimension(); i++) {

  362.                 if (gi.getEntry(i) > yi.getEntry(i) / ri.getEntry(i)) {
  363.                     mask.setEntry(i, 0.0);

  365.                 } else {
  366.                     viri.setEntry(i, yi.getEntry(i) / ri.getEntry(i));
  367.                 }

  369.             }
  370.             RealVector firstTerm = jacob.transpose().operate(yi.ebeMultiply(mask));
  371.             RealVector secondTerm = jacob.transpose().operate(gi.ebeMultiply(ri.ebeMultiply(mask)));

  373.             partial -= firstTerm.dotProduct(dx) - secondTerm.dotProduct(dx) + (gi.ebeMultiply(mask)).dotProduct(dyi.subtract(yi)) + viri.dotProduct(dyi.subtract(yi));
  374.             // partial -= firstTerm.dotProduct(dx) - secondTerm.dotProduct(dx) + g.dotProduct(dyi.ebeMultiply(mask));
  375.         }

  377.         return partial;
  378.     }

  380.     private RealVector penaltyFunctionGradX(RealVector currentGrad, RealVector x, RealVector y, RealVector r) {

  382.         int me = 0;
  383.         int mi;
  384.         RealVector partial = currentGrad.copy();
  385.         if (getEqConstraint() != null) {
  386.             me = getEqConstraint().dimY();
  387.             RealVector re = r.getSubVector(0, me);
  388.             RealVector ye = y.getSubVector(0, me);

  390.             RealVector ge = this.getEqConstraint().value(x).subtract(getEqConstraint().getLowerBound());
  391.             RealMatrix jacob = this.getEqConstraint().jacobian(x);

  393.             RealVector firstTerm = jacob.transpose().operate(ye);
  394.             RealVector secondTerm = jacob.transpose().operate(ge.ebeMultiply(re));

  396.             partial = partial.subtract(firstTerm.subtract(secondTerm));
  397.         }

  399.         if (getIqConstraint() != null) {
  400.             mi = getIqConstraint().dimY();

  402.             RealVector ri = r.getSubVector(me, mi);

  404.             RealVector yi = y.getSubVector(me, mi);
  405.             RealVector gi = this.getIqConstraint().value(x).subtract(getIqConstraint().getLowerBound());
  406.             RealMatrix jacob = this.getIqConstraint().jacobian(x);

  408.             RealVector mask = new ArrayRealVector(mi, 1.0);

  410.             for (int i = 0; i < gi.getDimension(); i++) {

  412.                 if (gi.getEntry(i) > yi.getEntry(i) / ri.getEntry(i)) {
  413.                     mask.setEntry(i, 0.0);

  415.                 }

  417.             }
  418.             RealVector firstTerm = jacob.transpose().operate(yi.ebeMultiply(mask));
  419.             RealVector secondTerm = jacob.transpose().operate(gi.ebeMultiply(ri.ebeMultiply(mask)));
  420.             partial = partial.subtract(firstTerm.subtract(secondTerm));
  421.         }

  423.         return partial;
  424.     }

  426.     private RealVector penaltyFunctionGradY(RealVector x, RealVector y, RealVector r) {

  428.         int me = 0;
  429.         int mi;
  430.         RealVector partial = new ArrayRealVector(y.getDimension());
  431.         if (getEqConstraint() != null) {
  432.             me = getEqConstraint().dimY();
  433.             RealVector g = this.getEqConstraint().value(x).subtract(getEqConstraint().getLowerBound());
  434.             partial.setSubVector(0, g.mapMultiply(-1.0));
  435.         }

  437.         if (getIqConstraint() != null) {
  438.             mi = getIqConstraint().dimY();

  440.             RealVector ri = r.getSubVector(me, mi);

  442.             RealVector yi = y.getSubVector(me, mi);
  443.             RealVector gi = this.getIqConstraint().value(x).subtract(getIqConstraint().getLowerBound());

  445.             RealVector mask = new ArrayRealVector(mi, 1.0);

  447.             RealVector viri = new ArrayRealVector(mi, 0.0);

  449.             for (int i = 0; i < gi.getDimension(); i++) {

  451.                 if (gi.getEntry(i) > yi.getEntry(i) / ri.getEntry(i)) {
  452.                     mask.setEntry(i, 0.0);

  454.                 } else {
  455.                     viri.setEntry(i, yi.getEntry(i) / ri.getEntry(i));
  456.                 }

  458.             }

  460.             partial.setSubVector(me, gi.ebeMultiply(mask).add(viri).mapMultiply(-1.0));
  461.         }

  463.         return partial;
  464.     }

  466.     private RealVector updateRj(RealMatrix H, RealVector y, RealVector dx, RealVector dy, RealVector r, double additional) { //r = updateRj(currentH,dx,y,u,r,sigm);
  467.         //CALCULATE SIGMA VECTOR THAT DEPEND FROM ITERATION
  468.         RealVector sigma = new ArrayRealVector(r.getDimension());
  469.         for (int i = 0; i < sigma.getDimension(); i++) {
  470.             final double appoggio = iterations.getCount() / FastMath.sqrt(r.getEntry(i));
  471.             sigma.setEntry(i, FastMath.min(1.0, appoggio));
  472.         }

  474.         int me = 0;
  475.         int mi = 0;
  476.         if (getEqConstraint() != null) {
  477.             me = getEqConstraint().dimY();
  478.         }
  479.         if (getIqConstraint() != null) {
  480.             mi = getIqConstraint().dimY();
  481.         }

  483.         RealVector sigmar = sigma.ebeMultiply(r);
  484.         //(u-v)^2 or (ru-v)
  485.         RealVector numerator = ((dy.subtract(y)).ebeMultiply(dy.subtract(y))).mapMultiply(2.0 * (mi + me));

  487.         double denominator = dx.dotProduct(H.operate(dx)) * (1.0 - additional);
  488.         RealVector r1 = r.copy();
  489.         if (getEqConstraint() != null) {
  490.             for (int i = 0; i < me; i++) {
  491.                 r1.setEntry(i, FastMath.max(sigmar.getEntry(i), numerator.getEntry(i) / denominator));
  492.             }
  493.         }
  494.         if (getIqConstraint() != null) {
  495.             for (int i = 0; i < mi; i++) {
  496.                 r1.setEntry(me + i, FastMath.max(sigmar.getEntry(me + i), numerator.getEntry(me + i) / denominator));
  497.             }
  498.         }


  501.         return r1;
  502.     }

  504.     private double updateRho(RealVector dx, RealVector dy, RealMatrix H, RealMatrix jacobianG, double additionalVariable) {

  506.         double num = 10.0 * FastMath.pow(dx.dotProduct(jacobianG.transpose().operate(dy)), 2);
  507.         double den = (1.0 - additionalVariable) * (1.0 - additionalVariable) * dx.dotProduct(H.operate(dx));
  508.         //double den = (1.0 - additionalVariable) * dx.dotProduct(H.operate(dx));

  510.         return FastMath.max(10.0, num / den);
  511.     }

  513.     private LagrangeSolution solveQP(RealVector x, RealVector y, double rho) {

  515.         RealMatrix H = hessian;
  516.         RealVector g = functionGradient;

  518.         int me = 0;
  519.         int mi = 0;
  520.         int add = 0;
  521.         boolean violated = false;
  522.         if (getEqConstraint() != null) {
  523.             me = getEqConstraint().dimY();
  524.         }
  525.         if (getIqConstraint() != null) {

  527.             mi = getIqConstraint().dimY();
  528.             violated = inequalityEval.subtract(getIqConstraint().getLowerBound()).getMinValue() <= getSettings().getEps() ||
  529.                        y.getMaxValue() >= 0;

  531.         }
  532.         // violated = true;
  533.         if (me > 0 || violated) {
  534.             add = 1;

  536.         }

  538.         RealMatrix H1 = new Array2DRowRealMatrix(H.getRowDimension() + add, H.getRowDimension() + add);
  539.         H1.setSubMatrix(H.getData(), 0, 0);
  540.         if (add == 1) {
  541.             H1.setEntry(H.getRowDimension(), H.getRowDimension(), rho);
  542.         }

  544.         RealVector g1 = new ArrayRealVector(g.getDimension() + add);
  545.         g1.setSubVector(0, g);

  547.         LinearEqualityConstraint eqc = null;
  548.         RealVector conditioneq;
  549.         if (getEqConstraint() != null) {
  550.             RealMatrix eqJacob = constraintJacob.getSubMatrix(0, me - 1, 0, x.getDimension() - 1);

  552.             RealMatrix Ae = new Array2DRowRealMatrix(me, x.getDimension() + add);
  553.             RealVector be = new ArrayRealVector(me);
  554.             Ae.setSubMatrix(eqJacob.getData(), 0, 0);
  555.             conditioneq = this.equalityEval.subtract(getEqConstraint().getLowerBound());
  556.             Ae.setColumnVector(x.getDimension(), conditioneq.mapMultiply(-1.0));

  558.             be.setSubVector(0, getEqConstraint().getLowerBound().subtract(this.equalityEval));
  559.             eqc = new LinearEqualityConstraint(Ae, be);

  561.         }
  562.         LinearInequalityConstraint iqc = null;

  564.         if (getIqConstraint() != null) {
  565. //
  566.             RealMatrix iqJacob = constraintJacob.getSubMatrix(me, me + mi - 1, 0, x.getDimension() - 1);

  568.             RealMatrix Ai = new Array2DRowRealMatrix(mi, x.getDimension() + add);
  569.             RealVector bi = new ArrayRealVector(mi);
  570.             Ai.setSubMatrix(iqJacob.getData(), 0, 0);

  572.             RealVector conditioniq = this.inequalityEval.subtract(getIqConstraint().getLowerBound());

  574.             if (add == 1) {

  576.                 for (int i = 0; i < conditioniq.getDimension(); i++) {

  578.                     if (!(conditioniq.getEntry(i) <= getSettings().getEps() || y.getEntry(me + i) > 0)) {
  579.                         conditioniq.setEntry(i, 0);
  580.                     }
  581.                 }

  583.                 Ai.setColumnVector(x.getDimension(), conditioniq.mapMultiply(-1.0));

  585.             }
  586.             bi.setSubVector(0, getIqConstraint().getLowerBound().subtract(this.inequalityEval));
  587.             iqc = new LinearInequalityConstraint(Ai, bi);

  589.         }
  590.         LinearBoundedConstraint bc = null;
  591.         if (add == 1) {

  593.             RealMatrix sigmaA = new Array2DRowRealMatrix(1, x.getDimension() + 1);
  594.             sigmaA.setEntry(0, x.getDimension(), 1.0);
  595.             ArrayRealVector lb = new ArrayRealVector(1, 0.0);
  596.             ArrayRealVector ub = new ArrayRealVector(1, 1.0);

  598.             bc = new LinearBoundedConstraint(sigmaA, lb, ub);

  600.         }

  602.         QuadraticFunction q = new QuadraticFunction(H1, g1, 0);

  604.         ADMMQPOptimizer solver = new ADMMQPOptimizer();
  605.         LagrangeSolution sol = solver.optimize(new ObjectiveFunction(q), iqc, eqc, bc);

  607.         final double sigma;
  608.         if (add == 1) {
  609.             sigma = sol.getX().getEntry(x.getDimension());
  610.         } else {
  611.             sigma = 0;
  612.         }

  614.         return new LagrangeSolution(sol.getX().getSubVector(0, x.getDimension()),
  615.                                     sol.getLambda().getSubVector(0, me + mi),
  616.                                     sigma);

  618.     }

  620.     private RealMatrix computeJacobianConstraint(RealVector x) {
  621.         int me = 0;
  622.         int mi = 0;
  623.         RealMatrix je = null;
  624.         RealMatrix ji = null;
  625.         if (this.getEqConstraint() != null) {
  626.             me = this.getEqConstraint().dimY();
  627.             je = this.getEqConstraint().jacobian(x);
  628.         }

  630.         if (this.getIqConstraint() != null) {
  631.             mi = this.getIqConstraint().dimY();
  632.             ji = this.getIqConstraint().jacobian(x);
  633.         }

  635.         RealMatrix jacobian = new Array2DRowRealMatrix(me + mi, x.getDimension());
  636.         if (me > 0) {
  637.             jacobian.setSubMatrix(je.getData(), 0, 0);
  638.         }
  639.         if (mi > 0) {
  640.             jacobian.setSubMatrix(ji.getData(), me, 0);
  641.         }

  643.         return jacobian;
  644.     }
  645.     /*
  646.      *DAMPED BFGS FORMULA
  647.      */

  649.     private RealMatrix BFGSFormula(RealMatrix oldH, RealVector dx, double alfa, RealVector newGrad, RealVector oldGrad) {

  651.         RealMatrix oldH1 = oldH.copy();
  652.         RealVector y1 = newGrad.subtract(oldGrad);
  653.         RealVector s = dx.mapMultiply(alfa);
  654.         double theta = 1.0;
  655.         if (s.dotProduct(y1) < 0.2 * s.dotProduct(oldH.operate(s))) {
  656.             theta = 0.8 * s.dotProduct(oldH.operate(s)) / (s.dotProduct(oldH.operate(s)) - s.dotProduct(y1));
  657.         }

  659.         RealVector y = y1.mapMultiply(theta).add((oldH.operate(s)).mapMultiply(1.0 - theta));

  661.         RealMatrix firstTerm = y.outerProduct(y).scalarMultiply(1.0 / s.dotProduct(y));
  662.         RealMatrix secondTerm = oldH1.multiply(s.outerProduct(s)).multiply(oldH);
  663.         double thirtTerm = s.dotProduct(oldH1.operate(s));
  664.         RealMatrix Hnew = oldH1.add(firstTerm).subtract(secondTerm.scalarMultiply(1.0 / thirtTerm));
  665.         //RESET HESSIAN IF NOT POSITIVE DEFINITE
  666.         EigenDecompositionSymmetric dsX = new EigenDecompositionSymmetric(Hnew);
  667.         double min = new ArrayRealVector(dsX.getEigenvalues()).getMinValue();
  668.         if (min < 0) {
  669.             Hnew = MatrixUtils.createRealIdentityMatrix(oldH.getRowDimension());
  670.         }
  671.         return Hnew;

  673.     }

  675. }
Created with JaCoCo 0.8.12.202403310830