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} < 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;
}
}