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