MeritFunctionL2.java

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

import org.hipparchus.linear.ArrayRealVector;
import org.hipparchus.linear.RealMatrix;
import org.hipparchus.linear.RealVector;
import org.hipparchus.util.FastMath;
import org.hipparchus.util.Precision;


/**
 * Implementation of the Augmented Lagrangian Merit Function.
 * <p>
 * This class implements the penalty function used for the line search procedure in
 * Sequential Quadratic Programming (SQP), as described by <b>Klaus Schittkowski (1982)</b>.
 * </p>
 * <p>
 * The function, often denoted as \(\Phi(x, v, r)\), combines the objective function
 * with equality and inequality constraints using an Augmented Lagrangian formulation.
 * This approach ensures that the search direction \(d\) generated by the QP subproblem
 * is a descent direction for the merit function, provided the penalty parameters \(r_j\)
 * are sufficiently large.
 * </p>
 * * @see <a href="https://apps.dtic.mil/sti/tr/pdf/ADA115667.pdf">
 * Schittkowski (1982): On the convergence of a sequential quadratic programming method</a>
 */
public class MeritFunctionL2 {

    /** Initial value for penalty parameters (typically small, e.g., 1e-2). */
    private static final double SIGMA_INIT = 1.0e-2;

    /** Maximum allowed value for penalty parameters to prevent numerical ill-conditioning. */
    private static final double R_MAX = 1.0e9;

    /** Objective function \(f(x)\). */
    private final TwiceDifferentiableFunction objective;

    /** Equality constraints \(g_j(x) = 0\). */
    private final Constraint eqConstraint;

    /** Inequality constraints \(g_j(x) \ge 0\). */
    private final Constraint iqConstraint;

    /** Linear bounds constraints treated as inequalities. */
    private final LinearInequalityConstraint bounds;

    /** Current point in the primal space \(x\). */
    private RealVector x;

    /** Current estimate of Lagrange multipliers \(y\). */
    private RealVector y;

    /** Search direction \(d\) (solution of the QP subproblem). */
    private RealVector       dx;

    /** Multipliers \(u\) obtained from the solution of the current QP subproblem. */
    private RealVector u;

    /** * Penalty parameters vector \(r\).
     * According to Schittkowski, \(r_j\) must satisfy specific descent conditions.
     */
    private final RealVector r;

    /** Penalty parameters from the previous iteration to maintain stability. */
    private final RealVector rOld;

    /** Gradient of the objective function \(\nabla f(x)\). */
    private RealVector J;

    /** Directional derivative of the merit function in the direction \(dx\). */
    private double penaltyGradient;

    /** Evaluation of the objective function at the current trial point. */
    private double objEval;

    /** Residuals of equality constraints \(h(x) = f(x) - L\). */
    private RealVector eqEval;

    /** Residuals of inequality constraints \(g(x) = f(x) - L\). */
    private RealVector iqEval;

    /** Residuals of box constraints \(b(x) = Ax - L\). */
    private RealVector bEval;

    /** Total value of the Augmented Lagrangian penalty function. */
    private double pEval;

    /** Jacobian matrix of equality constraints. */
    private RealMatrix JE;

    /** Jacobian matrix of inequality constraints. */
    private RealMatrix JI;

    /** Jacobian matrix of box constraints. */
    private RealMatrix JB;

    /**
     * Constructor for the Merit Function.
     *
     * @param objective Objective function to minimize
     * @param eqConstraint Equality constraints
     * @param iqConstraint Inequality constraints
     * @param bounds Linear box constraints
     * @param x Initial starting point
     */
    public MeritFunctionL2(final TwiceDifferentiableFunction objective,
                           final Constraint eqConstraint,
                           final Constraint iqConstraint,
                           final LinearInequalityConstraint bounds,
                           final RealVector x) {
        this.objective = objective;
        this.eqConstraint = eqConstraint;
        this.iqConstraint = iqConstraint;
        this.bounds=bounds;
        this.x = new ArrayRealVector(x);
        int me = 0;
        int mi = 0;
        int mb = 0;
        if (this.eqConstraint != null) {
            me = this.eqConstraint.dimY();
        }
        if (this.iqConstraint != null) {
            mi = this.iqConstraint.dimY();
        }

        if (this.bounds != null) {
            mb = this.bounds.dimY();
            JB=this.bounds.jacobian(null);
        }


        final int m = me + mi + mb;
        this.dx = new ArrayRealVector(x.getDimension());
        this.y = new ArrayRealVector(m);
        this.u = new ArrayRealVector(m);
        this.r = new ArrayRealVector(m, SIGMA_INIT);
        this.rOld=new ArrayRealVector(m, SIGMA_INIT);
        this.J = new ArrayRealVector(x.getDimension());
        this.eqEval = new ArrayRealVector(me);
        this.iqEval = new ArrayRealVector(mi);
        this.bEval = new ArrayRealVector(mb);
        //this evaluate objective function contraints function and penaly
        this.value(0);
    }

    /** * Updates internal states with the results from the QP subproblem.
     * <p>
     * This method must be called before performing a line search to ensure gradients
     * and search directions are consistent with the current iterate.
     * </p>
     * @param newJ Gradient of objective at current x
     * @param newJE Gradient of equality at current x
     * @param newJI Gradient of inequality at current x
     * @param newX Current iterate
     * @param newY Lagrange multipliers from the previous iteration
     * @param newDx Search direction solution from the QP subproblem
     * @param newU Multipliers solution from the QP subproblem
     */
    public void update(final RealVector newJ, final RealMatrix newJE, final RealMatrix newJI,
                       final RealVector newX, final RealVector newY,
                       final RealVector newDx, final RealVector newU) {
        this.J                = newJ;
        this.JE               = newJE;
        this.JI               = newJI;
        this.x                = newX;
        this.y                = newY;
        this.dx               = newDx;
        this.u                = newU;
        this.penaltyGradient = gradient();
    }

    /**
     * Returns the directional derivative of the penalty function.
     * @return \(\nabla \Phi(x)^T d\)
     */
    double getGradient() {
        return this.penaltyGradient;
    }

    /**
     * Returns the current primal point.
     * @return current x
     */
    RealVector getX() {
        return this.x;
    }

    /**
     * Returns the value of the objective function at the current iterate.
     * @return \(f(x)\)
     */
    double getObjEval() {
        return this.objEval;
    }

    /**
     * Returns the residuals of the inequality constraints.
     * @return \(g(x) - L\)
     */
    RealVector getIqEval() {
        return this.iqEval;
    }

     /**
     * Returns the residuals of the equality constraints.
     * @return \(h(x) - L\)
     */
    RealVector getEqEval() {
        return this.eqEval;
    }

    /**
     * Returns the residuals of the box constraints.
     * @return \(Ax - L\)
     */
    RealVector getBEval() {
        return this.bEval;
    }

    /**
     * Returns the total value of the merit function (objective + penalty terms).
     * @return \(\Phi(x, v, r)\)
     */
    double getPenaltyEval() {
        return this.pEval;
    }

    /**
     * Computes the merit function value at the current point (\(\alpha = 0\)).
     * Internal helper for initialization and gradient consistency.
     * @return current penalty value
     */
    public double value0() {
        RealVector xAlpha = x;
        RealVector yAlpha = null;
        if (y.getDimension() > 0) {
            yAlpha = y;
        }


        double penalty =objEval;

        int me = 0;
        if (eqConstraint != null) {
            me = eqConstraint.dimY();
            RealVector re = r.getSubVector(0, me);
            RealVector ye = yAlpha.getSubVector(0, me);

            // eqEval already contains (f(x) - lowerBound)
            RealVector g = this.eqEval;

            RealVector g2 = g.ebeMultiply(g);
            penalty -= ye.dotProduct(g) - 0.5 * re.dotProduct(g2);
        }

        int mi = 0;
        if (iqConstraint != null) {
            mi = iqConstraint.dimY();
            RealVector ri = r.getSubVector(me, mi);
            RealVector yi = yAlpha.getSubVector(me, mi);

            RealVector yk = yAlpha.getSubVector(me, mi);


            // iqEval already contains (f(x) - lowerBound)
            RealVector gk = this.iqEval;

            RealVector g = new ArrayRealVector(gk);
            RealVector mask = new ArrayRealVector(g.getDimension(), 1.0);

            for (int i = 0; i < gk.getDimension(); i++) {
                if (gk.getEntry(i) > (yk.getEntry(i) / ri.getEntry(i))) {
                    mask.setEntry(i, 0.0);
                    penalty -= 0.5 * yi.getEntry(i) * yi.getEntry(i) / ri.getEntry(i);
                }
            }

            RealVector g2 = g.ebeMultiply(g.ebeMultiply(mask));
            penalty -= yi.dotProduct(g.ebeMultiply(mask)) - 0.5 * ri.dotProduct(g2);
        }

        int mb;
        if (bounds != null) {
            mb = bounds.dimY();
            RealVector ri = r.getSubVector(me+mi, mb);
            RealVector yi = yAlpha.getSubVector(me+mi, mb);

            RealVector yk = yAlpha.getSubVector(me+mi, mb);


            // bEval already contains (Ax - lowerBound)
            RealVector gk = this.bEval;

            RealVector g = new ArrayRealVector(gk);
            RealVector mask = new ArrayRealVector(g.getDimension(), 1.0);

            for (int i = 0; i < gk.getDimension(); i++) {
                if (gk.getEntry(i) > (yk.getEntry(i) / ri.getEntry(i))) {
                    mask.setEntry(i, 0.0);
                    penalty -= 0.5 * yi.getEntry(i) * yi.getEntry(i) / ri.getEntry(i);
                }
            }

            RealVector g2 = g.ebeMultiply(g.ebeMultiply(mask));
            penalty -= yi.dotProduct(g.ebeMultiply(mask)) - 0.5 * ri.dotProduct(g2);
        }

        return penalty;
    }

    /**
     * Evaluate merit function for a given step alpha.
     * <p>
     * Optimized version to minimize overhead while maintaining strict numerical
     * consistency with the original backup summation order to prevent floating-point drift.
     * </p>
     * @param alpha line search step length
     * @return penalty value at x + alpha*dx
     */

   public double value(double alpha) {
        RealVector xAlpha = x.add(dx.mapMultiply(alpha));

        RealVector yAlpha = null;
        if (y.getDimension() > 0) {
            yAlpha = y.add(u.subtract(y).mapMultiply(alpha));
        }

        objEval = this.objective.value(xAlpha);
        double penalty =objEval;

        int me = 0;
        if (eqConstraint != null) {
            me = eqConstraint.dimY();
            RealVector re = r.getSubVector(0, me);
            RealVector ye = yAlpha.getSubVector(0, me);
            // Save evaluation directly as residual (f(x) - lower)
            eqEval = eqConstraint.value(xAlpha).subtract(eqConstraint.getLowerBound());
            RealVector g = eqEval;

            RealVector g2 = g.ebeMultiply(g);
            penalty -= ye.dotProduct(g) - 0.5 * re.dotProduct(g2);
        }

        int mi = 0;
        if (iqConstraint != null) {
            mi = iqConstraint.dimY();
            RealVector ri = r.getSubVector(me, mi);
            RealVector yi = yAlpha.getSubVector(me, mi);

            RealVector yk = yAlpha.getSubVector(me, mi);

            // Save evaluation directly as residual (f(x) - lower)
            iqEval = iqConstraint.value(xAlpha).subtract(iqConstraint.getLowerBound());
            RealVector gk = iqEval;

            RealVector g = new ArrayRealVector(gk);
            RealVector mask = new ArrayRealVector(g.getDimension(), 1.0);

            for (int i = 0; i < gk.getDimension(); i++) {
                // Logic according to Schittkowski for handling inequality penalty terms
                // If g_j > y_j / r_j, the constraint is considered sufficiently satisfied (inactive in penalty)
                if (gk.getEntry(i) > (yk.getEntry(i) / ri.getEntry(i))) {
                    mask.setEntry(i, 0.0);
                    penalty -= 0.5 * yi.getEntry(i) * yi.getEntry(i) / ri.getEntry(i);
                }
            }

            RealVector g2 = g.ebeMultiply(g.ebeMultiply(mask));
            penalty -= yi.dotProduct(g.ebeMultiply(mask)) - 0.5 * ri.dotProduct(g2);
        }

        int mb;
        if (bounds != null) {
            mb = bounds.dimY();
            RealVector ri = r.getSubVector(me + mi, mb);
            RealVector yi = yAlpha.getSubVector(me + mi, mb);

            RealVector yk = yAlpha.getSubVector(me + mi, mb);

            // Save evaluation directly as residual (Ax - lower)
            bEval = bounds.value(xAlpha).subtract(bounds.getLowerBound());
            RealVector gk = bEval;

            RealVector g = new ArrayRealVector(gk);
            RealVector mask = new ArrayRealVector(g.getDimension(), 1.0);

            for (int i = 0; i < gk.getDimension(); i++) {
                if (gk.getEntry(i) > (yk.getEntry(i) / ri.getEntry(i))) {
                    mask.setEntry(i, 0.0);
                    penalty -= 0.5 * yi.getEntry(i) * yi.getEntry(i) / ri.getEntry(i);
                }
            }

            RealVector g2 = g.ebeMultiply(g.ebeMultiply(mask));
            penalty -= yi.dotProduct(g.ebeMultiply(mask)) - 0.5 * ri.dotProduct(g2);
        }
        pEval = penalty;
        return penalty;
    }

    /**
     * Computes the directional derivative of the Augmented Lagrangian.
     * <p>
     * This derivative is used to check the Armijo-like descent condition
     * during the line search.
     * </p>
     * @return the total directional derivative
     */
    private double gradient() {
        if (y.getDimension() > 0) {
            return gradX().dotProduct(dx) + gradY().dotProduct(u.subtract(y));
        } else {
            return gradX().dotProduct(dx);
        }
    }


    /**
     * Computes the gradient of the merit function with respect to x.
     * @return \(\nabla_x \Phi(x, v, r)\)
     */
     public RealVector gradX() {
        RealVector partial = J;
        int me = 0;
        int mi = 0;
        int mb = 0;

        if (eqConstraint != null) {
            me = eqConstraint.dimY();
            RealVector re = r.getSubVector(0, me);
            RealVector ye = y.getSubVector(0, me);

            // eqEval is already residual
            RealVector ge = this.eqEval;
            RealMatrix jacob = JE;
            RealVector term = jacob.preMultiply(ye.subtract(ge.ebeMultiply(re)));
            partial = partial.subtract(term);
        }

        if (iqConstraint != null) {
            mi = iqConstraint.dimY();

            RealVector ri = r.getSubVector(me, mi);
            RealVector yi = y.getSubVector(me, mi);
            // iqEval is already residual
            RealVector gi = this.iqEval;
            RealMatrix jacob = JI;
            RealVector mask = new ArrayRealVector(mi, 1.0);

            for (int i = 0; i < gi.getDimension(); i++) {
                if (gi.getEntry(i) > yi.getEntry(i) / ri.getEntry(i)) {
                    mask.setEntry(i, 0.0);
                }
            }

            RealVector term=jacob.preMultiply((yi.subtract(gi.ebeMultiply(ri))).ebeMultiply(mask));
            partial=partial.subtract(term);
        }

        if (bounds != null) {
            mb = bounds.dimY();

            RealVector rb = r.getSubVector(me+mi, mb);

            RealVector yb = y.getSubVector(me+mi, mb);

            // bEval is already residual
            RealVector gb = this.bEval;

            RealMatrix jacob = JB;
            RealVector maskb = new ArrayRealVector(mb, 1.0);

            for (int i = 0; i < gb.getDimension(); i++) {
                if (gb.getEntry(i) > yb.getEntry(i) / rb.getEntry(i)) {
                    maskb.setEntry(i, 0.0);
                }
            }

            RealVector termb=jacob.preMultiply((yb.subtract(gb.ebeMultiply(rb))).ebeMultiply(maskb));
            partial=partial.subtract(termb);
        }

        return partial;
    }


    /**
     * Computes the gradient of the merit function with respect to the multipliers y.
     * @return \(\nabla_y \Phi(x, v, r)\)
     */
     public RealVector gradY() {

        int me = 0;
        int mi = 0;
        int mb = 0;
        RealVector partial = new ArrayRealVector(y.getDimension());
        if (eqConstraint != null) {
            me = eqConstraint.dimY();
            // eqEval is residual
            RealVector g = this.eqEval;
            partial.setSubVector(0, g.mapMultiply(-1.0));
        }

        if (iqConstraint != null) {
            mi = iqConstraint.dimY();

            RealVector ri = r.getSubVector(me, mi);
            RealVector riOld = rOld.getSubVector(me, mi);
            RealVector yi = y.getSubVector(me, mi);
            // iqEval is residual
            RealVector gi = this.iqEval;
            RealVector viri = new ArrayRealVector(mi, 0.0);
            for (int i = 0; i < gi.getDimension(); i++) {
                viri.setEntry(i,
                              gi.getEntry(i) > yi.getEntry(i) / ri.getEntry(i) ?
                              -yi.getEntry(i)/riOld.getEntry(i) :
                              -gi.getEntry(i));
            }

            partial.setSubVector(me, viri);
        }
        if (bounds != null) {
            mb = bounds.dimY();

            RealVector rb = r.getSubVector(me+mi, mb);
            RealVector rbOld = rOld.getSubVector(me+mi, mb);
            RealVector yb = y.getSubVector(me+mi, mb);
            // bEval is residual
            RealVector gb = this.bEval;
            RealVector viri = new ArrayRealVector(mb, 0.0);
            for (int i = 0; i < gb.getDimension(); i++) {
                viri.setEntry(i,
                              gb.getEntry(i) > yb.getEntry(i) / rb.getEntry(i) ?
                              -yb.getEntry(i)/rbOld.getEntry(i) :
                              -gb.getEntry(i));
            }

            partial.setSubVector(me+mi, viri);
        }

        return partial;
    }

    /**
     * Update the weight vector \(r_j\) (Penalty Parameters).
     * <p>
     * This is a critical step in Schittkowski's algorithm. The penalty parameters \(r_j\)
     * are updated after solving the QP subproblem but before the line search.
     * They are chosen such that the search direction \(dx\) is a descent direction for
     * the merit function: \(\nabla \Phi(x, v, r)^T d_{QP} &lt; 0\).
     * </p>
     * @param H Hessian Matrix (or its BFGS approximation)
     * @param newY last estimate of multipliers
     * @param newDx primal direction solution from QP
     * @param newU dual direction (multipliers) from QP
     * @param sigmaValue value of the additional slack variable in augmented QP
     * @param iterations current iteration count
     */
     public void updateRj(RealMatrix H,
                     RealVector newY,
                     RealVector newDx,
                     RealVector newU,
                     double sigmaValue,
                     int iterations) {

    if (newY == null || newY.getDimension() == 0) {
        return;
    }
    this.rOld.setSubVector(0, r.copy());
    final int m = r.getDimension();
    // d^T H d: curvature of the objective function approximation
    final double dbd = newDx.dotProduct(H.operate(newDx));
    final int iter=iterations+1;


    double dbdsigma = R_MAX;
    if (dbd * (1.0 - sigmaValue)  > Precision.SAFE_MIN) {
        // Formula to ensure descent: r_j must be proportional to (u_j - y_j)^2 / d^T H d
        dbdsigma = (2.0 * m) / (dbd * (1.0 - sigmaValue));
    }

    for (int j = 0; j < m; j++) {
        final double rj = r.getEntry(j);
        final double diff = newU.getEntry(j) - newY.getEntry(j);
        final double rNew =  Math.min(R_MAX, dbdsigma * diff * diff);

        // Increase r_j if necessary to maintain descent condition
        final double rjSigmaj  = Math.min(rj, iter * Math.sqrt(rj));
        r.setEntry(j, Math.max(rNew, rjSigmaj));
    }
}



    /** * Resets the penalty parameters vector \(r\) to its initial values.
     * Useful when the optimizer performs a restart or recovery from a bad step.
     */
    public void resetRj() {
        if (y.getDimension() > 0) {
            this.r.set(SIGMA_INIT);
            this.rOld.set(SIGMA_INIT);
        }
     }

    /** * Returns the current search direction.
     * @return search direction dx
     */
    RealVector getDx() {
       return  this.dx;
    }

    /**
     * Increases the penalty parameters globally.
     * Used in augmented QP mode when the standard descent conditions are not met.
     */
    public void rUp()
    {
        if (r != null) {
            for(int i = 0; i < r.getDimension(); i++) {
                r.setEntry(i, FastMath.min(1.0e9, r.getEntry(i) * 10.0));
            }
        }
    }

    /**
     * Returns the penalty parameters vector.
     * @return vector r
     */
    public RealVector getR()
    {
            return this.r;
    }

    /**
     * Returns the maximum allowed penalty value.
     * @return rMax
     */
    public double getRmax()
    {
            return this.R_MAX;
    }
}