private OptVector[] CalculateLagrangianHessian(
Func <double[], double> lagrangian,
OptVector[] lagrangianHessian,
OptVector xNew,
OptVector xOld,
OptVector step)
{
OptVector s = step;
OptVector y = new OptVector(numericalDerivative.EvaluatePartialDerivative(lagrangian, xNew.MinArray, 1)) -
new OptVector(numericalDerivative.EvaluatePartialDerivative(lagrangian, xOld.MinArray, 1));
OptVector[] yy = OptVector.Mult(y, y);
double ys = y * s;
OptVector partialDenom = OptVector.Mult(lagrangianHessian, s);
OptVector[] num = OptVector.Mult(partialDenom, OptVector.Mult(s, lagrangianHessian));
double denom = -1.0 * s * partialDenom;
#region Positive definiteness
if (ys < 1E-15)
{
double theta = 0.999999;
OptVector yNew = new OptVector(y);
double ysNew = ys;
while (ysNew < lambda * s * partialDenom &&
theta >= 0.0)
{
yNew = y * theta + (1.0 - theta) * partialDenom;
ysNew = yNew * s;
theta = theta - 1E-5;
}
y = yNew;
ys = ysNew;
yy = OptVector.Mult(y, y);
}
#endregion
if (ys == 0.0)
{
if (step.Length() > 0)
{
return(OptVector.GetIdentity(xNew.Count, step));
}
return(OptVector.GetIdentity(xNew.Count));
}
OptVector[] addParam1 = OptVector.Div(yy, ys);
OptVector[] addParam2 = OptVector.Div(num, denom);
return(OptVector.Sum(OptVector.Sum(lagrangianHessian, addParam1), addParam2));
}
private double[] Execute(
Func <double[], double> f,
List <Func <double[], double> > equalityConstraints,
List <Func <double[], double> > inequalityConstraints,
double[] lowerBound,
double[] upperBound,
double[] startValues,
int nIter)
{
OptVector xOld = new OptVector(startValues);
OptVector xNew = new OptVector(xOld);
OptVector lastFeasibleSolution = new OptVector(xOld);
List <Func <double[], double> > eqConstraints = new List <Func <double[], double> >();
if (equalityConstraints != null)
{
eqConstraints = new List <Func <double[], double> >(equalityConstraints);
}
List <Func <double[], double> > inqConstraints = new List <Func <double[], double> >();
if (inequalityConstraints != null)
{
inqConstraints = new List <Func <double[], double> >(inequalityConstraints);
}
var boundsConstraints = CreateBoundsConstraints(lowerBound, upperBound);
OptVector lambdaEq = new OptVector(eqConstraints.Count);
List <InequalityConstraintProperties> inequalityConstraintsProp = new List <InequalityConstraintProperties>();
foreach (var item in inqConstraints)
{
inequalityConstraintsProp.Add(new InequalityConstraintProperties(false, 0.0, item, false, 0.0, false, -1));
}
foreach (var item in boundsConstraints)
{
inequalityConstraintsProp.Add(new InequalityConstraintProperties(false, 0.0, item.Function, false, 0.0, true, item.Index));
}
OptVector[] hessian = OptVector.GetIdentity(startValues.Length);
int iqIndexStart = -1;
SetInequalityActiveConstraint(xNew, ref inequalityConstraintsProp, ref iqIndexStart);
Func <double[], double> lagrangian = BuildLagrangian(f, eqConstraints, inequalityConstraintsProp, lambdaEq.MinArray);
double directionValue = 0.0;
double lastMinValue = double.MaxValue;
double[] eqPenaltyParams = GetAndSetPenaltyParam(
f,
eqConstraints,
ref inequalityConstraintsProp,
xOld);
for (int i = 0; i < nIter; i++)
{
OptVector lambdaIq = new OptVector(inequalityConstraintsProp.Where(x => x.IsActive).Select(x => x.Lambda).ToArray());
OptVector direction = GetGradientDirection(
xNew,
eqConstraints,
lambdaEq,
inequalityConstraintsProp,
hessian,
lagrangian,
lambdaIq);
OptVector directionX = ExtractX(direction, xOld);
lambdaIq = lambdaIq + ExtractInequalityLambda(direction, xOld, lambdaEq, lambdaIq);
if (RemoveNegativeLambda(ref inequalityConstraintsProp, lambdaIq))
{
lagrangian = BuildLagrangian(f, eqConstraints, inequalityConstraintsProp, lambdaEq.MinArray);
continue;
}
directionValue = directionX * directionX;
if (directionValue < Precision)
{
inequalityConstraintsProp = SetInequalityLambda(inequalityConstraintsProp, lambdaIq);
SetInequalityActiveConstraint(xNew, ref inequalityConstraintsProp, ref iqIndexStart);
if (EarlyExit &&
!inequalityConstraintsProp.Any(x => !x.IsValid))
{
lastFeasibleSolution = xNew;
break;
}
lagrangian = BuildLagrangian(f, eqConstraints, inequalityConstraintsProp, lambdaEq.MinArray);
}
else
{
double step = ComputeStepLength(
f,
equalityConstraints,
inequalityConstraintsProp,
directionX,
xOld,
1.0,
eqPenaltyParams);
OptVector stepValue = step * directionX;
xNew = xOld + stepValue;
OptVector gradient = xNew - xOld;
if (gradient.Length() == 0.0 ||
stepValue.Length() < 1E-10)
{
OptVector nDirection = directionX.Normalize();
step = ComputeStepLength(
f,
equalityConstraints,
inequalityConstraintsProp,
nDirection,
xOld,
1.0,
eqPenaltyParams);
OptVector stVal = step * nDirection;
if (stVal.Length() < localMinCheck)
{
stepValue = nDirection;
hessian = OptVector.GetIdentity(xNew.Count);
}
else
{
stepValue = stVal;
}
xNew = xOld + stepValue;
}
if (xNew.MinArray.Contains(double.NaN))
{
xNew = lastFeasibleSolution;
xOld = xNew;
hessian = OptVector.GetIdentity(xNew.Count);
foreach (var item in inequalityConstraintsProp)
{
item.Lambda = 0.0;
}
lambdaEq = new OptVector(eqConstraints.Count);
eqPenaltyParams = GetAndSetPenaltyParam(
f,
eqConstraints,
ref inequalityConstraintsProp,
xNew);
lagrangian = BuildLagrangian(f, eqConstraints, inequalityConstraintsProp, lambdaEq.MinArray);
continue;
}
lambdaEq = lambdaEq + ExtractEqualityLambda(direction, xOld, lambdaEq);
inequalityConstraintsProp = SetInequalityLambda(inequalityConstraintsProp, lambdaIq);
SetInequalityActiveConstraint(xNew, ref inequalityConstraintsProp, ref iqIndexStart);
hessian = CalculateLagrangianHessian(lagrangian, hessian, xNew, xOld, stepValue);
lagrangian = BuildLagrangian(f, eqConstraints, inequalityConstraintsProp, lambdaEq.MinArray);
//Set penalty params
eqPenaltyParams = SetPenaltyEqParam(eqPenaltyParams, lambdaEq);
SetPenaltyIqParam(ref inequalityConstraintsProp);
//Penalty Parameters
double newEqConstraintsViolation = GetEqualityConstraintsViolation(eqConstraints, xNew);
double minValue = f(xNew.MinArray);
if (minValue < lastMinValue &&
!inequalityConstraintsProp.Any(x => !x.IsValid) &&
newEqConstraintsViolation < equalityConstraintTol)
{
lastFeasibleSolution = xNew;
lastMinValue = minValue;
}
xOld = xNew;
}
}
return(lastFeasibleSolution.MinArray);
}
