public double GetAlpha(
OptVector[] A,
OptVector p,
double num)
{
var denom = p * OptVector.Mult(A, p);
if (denom == 0.0)
{
return(1.0);
}
return(num / denom);
}
public OptVector Solve(
OptVector[] A,
OptVector b,
OptVector startX,
int nIter)
{
OptVector[] normA = A;
OptVector normb = b;
if (!OptVector.Equals(A, OptVector.Transpose(A)))
{
OptVector[] At = OptVector.Transpose(A);
normA = OptVector.Mult(At, A);
normb = OptVector.Mult(At, b);
}
OptVector rNew = normb - OptVector.Mult(normA, startX);
OptVector p = rNew;
OptVector x = new OptVector(startX);
double r2Old = rNew * rNew;
double alpha = 1.0;
double beta = 1.0;
for (int i = 0; i < nIter; i++)
{
alpha = GetAlpha(normA, p, r2Old);
x = x + alpha * p;
rNew = rNew - alpha * OptVector.Mult(normA, p);
double r2New = rNew * rNew;
if (r2New < Precision)
{
return(x);
}
beta = GetBeta(r2New, r2Old);
p = rNew + beta * p;
r2Old = r2New;
}
return(x);
}
static void TestFunc()
{
OptVector a = new OptVector(new double[] { 1, 2 });
OptVector b = new OptVector(new double[] { 3, 4 });
OptVector c = new OptVector(new double[] { 1, 3 });
OptVector d = new OptVector(new double[] { 4, 7 });
var res = OptVector.Mult(a, b);
var res1 = OptVector.SubtractFromIdentity(res);
var res2 = OptVector.Div(res, 2);
OptVector[] aa = new OptVector[] { a, b };
OptVector[] bb = new OptVector[] { c, d };
var res3 = OptVector.Mult(res, bb);
var res4 = OptVector.Sum(res, bb);
var res5 = OptVector.Mult(res, c);
}
static void TestCGMethod()
{
MINRES minres = new MINRES();
CGMethod cg = new CGMethod();
OptVector[] A2 = new OptVector[3];
A2[0] = new OptVector(new double[] { 2, 1, 3 });
A2[1] = new OptVector(new double[] { 2, 6, 8 });
A2[2] = new OptVector(new double[] { 6, 8, 18 });
OptVector b2 = new OptVector(new double[] { 1, 3, 5 });
var minresSol = minres.Solve(A2, b2, new OptVector(new double[3]), 50);
var sol2 = cg.Solve(A2, b2, new OptVector(new double[3]), 50);
OptVector[] A = new OptVector[3];
A[0] = new OptVector(new double[] { 2, 0, 1 });
A[1] = new OptVector(new double[] { 1, 6, 0 });
A[2] = new OptVector(new double[] { 3, 2, 3 });
OptVector x = new OptVector(new double[] { 2, 5, 7 });
var sol = cg.Solve(A, x, new OptVector(new double[3]), 50);
var solm = minres.Solve(A, x, new OptVector(new double[3]), 50);
OptVector[] A1 = new OptVector[3];
A1[0] = new OptVector(new double[] { 3, 1, -6 });
A1[1] = new OptVector(new double[] { 2, 1, -5 });
A1[2] = new OptVector(new double[] { 6, -3, 3 });
OptVector b1 = new OptVector(new double[] { -10, -8, 0 });
var sol1 = cg.Solve(A1, b1, new OptVector(new double[3]), 200);
var solm1 = minres.Solve(A1, b1, new OptVector(new double[3]), 200);
OptVector diff = b1 - OptVector.Mult(A1, solm1);
OptVector diff1 = b1 - OptVector.Mult(A1, sol1);
}
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));
}
public OptVector Solve(
OptVector[] A,
OptVector b,
OptVector startX,
int nIter)
{
OptVector[] symmA = A;
OptVector normb = b;
//Symmetrize matrix
if (CheckSymmetry)
{
OptVector[] At = OptVector.Transpose(A);
if (!OptVector.Equals(A, At))
{
symmA = OptVector.Mult(At, A);
normb = OptVector.Mult(At, b);
}
}
OptVector v0 = new OptVector(b.Count);
OptVector v1 = normb - OptVector.Mult(symmA, startX);
double beta1 = v1.Length();
double betaN = 0.0;
double n = beta1;
double c0 = 1.0;
double c1 = 1.0;
double s0 = 0.0;
double s1 = 0.0;
OptVector w0 = new OptVector(v1.Count);
OptVector w_1 = new OptVector(v1.Count);
OptVector x = new OptVector(startX);
for (int i = 0; i < nIter; i++)
{
//Calculate Lanczos Vectors
OptVector v = (1.0 / beta1) * v1;
OptVector Av = OptVector.Mult(symmA, v);
double alpha = v * Av;
v1 = Av - alpha * v - beta1 * v0;
betaN = v1.Length();
//Calculate QR factors
double lambda = c1 * alpha - c0 * s1 * beta1;
double p1 = Math.Sqrt(lambda * lambda + betaN * betaN);
double p2 = s1 * alpha + c0 * c1 * beta1;
double p3 = s0 * beta1;
//Calculate New Givens Rotations
c0 = c1;
c1 = lambda / p1;
s0 = s1;
s1 = betaN / p1;
//Update Solution
OptVector w = (1.0 / p1) * (v - p3 * w_1 - p2 * w0);
x = x + c1 * n * w;
n = -s1 * n;
residual = Math.Abs(n);
if (residual < precisionConst)
{
break;
}
beta1 = betaN;
v0 = v;
w_1 = w0;
w0 = w;
}
return(x);
}
