public NVector Calculate(NVector par_initial, NVector t, NVector y_dat)
{
_result = 0;
m = t.N;
if (par_initial.N != n) throw new Exception("LevenbergMarquardt.Calculate: size mismatch parms");
this.p = par_initial;
this.t = t;
if (y_dat.N != m) throw new Exception("LevenbergMarquardt.Calculate: size mismatch t-y");
this.y_dat = y_dat;
// weight_sq = (m - n + 1) / y_dat.Dot(y_dat);
DOF = (double)(m - n + 1);
//initalize Jacobian and related matrices
y_hat = func(t, p);
y_old = y_hat;
if (Jfunc == null)
J = Jacobian(p, y_hat);
else
J = Jfunc(t, p);
NVector delta_y = y_dat - y_hat;
X2 = delta_y.Dot(delta_y);
JtJ = J.Transpose() * J;
Jtdy = J.Transpose() * delta_y;
iteration = 0;
if (Jtdy.Abs().Max() < eps[0])
{
_result = 1;
return p; //Good guess!!!
}
if (updateType == UpdateType.Marquardt)
lambda = 0.01D;
else
lambda = 0.01D * JtJ.Diag().Max();
bool stop = false;
/************************** Begin Main loop ***********************/
// y_hat = vector of y estimates for current value of parameters
// y_try = vector of y estimates for current trial value of parameters
// y_dat = given dependent values (fixed)
// y_old = vector of y estimates for previous value of parameters (used in Broyden estimate of J)
// t = given independent values (fixed)
// p = current accepted estimate of parameters
// h = last calculated (trial) increment for the parameters
// p_try = current trial value for the parameters
// p_old = previous accepted value of parameters (used in Broyden estimate of J)
// X2 = chi^2 of last accepted estimate
// X2_try = chi^2 of current trial estimate
// J = current estimate of Jacobian at p
while (!stop)
{
iteration++;
NVector h;
if (updateType == UpdateType.Marquardt)
h = Jtdy / (JtJ + lambda * JtJ.Diag().Diag());
else
h = Jtdy / (JtJ + lambda * NMMatrix.I(n));
NVector p_try = (p + h).Max(p_min).Min(p_max);
NVector y_try = func(t, p_try);
delta_y = y_dat - y_try;
double X2_try = delta_y.Dot(delta_y);
if (updateType == UpdateType.Quadratic)
{
alpha = Jtdy.Dot(h) / ((X2_try - X2) / 2D + 2D * Jtdy.Dot(h));
h = h * alpha;
p_try = (p_try + h).Max(p_min).Min(p_max);
delta_y = y_dat - func(t, p_try);
X2_try = delta_y .Dot(delta_y);
}
dX2 = X2_try - X2;
double rho = -dX2 / (2D * (lambda * h + Jtdy).Dot(h));
if (dX2 < 0D) //found a better estimate
{
X2 = X2_try;
p_old = p;
p = p_try;
y_old = y_hat;
y_hat = y_try;
if (iteration % (2 * n) == 0) //|| dX2 > 0 or is it rho > ep[3] ?
if (Jfunc == null)
J = Jacobian(p, y_hat);
else
J = Jfunc(t, p);
else
J = J + (y_hat - y_old - J * h).Cross(h) / h.Dot(h); //Broyden rank-1 update of J
JtJ = J.Transpose() * J;
Jtdy = J.Transpose() * delta_y;
switch (updateType)
{
case UpdateType.Marquardt:
lambda = Math.Max(lambda / lambda_DN_fac, 1E-7);
break;
case UpdateType.Quadratic:
lambda = Math.Max(lambda / (1 + alpha), 1E-7);
break;
case UpdateType.Nielsen:
lambda = lambda * Math.Max(1D / 3D, 1D - Math.Pow(2D * rho - 1D, 3));
nu = 2D;
break;
}
if (Jtdy.Abs().Max() < eps[0] && iteration > 2)
{
_result = 1;
stop = true;
}
else if ((h / p).Abs().Max() < eps[1] && iteration > 2)
{
_result = 2;
stop = true;
}
else if (X2 / (m - n + 1) < eps[2] && iteration > 2)
{
_result = 3;
stop = true;
}
}
else //Not a better estimate
{
if (iteration % (2 * n) == 0) //update J every 2n th no matter what
{
if (Jfunc == null)
J = Jacobian(p, y_hat);
else
J = Jfunc(t, p);
JtJ = J.Transpose() * J;
Jtdy = J.Transpose() * (y_dat - y_hat);
}
switch (updateType)
{
case UpdateType.Marquardt:
lambda = Math.Min(lambda * lambda_UP_fac, 1E7);
break;
case UpdateType.Quadratic:
lambda = lambda + Math.Abs(dX2 / (2D * alpha));
break;
case UpdateType.Nielsen:
lambda = lambda * nu;
nu *= 2D;
break;
}
}
if (iteration > MaxIter && !stop)
{
_result = -1;
return p;
}
}
/************************** End Main loop ************************/
return p;
}
static void Main2(string[] args)
{
NVector A = new NVector(new double[] { 1, 3, 5, -2, 0 });
NVector B = new NVector(new double[] { -1, -2, 3, 1, 2 });
NVector C = A + B;
Console.WriteLine("A =" + A.ToString("0.000"));
Console.WriteLine("B =" + B.ToString("0.000"));
Console.WriteLine("A+B =" + C.ToString("0.000"));
double p = A.Dot(B);
NMMatrix E = A.Cross(B);
Console.WriteLine("A x B =" + E.ToString("0.000"));
E[4, 0] = -2;
E[4, 1] = 3;
E[4, 2] = 5;
E[4, 3] = -5;
E[4, 4] = 7;
E[0, 0] = -7;
E[1, 3] = -3;
E[3, 4] = -3.5;
E[2, 4] = -2;
NMMatrix H = new NMMatrix(new double[,] { { 5, 3, -2 ,1}, { 0, 3, 2,-3 }, { 4, 2, 3,2 }, { -6, 2, 8,-5 } });
Console.WriteLine("H =" + H.ToString("0.000"));
NVector V = new NVector(new double[] { 1, -1, 3, -2 });
Console.WriteLine("V =" + V.ToString("0.000"));
Console.WriteLine(" V / H =" + (V / H).ToString("0.0000"));
NMMatrix HI = H.Inverse();
Console.WriteLine("Inverse H =" + HI.ToString("0.000"));
Console.WriteLine("H * HI " + (H * HI).ToString("0.00000"));
Console.ReadKey();
NVector F = C / E;
NMMatrix G = E.Inverse();
NMMatrix N = (G * E - NMMatrix.I(5)).Apply((LinearAlgebra.F)Math.Abs);
double e = N.Max();
Console.WriteLine((e*1E15).ToString("0.00"));
Console.ReadKey();
}
