public static NVector operator *(double a, NVector B) { NVector C = new NVector(B); for (int i = 0; i < B._n; i++) C._vector[i] *= a; return C; }
public NVector(NVector A) { _vector = new double[A._n]; _n = A._n; for (int i = 0; i < A._n; i++) _vector[i] = A._vector[i]; }
public static NVector operator *(NVector A, NVector B) { if (A._n != B._n) throw new Exception("NVector operator *: incompatable vector size"); NVector C = new NVector(A); for (int i = 0; i < A._n; i++) C._vector[i] *= B._vector[i]; return C; }
public LevenbergMarquardt(Function func, JFunc Jfunc, NVector p_min, NVector p_max, NVector dp, double[] eps, UpdateType updateType) { this.func = func; this.Jfunc = Jfunc; n = p_min.N; this.p_min = p_min; if (p_max.N != n) throw new Exception("LevenbergMarquardt: size mismatch p_max"); this.p_max = p_max; if (Jfunc == null) { if (dp.N != n) throw new Exception("LevenbergMarquardt: size mismatch dp"); this.dp = dp; } MaxIter = 50 * n; this.eps = eps; this.updateType = updateType; }
public NVector Min(NVector A) { NVector B = new NVector(this); for (int i = 0; i < _n; i++) if (A._vector[i] < _vector[i]) B._vector[i] = A._vector[i]; return B; }
public double Dot(NVector A) { if (A._n != _n) throw new Exception("NVector.Dot: incompatable sizes"); double c = 0D; for (int i = 0; i < A._n; i++) c += A._vector[i] * _vector[i]; return c; }
static NVector func(NVector t, NVector p) { //parameters: A, B, C, a, b, t0 NVector y = new NVector(t.N); for (int i = 0; i < t.N; i++) { double t0 = t[i] - p[5]; double ebt = Math.Exp(-p[4] * t0); y[i] = p[2] + (p[1] - p[2]) * (1D - ebt); if (t0 > 0) { y[i] += p[0] * ebt * (1D - Math.Exp(-p[3] * t0)); } } return y; }
private NMMatrix Jacobian(NVector p, NVector y) { NVector ps = new NVector(p); //save a copy NMMatrix J = new NMMatrix(m, n); //creating a new J from scratch double del_p; for (int j = 0; j < n; j++) { del_p = Math.Max(dp[j] * Math.Abs(p[j]), dp[j]); p[j] = ps[j] + del_p; NVector y1 = func(t, p); if (dp[j] != 0D) //forward or backward difference J.ReplaceColumn(j, (y1 - y) / del_p); else //central difference { p[j] = ps[j] - del_p; J.ReplaceColumn(j, (y1 - func(t, p)) / (2D * del_p)); } p[j] = ps[j]; //restore this value } return J; }
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; }
public static NVector Uniform(double c, int dim) { NVector A = new NVector(dim); for (int i = 0; i < dim; i++) A._vector[i] = c; return A; }
public static NVector operator /(NVector A, double b) { NVector C = new NVector(A); for (int i = 0; i < A._n; i++) C._vector[i] /= b; return C; }
public void ReplaceColumn(int col, NVector V) { if (col < 0 || col >= this.M) throw new Exception("NMMatrix.ReplaceColumn: invalid column number"); for (int j = 0; j < _n; j++) this[j, col] = V[j]; }
public NVector ExtractColumn(int col) { if (col < 0 || col >= this.M) throw new Exception("NMMatrix.ExtractColumn: invalid column number"); NVector V = new NVector(N); for (int j = 0; j < _n; j++) V[j] = this[j, col]; return V; }
public NVector Diag() { if (_n != _m) throw new Exception("NMMatrix.Diag: non-square matrix"); NVector A = new NVector(_n); for (int i = 0; i < _n; i++) A[i] = _matrix[i, i]; return A; }
public NMMatrix Augment(NVector V) { if (N != V.N) throw new Exception("NMMatrix.Augment(Vector): incompatable sizes"); NMMatrix B = new NMMatrix(N, M + 1); for (int i = 0; i < N; i++) { for (int j = 0; j < M; j++) B[i, j] = this[i, j]; B[i, M] = V[i]; } return B; }
static NMMatrix Jfunc(NVector t, NVector p) { double eat; double ebt; NMMatrix J = new NMMatrix(t.N, p.N); for (int i = 0; i < t.N; i++) { double t0 = t[i] - p[5]; eat = Math.Exp(-p[3] * t0); ebt = Math.Exp(-p[4] * t0); J[i, 1] = 1D - ebt; //B J[i, 2] = ebt; //C J[i, 4] = p[1] - p[2]; //beta J[i, 5] = (p[2] - p[1]) * p[4]; //t0 if (t0 > 0D) //UnitStep portion { J[i, 0] = ebt * (1D - eat); //A J[i, 3] = p[0] * t0 * eat * ebt; //alpha J[i, 4] -= p[0] * (1D - eat); //beta J[i, 5] += p[0] * (p[4] * (1D - eat) - p[3] * eat); //t0 } J[i, 4] *= t0 * ebt; //beta J[i, 5] *= ebt; //t0 } return J; }
public static NVector operator *(NMMatrix A, NVector B) { int l1 = A.N; int l2 = A.M; if (l2 != B._n) throw new Exception("NVector.Mul: incompatable sizes"); NVector C = new NVector(l1); for (int i = 0; i < l1; i++) { double c = 0D; for (int j = 0; j < l2; j++) c += A[i, j] * B._vector[j]; C._vector[i] = c; } return C; }
public NVector Abs() { NVector A = new NVector(this); for (int i = 0; i < _n; i++) A._vector[i] = Math.Abs(_vector[i]); return A; }
private NMMatrix Broyden(NVector p_old, NVector y_old, NMMatrix J, NVector p, NVector y) { NVector h = p - p_old; J = J + (y - y_old - J * h).Cross(h) / h.Dot(h); return J; }
public NVector Apply(F func) { NVector A = new NVector(_n); for (int i = 0; i < _n; i++) A._vector[i] = func(_vector[i]); return A; }
public NMMatrix Cross(NVector A) { NMMatrix C = new NMMatrix(_n, A._n); for (int i = 0; i < _n; i++) for (int j = 0; j < A._n; j++) C[i,j] += _vector[i] * A._vector[j]; return C; }
private static bool fitSignal(double[] d, int beforeTime, eventTime current, int afterTime, double samplingRate, double minD, double maxD) { LevenbergMarquardt LM = new LevenbergMarquardt(func, Jfunc, new NVector(new double[] { minD - maxD, 2 * minD, 2 * minD, 0.25, 0.005, -0.25 }), new NVector(new double[] { maxD - minD, 2 * maxD, 2 * maxD, 40, 0.1, 0.5 }), null, new double[] { 0.0001, 0.00001, 0.00001, 0.01 }, LevenbergMarquardt.UpdateType.Marquardt); //set up LM processor, parameters and limits //determine subset of data around the detection signal int start = Math.Max(0, Math.Min(current.startTime - (beforeTime + (int)(deadtimeSecsAfter * samplingRate)), (int)(maxSecsBefore * samplingRate))); //up to 5 seconds before double newTOffset = (double)start / samplingRate; int dataLength = start + Math.Max(current.filterLength, Math.Min(afterTime - current.filterLength - current.startTime, (int)(maxSecsAfter * samplingRate))); //up to 40 seconds after start = current.startTime - start; dataLength = Math.Min(dataLength, d.Length - start); //watch for overrun past end of data array double max = double.MinValue; for (int v = current.startTime; v < current.startTime + current.length; v++) max = Math.Max(max, Math.Abs(d[v])); //Get signal max for initial estimate of A NVector t = new NVector(dataLength); for (int ti = 0; ti < dataLength; ti++) t[ti] = (double)ti / samplingRate - newTOffset; //create independent variable array NVector y = new NVector(dataLength); for (int i = 0; i < dataLength; i++) y[i] = d[start + i]; //create dependent variable array NVector p = LM.Calculate( new NVector(new double[] { current.sign * max, /* A */ d[current.startTime], /* B */ d[current.startTime], /* C */ 4D, /* alpha */ 0.04, /* beta */ 0D }), /* t0 */ t, y); //fit signal using Levenberg-Marquardt algorithm current.A = p[0]; //parse estimated parameters out current.B = p[1]; current.C = p[2]; current.a = p[3]; current.b = p[4]; if (LM.Result > 0) current.t0 += (int)(p[5] * samplingRate); //offset starting time by new t0, only if fit found current.chiSquare = LM.ChiSquare; //remember Chi square return LM.Result > 0; }