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