void VarproFunction::computeJtJmulE( const gsl_matrix* R, const gsl_matrix* E, gsl_matrix *out, int useJtJ ) {
gsl_vector vecE = gsl_vector_const_view_array(E->data, E->size1 * E->size2).vector;
gsl_vector vecOut = gsl_vector_const_view_array(out->data, out->size1 * out->size2).vector;
if (R->size1 * R->size2 != 0) {
computeFuncAndPseudoJacobianLs(R, myTmpEye, NULL, myTmpJac2);
} /* Otherwise use the precomputed Jacobian */
if (useJtJ) {
if (R->size1 * R->size2 != 0) {
gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, myTmpJac2, myTmpJac2, 0, myTmpJtJ);
} /* Otherwise use the precomputed JtJ */
gsl_blas_dgemv(CblasNoTrans, 2.0, myTmpJtJ, &vecE, 0.0, &vecOut);
} else {
gsl_blas_dgemv(CblasNoTrans, 1.0, myTmpJac2, &vecE, 0.0, myTmpJacobianCol);
gsl_blas_dgemv(CblasTrans, 2.0, myTmpJac2, myTmpJacobianCol, 0.0, &vecOut);
}
}
static void
kowalik_checksol(const double x[], const double sumsq,
const double epsrel, const char *sname,
const char *pname)
{
size_t i;
gsl_vector_const_view v = gsl_vector_const_view_array(x, kowalik_P);
const double norm = gsl_blas_dnrm2(&v.vector);
const double sumsq_exact1 = 3.075056038492370e-04;
const double kowalik_x1[kowalik_P] = { 1.928069345723978e-01,
1.912823290344599e-01,
1.230565070690708e-01,
1.360623308065148e-01 };
const double sumsq_exact2 = 0.00102734304869549252;
const double kowalik_x2[kowalik_P] = { GSL_NAN, /* inf */
-14.0758834005984603,
GSL_NAN, /* -inf */
GSL_NAN }; /* -inf */
const double *kowalik_x;
double sumsq_exact;
if (norm < 10.0)
{
kowalik_x = kowalik_x1;
sumsq_exact = sumsq_exact1;
}
else
{
kowalik_x = kowalik_x2;
sumsq_exact = sumsq_exact2;
}
gsl_test_rel(sumsq, sumsq_exact, epsrel, "%s/%s sumsq",
sname, pname);
for (i = 0; i < kowalik_P; ++i)
{
if (!gsl_finite(kowalik_x[i]))
continue;
gsl_test_rel(x[i], kowalik_x[i], epsrel, "%s/%s i="F_ZU,
sname, pname, i);
}
}
void GenerateRandMVnorm(gsl_rng * r, const size_t numSamples, const double * mu, const gsl_matrix *sigChol, const size_t NumParam, gsl_matrix * returnSamples)
{
// generate standard normal random samples
for(size_t i = 0; i < numSamples; i++)
for(size_t j = 0; j < NumParam; j++)
gsl_matrix_set(returnSamples, i, j, gsl_ran_gaussian(r, 1.0));
gsl_blas_dtrmm(CblasRight, CblasLower, CblasTrans, CblasNonUnit, 1.0, sigChol, returnSamples); // matrix multiplcation stdNormSamples %*% sigChol, for upper triangular matrix
// add mu to each row
gsl_vector_const_view vecMu = gsl_vector_const_view_array(mu, NumParam);
#pragma omp parallel for
for(size_t i = 0; i < numSamples; i++){
gsl_vector_view tmpRow = gsl_matrix_row(returnSamples, i);
gsl_vector_add(&tmpRow.vector, &vecMu.vector);
}
return;
}
static void
biggs_checksol(const double x[], const double sumsq,
const double epsrel, const char *sname,
const char *pname)
{
const double sumsq_exact = 0.0;
const double biggs_x[biggs_P] = { 1.0, 10.0, 1.0, 5.0, 4.0, 3.0 };
const double norm_exact = 12.3288280059380;
gsl_vector_const_view v = gsl_vector_const_view_array(biggs_x, biggs_P);
double norm = gsl_blas_dnrm2(&v.vector);
gsl_test_rel(sumsq, sumsq_exact, epsrel, "%s/%s sumsq",
sname, pname);
/*
* the solution vector is not unique due to permutations, so test
* the norm instead of individual elements
*/
gsl_test_rel(norm, norm_exact, epsrel, "%s/%s norm",
sname, pname);
}
/* Calculate Hamiltonian energy equation (13) of Girolami and Calderhead (2011).
* Arguments:
* N: number of parameters.
* Lx: unormalised posterior density at current parameter values.
* cholM: cholesky factor of the metric tensor evaluated at current parameter values.
* invM: inverse of metric tensor evaluated at current parameter values.
* p: value of the momentum parameters.
* state: a pointer to internal working storage for RMHMC.
* Result:
* returns the log of the Hamiltonian energy.
*/
static double calculateHamiltonian(int N, double Lx, const double* cholM, const double* invM,
const double* p, const rmhmc_params* state){
double logDetM = 0;
int d;
gsl_vector_view a_v = gsl_vector_view_array(state->atmp, N);
gsl_vector_const_view p_v = gsl_vector_const_view_array(p, N);
gsl_matrix_const_view invM_v = gsl_matrix_const_view_array(invM, N, N);
gsl_matrix_const_view cholM_v = gsl_matrix_const_view_array(cholM, N, N);
//gsl_blas_dsymv(CblasUpper, 1.0, &invM_v.matrix, &p_v.vector, 0.0, &a_v.vector);
gsl_blas_dgemv(CblasNoTrans, 1.0, &invM_v.matrix, &p_v.vector, 0.0, &a_v.vector);
for (d = 0; d < N; d++)
logDetM += log(gsl_matrix_get(&cholM_v.matrix, d, d));
double quadTerm = 0;
gsl_blas_ddot(&p_v.vector, &a_v.vector, &quadTerm);
return Lx - logDetM - 0.5*quadTerm;
}
void GetMVNpdf(const gsl_matrix * mat, const double * mu, const gsl_matrix * sigmaInv, const gsl_matrix * sigmaChol, const size_t nPoints, const size_t nDim, double * returnVal)
{
double normConst = - log(2*M_PI)*nDim/2.0;
for(size_t j = 0; j < nDim; j++)
normConst -= log(gsl_matrix_get(sigmaChol, j, j));
gsl_vector_const_view vecMu = gsl_vector_const_view_array(mu, nDim);
#pragma omp parallel for
for(size_t i = 0; i < nPoints; i++){
gsl_vector * x1 = gsl_vector_alloc(nDim); // Note: allocating and freeing these every loop is not ideal, but needed for threadsafe. There might be a better way.
gsl_vector * x2 = gsl_vector_alloc(nDim);
gsl_matrix_get_row(x1, mat, i);
gsl_vector_sub(x1, &vecMu.vector);
gsl_blas_dsymv(CblasUpper, 1.0, sigmaInv, x1, 0.0, x2);
gsl_blas_ddot(x1, x2, &returnVal[i]);
returnVal[i] = exp(normConst - 0.5*returnVal[i]);
gsl_vector_free(x1);
gsl_vector_free(x2);
}
return;
}
static void
test_fdf(const gsl_multifit_nlinear_type * T,
const gsl_multifit_nlinear_parameters * params,
const double xtol, const double gtol, const double ftol,
const double epsrel,
test_fdf_problem *problem,
const double *wts)
{
gsl_multifit_nlinear_fdf *fdf = problem->fdf;
const size_t n = fdf->n;
const size_t p = fdf->p;
const size_t max_iter = 2500;
gsl_vector *x0 = gsl_vector_alloc(p);
gsl_vector_view x0v = gsl_vector_view_array(problem->x0, p);
gsl_multifit_nlinear_workspace *w =
gsl_multifit_nlinear_alloc (T, params, n, p);
const char *pname = problem->name;
char buf[2048];
char sname[2048];
int status, info;
sprintf(buf, "%s/%s/scale=%s/solver=%s",
gsl_multifit_nlinear_name(w),
params->trs->name,
params->scale->name,
params->solver->name);
if (problem->fdf->df == NULL)
{
if (params->fdtype == GSL_MULTIFIT_NLINEAR_FWDIFF)
strcat(buf, "/fdjac,forward");
else
strcat(buf, "/fdjac,center");
}
if (problem->fdf->fvv == NULL)
{
strcat(buf, "/fdfvv");
}
strcpy(sname, buf);
gsl_vector_memcpy(x0, &x0v.vector);
if (wts)
{
gsl_vector_const_view wv = gsl_vector_const_view_array(wts, n);
gsl_multifit_nlinear_winit(x0, &wv.vector, fdf, w);
}
else
gsl_multifit_nlinear_init(x0, fdf, w);
status = gsl_multifit_nlinear_driver(max_iter, xtol, gtol, ftol,
NULL, NULL, &info, w);
gsl_test(status, "%s/%s did not converge, status=%s",
sname, pname, gsl_strerror(status));
/* check solution */
test_fdf_checksol(sname, pname, epsrel, w, problem);
if (wts == NULL)
{
/* test again with weighting matrix W = I */
gsl_vector *wv = gsl_vector_alloc(n);
sprintf(sname, "%s/weighted", buf);
gsl_vector_memcpy(x0, &x0v.vector);
gsl_vector_set_all(wv, 1.0);
gsl_multifit_nlinear_winit(x0, wv, fdf, w);
status = gsl_multifit_nlinear_driver(max_iter, xtol, gtol, ftol,
NULL, NULL, &info, w);
gsl_test(status, "%s/%s did not converge, status=%s",
sname, pname, gsl_strerror(status));
test_fdf_checksol(sname, pname, epsrel, w, problem);
gsl_vector_free(wv);
}
gsl_multifit_nlinear_free(w);
gsl_vector_free(x0);
}
void Vector::setData(const double* array)
{
gsl_vector_const_view varray = gsl_vector_const_view_array(array, _V->size);
gsl_vector_memcpy(_V, &varray.vector);
}
static void
test_fdfridge(const gsl_multifit_fdfsolver_type * T, const double xtol,
const double gtol, const double ftol,
const double epsrel, const double x0_scale,
test_fdf_problem *problem, const double *wts)
{
gsl_multifit_function_fdf *fdf = problem->fdf;
const size_t n = fdf->n;
const size_t p = fdf->p;
const size_t max_iter = 1500;
gsl_vector *x0 = gsl_vector_alloc(p);
gsl_vector_view x0v = gsl_vector_view_array(problem->x0, p);
gsl_multifit_fdfridge *w = gsl_multifit_fdfridge_alloc (T, n, p);
const char *pname = problem->name;
char sname[2048];
int status, info;
double lambda = 0.0;
sprintf(sname, "ridge/%s", gsl_multifit_fdfridge_name(w));
/* scale starting point x0 */
gsl_vector_memcpy(x0, &x0v.vector);
test_scale_x0(x0, x0_scale);
/* test undamped case with lambda = 0 */
if (wts)
{
gsl_vector_const_view wv = gsl_vector_const_view_array(wts, n);
gsl_multifit_fdfridge_wset(w, fdf, x0, lambda, &wv.vector);
}
else
gsl_multifit_fdfridge_set(w, fdf, x0, lambda);
status = gsl_multifit_fdfridge_driver(w, max_iter, xtol, gtol,
ftol, &info);
gsl_test(status, "%s/%s did not converge, status=%s",
sname, pname, gsl_strerror(status));
/* check solution */
test_fdf_checksol(sname, pname, epsrel, w->s, problem);
/* test for self consisent solution with L = \lambda I */
{
const double eps = 1.0e-10;
gsl_matrix *L = gsl_matrix_calloc(p, p);
gsl_vector_view diag = gsl_matrix_diagonal(L);
gsl_multifit_fdfridge *w2 = gsl_multifit_fdfridge_alloc (T, n, p);
gsl_vector *y0 = gsl_vector_alloc(p);
size_t i;
/* pick some value for lambda and set L = \lambda I */
lambda = 5.0;
gsl_vector_set_all(&diag.vector, lambda);
/* scale initial vector */
gsl_vector_memcpy(x0, &x0v.vector);
test_scale_x0(x0, x0_scale);
gsl_vector_memcpy(y0, x0);
if (wts)
{
gsl_vector_const_view wv = gsl_vector_const_view_array(wts, n);
gsl_multifit_fdfridge_wset(w, fdf, x0, lambda, &wv.vector);
gsl_multifit_fdfridge_wset3(w2, fdf, y0, L, &wv.vector);
}
else
{
gsl_multifit_fdfridge_set(w, fdf, x0, lambda);
gsl_multifit_fdfridge_set3(w2, fdf, y0, L);
}
/* solve with scalar lambda routine */
status = gsl_multifit_fdfridge_driver(w, max_iter, xtol, gtol,
ftol, &info);
gsl_test(status, "%s/lambda/%s did not converge, status=%s",
sname, pname, gsl_strerror(status));
/* solve with general matrix routine */
status = gsl_multifit_fdfridge_driver(w2, max_iter, xtol, gtol,
ftol, &info);
gsl_test(status, "%s/L/%s did not converge, status=%s",
sname, pname, gsl_strerror(status));
/* test x = y */
for (i = 0; i < p; ++i)
{
double xi = gsl_vector_get(w->s->x, i);
double yi = gsl_vector_get(w2->s->x, i);
if (fabs(xi) < eps)
{
gsl_test_abs(yi, xi, eps, "%s/%s ridge lambda=%g i="F_ZU,
sname, pname, lambda, i);
}
else
{
gsl_test_rel(yi, xi, eps, "%s/%s ridge lambda=%g i="F_ZU,
sname, pname, lambda, i);
}
}
gsl_matrix_free(L);
gsl_vector_free(y0);
gsl_multifit_fdfridge_free(w2);
}
gsl_multifit_fdfridge_free(w);
gsl_vector_free(x0);
}
static void
test_fdf(const gsl_multifit_fdfsolver_type * T, const double xtol,
const double gtol, const double ftol,
const double epsrel, const double x0_scale,
test_fdf_problem *problem,
const double *wts)
{
gsl_multifit_function_fdf *fdf = problem->fdf;
const size_t n = fdf->n;
const size_t p = fdf->p;
const size_t max_iter = 1500;
gsl_vector *x0 = gsl_vector_alloc(p);
gsl_vector_view x0v = gsl_vector_view_array(problem->x0, p);
gsl_multifit_fdfsolver *s = gsl_multifit_fdfsolver_alloc (T, n, p);
const char *pname = problem->name;
char sname[2048];
int status, info;
sprintf(sname, "%s/scale=%g%s",
gsl_multifit_fdfsolver_name(s), x0_scale,
problem->fdf->df ? "" : "/fdiff");
/* scale starting point x0 */
gsl_vector_memcpy(x0, &x0v.vector);
test_scale_x0(x0, x0_scale);
if (wts)
{
gsl_vector_const_view wv = gsl_vector_const_view_array(wts, n);
gsl_multifit_fdfsolver_wset(s, fdf, x0, &wv.vector);
}
else
gsl_multifit_fdfsolver_set(s, fdf, x0);
status = gsl_multifit_fdfsolver_driver(s, max_iter, xtol, gtol,
ftol, &info);
gsl_test(status, "%s/%s did not converge, status=%s",
sname, pname, gsl_strerror(status));
/* check solution */
test_fdf_checksol(sname, pname, epsrel, s, problem);
if (wts == NULL)
{
/* test again with weighting matrix W = I */
gsl_vector *wv = gsl_vector_alloc(n);
sprintf(sname, "%s/scale=%g%s/weights",
gsl_multifit_fdfsolver_name(s), x0_scale,
problem->fdf->df ? "" : "/fdiff");
gsl_vector_memcpy(x0, &x0v.vector);
test_scale_x0(x0, x0_scale);
gsl_vector_set_all(wv, 1.0);
gsl_multifit_fdfsolver_wset(s, fdf, x0, wv);
status = gsl_multifit_fdfsolver_driver(s, max_iter, xtol, gtol,
ftol, &info);
gsl_test(status, "%s/%s did not converge, status=%s",
sname, pname, gsl_strerror(status));
test_fdf_checksol(sname, pname, epsrel, s, problem);
gsl_vector_free(wv);
}
gsl_multifit_fdfsolver_free(s);
gsl_vector_free(x0);
}
int
main(int argc, char **argv)
{
size_t order, breakpoints, i;
gsl_ieee_env_setup();
argc = 0; /* prevent warnings about unused parameters */
argv = 0;
for (order = 1; order < 10; order++)
{
for (breakpoints = 2; breakpoints < 100; breakpoints++)
{
double a = -1.23 * order, b = 45.6 * order;
gsl_bspline_workspace *bw = gsl_bspline_alloc(order, breakpoints);
gsl_bspline_deriv_workspace *dbw = gsl_bspline_deriv_alloc(order);
gsl_bspline_knots_uniform(a, b, bw);
test_bspline(bw, dbw);
gsl_bspline_deriv_free(dbw);
gsl_bspline_free(bw);
}
}
for (order = 1; order < 10; order++)
{
for (breakpoints = 2; breakpoints < 100; breakpoints++)
{
double a = -1.23 * order, b = 45.6 * order;
gsl_bspline_workspace *bw = gsl_bspline_alloc(order, breakpoints);
gsl_bspline_deriv_workspace *dbw = gsl_bspline_deriv_alloc(order);
gsl_vector *k = gsl_vector_alloc(breakpoints);
for (i = 0; i < breakpoints; i++)
{
double f, x;
f = sqrt(i / (breakpoints - 1.0));
x = (1 - f) * a + f * b;
gsl_vector_set(k, i, x);
};
gsl_bspline_knots(k, bw);
test_bspline(bw, dbw);
gsl_vector_free(k);
gsl_bspline_deriv_free(dbw);
gsl_bspline_free(bw);
}
}
/* Spot check known 0th, 1st, 2nd derivative
evaluations for a particular k = 2 case. */
{
size_t i, j; /* looping */
const double xloc[4] = { 0.0, 1.0, 6.0, 7.0};
const double deriv[4][3] =
{
{ -1.0/2.0, 1.0/2.0, 0.0 },
{ -1.0/2.0, 1.0/2.0, 0.0 },
{ 0.0, -1.0/5.0, 1.0/5.0 },
{ 0.0, -1.0/5.0, 1.0/5.0 }
};
gsl_bspline_workspace *bw = gsl_bspline_alloc(2, 3);
gsl_bspline_deriv_workspace *dbw = gsl_bspline_deriv_alloc(2);
gsl_matrix *dB = gsl_matrix_alloc(gsl_bspline_ncoeffs(bw),
gsl_bspline_order(bw) + 1);
gsl_vector *breakpts = gsl_vector_alloc(3);
gsl_vector_set(breakpts, 0, 0.0);
gsl_vector_set(breakpts, 1, 2.0);
gsl_vector_set(breakpts, 2, 7.0);
gsl_bspline_knots(breakpts, bw);
for (i = 0; i < 4; ++i) /* at each location */
{
/* Initialize dB with poison to ensure we overwrite it */
gsl_matrix_set_all(dB, GSL_NAN);
gsl_bspline_deriv_eval(xloc[i], gsl_bspline_order(bw), dB, bw, dbw);
for (j = 0; j < gsl_bspline_ncoeffs(bw) ; ++j)
{
/* check basis function 1st deriv */
gsl_test_abs(gsl_matrix_get(dB, j, 1), deriv[i][j], GSL_DBL_EPSILON,
"b-spline k=%d basis #%d derivative %d at x = %f",
gsl_bspline_order(bw), j, 1, xloc[i]);
}
for (j = 0; j < gsl_bspline_ncoeffs(bw); ++j)
{
/* check k order basis function has k-th deriv equal to 0 */
gsl_test_abs(gsl_matrix_get(dB, j, gsl_bspline_order(bw)), 0.0,
GSL_DBL_EPSILON,
"b-spline k=%d basis #%d derivative %d at x = %f",
gsl_bspline_order(bw), j, gsl_bspline_order(bw),
xloc[i]);
}
}
gsl_matrix_free(dB);
gsl_bspline_deriv_free(dbw);
gsl_bspline_free(bw);
gsl_vector_free(breakpts);
}
/* Spot check known 0th, 1st, 2nd derivative
evaluations for a particular k = 3 case. */
{
size_t i, j; /* looping */
const double xloc[5] = { 0.0, 5.0, 9.0, 12.0, 15.0 };
const double eval[5][6] =
{
{ 4./25., 69./100., 3./ 20. , 0. , 0. , 0. },
{ 0. , 4./21. , 143./210. , 9./70., 0. , 0. },
{ 0. , 0. , 3./ 10. , 7./10., 0. , 0. },
{ 0. , 0. , 0. , 3./4. , 1./4., 0. },
{ 0. , 0. , 0. , 1./3. , 5./9., 1./9. }
};
const double deriv[5][6] =
{
{ -4./25., 3./50., 1./ 10., 0. , 0. , 0. },
{ 0. , -2./21., 1./105., 3./35., 0. , 0. },
{ 0. , 0. , -1./5. , 1./ 5., 0. , 0. },
{ 0. , 0. , 0. , -1./ 6., 1./6. , 0. },
{ 0. , 0. , 0. , -1./ 9., 1./27., 2./27. }
};
const double deriv2[5][6] =
{
{ 2./25., -17./150., 1.0/30.0 , 0.0 , 0. , 0. },
{ 0. , 1./ 42., -11.0/210.0, 1.0/35.0, 0. , 0. },
{ 0. , 0. , 1.0/15.0 ,-11.0/90.0, 1./18. , 0. },
{ 0. , 0. , 0.0 , 1.0/54.0, -7./162., 2./81. },
{ 0. , 0. , 0.0 , 1.0/54.0, -7./162., 2./81. }
};
gsl_bspline_workspace *bw = gsl_bspline_alloc(3, 5);
gsl_bspline_deriv_workspace *dbw = gsl_bspline_deriv_alloc(3);
gsl_matrix *dB = gsl_matrix_alloc(gsl_bspline_ncoeffs(bw),
gsl_bspline_order(bw) + 1);
gsl_vector *breakpts = gsl_vector_alloc(5);
gsl_vector_set(breakpts, 0, -3.0);
gsl_vector_set(breakpts, 1, 2.0);
gsl_vector_set(breakpts, 2, 9.0);
gsl_vector_set(breakpts, 3, 12.0);
gsl_vector_set(breakpts, 4, 21.0);
gsl_bspline_knots(breakpts, bw);
for (i = 0; i < 5; ++i) /* at each location */
{
/* Initialize dB with poison to ensure we overwrite it */
gsl_matrix_set_all(dB, GSL_NAN);
gsl_bspline_deriv_eval(xloc[i], gsl_bspline_order(bw), dB, bw, dbw);
/* check basis function evaluation */
for (j = 0; j < gsl_bspline_ncoeffs(bw); ++j)
{
gsl_test_abs(gsl_matrix_get(dB, j, 0), eval[i][j], GSL_DBL_EPSILON,
"b-spline k=%d basis #%d derivative %d at x = %f",
gsl_bspline_order(bw), j, 0, xloc[i]);
}
/* check 1st derivative evaluation */
for (j = 0; j < gsl_bspline_ncoeffs(bw); ++j)
{
gsl_test_abs(gsl_matrix_get(dB, j, 1), deriv[i][j], GSL_DBL_EPSILON,
"b-spline k=%d basis #%d derivative %d at x = %f",
gsl_bspline_order(bw), j, 1, xloc[i]);
}
/* check 2nd derivative evaluation */
for (j = 0; j < gsl_bspline_ncoeffs(bw); ++j)
{
gsl_test_abs(gsl_matrix_get(dB, j, 2), deriv2[i][j], GSL_DBL_EPSILON,
"b-spline k=%d basis #%d derivative %d at x = %f",
gsl_bspline_order(bw), j, 2, xloc[i]);
}
}
gsl_matrix_free(dB);
gsl_bspline_deriv_free(dbw);
gsl_bspline_free(bw);
gsl_vector_free(breakpts);
}
/* Check Greville abscissae functionality on a non-uniform k=1 */
{
size_t i; /* looping */
/* Test parameters */
const size_t k = 1;
const double bpoint_data[] = { 0.0, 0.2, 0.5, 0.75, 1.0 };
const size_t nbreak = sizeof(bpoint_data)/sizeof(bpoint_data[0]);
/* Expected results */
const double abscissae_data[] = { 0.1, 0.35, 0.625, 0.875 };
const size_t nabscissae = sizeof(abscissae_data)/sizeof(abscissae_data[0]);
gsl_vector_const_view bpoints = gsl_vector_const_view_array(bpoint_data, nbreak);
gsl_bspline_workspace *w = gsl_bspline_alloc(k, nbreak);
gsl_bspline_knots((const gsl_vector *) &bpoints, w);
gsl_test_int(nabscissae, gsl_bspline_ncoeffs(w),
"b-spline k=%d number of abscissae", k);
for (i = 0; i < nabscissae; ++i)
{
gsl_test_abs(gsl_bspline_greville_abscissa(i, w), abscissae_data[i], 2*k*GSL_DBL_EPSILON,
"b-spline k=%d Greville abscissa #%d at x = %f", k, i, abscissae_data[i]);
}
gsl_bspline_free(w);
}
/* Check Greville abscissae functionality on a non-uniform k=2 */
{
size_t i; /* looping */
/* Test parameters */
const size_t k = 2;
const double bpoint_data[] = { 0.0, 0.2, 0.5, 0.75, 1.0 };
const size_t nbreak = sizeof(bpoint_data)/sizeof(bpoint_data[0]);
/* Expected results */
const double abscissae_data[] = { 0.0, 0.2, 0.5, 0.75, 1.0 };
const size_t nabscissae = sizeof(abscissae_data)/sizeof(abscissae_data[0]);
gsl_vector_const_view bpoints = gsl_vector_const_view_array(bpoint_data, nbreak);
gsl_bspline_workspace *w = gsl_bspline_alloc(k, nbreak);
gsl_bspline_knots((const gsl_vector *) &bpoints, w);
gsl_test_int(nabscissae, gsl_bspline_ncoeffs(w),
"b-spline k=%d number of abscissae", k);
for (i = 0; i < nabscissae; ++i)
{
gsl_test_abs(gsl_bspline_greville_abscissa(i, w), abscissae_data[i], 2*k*GSL_DBL_EPSILON,
"b-spline k=%d Greville abscissa #%d at x = %f", k, i, abscissae_data[i]);
}
gsl_bspline_free(w);
}
/* Check Greville abscissae functionality on non-uniform k=3 */
{
size_t i; /* looping */
/* Test parameters */
const size_t k = 3;
const double bpoint_data[] = { 0.0, 0.2, 0.5, 0.75, 1.0 };
const size_t nbreak = sizeof(bpoint_data)/sizeof(bpoint_data[0]);
/* Expected results */
const double abscissae_data[] = { 0.0, 1.0/10.0, 7.0/20.0,
5.0/ 8.0, 7.0/ 8.0, 1.0 };
const size_t nabscissae = sizeof(abscissae_data)/sizeof(abscissae_data[0]);
gsl_vector_const_view bpoints = gsl_vector_const_view_array(bpoint_data, nbreak);
gsl_bspline_workspace *w = gsl_bspline_alloc(k, nbreak);
gsl_bspline_knots((const gsl_vector *) &bpoints, w);
gsl_test_int(nabscissae, gsl_bspline_ncoeffs(w),
"b-spline k=%d number of abscissae", k);
for (i = 0; i < nabscissae; ++i)
{
gsl_test_abs(gsl_bspline_greville_abscissa(i, w), abscissae_data[i], 2*k*GSL_DBL_EPSILON,
"b-spline k=%d Greville abscissa #%d at x = %f", k, i, abscissae_data[i]);
}
gsl_bspline_free(w);
}
/* Check Greville abscissae functionality on non-uniform k=4 */
{
size_t i; /* looping */
/* Test parameters */
const size_t k = 4;
const double bpoint_data[] = { 0.0, 0.2, 0.5, 0.75, 1.0 };
const size_t nbreak = sizeof(bpoint_data)/sizeof(bpoint_data[0]);
/* Expected results */
const double abscissae_data[] = { 0.0, 1.0/15.0, 7.0/30.0, 29.0/60.0,
3.0/ 4.0, 11.0/12.0, 1.0 };
const size_t nabscissae = sizeof(abscissae_data)/sizeof(abscissae_data[0]);
gsl_vector_const_view bpoints = gsl_vector_const_view_array(bpoint_data, nbreak);
gsl_bspline_workspace *w = gsl_bspline_alloc(k, nbreak);
gsl_bspline_knots((const gsl_vector *) &bpoints, w);
gsl_test_int(nabscissae, gsl_bspline_ncoeffs(w),
"b-spline k=%d number of abscissae", k);
for (i = 0; i < nabscissae; ++i)
{
gsl_test_abs(gsl_bspline_greville_abscissa(i, w), abscissae_data[i], 2*k*GSL_DBL_EPSILON,
"b-spline k=%d Greville abscissa #%d at x = %f", k, i, abscissae_data[i]);
}
gsl_bspline_free(w);
}
exit(gsl_test_summary());
}
int Cornea::computeCentre(const std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &led_pos, // LED locations
const std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &glint_pos,
std::vector<double> &gx_guesses,
Eigen::Vector3d ¢re,
double &err) {
// initialise the cornea tracker
create(led_pos, glint_pos);
/*
* Check out usage and more info about GSL:
* http://www.csse.uwa.edu.au/programming/gsl-1.0/gsl-ref_35.html
*/
const size_t n = 3 * pairsOfTwo(data.size()); // number of functions
const size_t p = gx_guesses.size(); // number of parameters
// initial guesses
gsl_vector_const_view x = gsl_vector_const_view_array(gx_guesses.data(), p);
gsl_multifit_function_fdf f;
f.f = &my_f; // function
f.df = &my_df; // derivative
f.fdf = &my_fdf; // both
f.n = n; // number of functions
f.p = p; // number of parameters
f.params = this; // additional parameter
const gsl_multifit_fdfsolver_type *T = gsl_multifit_fdfsolver_lmsder;
gsl_multifit_fdfsolver *solver = gsl_multifit_fdfsolver_alloc(T, n, p);
gsl_multifit_fdfsolver_set(solver, &f, &x.vector);
int status;
unsigned int iter = 0;
do {
iter++;
status = gsl_multifit_fdfsolver_iterate(solver);
if(status) {
break;
}
status = gsl_multifit_test_delta(solver->dx, solver->x, PRECISION, PRECISION);
}
while(status == GSL_CONTINUE && iter < MAX_ITER);
if(iter == MAX_ITER) {
printf("Cornea::computeCentre(): iter = MAX_ITER\n");
}
gsl_matrix *covar = gsl_matrix_alloc(p, p);
gsl_multifit_covar(solver->J, 0.0, covar);
// for(int row = 0; row < p; ++row) {
// for(int col = 0; col < p; ++col) {
// printf("%.2f ", covar->data[row * p + col]);
// }
// printf("\n");
// }
// printf("*****************************\n");
/***********************************************************************
* Compute the fit error
**********************************************************************/
err = 0;
for(size_t i = 0; i < p; i++) {
err += gsl_matrix_get(covar, i, i);
}
err = std::sqrt(err);
Eigen::Vector3d cw(0.0, 0.0, 0.0);
// cornea sphere radius
const double RHO = trackerSettings.RHO;
// remove this
double dMaxX = -10.0;
for(size_t i = 0; i < data.size(); ++i) {
const DATA_FOR_CORNEA_COMPUTATION &cur_data = data[i];
const double gx_guess = gsl_vector_get(solver->x, i);
const double B_aux = atan2(gx_guess * tan(cur_data.alpha_aux), (cur_data.l_aux - gx_guess));
// calculate the corneal sphere centers in the auxiliary coordinate systems
const Eigen::Vector3d c_aux(gx_guess - RHO * sin((cur_data.alpha_aux - B_aux) / 2.),
0.,
gx_guess * tan(cur_data.alpha_aux) + RHO * cos((cur_data.alpha_aux - B_aux) / 2.));
const Eigen::Vector3d tmp = cur_data.R * c_aux;
cw(0) += tmp(0);
cw(1) += tmp(1);
cw(2) += tmp(2);
if(tmp(0) > dMaxX) {
dMaxX = tmp(0);
}
// printf("%i: centre (mm): %.2f %.2f %.2f\n", (int)i, 1000.0*tmp(0), 1000.0*tmp(1), 1000.0*tmp(2));
}
const double nof_samples = (double)data.size();
centre << cw(0) / nof_samples, cw(1) / nof_samples, cw(2) / nof_samples;
// printf("Avg: %.2f %.2f %.2f\n", 1000.0*centre(0), 1000.0*centre(1), 1000.0*centre(2));
// printf("*********************************\n");
gsl_multifit_fdfsolver_free(solver);
gsl_matrix_free(covar);
return (int)iter;
}
/**
* Add signal s_mu = M_mu_nu A^nu within the given transient-window
* to given atoms.
*
* RETURN: SNR^2 of the injected signal
* and the effective AntennaPatternMatrix M_mu_nu for this signal.
*/
REAL8
XLALAddSignalToFstatAtomVector ( FstatAtomVector* atoms, /**< [in/out] atoms vectors containing antenna-functions and possibly noise {Fa,Fb} */
AntennaPatternMatrix *M_mu_nu, /**< [out] effective antenna-pattern matrix for the injected signal */
const PulsarAmplitudeVect A_Mu, /**< [in] input canonical amplitude vector A^mu = {A1,A2,A3,A4} */
transientWindow_t transientWindow /**< transient signal window */
)
{
int gslstat;
/* check input consistency */
if ( !atoms || !atoms->data ) {
XLALPrintError ( "%s: Invalid NULL input 'atoms'\n", __func__ );
XLAL_ERROR_REAL8 ( XLAL_EINVAL );
}
if ( !M_mu_nu ) {
XLALPrintError ( "%s: Invalid NULL input 'M_mu_nu'\n", __func__ );
XLAL_ERROR_REAL8 ( XLAL_EINVAL );
}
/* prepare transient-window support */
UINT4 t0, t1;
if ( XLALGetTransientWindowTimespan ( &t0, &t1, transientWindow ) != XLAL_SUCCESS ) {
XLALPrintError ("%s: XLALGetTransientWindowTimespan() failed.\n", __func__ );
XLAL_ERROR_REAL8 ( XLAL_EFUNC );
}
/* prepare gsl-matrix for Mh_mu_nu = [ a^2, a*b ; a*b , b^2 ] */
gsl_matrix *Mh_mu_nu;
if ( (Mh_mu_nu = gsl_matrix_calloc ( 4, 4 )) == NULL ) {
XLALPrintError ("%s: gsl_matrix_calloc ( 4, 4 ) failed.\n", __func__ );
XLAL_ERROR_REAL8 ( XLAL_ENOMEM );
}
gsl_vector_const_view A_Mu_view = gsl_vector_const_view_array ( A_Mu, 4 );
REAL8 TAtom = atoms->TAtom;
UINT4 numAtoms = atoms->length;
UINT4 alpha;
REAL8 Ad = 0, Bd = 0, Cd = 0; // usual non-transient antenna-pattern functions
REAL8 Ap = 0, Bp = 0, Cp = 0; // "effective" transient-CW antenna-pattern matrix M'_mu_nu
for ( alpha=0; alpha < numAtoms; alpha ++ )
{
UINT4 ti = atoms->data[alpha].timestamp;
REAL8 win = XLALGetTransientWindowValue ( ti, t0, t1, transientWindow.tau, transientWindow.type );
if ( win == 0 )
continue;
/* compute sh_mu = sqrt(gamma/2) * Mh_mu_nu A^nu * win, where Mh_mu_nu is now just
* the per-atom block matrix [a^2, ab; ab, b^2 ]
* where Sn=1, so gamma = Sinv*TAtom = TAtom
*/
// NOTE: for sh_mu: only LINEAR in window-function, NOT quadratic! -> see notes
REAL8 a2 = win * atoms->data[alpha].a2_alpha;
REAL8 b2 = win * atoms->data[alpha].b2_alpha;
REAL8 ab = win * atoms->data[alpha].ab_alpha;
Ad += a2;
Bd += b2;
Cd += ab;
// we also compute M'_mu_nu, which will be used to estimate optimal SNR
// NOTE: M'_mu_nu is QUADRATIC in window-function!, so we multiply by win again
Ap += win * a2;
Bp += win * b2;
Cp += win * ab;
/* upper-left block */
gsl_matrix_set ( Mh_mu_nu, 0, 0, a2 );
gsl_matrix_set ( Mh_mu_nu, 1, 1, b2 );
gsl_matrix_set ( Mh_mu_nu, 0, 1, ab );
gsl_matrix_set ( Mh_mu_nu, 1, 0, ab );
/* lower-right block: +2 on all components */
gsl_matrix_set ( Mh_mu_nu, 2, 2, a2 );
gsl_matrix_set ( Mh_mu_nu, 3, 3, b2 );
gsl_matrix_set ( Mh_mu_nu, 2, 3, ab );
gsl_matrix_set ( Mh_mu_nu, 3, 2, ab );
/* placeholder for 4-vector sh_mu */
PulsarAmplitudeVect sh_mu = {0,0,0,0};
gsl_vector_view sh_mu_view = gsl_vector_view_array ( sh_mu, 4 );
/* int gsl_blas_dgemv (CBLAS_TRANSPOSE_t TransA, double alpha, const gsl_matrix * A, const gsl_vector * x, double beta, gsl_vector * y)
* compute the matrix-vector product and sum y = \alpha op(A) x + \beta y, where op(A) = A, A^T, A^H
* for TransA = CblasNoTrans, CblasTrans, CblasConjTrans.
*
* sh_mu = sqrt(gamma/2) * Mh_mu_nu A^nu, where here gamma = TAtom, as Sinv=1 for multi-IFO value, and weights for SinvX!=1 have already been absorbed in atoms through XLALComputeMultiAMCoeffs()
*/
REAL8 norm_s = sqrt(TAtom / 2.0);
if ( (gslstat = gsl_blas_dgemv (CblasNoTrans, norm_s, Mh_mu_nu, &A_Mu_view.vector, 0.0, &sh_mu_view.vector)) != 0 ) {
XLALPrintError ( "%s: gsl_blas_dgemv(L * norm) failed: %s\n", __func__, gsl_strerror (gslstat) );
XLAL_ERROR_REAL8 ( XLAL_EFAILED );
}
/* add this signal to the atoms, using the relation Fa,Fb <--> x_mu: see Eq.(72) in CFSv2-LIGO-T0900149-v2.pdf */
REAL8 s1,s2,s3,s4;
s1 = sh_mu[0];
s2 = sh_mu[1];
s3 = sh_mu[2];
s4 = sh_mu[3];
atoms->data[alpha].Fa_alpha += crectf( s1, - s3 );
atoms->data[alpha].Fb_alpha += crectf( s2, - s4 );
} /* for alpha < numAtoms */
/* compute optimal SNR^2 expected for this signal,
* using rho2 = A^mu M'_mu_nu A^nu = T/Sn( A' [A1^2+A3^2] + 2C' [A1A2 +A3A4] + B' [A2^2+A4^2])
* NOTE: here we use the "effective" transient-CW antenna-pattern matrix M'_mu_nu
*/
REAL8 A1 = A_Mu[0];
REAL8 A2 = A_Mu[1];
REAL8 A3 = A_Mu[2];
REAL8 A4 = A_Mu[3];
REAL8 rho2 = TAtom * ( Ap * ( SQ(A1) + SQ(A3) ) + 2.0*Cp * ( A1*A2 + A3*A4 ) + Bp * ( SQ(A2) + SQ(A4) ) );
/* return "effective" transient antenna-pattern matrix */
M_mu_nu->Sinv_Tsft = TAtom; /* everything here in units of Sn, so effectively Sn=1 */
M_mu_nu->Ad = Ap;
M_mu_nu->Bd = Bp;
M_mu_nu->Cd = Cp;
M_mu_nu->Dd = Ap * Bp - Cp * Cp;
/* free memory */
gsl_matrix_free ( Mh_mu_nu );
/* return SNR^2 */
return rho2;
} /* XLALAddSignalToFstatAtomVector() */
