private (Matrix <double>, Matrix <double>) Filter(Matrix <double> A, Matrix <double> B) { var Ap = A.SubMatrix(0, 1, 0, A.ColumnCount); var Bp = B.SubMatrix(0, 1, 0, B.ColumnCount); for (int i = 1; i < A.RowCount; i++) { var At = SignalUtils.Stack(Ap, A.SubMatrix(i, 1, 0, A.ColumnCount)); var Bt = SignalUtils.Stack(Bp, B.SubMatrix(i, 1, 0, B.ColumnCount)); if (At.Rank() == At.RowCount) { Ap = At; Bp = Bt; } else if (SignalUtils.Append(At, Bt).Rank() != At.RowCount) { logger.Debug($"Same constrains detected at NO.{i}, which will be ignored."); } else { logger.Debug(SignalUtils.SPrint(At, "At")); logger.Debug(SignalUtils.SPrint(Bt, "Bt")); throw logger.Error(new LigralException($"Conflict constrains detected at NO.{i}.")); } } return(Ap, Bp); }
public override Matrix <double> Optimize(MFunc cost, Matrix <double> x0, MFunc equal, MFunc bound) { int n = x0.RowCount; if (x0.ColumnCount != 1) { throw logger.Error(new LigralException($"Only x with shape nx1 is supported, but we got {n}x{x0.ColumnCount}")); } var x = x0; var c = cost(x); for (int i = 0; i < MaximumIteration; i++) { var H = Algorithm.RoughHessian(cost, x); var C = Algorithm.RoughPartial(cost, x); var Ae = Algorithm.RoughPartial(equal, x); var Be = equal(x); if (Ae != null) { Ae = Ae.Transpose(); (Ae, Be) = Filter(Ae, Be); if (Be.ColumnCount != 1) { throw logger.Error(new LigralException($"Only h(x) with shape mx1 is supported, but we got {Be.RowCount}x{Be.ColumnCount}")); } } logger.Debug(SignalUtils.SPrint(Ae, "Ae")); logger.Debug(SignalUtils.SPrint(Be, "Be")); var Ab = Algorithm.RoughPartial(bound, x); var Bb = bound(x); if (Ab != null) { Ab = Ab.Transpose(); int bRowCount = Bb.RowCount; for (int j = 0, k = 0; j < bRowCount; j++) { if (Bb[j - k, 0] < BoundaryConstrainTolerant) { Ab = Ab.RemoveRow(j - k); Bb = Bb.RemoveRow(j - k); k++; } } } logger.Debug(SignalUtils.SPrint(Ab, "Ab")); logger.Debug(SignalUtils.SPrint(Bb, "Bb")); (var A, var B) = Filter(Ab, Bb, Ae, Be); logger.Debug(SignalUtils.SPrint(A, "A")); logger.Debug(SignalUtils.SPrint(B, "B")); if (B != null && B.ColumnCount != 1) { throw logger.Error(new LigralException($"Only g(x) with shape px1 is supported, but we got {B.RowCount-Be.RowCount}x{B.ColumnCount}")); } var build = Matrix <double> .Build; int mp = B == null?0:B.RowCount; var K = SignalUtils.Stack( SignalUtils.Append(H, A == null?A:A.Transpose()), SignalUtils.Append(A, mp == 0?null:build.Dense(mp, mp, 0)) ); if (K == null) { logger.Warn("No constrain is given, optimizer quit"); return(x); } logger.Debug(SignalUtils.SPrint(K, "K")); var p = SignalUtils.Stack(-C, -B); logger.Debug(SignalUtils.SPrint(p, "p")); var r = K.Solve(p); var s = r.SubMatrix(0, n, 0, 1); logger.Debug(SignalUtils.SPrint(s, "s")); var lambda = r.SubMatrix(n, mp, 0, 1); logger.Debug(SignalUtils.SPrint(lambda, "lambda")); x += s; var cp = cost(x); if (lambda.RowAbsoluteSums().ForAll(sum => sum < OptimizationStopLambdaTolerant)) { logger.Info("SQP Optimizer stops as lambda is close to zero"); break; } else if ((cp - c).RowAbsoluteSums().ForAll(sum => sum < OptimizationStopCostTolerant)) { logger.Info("SQP Optimizer stops as costs did not change."); break; } else { c = cp; } } return(x); }