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