int minimise(gsl_vector * v_alpha_zero, gsl_vector * v_alpha_A, gsl_matrix * m_V_alpha_zero, gsl_vector * v_d, gsl_matrix * m_D, gsl_vector * v_alpha, gsl_matrix * m_V_alpha, double * chi2) {
size_t height = v_alpha_zero->size;
gsl_vector * v_delta_alpha = gsl_vector_calloc(height);
gsl_vector_memcpy(v_delta_alpha,v_alpha_zero); /* delta_alpha = alpha_zero - alpha_A*/
gsl_vector_sub(v_delta_alpha,v_alpha_A);
gsl_matrix * m_T1 = gsl_matrix_calloc(TRACK_NBR*CONSTRAINTS,height); /* Temporary matrix */
gsl_matrix * m_T2 = gsl_matrix_calloc(TRACK_NBR*CONSTRAINTS,TRACK_NBR*CONSTRAINTS); /* Temporary matrix */
gsl_vector * v_T3 = gsl_vector_calloc(TRACK_NBR*CONSTRAINTS); /* Temporary vector */
gsl_matrix * m_V_D = gsl_matrix_calloc(TRACK_NBR*CONSTRAINTS,TRACK_NBR*CONSTRAINTS);
/* V_D */
gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,m_D,m_V_alpha_zero,0.0,m_T1); /* T1 = D*V_alpha_zero */
gsl_blas_dgemm(CblasNoTrans,CblasTrans,1.0,m_T1,m_D,0.0,m_T2); /* T2 = T1*D' */
/* Ok */
gsl_blas_dgemv(CblasNoTrans,1.0,m_D,v_delta_alpha,0.0,v_T3); /* T3 = D*delta_alpha */
gsl_vector_add(v_T3,v_d); /* T3 = T3 + d */
/* Ok */
gsl_vector * v_lambda = gsl_vector_calloc(TRACK_NBR*CONSTRAINTS);
/* lambda = inv(T2)*T3 */
int s;
gsl_permutation * p = gsl_permutation_alloc (TRACK_NBR*CONSTRAINTS);
gsl_linalg_LU_decomp (m_T2, p, &s);
gsl_linalg_LU_solve(m_T2,p,v_T3,v_lambda);
/* Ok */
gsl_matrix * m_T4 = gsl_matrix_calloc(height,TRACK_NBR*CONSTRAINTS);
gsl_vector * v_T5 = gsl_vector_calloc(height);
gsl_blas_dgemm(CblasNoTrans,CblasTrans,1.0,m_V_alpha_zero,m_D,0.0,m_T4); /* T4 = V_alpha_zero*m_D' */
gsl_blas_dgemv(CblasNoTrans,1.0,m_T4,v_lambda,0.0,v_T5); /* T5 = T4*lambda */
gsl_vector_memcpy(v_alpha,v_alpha_A); /* alpha = alpha_A */
gsl_vector_sub(v_alpha,v_T5); /* alpha = alpha_A - T5*/
/* Ok */
/* Compute V_alpha */
gsl_matrix * m_T6 = gsl_matrix_calloc(TRACK_NBR*CONSTRAINTS,TRACK_NBR*CONSTRAINTS);
gsl_matrix * m_T7 = gsl_matrix_calloc(height,TRACK_NBR*CONSTRAINTS);
gsl_matrix * m_T8 = gsl_matrix_calloc(height,height);
gsl_matrix * m_T9 = gsl_matrix_calloc(height,height);
gsl_matrix_memcpy(m_V_alpha,m_V_alpha_zero); /* V_alpha = V_alpha_zero */
/* T6 = inv(T2) */
gsl_linalg_LU_invert(m_T2,p,m_T6); /* See before for LU_decomp(m_T2,...)*/
/* T7 = T4 * T6 */
gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,m_T4,m_T6,0.0,m_T7);
/* T8 = T7 * D */
gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,m_T7,m_D,0.0,m_T8);
/* T9 = T8 * V_alpha_zero */
gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,m_T8,m_V_alpha_zero,0.0,m_T9);
gsl_matrix_sub(m_V_alpha,m_T9);
/* Ok */
/* Compute chi2 */
/* chi2 = lambda' * T3 */
gsl_blas_ddot(v_lambda,v_T3,chi2);
gsl_vector_free(v_T5);
/* Clean the mess */
gsl_matrix_free(m_T1);
gsl_matrix_free(m_T2);
gsl_vector_free(v_T3);
gsl_matrix_free(m_T4);
gsl_matrix_free(m_T6);
gsl_matrix_free(m_T7);
gsl_matrix_free(m_T8);
gsl_matrix_free(m_T9);
gsl_permutation_free(p);
gsl_matrix_free(m_V_D);
gsl_vector_free(v_delta_alpha);
gsl_vector_free(v_lambda);
}
int
main(int argc, char *argv[])
{
gen_workspace *gen_workspace_p;
lapack_workspace *lapack_workspace_p;
size_t N;
int c;
int lower;
int upper;
int incremental;
size_t nmat;
gsl_matrix *A, *B;
gsl_rng *r;
int s;
int compute_schur;
size_t i;
gsl_ieee_env_setup();
gsl_rng_env_setup();
N = 30;
lower = -10;
upper = 10;
incremental = 0;
nmat = 0;
compute_schur = 0;
while ((c = getopt(argc, argv, "ic:n:l:u:z")) != (-1))
{
switch (c)
{
case 'i':
incremental = 1;
break;
case 'n':
N = strtol(optarg, NULL, 0);
break;
case 'l':
lower = strtol(optarg, NULL, 0);
break;
case 'u':
upper = strtol(optarg, NULL, 0);
break;
case 'c':
nmat = strtoul(optarg, NULL, 0);
break;
case 'z':
compute_schur = 1;
break;
case '?':
default:
printf("usage: %s [-i] [-z] [-n size] [-l lower-bound] [-u upper-bound] [-c num]\n", argv[0]);
exit(1);
break;
} /* switch (c) */
}
A = gsl_matrix_alloc(N, N);
B = gsl_matrix_alloc(N, N);
gen_workspace_p = gen_alloc(N, compute_schur);
lapack_workspace_p = lapack_alloc(N);
r = gsl_rng_alloc(gsl_rng_default);
if (incremental)
{
make_start_matrix(A, lower);
/* we need B to be non-singular */
make_random_integer_matrix(B, r, lower, upper);
}
fprintf(stderr, "testing N = %d", N);
if (incremental)
fprintf(stderr, " incrementally");
else
fprintf(stderr, " randomly");
fprintf(stderr, " on element range [%d, %d]", lower, upper);
if (compute_schur)
fprintf(stderr, ", with Schur vectors");
fprintf(stderr, "\n");
while (1)
{
if (nmat && (count >= nmat))
break;
++count;
if (!incremental)
{
make_random_matrix(A, r, lower, upper);
make_random_matrix(B, r, lower, upper);
}
else
{
s = inc_matrix(A, lower, upper);
if (s)
break; /* all done */
make_random_integer_matrix(B, r, lower, upper);
}
/*if (count != 89120)
continue;*/
/* make copies of matrices */
gsl_matrix_memcpy(gen_workspace_p->A, A);
gsl_matrix_memcpy(gen_workspace_p->B, B);
gsl_matrix_transpose_memcpy(lapack_workspace_p->A, A);
gsl_matrix_transpose_memcpy(lapack_workspace_p->B, B);
/* compute eigenvalues with LAPACK */
s = lapack_proc(lapack_workspace_p);
if (s != GSL_SUCCESS)
{
printf("LAPACK failed, case %lu\n", count);
exit(1);
}
#if 0
print_matrix(A, "A");
print_matrix(B, "B");
gsl_matrix_transpose(lapack_workspace_p->A);
gsl_matrix_transpose(lapack_workspace_p->B);
print_matrix(lapack_workspace_p->A, "S_lapack");
print_matrix(lapack_workspace_p->B, "T_lapack");
#endif
/* compute eigenvalues with GSL */
s = gen_proc(gen_workspace_p);
if (s != GSL_SUCCESS)
{
printf("=========== CASE %lu ============\n", count);
printf("Failed to converge: found %u eigenvalues\n",
gen_workspace_p->n_evals);
print_matrix(A, "A");
print_matrix(B, "B");
print_matrix(gen_workspace_p->A, "Af");
print_matrix(gen_workspace_p->B, "Bf");
print_matrix(lapack_workspace_p->A, "Ae");
print_matrix(lapack_workspace_p->B, "Be");
exit(1);
}
#if 0
print_matrix(gen_workspace_p->A, "S_gsl");
print_matrix(gen_workspace_p->B, "T_gsl");
#endif
/* compute alpha / beta vectors */
for (i = 0; i < N; ++i)
{
double beta;
gsl_complex alpha, z;
beta = gsl_vector_get(gen_workspace_p->beta, i);
if (beta == 0.0)
GSL_SET_COMPLEX(&z, GSL_POSINF, GSL_POSINF);
else
{
alpha = gsl_vector_complex_get(gen_workspace_p->alpha, i);
z = gsl_complex_div_real(alpha, beta);
}
gsl_vector_complex_set(gen_workspace_p->evals, i, z);
beta = gsl_vector_get(lapack_workspace_p->beta, i);
GSL_SET_COMPLEX(&alpha,
lapack_workspace_p->alphar[i],
lapack_workspace_p->alphai[i]);
if (beta == 0.0)
GSL_SET_COMPLEX(&z, GSL_POSINF, GSL_POSINF);
else
z = gsl_complex_div_real(alpha, beta);
gsl_vector_complex_set(lapack_workspace_p->evals, i, z);
gsl_vector_complex_set(lapack_workspace_p->alpha, i, alpha);
}
#if 0
gsl_sort_vector(gen_workspace_p->beta);
gsl_sort_vector(lapack_workspace_p->beta);
sort_complex_vector(gen_workspace_p->alpha);
sort_complex_vector(lapack_workspace_p->alpha);
s = test_alpha(gen_workspace_p->alpha,
lapack_workspace_p->alpha,
A,
B,
"gen",
"lapack");
s = test_beta(gen_workspace_p->beta,
lapack_workspace_p->beta,
A,
B,
"gen",
"lapack");
#endif
#if 1
sort_complex_vector(gen_workspace_p->evals);
sort_complex_vector(lapack_workspace_p->evals);
s = test_evals(gen_workspace_p->evals,
lapack_workspace_p->evals,
A,
B,
"gen",
"lapack");
#endif
if (compute_schur)
{
test_schur(A,
gen_workspace_p->A,
gen_workspace_p->Q,
gen_workspace_p->Z);
test_schur(B,
gen_workspace_p->B,
gen_workspace_p->Q,
gen_workspace_p->Z);
}
}
gsl_matrix_free(A);
gsl_matrix_free(B);
gen_free(gen_workspace_p);
lapack_free(lapack_workspace_p);
if (r)
gsl_rng_free(r);
return 0;
} /* main() */
static int
test_COD_lssolve_eps(const gsl_matrix * m, const double * actual, const double eps, const char *desc)
{
int s = 0;
size_t i, M = m->size1, N = m->size2;
gsl_vector * lhs = gsl_vector_alloc(M);
gsl_vector * rhs = gsl_vector_alloc(M);
gsl_matrix * QRZT = gsl_matrix_alloc(M, N);
gsl_vector * tau_Q = gsl_vector_alloc(GSL_MIN(M, N));
gsl_vector * tau_Z = gsl_vector_alloc(GSL_MIN(M, N));
gsl_vector * work = gsl_vector_alloc(N);
gsl_vector * x = gsl_vector_alloc(N);
gsl_vector * r = gsl_vector_alloc(M);
gsl_vector * res = gsl_vector_alloc(M);
gsl_permutation * perm = gsl_permutation_alloc(N);
size_t rank;
gsl_matrix_memcpy(QRZT, m);
for (i = 0; i < M; i++)
gsl_vector_set(rhs, i, i + 1.0);
s += gsl_linalg_COD_decomp(QRZT, tau_Q, tau_Z, perm, &rank, work);
s += gsl_linalg_COD_lssolve(QRZT, tau_Q, tau_Z, perm, rank, rhs, x, res);
for (i = 0; i < N; i++)
{
double xi = gsl_vector_get(x, i);
gsl_test_rel(xi, actual[i], eps,
"%s (%3lu,%3lu)[%lu]: %22.18g %22.18g\n",
desc, M, N, i, xi, actual[i]);
}
/* compute residual r = b - m x */
if (M == N)
{
gsl_vector_set_zero(r);
}
else
{
gsl_vector_memcpy(r, rhs);
gsl_blas_dgemv(CblasNoTrans, -1.0, m, x, 1.0, r);
}
for (i = 0; i < N; i++)
{
double r1 = gsl_vector_get(res, i);
double r2 = gsl_vector_get(r, i);
if (fabs(r2) < 1.0e3 * GSL_DBL_EPSILON)
{
gsl_test_abs(r1, r2, 10.0 * eps,
"%s res (%3lu,%3lu)[%lu]: %22.18g %22.18g\n",
desc, M, N, i, r1, r2);
}
else
{
gsl_test_rel(r1, r2, eps,
"%s res (%3lu,%3lu)[%lu]: %22.18g %22.18g\n",
desc, M, N, i, r1, r2);
}
}
gsl_vector_free(r);
gsl_vector_free(res);
gsl_vector_free(x);
gsl_vector_free(tau_Q);
gsl_vector_free(tau_Z);
gsl_matrix_free(QRZT);
gsl_vector_free(rhs);
gsl_vector_free(lhs);
gsl_vector_free(work);
gsl_permutation_free(perm);
return s;
}
int
gsl_linalg_pcholesky_invert(const gsl_matrix * LDLT, const gsl_permutation * p,
gsl_matrix * Ainv)
{
const size_t M = LDLT->size1;
const size_t N = LDLT->size2;
if (M != N)
{
GSL_ERROR ("LDLT matrix must be square", GSL_ENOTSQR);
}
else if (LDLT->size1 != p->size)
{
GSL_ERROR ("matrix size must match permutation size", GSL_EBADLEN);
}
else if (Ainv->size1 != Ainv->size2)
{
GSL_ERROR ("Ainv matrix must be square", GSL_ENOTSQR);
}
else if (Ainv->size1 != M)
{
GSL_ERROR ("Ainv matrix has wrong dimensions", GSL_EBADLEN);
}
else
{
size_t i, j;
gsl_vector_view v1, v2;
/* invert the lower triangle of LDLT */
gsl_matrix_memcpy(Ainv, LDLT);
gsl_linalg_tri_lower_unit_invert(Ainv);
/* compute sqrt(D^{-1}) L^{-1} in the lower triangle of Ainv */
for (i = 0; i < N; ++i)
{
double di = gsl_matrix_get(LDLT, i, i);
double sqrt_di = sqrt(di);
for (j = 0; j < i; ++j)
{
double *Lij = gsl_matrix_ptr(Ainv, i, j);
*Lij /= sqrt_di;
}
gsl_matrix_set(Ainv, i, i, 1.0 / sqrt_di);
}
/*
* The lower triangle of Ainv now contains D^{-1/2} L^{-1}. Now compute
* A^{-1} = L^{-T} D^{-1} L^{-1}
*/
for (i = 0; i < N; ++i)
{
double aii = gsl_matrix_get(Ainv, i, i);
if (i < N - 1)
{
double tmp;
v1 = gsl_matrix_subcolumn(Ainv, i, i, N - i);
gsl_blas_ddot(&v1.vector, &v1.vector, &tmp);
gsl_matrix_set(Ainv, i, i, tmp);
if (i > 0)
{
gsl_matrix_view m = gsl_matrix_submatrix(Ainv, i + 1, 0, N - i - 1, i);
v1 = gsl_matrix_subcolumn(Ainv, i, i + 1, N - i - 1);
v2 = gsl_matrix_subrow(Ainv, i, 0, i);
gsl_blas_dgemv(CblasTrans, 1.0, &m.matrix, &v1.vector, aii, &v2.vector);
}
}
else
{
v1 = gsl_matrix_row(Ainv, N - 1);
gsl_blas_dscal(aii, &v1.vector);
}
}
/* copy lower triangle to upper */
gsl_matrix_transpose_tricpy('L', 0, Ainv, Ainv);
/* now apply permutation p to the matrix */
/* compute L^{-T} D^{-1} L^{-1} P^T */
for (i = 0; i < N; ++i)
{
v1 = gsl_matrix_row(Ainv, i);
gsl_permute_vector_inverse(p, &v1.vector);
}
/* compute P L^{-T} D^{-1} L^{-1} P^T */
for (i = 0; i < N; ++i)
{
v1 = gsl_matrix_column(Ainv, i);
gsl_permute_vector_inverse(p, &v1.vector);
}
return GSL_SUCCESS;
}
}
int
gsl_multifit_linear (const gsl_matrix * X,
const gsl_vector * y,
gsl_vector * c,
gsl_matrix * cov,
double *chisq, gsl_multifit_linear_workspace * work)
{
if (X->size1 != y->size)
{
GSL_ERROR
("number of observations in y does not match rows of matrix X",
GSL_EBADLEN);
}
else if (X->size2 != c->size)
{
GSL_ERROR ("number of parameters c does not match columns of matrix X",
GSL_EBADLEN);
}
else if (cov->size1 != cov->size2)
{
GSL_ERROR ("covariance matrix is not square", GSL_ENOTSQR);
}
else if (c->size != cov->size1)
{
GSL_ERROR
("number of parameters does not match size of covariance matrix",
GSL_EBADLEN);
}
else if (X->size1 != work->n || X->size2 != work->p)
{
GSL_ERROR
("size of workspace does not match size of observation matrix",
GSL_EBADLEN);
}
else
{
const size_t n = X->size1;
const size_t p = X->size2;
size_t i, j;
gsl_matrix *A = work->A;
gsl_matrix *Q = work->Q;
gsl_matrix *QSI = work->QSI;
gsl_vector *S = work->S;
gsl_vector *xt = work->xt;
gsl_vector *D = work->D;
/* Copy X to workspace, A <= X */
gsl_matrix_memcpy (A, X);
/* Balance the columns of the matrix A */
gsl_linalg_balance_columns (A, D);
/* Decompose A into U S Q^T */
gsl_linalg_SV_decomp_mod (A, QSI, Q, S, xt);
/* Solve y = A c for c */
gsl_blas_dgemv (CblasTrans, 1.0, A, y, 0.0, xt);
/* Scale the matrix Q, Q' = Q S^-1 */
gsl_matrix_memcpy (QSI, Q);
for (j = 0; j < p; j++)
{
gsl_vector_view column = gsl_matrix_column (QSI, j);
double alpha = gsl_vector_get (S, j);
if (alpha != 0)
alpha = 1.0 / alpha;
gsl_vector_scale (&column.vector, alpha);
}
gsl_vector_set_zero (c);
gsl_blas_dgemv (CblasNoTrans, 1.0, QSI, xt, 0.0, c);
/* Unscale the balancing factors */
gsl_vector_div (c, D);
/* Compute chisq, from residual r = y - X c */
{
double s2 = 0, r2 = 0;
for (i = 0; i < n; i++)
{
double yi = gsl_vector_get (y, i);
gsl_vector_const_view row = gsl_matrix_const_row (X, i);
double y_est, ri;
gsl_blas_ddot (&row.vector, c, &y_est);
ri = yi - y_est;
r2 += ri * ri;
}
s2 = r2 / (n - p);
*chisq = r2;
/* Form variance-covariance matrix cov = s2 * (Q S^-1) (Q S^-1)^T */
for (i = 0; i < p; i++)
{
gsl_vector_view row_i = gsl_matrix_row (QSI, i);
double d_i = gsl_vector_get (D, i);
for (j = i; j < p; j++)
{
gsl_vector_view row_j = gsl_matrix_row (QSI, j);
double d_j = gsl_vector_get (D, j);
double s;
gsl_blas_ddot (&row_i.vector, &row_j.vector, &s);
gsl_matrix_set (cov, i, j, s * s2 / (d_i * d_j));
gsl_matrix_set (cov, j, i, s * s2 / (d_i * d_j));
}
}
}
return GSL_SUCCESS;
}
}
AnovaTest::AnovaTest(mv_Method *mm, gsl_matrix *Y, gsl_matrix *X, gsl_matrix *isXvarIn):mmRef(mm), Yref(Y), Xref(X), inRef(isXvarIn)
{
unsigned int hid, aid;
unsigned int i, j, count;
nModels=inRef->size1, nParam=Xref->size2;
nRows=Yref->size1, nVars=Yref->size2;
// printf("initialize public variables: stats\n");
multstat=(double *)malloc((nModels-1)*sizeof(double));
Pmultstat = (double *)malloc((nModels-1)*sizeof(double));
for (j=0; j<nModels-1; j++) *(Pmultstat+j)=0.0;
dfDiff = (unsigned int *)malloc((nModels-1)*sizeof(unsigned int));
statj = gsl_matrix_alloc(nModels-1, nVars);
Pstatj = gsl_matrix_alloc(nModels-1, nVars);
gsl_matrix_set_zero(Pstatj);
bStatj = gsl_vector_alloc(nVars);
Hats = (mv_mat *)malloc(nModels*sizeof(mv_mat));
sortid = (gsl_permutation **)malloc((nModels-1)*sizeof(gsl_permutation *));
for (i=0; i<nModels; i++ ) {
// Hats[i]
Hats[i].mat=gsl_matrix_alloc(nRows, nRows);
Hats[i].SS=gsl_matrix_alloc(nVars, nVars);
Hats[i].R=gsl_matrix_alloc(nVars, nVars);
Hats[i].Res=gsl_matrix_alloc(nRows, nVars);
Hats[i].Y = gsl_matrix_alloc(nRows, nVars);
Hats[i].sd = gsl_vector_alloc(nVars);
count = 0;
for (j=0; j<nParam; j++){
count+=(unsigned int)gsl_matrix_get(inRef, i, j);
}
// printf("count=%d \n", count);
Hats[i].X = gsl_matrix_alloc(nRows, count);
Hats[i].Coef=gsl_matrix_alloc(count, nVars);
gsl_vector_view refi=gsl_matrix_row(inRef, i);
subX(Xref, &refi.vector, Hats[i].X);
calcSS(Yref, &(Hats[i]), mmRef);
// displaymatrix(Hats[i].SS, "SS");
}
for (i=1; i<nModels; i++) {
hid = i; aid = i-1;
if ( mmRef->resamp != CASEBOOT ) {
// fit = Y- resi
gsl_matrix_memcpy (Hats[i].Y, Yref);
gsl_matrix_sub (Hats[i].Y, Hats[i].Res);
}
gsl_vector_view statij = gsl_matrix_row(statj, aid);
testStatCalc(&(Hats[hid]), &(Hats[aid]), mmRef, TRUE, (multstat+aid), &statij.vector);
dfDiff[aid] = Hats[aid].X->size2-Hats[hid].X->size2;
// sortid
sortid[aid] = gsl_permutation_alloc(nVars);
gsl_sort_vector_index (sortid[aid], &statij.vector);
// rearrange sortid in descending order
gsl_permutation_reverse (sortid[aid]);
}
// initialize resampling indices
// getBootID(); done in R
bootID = NULL;
// Initialize GSL rnd environment variables
const gsl_rng_type *T;
gsl_rng_env_setup();
T = gsl_rng_default;
// an mt19937 generator with a seed of 0
rnd = gsl_rng_alloc(T);
if (mmRef->reprand!=TRUE){
struct timeval tv; // seed generation based on time
gettimeofday(&tv, 0);
unsigned long mySeed=tv.tv_sec + tv.tv_usec;
gsl_rng_set(rnd, mySeed); // reset seed
}
// printf("Anova test initialized.\n");
}
static int
md_eigen(lua_State *L) /* (-1,+2,e) */
{
mMatReal *m = qlua_checkMatReal(L, 1);
gsl_matrix_view mx;
gsl_eigen_symmv_workspace *w;
gsl_vector *ev;
mVecReal *lambda;
mMatReal *trans;
mMatReal *tmp;
int n;
int i;
int lo, hi;
switch (lua_gettop(L)) {
case 1:
if (m->l_size != m->r_size)
return luaL_error(L, "matrix:eigen() expects square matrix");
lo = 0;
hi = m->l_size;
break;
case 2:
lo = 0;
hi = luaL_checkint(L, 2);
if ((hi > m->l_size) || (hi > m->r_size))
return slice_out(L);
break;
case 3:
lo = luaL_checkint(L, 2);
hi = luaL_checkint(L, 3);
if ((lo >= hi) ||
(lo > m->l_size) || (lo > m->r_size) ||
(hi > m->l_size) || (hi > m->r_size))
return slice_out(L);
break;
default:
return luaL_error(L, "matrix:eigen(): illegal arguments");
}
n = hi - lo;
mx = gsl_matrix_submatrix(m->m, lo, lo, n, n);
tmp = qlua_newMatReal(L, n, n);
gsl_matrix_memcpy(tmp->m, &mx.matrix);
lambda = qlua_newVecReal(L, n);
trans = qlua_newMatReal(L, n, n);
ev = new_gsl_vector(L, n);
w = gsl_eigen_symmv_alloc(n);
if (w == 0) {
lua_gc(L, LUA_GCCOLLECT, 0);
w = gsl_eigen_symmv_alloc(n);
if (w == 0)
luaL_error(L, "not enough memory");
}
if (gsl_eigen_symmv(tmp->m, ev, trans->m, w))
luaL_error(L, "matrix:eigen() failed");
if (gsl_eigen_symmv_sort(ev, trans->m, GSL_EIGEN_SORT_VAL_ASC))
luaL_error(L, "matrix:eigen() eigenvalue ordering failed");
for (i = 0; i < n; i++)
lambda->val[i] = gsl_vector_get(ev, i);
gsl_vector_free(ev);
gsl_eigen_symmv_free(w);
return 2;
}
int Holling2(double t, const double y[], double ydot[], void *params){
double alpha = 0.3; // respiration
double lambda = 0.65; // ecologic efficiency
double hand = 0.35; // handling time
double beta = 0.5; // intraspecific competition
double aij = 6.0; // attack rate
int i, j,l = 0; // Hilfsvariablen
double rowsum = 0;
double colsum = 0;
//-- Struktur zerlegen-------------------------------------------------------------------------------------------------------------------------------
struct foodweb *nicheweb = (struct foodweb *)params; // pointer cast from (void*) to (struct foodweb*)
//printf("t in Holling 2=%f\n", t);
gsl_vector *network = (nicheweb->network); // Inhalt: A+linksA+Y+linksY+Massen+Trophische_Level = (Rnum+S)²+1+Y²+1+(Rnum+S)+S
int S = nicheweb->S;
int Y = nicheweb->Y;
int Rnum = nicheweb->Rnum;
double d = nicheweb->d;
int Z = nicheweb->Z;
double dij = pow(10, d);
double nu,mu, tau;
int SpeciesNumber;
tau = gsl_vector_get(nicheweb->migrPara,0);
mu = gsl_vector_get(nicheweb->migrPara,1);
if((int)nu!=0)
{
//printf("nu ist nicht null sondern %f\n",nu);
}
nu = gsl_vector_get(nicheweb->migrPara,2);
SpeciesNumber = gsl_vector_get(nicheweb->migrPara,3);
double tlast = gsl_vector_get(nicheweb->migrPara,4);
if(SpeciesNumber!= 0)
{
//printf("SpeciesNumber %i\n", SpeciesNumber);
}
//printf("t oben %f\n",t);
//int len = (Rnum+S)*(Rnum+S)+2+Y*Y+(Rnum+S)+S;
gsl_vector_view A_view = gsl_vector_subvector(network, 0, (Rnum+S)*(Rnum+S)); // Fressmatrix A als Vektor
gsl_matrix_view EA_mat = gsl_matrix_view_vector(&A_view.vector, (Rnum+S), (Rnum+S)); // A als Matrix_view
gsl_matrix *EAmat = &EA_mat.matrix; // A als Matrix
gsl_vector_view D_view = gsl_vector_subvector(network, (Rnum+S)*(Rnum+S)+1, Y*Y); // Migrationsmatrix D als Vektor
gsl_matrix_view ED_mat = gsl_matrix_view_vector(&D_view.vector, Y, Y); // D als Matrixview
gsl_matrix *EDmat = &ED_mat.matrix; // D als Matrix
gsl_vector_view M_vec = gsl_vector_subvector(network, ((Rnum+S)*(Rnum+S))+1+(Y*Y)+1, (Rnum+S)); // Massenvektor
gsl_vector *Mvec = &M_vec.vector;
//-- verändere zu dem gewünschten Zeitpunkt Migrationsmatrix
if( (t > tau) && (tlast < tau))
{
//printf("mu ist %f\n", gsl_vector_get(nicheweb->migrPara,1));
//printf("nu ist %f\n", nu);
gsl_vector_set(nicheweb->migrPara,4,t);
//printf("Setze Link für gewünschte Migration\n");
//printf("t oben %f\n",t);
gsl_matrix_set(EDmat, nu, mu, 1.);
int m;
// for(l = 0; l< Y;l++)
// {
// for(m=0;m<Y;m++)
// {
// printf("%f\t",gsl_matrix_get(EDmat,l,m));
// }
// printf("\n");
// }
}
else
{
gsl_matrix_set_zero(EDmat);
}
// printf("\ncheckpoint Holling2 I\n");
// printf("\nS = %i\n", S);
// printf("\nS + Rnum = %i\n", S+Rnum);
//
// printf("\nSize A_view = %i\n", (int)A_view.vector.size);
// printf("\nSize D_view = %i\n", (int)D_view.vector.size);
// printf("\nSize M_vec = %i\n", (int)M_vec.vector.size);
// for(i=0; i<(Rnum+S)*Y; i++){
// printf("\ny = %f\n", y[i]);
// }
// for(i=0; i<(Rnum+S)*Y; i++){
// printf("\nydot = %f\n", ydot[i]);
// }
//--zusätzliche Variablen anlegen-------------------------------------------------------------------------------------------------------------
double ytemp[(Rnum+S)*Y];
for(i=0; i<(Rnum+S)*Y; i++) ytemp[i] = y[i]; // temp array mit Kopie der Startwerte
for(i=0; i<(Rnum+S)*Y; i++) ydot[i] = 0; // Ergebnis, in das evolve_apply schreibt
gsl_vector_view yfddot_vec = gsl_vector_view_array(ydot, (Rnum+S)*Y); //Notiz: vector_view_array etc. arbeiten auf den original Daten der ihnen zugeordneten Arrays/Vektoren!
gsl_vector *yfddotvec = &yfddot_vec.vector; // zum einfacheren Rechnen ydot über vector_view_array ansprechen
gsl_vector_view yfd_vec = gsl_vector_view_array(ytemp, (Rnum+S)*Y);
gsl_vector *yfdvec = &yfd_vec.vector; // Startwerte der Populationen
//-- neue Objekte zum Rechnen anlegen--------------------------------------------------------------------------------------------------------
gsl_matrix *AFgsl = gsl_matrix_calloc(Rnum+S, Rnum+S); // matrix of foraging efforts
gsl_matrix *ADgsl = gsl_matrix_calloc(Y,Y); // matrix of migration efforts
gsl_matrix *Emat = gsl_matrix_calloc(Rnum+S, Rnum+S); // gsl objects for calculations of populations
gsl_vector *tvec = gsl_vector_calloc(Rnum+S);
gsl_vector *rvec = gsl_vector_calloc(Rnum+S);
gsl_vector *svec = gsl_vector_calloc(Rnum+S);
gsl_matrix *Dmat = gsl_matrix_calloc(Y,Y); // gsl objects for calculations of migration
gsl_vector *d1vec = gsl_vector_calloc(Y);
gsl_vector *d2vec = gsl_vector_calloc(Y);
gsl_vector *d3vec = gsl_vector_calloc(Y);
// printf("\ncheckpoint Holling2 III\n");
//-- Einzelne Patches lösen------------------------------------------------------------------------------------------------------------
for(l=0; l<Y; l++) // start of patch solving
{
gsl_matrix_set_zero(AFgsl); // Objekte zum Rechnen vor jedem Patch nullen
gsl_matrix_set_zero(Emat);
gsl_vector_set_zero(tvec);
gsl_vector_set_zero(rvec);
gsl_vector_set_zero(svec);
gsl_vector_view ydot_vec = gsl_vector_subvector(yfddotvec, (Rnum+S)*l, (Rnum+S)); // enthält ydot von Patch l
gsl_vector *ydotvec = &ydot_vec.vector;
gsl_vector_view y_vec = gsl_vector_subvector(yfdvec, (Rnum+S)*l, (Rnum+S)); // enthält Startwerte der Population in l
gsl_vector *yvec = &y_vec.vector;
gsl_matrix_memcpy(AFgsl, EAmat);
for(i=0; i<Rnum+S; i++)
{
gsl_vector_view rowA = gsl_matrix_row(AFgsl,i);
rowsum = gsl_blas_dasum(&rowA.vector);
if(rowsum !=0 )
{
for(j=0; j<Rnum+S; j++)
gsl_matrix_set(AFgsl, i, j, (gsl_matrix_get(AFgsl,i,j)/rowsum)); // normiere Beute Afgsl = A(Beutelinks auf 1 normiert) = f(i,j)
}
}
gsl_matrix_memcpy(Emat, EAmat); // Emat = A
gsl_matrix_scale(Emat, aij); // Emat(i,j) = a(i,j)
gsl_matrix_mul_elements(Emat, AFgsl); // Emat(i,j) = a(i,j)*f(i,j)
gsl_vector_memcpy(svec, yvec); // s(i) = y(i)
gsl_vector_scale(svec, hand); // s(i) = y(i)*h
gsl_blas_dgemv(CblasNoTrans, 1, Emat, svec, 0, rvec); // r(i) = Sum_k h*a(i,k)*f(i,k)*y(k)
gsl_vector_add_constant(rvec, 1); // r(i) = 1+Sum_k h*a(i,k)*f(i,k)*y(k)
gsl_vector_memcpy(tvec, Mvec); // t(i) = masse(i)^(-0.25)
gsl_vector_div(tvec, rvec); // t(i) = masse(i)^(-0.25)/(1+Sum_k h*a(i,k)*f(i,k)*y(k))
gsl_vector_mul(tvec, yvec); // t(i) = masse(i)^(-0.25)*y(i)/(1+Sum_k h*a(i,k)*f(i,k)*y(k))
gsl_blas_dgemv(CblasTrans, 1, Emat, tvec, 0, rvec); // r(i) = Sum_j a(j,i)*f(j,i)*t(j)
gsl_vector_mul(rvec, yvec); // r(i) = Sum_j a(j,i)*f(j,i)*t(j)*y(i) [rvec: Praedation]
gsl_blas_dgemv(CblasNoTrans, lambda, Emat, yvec, 0, ydotvec); // ydot(i) = Sum_j lambda*a(i,j)*f(i,j)*y(j)
gsl_vector_mul(ydotvec, tvec); // ydot(i) = Sum_j lambda*a(i,j)*f(i,j)*y(j)*t(i)
gsl_vector_memcpy(svec, Mvec);
gsl_vector_scale(svec, alpha); // s(i) = alpha*masse^(-0.25) [svec=Respiration bzw. Mortalitaet]
gsl_vector_memcpy(tvec, Mvec);
gsl_vector_scale(tvec, beta); // t(i) = beta*masse^(-0.25)
gsl_vector_mul(tvec, yvec); // t(i) = beta*y(i)
gsl_vector_add(svec, tvec); // s(i) = alpha*masse^(-0.25)+beta*y(i)
gsl_vector_mul(svec, yvec); // s(i) = alpha*masse^(-0.25)*y(i)+beta*y(i)*y(i)
gsl_vector_add(svec, rvec); // [svec: Respiration, competition und Praedation]
gsl_vector_sub(ydotvec, svec); // ydot(i) = Fressen-Respiration-Competition-Praedation
for(i=0; i<Rnum; i++)
gsl_vector_set(ydotvec, i, 0.0); // konstante Ressourcen
}// Ende Einzelpatch, Ergebnis steht in ydotvec
// printf("\ncheckpoint Holling2 IV\n");
//-- Migration lösen---------------------------------------------------------------------------------------------------------
gsl_vector *ydottest = gsl_vector_calloc(Y);
double ydotmigr = gsl_vector_get(nicheweb->migrPara, 5);
int count=0,m;
for(l = 0; l< Y;l++)
{
for(m=0;m<Y;m++)
{
count += gsl_matrix_get(EDmat,l,m);
}
}
// if(count!=0)
// {
// //printf("count %i\n",count);
// //printf("t unten %f\n",t);
// //printf("tau %f\n",tau);
// for(l = 0; l< Y;l++)
// {
// for(m=0;m<Y;m++)
// {
// printf("%f\t",gsl_matrix_get(EDmat,l,m));
// }
// printf("\n");
// }
// }
double max = gsl_matrix_max(EDmat);
for(l = Rnum; l< Rnum+S; l++) // start of migration solving
{
if(l == SpeciesNumber+Rnum && max !=0)
{
//printf("max ist %f\n",max);
//printf("l ist %i\n",l);
gsl_matrix_set_zero(ADgsl); // reset gsl objects for every patch
gsl_matrix_set_zero(Dmat);
gsl_vector_set_zero(d1vec);
gsl_vector_set_zero(d2vec);
gsl_vector_set_zero(d3vec);
gsl_vector_set_zero(ydottest);
// Untervektor von yfddot (enthält ydot[]) mit offset l (Rnum...Rnum+S) und Abstand zwischen den Elementen (stride) von Rnum+S.
// Dies ergibt gerade die Größe einer Spezies in jedem Patch in einem Vektor
gsl_vector_view dydot_vec = gsl_vector_subvector_with_stride(yfddotvec, l, (Rnum+S), Y); // ydot[]
gsl_vector *dydotvec = &dydot_vec.vector;
gsl_vector_view dy_vec = gsl_vector_subvector_with_stride(yfdvec, l, (Rnum+S), Y); // Startgrößen der Spezies pro Patch
gsl_vector *dyvec = &dy_vec.vector;
gsl_matrix_memcpy(ADgsl, EDmat); // ADgsl = D
if(nicheweb->M == 1) // umschalten w: patchwise (Abwanderung aus jedem Patch gleich), sonst linkwise (Abwanderung pro link gleich)
{
for(i=0; i<Y; i++)
{
gsl_vector_view colD = gsl_matrix_column(ADgsl, i); // Spalte i aus Migrationsmatrix
colsum = gsl_blas_dasum(&colD.vector);
if(colsum!=0)
{
for(j=0;j<Y;j++)
gsl_matrix_set(ADgsl,j,i,(gsl_matrix_get(ADgsl,j,i)/colsum)); // ADgsl: D mit normierten Links
}
}
}
gsl_matrix_memcpy(Dmat, EDmat); // Dmat = D
gsl_matrix_scale(Dmat, dij); // Dmat(i,j) = d(i,j) (Migrationsstärke)
gsl_matrix_mul_elements(Dmat, ADgsl); // Dmat(i,j) = d(i,j)*xi(i,j) (skalierte und normierte Migrationsmatrix)
gsl_vector_set_all(d1vec, 1/gsl_vector_get(Mvec, l)); // d1(i)= m(l)^0.25
gsl_vector_mul(d1vec, dyvec); // d1(i)= m(l)^0.25*y(i)
gsl_blas_dgemv(CblasNoTrans, 1, Dmat, d1vec, 0, d2vec); // d2(i)= Sum_j d(i,j)*xi(i,j)*m(l)^0.25*y(j)
gsl_vector_set_all(d1vec, 1); // d1(i)= 1
gsl_blas_dgemv(CblasTrans, 1, Dmat, d1vec, 0, d3vec); // d3(i)= Sum_j d(i,j)*xi(i,j)
gsl_vector_scale(d3vec, 1/gsl_vector_get(Mvec,l)); // d3(i)= Sum_j d(i,j)*xi(i,j)*m(l)^0.25
gsl_vector_mul(d3vec, dyvec); // d3(i)= Sum_j d(i,j)*xi(i,j)*m(l)^0.25*y(i)
gsl_vector_add(ydottest,d2vec);
gsl_vector_sub(ydottest,d3vec);
//printf("d2vec ist %f\n",gsl_vector_get(d2vec,0));
//printf("d3vec ist %f\n",gsl_vector_get(d3vec,0));
//if(gsl_vector_get(ydottest,mu)!=0)
//{
ydotmigr += gsl_vector_get(ydottest,nu);
gsl_vector_set(nicheweb->migrPara,5,ydotmigr);
//printf("ydotmigr ist %f\n",gsl_vector_get(nicheweb->migrPara,5));
// if(ydotmigr !=0)
// {
// printf("ydottest aufaddiert ist %f\n",ydotmigr);
// printf("ydottest aufaddiert ist %f\n",gsl_vector_get(nicheweb->migrPara,5));
// }
gsl_vector_add(dydotvec, d2vec); //
gsl_vector_sub(dydotvec, d3vec); // Ergebnis in dydotvec (also ydot[]) = Sum_j d(i,j)*xi(i,j)*m(l)^0.25*y(j) - Sum_j d(i,j)*xi(i,j)*m(l)^0.25*y(i)
}
}// Patch i gewinnt das was aus allen j Patches zuwandert und verliert was von i auswandert
//printf("ydot ist %f\n",gsl_vector_get(ydottest,0));
//printf("\ncheckpoint Holling2 V\n");
/*
for(i=0; i<(Rnum+S)*Y; i++){
printf("\ny = %f\tydot=%f\n", y[i], ydot[i]);
}
*/
//--check for fixed point attractor-----------------------------------------------------------------------------------
if(t>7800){
gsl_vector_set(nicheweb->fixpunkte, 0, 0);
gsl_vector_set(nicheweb->fixpunkte, 1, 0);
gsl_vector_set(nicheweb->fixpunkte, 2, 0);
int fix0 = (int)gsl_vector_get(nicheweb->fixpunkte, 0);
int fix1 = (int)gsl_vector_get(nicheweb->fixpunkte, 1);
int fix2 = (int)gsl_vector_get(nicheweb->fixpunkte, 2);
//printf("t unten = %f\n", t);
for(i=0; i<(Rnum+S)*Y; i++)
{
if(y[i] <= 0)
{
fix0++;
fix1++;
fix2++;
}
else
{
if((ydot[i]/y[i]<0.0001) || (ydot[i]<0.0001)) fix0++;
if(ydot[i]/y[i]<0.0001) fix1++;
if(ydot[i]<0.0001) fix2++;
}
}
if(fix0==(Rnum+S)*Y) gsl_vector_set(nicheweb->fixpunkte, 3, 1);
if(fix1==(Rnum+S)*Y) gsl_vector_set(nicheweb->fixpunkte, 4, 1);
if(fix2==(Rnum+S)*Y) gsl_vector_set(nicheweb->fixpunkte, 5, 1);
}
//--Speicher leeren-----------------------------------------------------------------------------------------------------
gsl_matrix_free(Emat);
gsl_matrix_free(Dmat);
gsl_matrix_free(AFgsl);
gsl_matrix_free(ADgsl);
gsl_vector_free(tvec);
gsl_vector_free(rvec);
gsl_vector_free(svec);
gsl_vector_free(d1vec);
gsl_vector_free(d2vec);
gsl_vector_free(d3vec);
gsl_vector_free(ydottest);
// printf("\nCheckpoint Holling2 VI\n");
return GSL_SUCCESS;
}
static int
set (void *vstate, gsl_multifit_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_matrix * J, gsl_vector * dx, int scale)
{
lmder_state_t *state = (lmder_state_t *) vstate;
gsl_matrix *r = state->r;
gsl_vector *tau = state->tau;
gsl_vector *diag = state->diag;
gsl_vector *work1 = state->work1;
gsl_permutation *perm = state->perm;
int signum;
/* Evaluate function at x */
/* return immediately if evaluation raised error */
{
int status = GSL_MULTIFIT_FN_EVAL_F_DF (fdf, x, f, J);
if (status)
return status;
}
state->par = 0;
state->iter = 1;
state->fnorm = enorm (f);
gsl_vector_set_all (dx, 0.0);
/* store column norms in diag */
if (scale)
{
compute_diag (J, diag);
}
else
{
gsl_vector_set_all (diag, 1.0);
}
/* set delta to 100 |D x| or to 100 if |D x| is zero */
state->xnorm = scaled_enorm (diag, x);
state->delta = compute_delta (diag, x);
/* Factorize J into QR decomposition */
gsl_matrix_memcpy (r, J);
gsl_linalg_QRPT_decomp (r, tau, perm, &signum, work1);
gsl_vector_set_zero (state->rptdx);
gsl_vector_set_zero (state->w);
/* Zero the trial vector, as in the alloc function */
gsl_vector_set_zero (state->f_trial);
#ifdef DEBUG
printf("r = "); gsl_matrix_fprintf(stdout, r, "%g");
printf("perm = "); gsl_permutation_fprintf(stdout, perm, "%d");
printf("tau = "); gsl_vector_fprintf(stdout, tau, "%g");
#endif
return GSL_SUCCESS;
}
int main(int argc, char **argv) {
const int MAX_ITER = 50;
const double RELTOL = 1e-2;
const double ABSTOL = 1e-4;
/*
* Some bookkeeping variables for MPI. The 'rank' of a process is its numeric id
* in the process pool. For example, if we run a program via `mpirun -np 4 foo', then
* the process ranks are 0 through 3. Here, N and size are the total number of processes
* running (in this example, 4).
*/
int rank;
int size;
MPI_Init(&argc, &argv); // Initialize the MPI execution environment
MPI_Comm_rank(MPI_COMM_WORLD, &rank); // Determine current running process
MPI_Comm_size(MPI_COMM_WORLD, &size); // Total number of processes
double N = (double) size; // Number of subsystems/slaves for ADMM
/* Read in local data */
int skinny; // A flag indicating whether the matrix A is fat or skinny
FILE *f;
int m, n;
int row, col;
double entry;
/*
* Subsystem n will look for files called An.dat and bn.dat
* in the current directory; these are its local data and do not need to be
* visible to any other processes. Note that
* m and n here refer to the dimensions of the *local* coefficient matrix.
*/
/* Read A */
char s[20];
sprintf(s, "data/A%d.dat", rank + 1);
printf("[%d] reading %s\n", rank, s);
f = fopen(s, "r");
if (f == NULL) {
printf("[%d] ERROR: %s does not exist, exiting.\n", rank, s);
exit(EXIT_FAILURE);
}
mm_read_mtx_array_size(f, &m, &n);
gsl_matrix *A = gsl_matrix_calloc(m, n);
for (int i = 0; i < m*n; i++) {
row = i % m;
col = floor(i/m);
fscanf(f, "%lf", &entry);
gsl_matrix_set(A, row, col, entry);
}
fclose(f);
/* Read b */
sprintf(s, "data/b%d.dat", rank + 1);
printf("[%d] reading %s\n", rank, s);
f = fopen(s, "r");
if (f == NULL) {
printf("[%d] ERROR: %s does not exist, exiting.\n", rank, s);
exit(EXIT_FAILURE);
}
mm_read_mtx_array_size(f, &m, &n);
gsl_vector *b = gsl_vector_calloc(m);
for (int i = 0; i < m; i++) {
fscanf(f, "%lf", &entry);
gsl_vector_set(b, i, entry);
}
fclose(f);
m = A->size1;
n = A->size2;
skinny = (m >= n);
/*
* These are all variables related to ADMM itself. There are many
* more variables than in the Matlab implementation because we also
* require vectors and matrices to store various intermediate results.
* The naming scheme follows the Matlab version of this solver.
*/
double rho = 1.0;
gsl_vector *x = gsl_vector_calloc(n);
gsl_vector *u = gsl_vector_calloc(n);
gsl_vector *z = gsl_vector_calloc(n);
gsl_vector *y = gsl_vector_calloc(n);
gsl_vector *r = gsl_vector_calloc(n);
gsl_vector *zprev = gsl_vector_calloc(n);
gsl_vector *zdiff = gsl_vector_calloc(n);
gsl_vector *q = gsl_vector_calloc(n);
gsl_vector *w = gsl_vector_calloc(n);
gsl_vector *Aq = gsl_vector_calloc(m);
gsl_vector *p = gsl_vector_calloc(m);
gsl_vector *Atb = gsl_vector_calloc(n);
double send[3]; // an array used to aggregate 3 scalars at once
double recv[3]; // used to receive the results of these aggregations
double nxstack = 0;
double nystack = 0;
double prires = 0;
double dualres = 0;
double eps_pri = 0;
double eps_dual = 0;
/* Precompute and cache factorizations */
gsl_blas_dgemv(CblasTrans, 1, A, b, 0, Atb); // Atb = A^T b
/*
* The lasso regularization parameter here is just hardcoded
* to 0.5 for simplicity. Using the lambda_max heuristic would require
* network communication, since it requires looking at the *global* A^T b.
*/
double lambda = 0.5;
if (rank == 0) {
printf("using lambda: %.4f\n", lambda);
}
gsl_matrix *L;
/* Use the matrix inversion lemma for efficiency; see section 4.2 of the paper */
if (skinny) {
/* L = chol(AtA + rho*I) */
L = gsl_matrix_calloc(n,n);
gsl_matrix *AtA = gsl_matrix_calloc(n,n);
gsl_blas_dsyrk(CblasLower, CblasTrans, 1, A, 0, AtA);
gsl_matrix *rhoI = gsl_matrix_calloc(n,n);
gsl_matrix_set_identity(rhoI);
gsl_matrix_scale(rhoI, rho);
gsl_matrix_memcpy(L, AtA);
gsl_matrix_add(L, rhoI);
gsl_linalg_cholesky_decomp(L);
gsl_matrix_free(AtA);
gsl_matrix_free(rhoI);
} else {
/* L = chol(I + 1/rho*AAt) */
L = gsl_matrix_calloc(m,m);
gsl_matrix *AAt = gsl_matrix_calloc(m,m);
gsl_blas_dsyrk(CblasLower, CblasNoTrans, 1, A, 0, AAt);
gsl_matrix_scale(AAt, 1/rho);
gsl_matrix *eye = gsl_matrix_calloc(m,m);
gsl_matrix_set_identity(eye);
gsl_matrix_memcpy(L, AAt);
gsl_matrix_add(L, eye);
gsl_linalg_cholesky_decomp(L);
gsl_matrix_free(AAt);
gsl_matrix_free(eye);
}
/* Main ADMM solver loop */
int iter = 0;
if (rank == 0) {
printf("%3s %10s %10s %10s %10s %10s\n", "#", "r norm", "eps_pri", "s norm", "eps_dual", "objective");
}
double startAllTime, endAllTime;
startAllTime = MPI_Wtime();
while (iter < MAX_ITER) {
/* u-update: u = u + x - z */
gsl_vector_sub(x, z);
gsl_vector_add(u, x);
/* x-update: x = (A^T A + rho I) \ (A^T b + rho z - y) */
gsl_vector_memcpy(q, z);
gsl_vector_sub(q, u);
gsl_vector_scale(q, rho);
gsl_vector_add(q, Atb); // q = A^T b + rho*(z - u)
double tmp, tmpq;
gsl_blas_ddot(x, x, &tmp);
gsl_blas_ddot(q, q, &tmpq);
if (skinny) {
/* x = U \ (L \ q) */
gsl_linalg_cholesky_solve(L, q, x);
} else {
/* x = q/rho - 1/rho^2 * A^T * (U \ (L \ (A*q))) */
gsl_blas_dgemv(CblasNoTrans, 1, A, q, 0, Aq);
gsl_linalg_cholesky_solve(L, Aq, p);
gsl_blas_dgemv(CblasTrans, 1, A, p, 0, x); /* now x = A^T * (U \ (L \ (A*q)) */
gsl_vector_scale(x, -1/(rho*rho));
gsl_vector_scale(q, 1/rho);
gsl_vector_add(x, q);
}
/*
* Message-passing: compute the global sum over all processors of the
* contents of w and t. Also, update z.
*/
gsl_vector_memcpy(w, x);
gsl_vector_add(w, u); // w = x + u
gsl_blas_ddot(r, r, &send[0]);
gsl_blas_ddot(x, x, &send[1]);
gsl_blas_ddot(u, u, &send[2]);
send[2] /= pow(rho, 2);
gsl_vector_memcpy(zprev, z);
// could be reduced to a single Allreduce call by concatenating send to w
MPI_Allreduce(w->data, z->data, n, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
MPI_Allreduce(send, recv, 3, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
prires = sqrt(recv[0]); /* sqrt(sum ||r_i||_2^2) */
nxstack = sqrt(recv[1]); /* sqrt(sum ||x_i||_2^2) */
nystack = sqrt(recv[2]); /* sqrt(sum ||y_i||_2^2) */
gsl_vector_scale(z, 1/N);
soft_threshold(z, lambda/(N*rho));
/* Termination checks */
/* dual residual */
gsl_vector_memcpy(zdiff, z);
gsl_vector_sub(zdiff, zprev);
dualres = sqrt(N) * rho * gsl_blas_dnrm2(zdiff); /* ||s^k||_2^2 = N rho^2 ||z - zprev||_2^2 */
/* compute primal and dual feasibility tolerances */
eps_pri = sqrt(n*N)*ABSTOL + RELTOL * fmax(nxstack, sqrt(N)*gsl_blas_dnrm2(z));
eps_dual = sqrt(n*N)*ABSTOL + RELTOL * nystack;
if (rank == 0) {
printf("%3d %10.4f %10.4f %10.4f %10.4f %10.4f\n", iter,
prires, eps_pri, dualres, eps_dual, objective(A, b, lambda, z));
}
if (prires <= eps_pri && dualres <= eps_dual) {
break;
}
/* Compute residual: r = x - z */
gsl_vector_memcpy(r, x);
gsl_vector_sub(r, z);
iter++;
}
/* Have the master write out the results to disk */
if (rank == 0) {
endAllTime = MPI_Wtime();
printf("Elapsed time is: %lf \n", endAllTime - startAllTime);
f = fopen("data/solution.dat", "w");
gsl_vector_fprintf(f, z, "%lf");
fclose(f);
}
MPI_Finalize(); /* Shut down the MPI execution environment */
/* Clear memory */
gsl_matrix_free(A);
gsl_matrix_free(L);
gsl_vector_free(b);
gsl_vector_free(x);
gsl_vector_free(u);
gsl_vector_free(z);
gsl_vector_free(y);
gsl_vector_free(r);
gsl_vector_free(w);
gsl_vector_free(zprev);
gsl_vector_free(zdiff);
gsl_vector_free(q);
gsl_vector_free(Aq);
gsl_vector_free(Atb);
gsl_vector_free(p);
return EXIT_SUCCESS;
}
// measurement update (correction)
bool DiscreteExtendedKalmanFilter::updateMeasurement(const size_t step, const gsl_vector *actualMeasurement, const gsl_vector *input)
{
if (!x_hat_ || /*!y_hat_ ||*/ !P_ || !K_) return false;
const gsl_vector *h_eval = system_.evaluateMeasurementEquation(step, x_hat_, input, NULL); // h = h(k, x(k), u(k), 0)
const gsl_matrix *Cd = system_.getOutputMatrix(step, x_hat_); // Cd(k) = dh(k, x-(k), u(k), 0)/dx
#if 0
const gsl_matrix *V = system_.getMeasurementNoiseCouplingMatrix(step); // V(k) = dh(k, x-(k), u(k), 0)/dv
const gsl_matrix *R = system_.getMeasurementNoiseCovarianceMatrix(step); // R(k)
#else
const gsl_matrix *Rd = system_.getMeasurementNoiseCovarianceMatrix(step); // Rd(k) = V(k) * R(k) * V(k)^T
#endif
if (!Cd || !Rd || !h_eval || !actualMeasurement) return false;
// 1. calculate Kalman gain: K(k) = P-(k) * Cd(k)^T * (Cd(k) * P-(k) * Cd(k)^T + Rd(k))^-1 where Cd(k) = dh(k, x-(k), u(k), 0)/dx, Rd(k) = V(k) * R(k) * V(k)^T, V(k) = dh(k, x-(k), u(k), 0)/dv
// inverse of matrix using LU decomposition
gsl_matrix_memcpy(RR_, Rd);
if (GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, P_, Cd, 0.0, PCt_) ||
GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Cd, PCt_, 1.0, RR_))
return false;
int signum;
if (GSL_SUCCESS != gsl_linalg_LU_decomp(RR_, permutation_, &signum) ||
GSL_SUCCESS != gsl_linalg_LU_invert(RR_, permutation_, invRR_))
return false;
if (GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, PCt_, invRR_, 0.0, K_)) // calculate Kalman gain
return false;
// 2. update measurement: x(k) = x-(k) + K(k) * (y_tilde(k) - y_hat(k)) where y_hat(k) = h(k, x-(k), u(k), 0)
#if 0
// save an estimated measurement, y_hat
gsl_vector_memcpy(y_hat_, h_eval);
gsl_vector_memcpy(residual_, y_hat_);
if (GSL_SUCCESS != gsl_vector_sub(residual_, actualMeasurement) || // calculate residual = y_tilde(k) - y_hat(k)
GSL_SUCCESS != gsl_blas_dgemv(CblasNoTrans, -1.0, K_, residual_, 1.0, x_hat_)) // calculate x_hat(k)
return false;
#else
gsl_vector_memcpy(residual_, h_eval);
if (GSL_SUCCESS != gsl_vector_sub(residual_, actualMeasurement) || // calculate residual = y_tilde(k) - y_hat(k)
GSL_SUCCESS != gsl_blas_dgemv(CblasNoTrans, -1.0, K_, residual_, 1.0, x_hat_)) // calculate x_hat(k)
return false;
#endif
// 3. update covariance: P(k) = (I - K(k) * Cd(k)) * P-(k)
#if 0
// not working
gsl_matrix_set_identity(M_);
if (GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, K_, Cd, 1.0, M_) ||
GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, M_, P_, 0.0, P_))
return false;
#else
gsl_matrix_set_identity(M_);
if (GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, K_, Cd, 1.0, M_) ||
GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, M_, P_, 0.0, M2_))
return false;
gsl_matrix_memcpy(P_, M2_);
#endif
// preserve symmetry of P
gsl_matrix_transpose_memcpy(M_, P_);
gsl_matrix_add(P_, M_);
gsl_matrix_scale(P_, 0.5);
return true;
}
static int
multifit_wlinear_svd (const gsl_matrix * X,
const gsl_vector * w,
const gsl_vector * y,
double tol,
int balance,
size_t * rank,
gsl_vector * c,
gsl_matrix * cov,
double *chisq, gsl_multifit_linear_workspace * work)
{
if (X->size1 != y->size)
{
GSL_ERROR
("number of observations in y does not match rows of matrix X",
GSL_EBADLEN);
}
else if (X->size2 != c->size)
{
GSL_ERROR ("number of parameters c does not match columns of matrix X",
GSL_EBADLEN);
}
else if (w->size != y->size)
{
GSL_ERROR ("number of weights does not match number of observations",
GSL_EBADLEN);
}
else if (cov->size1 != cov->size2)
{
GSL_ERROR ("covariance matrix is not square", GSL_ENOTSQR);
}
else if (c->size != cov->size1)
{
GSL_ERROR
("number of parameters does not match size of covariance matrix",
GSL_EBADLEN);
}
else if (X->size1 != work->n || X->size2 != work->p)
{
GSL_ERROR
("size of workspace does not match size of observation matrix",
GSL_EBADLEN);
}
else
{
const size_t n = X->size1;
const size_t p = X->size2;
size_t i, j, p_eff;
gsl_matrix *A = work->A;
gsl_matrix *Q = work->Q;
gsl_matrix *QSI = work->QSI;
gsl_vector *S = work->S;
gsl_vector *t = work->t;
gsl_vector *xt = work->xt;
gsl_vector *D = work->D;
/* Scale X, A = sqrt(w) X */
gsl_matrix_memcpy (A, X);
for (i = 0; i < n; i++)
{
double wi = gsl_vector_get (w, i);
if (wi < 0)
wi = 0;
{
gsl_vector_view row = gsl_matrix_row (A, i);
gsl_vector_scale (&row.vector, sqrt (wi));
}
}
/* Balance the columns of the matrix A if requested */
if (balance)
{
gsl_linalg_balance_columns (A, D);
}
else
{
gsl_vector_set_all (D, 1.0);
}
/* Decompose A into U S Q^T */
gsl_linalg_SV_decomp_mod (A, QSI, Q, S, xt);
/* Solve sqrt(w) y = A c for c, by first computing t = sqrt(w) y */
for (i = 0; i < n; i++)
{
double wi = gsl_vector_get (w, i);
double yi = gsl_vector_get (y, i);
if (wi < 0)
wi = 0;
gsl_vector_set (t, i, sqrt (wi) * yi);
}
gsl_blas_dgemv (CblasTrans, 1.0, A, t, 0.0, xt);
/* Scale the matrix Q, Q' = Q S^-1 */
gsl_matrix_memcpy (QSI, Q);
{
double alpha0 = gsl_vector_get (S, 0);
p_eff = 0;
for (j = 0; j < p; j++)
{
gsl_vector_view column = gsl_matrix_column (QSI, j);
double alpha = gsl_vector_get (S, j);
if (alpha <= tol * alpha0) {
alpha = 0.0;
} else {
alpha = 1.0 / alpha;
p_eff++;
}
gsl_vector_scale (&column.vector, alpha);
}
*rank = p_eff;
}
gsl_vector_set_zero (c);
/* Solution */
gsl_blas_dgemv (CblasNoTrans, 1.0, QSI, xt, 0.0, c);
/* Unscale the balancing factors */
gsl_vector_div (c, D);
/* Form covariance matrix cov = (Q S^-1) (Q S^-1)^T */
for (i = 0; i < p; i++)
{
gsl_vector_view row_i = gsl_matrix_row (QSI, i);
double d_i = gsl_vector_get (D, i);
for (j = i; j < p; j++)
{
gsl_vector_view row_j = gsl_matrix_row (QSI, j);
double d_j = gsl_vector_get (D, j);
double s;
gsl_blas_ddot (&row_i.vector, &row_j.vector, &s);
gsl_matrix_set (cov, i, j, s / (d_i * d_j));
gsl_matrix_set (cov, j, i, s / (d_i * d_j));
}
}
/* Compute chisq, from residual r = y - X c */
{
double r2 = 0;
for (i = 0; i < n; i++)
{
double yi = gsl_vector_get (y, i);
double wi = gsl_vector_get (w, i);
gsl_vector_const_view row = gsl_matrix_const_row (X, i);
double y_est, ri;
gsl_blas_ddot (&row.vector, c, &y_est);
ri = yi - y_est;
r2 += wi * ri * ri;
}
*chisq = r2;
}
return GSL_SUCCESS;
}
}
static int
msbdf_update (void *vstate, const size_t dim, gsl_matrix * dfdy, double *dfdt,
const double t, const double *y, const gsl_odeiv2_system * sys,
gsl_matrix * M, gsl_permutation * p,
const size_t iter, size_t * nJ, size_t * nM,
const double tprev, const double failt,
const double gamma, const double gammaprev, const double hratio)
{
/* Evaluates Jacobian dfdy and updates iteration matrix M
if criteria for update is met.
*/
/* Jacobian is evaluated
- at first step
- if MSBDF_JAC_WAIT steps have been made without re-evaluation
- in case of a convergence failure if
--- change in gamma is small, or
--- convergence failure resulted in step size decrease
*/
const double c = 0.2;
const double gammarel = fabs (gamma / gammaprev - 1.0);
if (*nJ == 0 || *nJ > MSBDF_JAC_WAIT ||
(t == failt && (gammarel < c || hratio < 1.0)))
{
#ifdef DEBUG
printf ("-- evaluate jacobian\n");
#endif
int s = GSL_ODEIV_JA_EVAL (sys, t, y, dfdy->data, dfdt);
if (s == GSL_EBADFUNC)
{
return s;
}
if (s != GSL_SUCCESS)
{
msbdf_failurehandler (vstate, dim, t);
#ifdef DEBUG
printf ("-- FAIL at jacobian function evaluation\n");
#endif
return s;
}
/* Reset counter */
*nJ = 0;
}
/* Iteration matrix M (and it's LU decomposition) is generated
- at first step
- if MSBDF_M_WAIT steps have been made without an update
- if change in gamma is significant (e.g. change in step size)
- if previous step was rejected
*/
if (*nM == 0 || *nM > MSBDF_M_WAIT || gammarel >= c ||
t == tprev || t == failt)
{
#ifdef DEBUG
printf ("-- update M, gamma=%.5e\n", gamma);
#endif
size_t i;
gsl_matrix_memcpy (M, dfdy);
gsl_matrix_scale (M, -gamma);
for (i = 0; i < dim; i++)
{
gsl_matrix_set (M, i, i, gsl_matrix_get (M, i, i) + 1.0);
}
{
int signum;
int s = gsl_linalg_LU_decomp (M, p, &signum);
if (s != GSL_SUCCESS)
{
return GSL_FAILURE;
}
}
/* Reset counter */
*nM = 0;
}
return GSL_SUCCESS;
}
/* solve: min ||b - A x||^2 + lambda^2 ||x||^2 */
static int
test_COD_lssolve2_eps(const double lambda, const gsl_matrix * A, const gsl_vector * b, const double eps, const char *desc)
{
int s = 0;
size_t i, M = A->size1, N = A->size2;
gsl_vector * lhs = gsl_vector_alloc(M);
gsl_matrix * QRZT = gsl_matrix_alloc(M, N);
gsl_vector * tau_Q = gsl_vector_alloc(GSL_MIN(M, N));
gsl_vector * tau_Z = gsl_vector_alloc(GSL_MIN(M, N));
gsl_vector * work = gsl_vector_alloc(N);
gsl_vector * x = gsl_vector_alloc(N);
gsl_vector * x_aug = gsl_vector_alloc(N);
gsl_vector * r = gsl_vector_alloc(M);
gsl_vector * res = gsl_vector_alloc(M);
gsl_permutation * perm = gsl_permutation_alloc(N);
size_t rank;
/* form full rank augmented system B = [ A ; lambda*I_N ], f = [ rhs ; 0 ] and solve with QRPT */
{
gsl_vector_view v;
gsl_matrix_view m;
gsl_permutation *p = gsl_permutation_alloc(N);
gsl_matrix * B = gsl_matrix_calloc(M + N, N);
gsl_vector * f = gsl_vector_calloc(M + N);
gsl_vector * tau = gsl_vector_alloc(N);
gsl_vector * residual = gsl_vector_alloc(M + N);
int signum;
m = gsl_matrix_submatrix(B, 0, 0, M, N);
gsl_matrix_memcpy(&m.matrix, A);
m = gsl_matrix_submatrix(B, M, 0, N, N);
v = gsl_matrix_diagonal(&m.matrix);
gsl_vector_set_all(&v.vector, lambda);
v = gsl_vector_subvector(f, 0, M);
gsl_vector_memcpy(&v.vector, b);
/* solve: [ A ; lambda*I ] x_aug = [ b ; 0 ] */
gsl_linalg_QRPT_decomp(B, tau, p, &signum, work);
gsl_linalg_QRPT_lssolve(B, tau, p, f, x_aug, residual);
gsl_permutation_free(p);
gsl_matrix_free(B);
gsl_vector_free(f);
gsl_vector_free(tau);
gsl_vector_free(residual);
}
gsl_matrix_memcpy(QRZT, A);
s += gsl_linalg_COD_decomp(QRZT, tau_Q, tau_Z, perm, &rank, work);
{
gsl_matrix *S = gsl_matrix_alloc(rank, rank);
gsl_vector *workr = gsl_vector_alloc(rank);
s += gsl_linalg_COD_lssolve2(lambda, QRZT, tau_Q, tau_Z, perm, rank, b, x, res, S, workr);
gsl_matrix_free(S);
gsl_vector_free(workr);
}
for (i = 0; i < N; i++)
{
double xi = gsl_vector_get(x, i);
double yi = gsl_vector_get(x_aug, i);
gsl_test_rel(xi, yi, eps,
"%s (%3lu,%3lu)[%lu]: %22.18g %22.18g\n",
desc, M, N, i, xi, yi);
}
/* compute residual r = b - A x */
if (M == N)
{
gsl_vector_set_zero(r);
}
else
{
gsl_vector_memcpy(r, b);
gsl_blas_dgemv(CblasNoTrans, -1.0, A, x, 1.0, r);
}
for (i = 0; i < N; i++)
{
double xi = gsl_vector_get(res, i);
double yi = gsl_vector_get(r, i);
gsl_test_rel(xi, yi, sqrt(eps),
"%s res (%3lu,%3lu)[%lu]: %22.18g %22.18g\n",
desc, M, N, i, xi, yi);
}
gsl_vector_free(r);
gsl_vector_free(res);
gsl_vector_free(x);
gsl_vector_free(x_aug);
gsl_vector_free(tau_Q);
gsl_vector_free(tau_Z);
gsl_matrix_free(QRZT);
gsl_vector_free(lhs);
gsl_vector_free(work);
gsl_permutation_free(perm);
return s;
}
matrix<double>::matrix(const gsl_matrix* m)
{
_matrix = gsl_matrix_alloc(m->size1,m->size2);
gsl_matrix_memcpy(_matrix, m);
}
matrix<double>::matrix(const gsl_matrix& m)
{
_matrix = gsl_matrix_alloc(m.size1,m.size2);
gsl_matrix_memcpy(_matrix, &m);
}
/** Copy constructor */
matrix<double>::matrix(const matrix<double>& m)
{
_matrix = gsl_matrix_alloc(m.size_i(), m.size_j());
gsl_matrix_memcpy(_matrix, m.as_gsl_type_ptr());
}
/** *******************************************************************************************************************************************/
int generate_gaus_rv_inits(gsl_vector *myBeta,struct fnparams *gparams){
/** this is the SAME CODE as in the Gaussian case */
/** beta_hat= (X^T X)^{-1} X^T y **/
const datamatrix *designdata = ((struct fnparams *) gparams)->designdata;/** all design data inc Y and priors **/
const gsl_vector *Y = designdata->Y;/** response vector **/
const gsl_matrix *X = designdata->datamatrix_noRV ;/** design matrix - with one too few cols! **/
gsl_vector *vectmp1= gparams->vectmp1;/** numparams long*/
gsl_vector *vectmp2 = gparams->vectmp2;/** numparams long*/
gsl_matrix *mattmp2 = gparams->mattmp2;/** same dim as X*/
gsl_matrix *mattmp3 = gparams->mattmp3;/** p x p **/
gsl_matrix *mattmp4 = gparams->mattmp4;/** p x p **/
gsl_vector *vectmp1long = gparams->vectmp1long;/** scratch space **/
gsl_vector *vectmp2long = gparams->vectmp2long;/** scratch space **/
gsl_permutation *perm = gparams->perm;
unsigned int i;
int ss;
int haveError;
double variance=0.0;
double n=Y->size;/** no. observations **/
double m=X->size2;/** number of coefficients excluding tau-precision */
/*Rprintf("X: %d %d %d %d %d %d\n",X->size1,X->size2,mattmp2->size1,mattmp2->size2,mattmp3->size1,mattmp3->size2); */
gsl_matrix_memcpy(mattmp2,X);
gsl_blas_dgemm (CblasTrans, CblasNoTrans, /** mattmp3 is p x p matrix X^T X **/
1.0, X, mattmp2,
0.0, mattmp3);
gsl_permutation_init(perm);/** reset - might not be needed */
gsl_linalg_LU_decomp(mattmp3,perm,&ss);
gsl_set_error_handler_off();/**Turning off GSL Error handler as this may fail as mattmp3 may be singular */
haveError=gsl_linalg_LU_invert (mattmp3, perm, mattmp4);/** mattmp4 is now inv (X^T X) */
if(!haveError){/** no error */
/** copy Y into vectmp1long and +1 and take logs since poisson has log link - this is a fudge */
/*for(i=0;i<vectmp1long->size;i++){gsl_vector_set(vectmp1long,i,log(gsl_vector_get(Y,i)+DBL_MIN)/(log(1-gsl_vector_get(Y,i)+DBL_MIN)));} */
/*for(i=0;i<vectmp1long->size;i++){gsl_vector_set(vectmp1long,i,log(gsl_vector_get(Y,i)+1)/(log(1-gsl_vector_get(Y,i)+1)));} */
gsl_blas_dgemv (CblasTrans, 1.0, X, Y, 0.0, vectmp1); /** X^T Y */
gsl_blas_dgemv (CblasNoTrans, 1.0, mattmp4, vectmp1, 0.0, vectmp2);
for(i=0;i<myBeta->size-2;i++){gsl_vector_set(myBeta,i,gsl_vector_get(vectmp2,i));} /** size myBeta->size-2 as last two entries are precisions **/
} else {/** singular to set initial values all to zero **/
Rprintf("caught gsl error - singular matrix in initial guess estimates\n");
for(i=0;i<myBeta->size;i++){gsl_vector_set(myBeta,i,0.01);}}
gsl_set_error_handler (NULL);/** restore the error handler*/
/*Rprintf("inits\n");for(i=0;i<myBeta->size;i++){Rprintf("%10.15e ",gsl_vector_get(myBeta,i));} Rprintf("\n");*//** set to Least squares estimate */
/** now for variance estimate */
/** first get y_hat estimate */
gsl_blas_dgemv (CblasNoTrans, 1.0, X, vectmp2, 0.0, vectmp1long); /** vectmp1 is y_hat */
/*for(i=0;i<vectmp1long->size;i++){Rprintf("y_hat=%f\n",gsl_vector_get(vectmp1long,i));}*/
/*error("");*/
gsl_vector_scale(vectmp1long,-1.0);/** - y_hat */
gsl_vector_add(vectmp1long,Y);/** now have Y-y_hat (or -y_hat + Y) */
/*for(i=0;i<vectmp1long->size;i++){gsl_vector_set(vectmp1long,i,fabs(gsl_vector_get(vectmp1long,i)));}
for(i=0;i<vectmp1long->size;i++){Rprintf("y_hat=%f\n",gsl_vector_get(vectmp1long,i));}
for(i=0;i<vectmp1long->size;i++){gsl_vector_set(vectmp1long,i,log(gsl_vector_get(vectmp1long,i))/log(1-gsl_vector_get(vectmp1long,i)));}*/ /** errors on logit scale **/
/*gsl_vector_set_all(vectmp2long,1);*/
gsl_vector_memcpy(vectmp2long,vectmp1long);
gsl_blas_ddot (vectmp1long, vectmp2long, &variance);/** got sum((Y-Y_hat)^2) */
variance=variance/(n-m);/** unbiased estimator using denominator n-#term in regression equation **/
/* Rprintf("variance estimator=%f precision=%f\n",variance,1/variance);*/
/* variance=0.086;*/
/*variance=exp(gsl_vector_get(myBeta,0))/(1+exp(gsl_vector_get(myBeta,0)));*/
/** variance here is for plain glm - just split this 50/50 between residual error and group level variance **/
gsl_vector_set(myBeta,myBeta->size-2,1.0/(0.5*variance));/** - estimate for rv precision **/
gsl_vector_set(myBeta,myBeta->size-1,1.0/(0.5*variance));/** - estimate for residual precision **/
/*gsl_vector_set(myBeta,myBeta->size-1,1.0/variance); */
/*gsl_vector_set(myBeta,0,0.9 );gsl_vector_set(myBeta,1,0.9);gsl_vector_set(myBeta,2,1.5);*/
#ifdef junk
Rprintf("------------ TEMP: Using Fixed initial values from LME4----------------\n");
gsl_vector_set(myBeta,0,0.062044233);/** intercept */
gsl_vector_set(myBeta,1,-0.1229382094322);/** slope g2 */
gsl_vector_set(myBeta,2,1.0/0.1570366587829);/** group level precision */
gsl_vector_set(myBeta,3,1.0/0.8628565204966);/** residual precision */
#endif
/*Rprintf("inits\n");for(i=0;i<myBeta->size;i++){Rprintf("%10.15e ",gsl_vector_get(myBeta,i));} Rprintf("\n");*//** set to Least squares estimate */
return GSL_SUCCESS;
}
void fnIMIS(const size_t InitSamples, const size_t StepSamples, const size_t FinalResamples, const size_t MaxIter, const size_t NumParam, unsigned long int rng_seed, const char * runName)
{
// Declare and configure GSL RNG
gsl_rng * rng;
const gsl_rng_type * T;
gsl_rng_env_setup();
T = gsl_rng_default;
rng = gsl_rng_alloc (T);
gsl_rng_set(rng, rng_seed);
char strDiagnosticsFile[strlen(runName) + 15 +1];
char strResampleFile[strlen(runName) + 12 +1];
strcpy(strDiagnosticsFile, runName); strcat(strDiagnosticsFile, "Diagnostics.txt");
strcpy(strResampleFile, runName); strcat(strResampleFile, "Resample.txt");
FILE * diagnostics_file = fopen(strDiagnosticsFile, "w");
fprintf(diagnostics_file, "Seeded RNG: %zu\n", rng_seed);
fprintf(diagnostics_file, "Running IMIS. InitSamples: %zu, StepSamples: %zu, FinalResamples %zu, MaxIter %zu\n", InitSamples, StepSamples, FinalResamples, MaxIter);
// Setup IMIS arrays
gsl_matrix * Xmat = gsl_matrix_alloc(InitSamples + StepSamples*MaxIter, NumParam);
double * prior_all = (double*) malloc(sizeof(double) * (InitSamples + StepSamples*MaxIter));
double * likelihood_all = (double*) malloc(sizeof(double) * (InitSamples + StepSamples*MaxIter));
double * imp_weight_denom = (double*) malloc(sizeof(double) * (InitSamples + StepSamples*MaxIter)); // proportional to q(k) in stage 2c of Raftery & Bao
double * gaussian_sum = (double*) calloc(InitSamples + StepSamples*MaxIter, sizeof(double)); // sum of mixture distribution for mode
struct dst * distance = (struct dst *) malloc(sizeof(struct dst) * (InitSamples + StepSamples*MaxIter)); // Mahalanobis distance to most recent mode
double * imp_weights = (double*) malloc(sizeof(double) * (InitSamples + StepSamples*MaxIter));
double * tmp_MVNpdf = (double*) malloc(sizeof(double) * (InitSamples + StepSamples*MaxIter));
gsl_matrix * nearestX = gsl_matrix_alloc(StepSamples, NumParam);
double center_all[MaxIter][NumParam];
gsl_matrix * sigmaChol_all[MaxIter];
gsl_matrix * sigmaInv_all[MaxIter];
// Initial prior samples
sample_prior(rng, InitSamples, Xmat);
// Calculate prior covariance
double prior_invCov_diag[NumParam];
/*
The paper describing the algorithm uses the full prior covariance matrix.
This follows the code in the IMIS R package and diagonalizes the prior
covariance matrix to ensure invertibility.
*/
for(size_t i = 0; i < NumParam; i++){
gsl_vector_view tmpCol = gsl_matrix_subcolumn(Xmat, i, 0, InitSamples);
prior_invCov_diag[i] = gsl_stats_variance(tmpCol.vector.data, tmpCol.vector.stride, InitSamples);
prior_invCov_diag[i] = 1.0/prior_invCov_diag[i];
}
// IMIS steps
fprintf(diagnostics_file, "Step Var(w_i) MargLik Unique Max(w_i) ESS Time\n");
printf("Step Var(w_i) MargLik Unique Max(w_i) ESS Time\n");
time_t time1, time2;
time(&time1);
size_t imisStep = 0, numImisSamples;
for(imisStep = 0; imisStep < MaxIter; imisStep++){
numImisSamples = (InitSamples + imisStep*StepSamples);
// Evaluate prior and likelihood
if(imisStep == 0){ // initial stage
#pragma omp parallel for
for(size_t i = 0; i < numImisSamples; i++){
gsl_vector_const_view theta = gsl_matrix_const_row(Xmat, i);
prior_all[i] = prior(&theta.vector);
likelihood_all[i] = likelihood(&theta.vector);
}
} else { // imisStep > 0
#pragma omp parallel for
for(size_t i = InitSamples + (imisStep-1)*StepSamples; i < numImisSamples; i++){
gsl_vector_const_view theta = gsl_matrix_const_row(Xmat, i);
prior_all[i] = prior(&theta.vector);
likelihood_all[i] = likelihood(&theta.vector);
}
}
// Determine importance weights, find current maximum, calculate monitoring criteria
#pragma omp parallel for
for(size_t i = 0; i < numImisSamples; i++){
imp_weight_denom[i] = (InitSamples*prior_all[i] + StepSamples*gaussian_sum[i])/(InitSamples + StepSamples * imisStep);
imp_weights[i] = (prior_all[i] > 0)?likelihood_all[i]*prior_all[i]/imp_weight_denom[i]:0;
}
double sumWeights = 0.0;
for(size_t i = 0; i < numImisSamples; i++){
sumWeights += imp_weights[i];
}
double maxWeight = 0.0, varImpW = 0.0, entropy = 0.0, expectedUnique = 0.0, effSampSize = 0.0, margLik;
size_t maxW_idx;
#pragma omp parallel for reduction(+: varImpW, entropy, expectedUnique, effSampSize)
for(size_t i = 0; i < numImisSamples; i++){
imp_weights[i] /= sumWeights;
varImpW += pow(numImisSamples * imp_weights[i] - 1.0, 2.0);
entropy += imp_weights[i] * log(imp_weights[i]);
expectedUnique += (1.0 - pow((1.0 - imp_weights[i]), FinalResamples));
effSampSize += pow(imp_weights[i], 2.0);
}
for(size_t i = 0; i < numImisSamples; i++){
if(imp_weights[i] > maxWeight){
maxW_idx = i;
maxWeight = imp_weights[i];
}
}
for(size_t i = 0; i < NumParam; i++)
center_all[imisStep][i] = gsl_matrix_get(Xmat, maxW_idx, i);
varImpW /= numImisSamples;
entropy = -entropy / log(numImisSamples);
effSampSize = 1.0/effSampSize;
margLik = log(sumWeights/numImisSamples);
fprintf(diagnostics_file, "%4zu %8.2f %8.2f %8.2f %8.2f %8.2f %8.2f\n", imisStep, varImpW, margLik, expectedUnique, maxWeight, effSampSize, difftime(time(&time2), time1));
printf("%4zu %8.2f %8.2f %8.2f %8.2f %8.2f %8.2f\n", imisStep, varImpW, margLik, expectedUnique, maxWeight, effSampSize, difftime(time(&time2), time1));
time1 = time2;
// Check for convergence
if(expectedUnique > FinalResamples*(1.0 - exp(-1.0))){
break;
}
// Calculate Mahalanobis distance to current mode
GetMahalanobis_diag(Xmat, center_all[imisStep], prior_invCov_diag, numImisSamples, NumParam, distance);
// Find StepSamples nearest points
// (Note: this was a major bottleneck when InitSamples and StepResamples are large. qsort substantially outperformed GSL sort options.)
qsort(distance, numImisSamples, sizeof(struct dst), cmp_dst);
#pragma omp parallel for
for(size_t i = 0; i < StepSamples; i++){
gsl_vector_const_view tmpX = gsl_matrix_const_row(Xmat, distance[i].idx);
gsl_matrix_set_row(nearestX, i, &tmpX.vector);
}
// Calculate weighted covariance of nearestX
// (a) Calculate weights for nearest points 1...StepSamples
double weightsCov[StepSamples];
#pragma omp parallel for
for(size_t i = 0; i < StepSamples; i++){
weightsCov[i] = 0.5*(imp_weights[distance[i].idx] + 1.0/numImisSamples); // cov_wt function will normalize the weights
}
// (b) Calculate weighted covariance
sigmaChol_all[imisStep] = gsl_matrix_alloc(NumParam, NumParam);
covariance_weighted(nearestX, weightsCov, StepSamples, center_all[imisStep], NumParam, sigmaChol_all[imisStep]);
// (c) Do Cholesky decomposition and inverse of covariance matrix
gsl_linalg_cholesky_decomp(sigmaChol_all[imisStep]);
for(size_t j = 0; j < NumParam; j++) // Note: GSL outputs a symmetric matrix rather than lower tri, so have to set upper tri to zero
for(size_t k = j+1; k < NumParam; k++)
gsl_matrix_set(sigmaChol_all[imisStep], j, k, 0.0);
sigmaInv_all[imisStep] = gsl_matrix_alloc(NumParam, NumParam);
gsl_matrix_memcpy(sigmaInv_all[imisStep], sigmaChol_all[imisStep]);
gsl_linalg_cholesky_invert(sigmaInv_all[imisStep]);
// Sample new inputs
gsl_matrix_view newSamples = gsl_matrix_submatrix(Xmat, numImisSamples, 0, StepSamples, NumParam);
GenerateRandMVnorm(rng, StepSamples, center_all[imisStep], sigmaChol_all[imisStep], NumParam, &newSamples.matrix);
// Evaluate sampling probability from mixture distribution
// (a) For newly sampled points, sum over all previous centers
for(size_t pastStep = 0; pastStep < imisStep; pastStep++){
GetMVNpdf(&newSamples.matrix, center_all[pastStep], sigmaInv_all[pastStep], sigmaChol_all[pastStep], StepSamples, NumParam, tmp_MVNpdf);
#pragma omp parallel for
for(size_t i = 0; i < StepSamples; i++)
gaussian_sum[numImisSamples + i] += tmp_MVNpdf[i];
}
// (b) For all points, add weight for most recent center
gsl_matrix_const_view Xmat_curr = gsl_matrix_const_submatrix(Xmat, 0, 0, numImisSamples + StepSamples, NumParam);
GetMVNpdf(&Xmat_curr.matrix, center_all[imisStep], sigmaInv_all[imisStep], sigmaChol_all[imisStep], numImisSamples + StepSamples, NumParam, tmp_MVNpdf);
#pragma omp parallel for
for(size_t i = 0; i < numImisSamples + StepSamples; i++)
gaussian_sum[i] += tmp_MVNpdf[i];
} // loop over imisStep
//// FINISHED IMIS ROUTINE
fclose(diagnostics_file);
// Resample posterior outputs
int resampleIdx[FinalResamples];
walker_ProbSampleReplace(rng, numImisSamples, imp_weights, FinalResamples, resampleIdx); // Note: Random sampling routine used in R sample() function.
// Print results
FILE * resample_file = fopen(strResampleFile, "w");
for(size_t i = 0; i < FinalResamples; i++){
for(size_t j = 0; j < NumParam; j++)
fprintf(resample_file, "%.15e\t", gsl_matrix_get(Xmat, resampleIdx[i], j));
gsl_vector_const_view theta = gsl_matrix_const_row(Xmat, resampleIdx[i]);
fprintf(resample_file, "\n");
}
fclose(resample_file);
/*
// This outputs Xmat (parameter matrix), centers, and covariance matrices to files for debugging
FILE * Xmat_file = fopen("Xmat.txt", "w");
for(size_t i = 0; i < numImisSamples; i++){
for(size_t j = 0; j < NumParam; j++)
fprintf(Xmat_file, "%.15e\t", gsl_matrix_get(Xmat, i, j));
fprintf(Xmat_file, "%e\t%e\t%e\t%e\t%e\t\n", prior_all[i], likelihood_all[i], imp_weights[i], gaussian_sum[i], distance[i]);
}
fclose(Xmat_file);
FILE * centers_file = fopen("centers.txt", "w");
for(size_t i = 0; i < imisStep; i++){
for(size_t j = 0; j < NumParam; j++)
fprintf(centers_file, "%f\t", center_all[i][j]);
fprintf(centers_file, "\n");
}
fclose(centers_file);
FILE * sigmaInv_file = fopen("sigmaInv.txt", "w");
for(size_t i = 0; i < imisStep; i++){
for(size_t j = 0; j < NumParam; j++)
for(size_t k = 0; k < NumParam; k++)
fprintf(sigmaInv_file, "%f\t", gsl_matrix_get(sigmaInv_all[i], j, k));
fprintf(sigmaInv_file, "\n");
}
fclose(sigmaInv_file);
*/
// free memory allocated by IMIS
for(size_t i = 0; i < imisStep; i++){
gsl_matrix_free(sigmaChol_all[i]);
gsl_matrix_free(sigmaInv_all[i]);
}
// release RNG
gsl_rng_free(rng);
gsl_matrix_free(Xmat);
gsl_matrix_free(nearestX);
free(prior_all);
free(likelihood_all);
free(imp_weight_denom);
free(gaussian_sum);
free(distance);
free(imp_weights);
free(tmp_MVNpdf);
return;
}