int
gsl_linalg_COD_unpack(const gsl_matrix * QRZ, const gsl_vector * tau_Q,
const gsl_vector * tau_Z, const size_t rank, gsl_matrix * Q,
gsl_matrix * R, gsl_matrix * Z)
{
const size_t M = QRZ->size1;
const size_t N = QRZ->size2;
if (tau_Q->size != GSL_MIN (M, N))
{
GSL_ERROR ("size of tau_Q must be MIN(M,N)", GSL_EBADLEN);
}
else if (tau_Z->size != GSL_MIN (M, N))
{
GSL_ERROR ("size of tau_Z must be MIN(M,N)", GSL_EBADLEN);
}
else if (rank > GSL_MIN (M, N))
{
GSL_ERROR ("rank must be <= MIN(M,N)", GSL_EBADLEN);
}
else if (Q->size1 != M || Q->size2 != M)
{
GSL_ERROR ("Q must by M-by-M", GSL_EBADLEN);
}
else if (R->size1 != M || R->size2 != N)
{
GSL_ERROR ("R must by M-by-N", GSL_EBADLEN);
}
else if (Z->size1 != N || Z->size2 != N)
{
GSL_ERROR ("Z must by N-by-N", GSL_EBADLEN);
}
else
{
size_t i;
gsl_matrix_view R11 = gsl_matrix_submatrix(R, 0, 0, rank, rank);
gsl_matrix_const_view QRZ11 = gsl_matrix_const_submatrix(QRZ, 0, 0, rank, rank);
/* form Q matrix */
gsl_matrix_set_identity(Q);
for (i = GSL_MIN (M, N); i-- > 0;)
{
gsl_vector_const_view h = gsl_matrix_const_subcolumn (QRZ, i, i, M - i);
gsl_matrix_view m = gsl_matrix_submatrix (Q, i, i, M - i, M - i);
double ti = gsl_vector_get (tau_Q, i);
gsl_linalg_householder_hm (ti, &h.vector, &m.matrix);
}
/* form Z matrix */
gsl_matrix_set_identity(Z);
if (rank < N)
{
gsl_vector_view work = gsl_matrix_row(R, 0); /* temporary workspace, size N */
/* multiply I by Z from the right */
gsl_linalg_COD_matZ(QRZ, tau_Z, rank, Z, &work.vector);
}
/* copy rank-by-rank upper triangle of QRZ into R and zero the rest */
gsl_matrix_set_zero(R);
gsl_matrix_tricpy('U', 1, &R11.matrix, &QRZ11.matrix);
return GSL_SUCCESS;
}
}
int
gsl_linalg_hessenberg_unpack_accum(gsl_matrix * H, gsl_vector * tau,
gsl_matrix * V)
{
const size_t N = H->size1;
if (N != H->size2)
{
GSL_ERROR ("Hessenberg reduction requires square matrix",
GSL_ENOTSQR);
}
else if (N != tau->size)
{
GSL_ERROR ("tau vector must match matrix size", GSL_EBADLEN);
}
else if (N != V->size2)
{
GSL_ERROR ("V matrix has wrong dimension", GSL_EBADLEN);
}
else
{
size_t j; /* looping */
double tau_j; /* householder coefficient */
gsl_vector_view c, /* matrix column */
hv; /* householder vector */
gsl_matrix_view m;
if (N < 3)
{
/* nothing to do */
return GSL_SUCCESS;
}
for (j = 0; j < (N - 2); ++j)
{
c = gsl_matrix_column(H, j);
tau_j = gsl_vector_get(tau, j);
/*
* get a view to the householder vector in column j, but
* make sure hv(2) starts at the element below the
* subdiagonal, since hv(1) was never stored and is always
* 1
*/
hv = gsl_vector_subvector(&c.vector, j + 1, N - (j + 1));
/*
* Only operate on part of the matrix since the first
* j + 1 entries of the real householder vector are 0
*
* V -> V * U(j)
*
* Note here that V->size1 is not necessarily equal to N
*/
m = gsl_matrix_submatrix(V, 0, j + 1, V->size1, N - (j + 1));
/* apply right Householder matrix to V */
gsl_linalg_householder_mh(tau_j, &hv.vector, &m.matrix);
}
return GSL_SUCCESS;
}
} /* gsl_linalg_hessenberg_unpack_accum() */
void computePhysics(struct chain * chain, struct point a[])
{
int n = chain->number;
double mass = chain->totalMass/(chain->number+1.0);
gsl_matrix *U = gsl_matrix_alloc (n*3+1, n*3+1);
gsl_matrix_set_zero(U);
gsl_matrix_view M = gsl_matrix_submatrix(U, 0, 0, n*2, n*2);
gsl_matrix_view nC = gsl_matrix_submatrix(U, n*2, 0, n+1, n*2);
gsl_matrix_view nCt = gsl_matrix_submatrix(U, 0, n*2, n*2, n+1);
//Set Matrix M
for(int i = 0; i < n*2; i++)
gsl_matrix_set(&M.matrix, i, i, mass/n);
//Set Matrix NablaC
gsl_matrix_set(&nC.matrix, 0, 0, chain->p[1].x*2.0);
gsl_matrix_set(&nC.matrix, 0, 1, chain->p[1].y*2.0);
for(int i = 1; i < n; i++)
{
gsl_matrix_set(&nC.matrix, i, i*2-2, (chain->p[i].x - chain->p[i+1].x)*2.0);
gsl_matrix_set(&nC.matrix, i, i*2-1, (chain->p[i].y - chain->p[i+1].y)*2.0);
gsl_matrix_set(&nC.matrix, i, i*2, (chain->p[i+1].x - chain->p[i].x)*2.0);
gsl_matrix_set(&nC.matrix, i, i*2+1, (chain->p[i+1].y - chain->p[i].y)*2.0);
}
if(isConstraint)
{
gsl_matrix_set(&nC.matrix, n, n*2-2, chain->p[n].x*2.0);
gsl_matrix_set(&nC.matrix, n, n*2-1, chain->p[n].y*2.0 + 1.0);
}else
{
gsl_matrix_set(&nC.matrix, n, n*2-2, 0.0);
gsl_matrix_set(&nC.matrix, n, n*2-1, 0.0);
}
//Set Matrix NablaCt
gsl_matrix_set(&nCt.matrix, 0, 0, chain->p[1].x*2.0);
gsl_matrix_set(&nCt.matrix, 1, 0, chain->p[1].y*2.0);
for(int i = 1; i < n; i++)
{
gsl_matrix_set(&nCt.matrix, i*2-2, i, (chain->p[i].x - chain->p[i+1].x)*2.0);
gsl_matrix_set(&nCt.matrix, i*2-1, i, (chain->p[i].y - chain->p[i+1].y)*2.0);
gsl_matrix_set(&nCt.matrix, i*2, i, (chain->p[i+1].x - chain->p[i].x)*2.0);
gsl_matrix_set(&nCt.matrix, i*2+1, i, (chain->p[i+1].y - chain->p[i].y)*2.0);
}
if(isConstraint)
{
gsl_matrix_set(&nCt.matrix, n*2-2, n, chain->p[n].x*2.0);
gsl_matrix_set(&nCt.matrix, n*2-1, n, chain->p[n].y*2.0 + 1.0);
}else
{
gsl_matrix_set(&nCt.matrix, n*2-2, n, 0.0);
gsl_matrix_set(&nCt.matrix, n*2-1, n, 0.0);
}
gsl_matrix *V = gsl_matrix_alloc(n*3+1, n*3+1);
gsl_vector *s = gsl_vector_alloc(n*3+1);
gsl_vector *workvec = gsl_vector_alloc(n*3+1);
// for (int i = 0; i < n*2+n+1; i++)
// {
// for (int j = 0; j < n*2+n+1; j++)
// printf ("%.1f ", gsl_matrix_get (U, i, j));
// printf ("\n");
// }
gsl_linalg_SV_decomp(U, V, s, workvec);
//Filter
double max = gsl_vector_max(s);
for(int i=0; i<n*3+1; i++)
if(gsl_vector_get(s, i) < max * 0.000001)
gsl_vector_set(s, i, 0.0);
gsl_vector *b = gsl_vector_alloc(n*3+1);
gsl_vector *b1 = gsl_vector_alloc(n+1);
gsl_vector *x = gsl_vector_alloc(n*3+1);
gsl_vector_set_zero(b);
gsl_vector_set_zero(b1);
gsl_vector_view fext = gsl_vector_subvector(b, 0, n*2);
gsl_vector_view bs = gsl_vector_subvector(b, n*2, n+1);
//Set Vector fext
for(int i = 0; i < n; i++)
{
gsl_vector_set(&fext.vector, i*2, chain->f[i+1].x);
gsl_vector_set(&fext.vector, i*2+1, chain->f[i+1].y);
}
//Set Vector bs
gsl_vector_set(&bs.vector, 0, - chain->v[1].x*chain->v[1].x*2.0 - chain->v[1].y*chain->v[1].y*2.0);
for(int i = 1; i < n; i++)
gsl_vector_set(&bs.vector, i,
- (chain->v[i].x-chain->v[i+1].x)*chain->v[i].x*2.0 -
(chain->v[i].y-chain->v[i+1].y)*chain->v[i].y*2.0 -
(chain->v[i+1].x-chain->v[i].x)*chain->v[i+1].x*2.0 -
(chain->v[i+1].y-chain->v[i].y)*chain->v[i+1].y*2.0
);
if(isConstraint)
gsl_vector_set(&bs.vector, n, -chain->v[n].x*chain->v[n].x*2.0 - (chain->v[n].y*2.0+1.0)* chain->v[n].y);
else gsl_vector_set(&bs.vector, n, 0.0);
//Compute Baumgarte stabilization
gsl_vector_set(b1, 0, - chain->p[1].x*chain->v[1].x*2.0 - chain->p[1].y*chain->v[1].y*2.0);
for(int i = 1; i < n; i++)
gsl_vector_set(b1, i,
- (chain->p[i].x-chain->p[i+1].x)*chain->v[i].x*2.0 -
(chain->p[i].y-chain->p[i+1].y)*chain->v[i].y*2.0 -
(chain->p[i+1].x-chain->p[i].x)*chain->v[i+1].x*2.0 -
(chain->p[i+1].y-chain->p[i].y)*chain->v[i+1].y*2.0
);
if(isConstraint)
gsl_vector_set(b1, n, -chain->p[n].x*chain->v[n].x*2.0 - (chain->p[n].y*2.0+1.0)* chain->v[n].y);
else gsl_vector_set(b1, n, 0.0);
gsl_vector_scale(b1, BSALPHA * 2.0);
gsl_vector_add(&bs.vector, b1);
gsl_vector_set_zero(b1);
gsl_vector_set(b1, 0, - chain->p[1].x*chain->p[1].x - chain->p[1].y*chain->p[1].y + 0.01);
for(int i = 1; i < n; i++)
gsl_vector_set(b1, i,
- (chain->p[i+1].x-chain->p[i].x) * (chain->p[i+1].x-chain->p[i].x)
- (chain->p[i+1].y-chain->p[i].y) * (chain->p[i+1].y-chain->p[i].y) + 0.01
);
if(isConstraint)
gsl_vector_set(b1, n, - chain->p[n].x*chain->p[n].x - (chain->p[n].y+0.5) * (chain->p[n].y+0.5) + 0.25);
else gsl_vector_set(b1, n, 0.0);
gsl_vector_scale(b1, BSALPHA * BSALPHA);
gsl_vector_add(&bs.vector, b1);
//Solve The Equation
gsl_linalg_SV_solve(U, V, s, b, x);
for(int i=0; i<chain->number; i++)
{
a[i].x = gsl_vector_get(x, i*2);
a[i].y = gsl_vector_get(x, i*2+1);
}
// for (int i = n*2; i < n*3+1; i++)
// {
// printf ("%g ", gsl_vector_get(b, i));
// printf ("\n");
// }
// printf ("\n");
gsl_vector_free(x);
gsl_vector_free(b1);
gsl_vector_free(b);
gsl_vector_free(workvec);
gsl_vector_free(s);
gsl_matrix_free(V);
gsl_matrix_free(U);
}
int main(void)
{
int i,j;
// define the system
gsl_matrix* A = gsl_matrix_alloc(ROW,COL);
gsl_matrix* B = gsl_matrix_alloc(COL,COL);
gsl_matrix* BI = gsl_matrix_alloc(COL,COL);
gsl_matrix* R = gsl_matrix_alloc(COL,COL);
gsl_vector* b = gsl_vector_alloc(ROW);
gsl_vector* y = gsl_vector_alloc(COL);
gsl_vector* x = gsl_vector_alloc(COL);
// define the entries of matrix A and vector y
for (i = 0; i < COL; i++)
{
gsl_vector_set(y,i,sin(i)+cos(i*i));
for (j = 0; j < ROW; j++)
{
gsl_matrix_set (A, j, i, j*sin (i) + cos (j*i));
}
}
// copy COLxCOL submatrix of A into B for later
gsl_matrix_view a = gsl_matrix_submatrix(A,0,0,COL,COL);
gsl_matrix_memcpy(B,&a.matrix);
// construct the vector b
gsl_blas_dgemv(CblasNoTrans,1,A,y,0,b);
// perform QR decomposition on A and solve Ax = b for x
qr_dec(A,R);
qr_bak(A,R,b,x);
// find the norm of the vector x-y, if zero: the solution is correct
gsl_vector_sub(x,y);
printf("Solve Ax = b for x, where b = Ay, using QR decomp\n");
printf("Evaluating the deviation between x and y:\n");
printf("\t|x-y| =\t%g\n",gsl_blas_dnrm2(x));
// the abs. val of determinant from QR decomp and the inverse
gsl_matrix_memcpy(&a.matrix,B);
qr_dec(B,R);
double d = qr_absdet(R);
qr_inv(B,R,BI,x);
// to evaluate Binv, we measure the entrywise norm of B*Binv - I
gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1,&a.matrix,BI,0,R);
printf("\nCompute the inverse matrix Binv of B. The entrywise norm of B*Binv - I is\n");
printf("\t|B*Binv - I| =\t");
double sum = 0;
for(i=0; i < COL; i++)
{
gsl_matrix_set(R,i,i,gsl_matrix_get(R,i,i)-1);
for(j=0; j < COL; j++)
{
sum += pow(gsl_matrix_get(R,i,j),2);
}
}
printf("%g\n",sqrt(sum));
// determinant from GSL using LU decomp
gsl_permutation* p = gsl_permutation_alloc(COL);
gsl_linalg_LU_decomp(&a.matrix,p,&i);
double dgsl = fabs(gsl_linalg_LU_det(&a.matrix,i));
printf("\nCompare the algorithm for computation");
printf(" of the absolute value of the determinant\n");
printf("\t|det(A)|/|det(A)_gsl| - 1 =\t%g\n",d/dgsl-1);
gsl_vector_free(y);
gsl_vector_free(x);
gsl_vector_free(b);
gsl_matrix_free(A);
gsl_matrix_free(R);
gsl_matrix_free(B);
gsl_matrix_free(BI);
gsl_permutation_free(p);
return 0;
}
int prepareLambdas(gsl_vector * y,
gsl_matrix * U,
gsl_vector * D2,
gsl_vector * lambdaVeckHKB,
char * skhkbfilename,
char * sklwfilename,
gsl_vector * lambdaVeckLW,
int randomized,
int s)
{
double kHKB;
double kLW;
double crossprod;
double numerator;
double denominatorkHKB;
double denominatorkLW;
int lengthLambdaVec = lambdaVeckHKB->size;
gsl_matrix_view Uview; // a matrix view
int n = y->size;
int i, j;
gsl_vector * resid = gsl_vector_alloc(n);
gsl_matrix * H = gsl_matrix_alloc(n, n);
for(i = 0; i < lengthLambdaVec; i++)
{
gsl_matrix * diag = gsl_matrix_calloc((i+1), (i+1));
Uview = gsl_matrix_submatrix(U, 0, 0, n, (i + 1));
// Make the hat matrix
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, &Uview.matrix, &Uview.matrix, 0.0, H);
// make the fitted ys - put in the resid vector
gsl_blas_dgemv(CblasNoTrans, 1.0, H, y, 0.0, resid);
// make the denominaotor for kLW
if(sklwfilename != NULL)
{
gsl_blas_ddot(y, resid, &denominatorkLW);
}
// Make the residual vector
gsl_vector_scale(resid, -1);
gsl_vector_add(resid, y);
// make the crossproduct
gsl_blas_ddot(resid, resid, &crossprod);
// times it by i
numerator = crossprod * ((float) i + 1.0);
// this gives the numerator
// Make the denominator for kHKB
// Make the diagonal matrix
for(j = 0; j < diag->size1; j++)
{
gsl_matrix_set(diag, j, j, 1.0 / gsl_vector_get(D2, j));
}
//
// Make the matrix U diag D2
gsl_matrix * UD2 = gsl_matrix_alloc(n, (i + 1));
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &Uview.matrix, diag, 0.0, UD2);
// Make the matrix U diag D2 U' - put it into H
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, UD2, &Uview.matrix, 0.0, H);
// Make the matrix U diag D2 U' y - put it into resid
gsl_blas_dgemv(CblasNoTrans, 1.0, H, y, 0.0, resid);
// Make the dot product
gsl_blas_ddot(y, resid, &denominatorkHKB);
// put in the matrix
if(skhkbfilename != NULL)
{
gsl_blas_ddot(y, resid, &denominatorkHKB);
denominatorkHKB = ((float) n - (float) i - 1.0) * denominatorkHKB;
kHKB = numerator / denominatorkHKB;
gsl_vector_set(lambdaVeckHKB, i, kHKB);
}
if(sklwfilename != NULL)
{
denominatorkLW = ((float) n - (float) i - 1.0) * denominatorkLW;
kLW = numerator / denominatorkLW;
gsl_vector_set(lambdaVeckLW, i, kLW);
}
gsl_matrix_free(UD2);
gsl_matrix_free(diag);
}
if(randomized)
{
gsl_rng * rndm = gsl_rng_alloc(gsl_rng_mt19937);
double weight;
gsl_rng_set(rndm, s);
for(i=0; i<lambdaVeckHKB->size; i++)
{
weight = gsl_ran_flat(rndm, 0.2, 1.0);
gsl_vector_set(lambdaVeckHKB, i, weight * gsl_vector_get(lambdaVeckHKB, i));
weight = gsl_ran_flat(rndm, 0.2, 1.0);
gsl_vector_set(lambdaVeckLW, i, weight * gsl_vector_get(lambdaVeckLW, i));
}
gsl_rng_free(rndm);
}
gsl_vector_free(resid);
gsl_matrix_free(H);
return 0;
}
static void set_coef_mat_A(struct mvar_model *model, struct mvar_fit *fit, gsl_matrix *aug_A)
{
gsl_matrix_view mat_view = gsl_matrix_submatrix(aug_A, 0, 1, aug_A->size1, aug_A->size2 - 1);
gsl_matrix_memcpy(model->A, &mat_view.matrix);
}
int
gsl_linalg_PTLQ_decomp (gsl_matrix * A, gsl_vector * tau, gsl_permutation * p, int *signum, gsl_vector * norm)
{
const size_t N = A->size1;
const size_t M = A->size2;
if (tau->size != GSL_MIN (M, N))
{
GSL_ERROR ("size of tau must be MIN(M,N)", GSL_EBADLEN);
}
else if (p->size != N)
{
GSL_ERROR ("permutation size must be N", GSL_EBADLEN);
}
else if (norm->size != N)
{
GSL_ERROR ("norm size must be N", GSL_EBADLEN);
}
else
{
size_t i;
*signum = 1;
gsl_permutation_init (p); /* set to identity */
/* Compute column norms and store in workspace */
for (i = 0; i < N; i++)
{
gsl_vector_view c = gsl_matrix_row (A, i);
double x = gsl_blas_dnrm2 (&c.vector);
gsl_vector_set (norm, i, x);
}
for (i = 0; i < GSL_MIN (M, N); i++)
{
/* Bring the column of largest norm into the pivot position */
double max_norm = gsl_vector_get(norm, i);
size_t j, kmax = i;
for (j = i + 1; j < N; j++)
{
double x = gsl_vector_get (norm, j);
if (x > max_norm)
{
max_norm = x;
kmax = j;
}
}
if (kmax != i)
{
gsl_matrix_swap_rows (A, i, kmax);
gsl_permutation_swap (p, i, kmax);
gsl_vector_swap_elements(norm,i,kmax);
(*signum) = -(*signum);
}
/* Compute the Householder transformation to reduce the j-th
column of the matrix to a multiple of the j-th unit vector */
{
gsl_vector_view c_full = gsl_matrix_row (A, i);
gsl_vector_view c = gsl_vector_subvector (&c_full.vector,
i, M - i);
double tau_i = gsl_linalg_householder_transform (&c.vector);
gsl_vector_set (tau, i, tau_i);
/* Apply the transformation to the remaining columns */
if (i + 1 < N)
{
gsl_matrix_view m = gsl_matrix_submatrix (A, i +1, i, N - (i+1), M - i);
gsl_linalg_householder_mh (tau_i, &c.vector, &m.matrix);
}
}
/* Update the norms of the remaining columns too */
if (i + 1 < M)
{
for (j = i + 1; j < N; j++)
{
double x = gsl_vector_get (norm, j);
if (x > 0.0)
{
double y = 0;
double temp= gsl_matrix_get (A, j, i) / x;
if (fabs (temp) >= 1)
y = 0.0;
else
y = x * sqrt (1 - temp * temp);
/* recompute norm to prevent loss of accuracy */
if (fabs (y / x) < sqrt (20.0) * GSL_SQRT_DBL_EPSILON)
{
gsl_vector_view c_full = gsl_matrix_row (A, j);
gsl_vector_view c =
gsl_vector_subvector(&c_full.vector,
i+1, M - (i+1));
y = gsl_blas_dnrm2 (&c.vector);
}
gsl_vector_set (norm, j, y);
}
}
}
}
return GSL_SUCCESS;
}
}
static gsl_matrix_view mat_view_R11(struct mvar_fit *fit, gsl_matrix *R)
{
return gsl_matrix_submatrix(R, 0, 0, fit->nr_params, fit->nr_params);
}
static gsl_matrix_view mat_view_R22(struct mvar_fit *fit, gsl_matrix *R)
{
return gsl_matrix_submatrix(R, fit->nr_params, fit->nr_params, fit->m, fit->m);
}
int
gsl_linalg_hessenberg_decomp(gsl_matrix *A, gsl_vector *tau)
{
const size_t N = A->size1;
if (N != A->size2)
{
GSL_ERROR ("Hessenberg reduction requires square matrix",
GSL_ENOTSQR);
}
else if (N != tau->size)
{
GSL_ERROR ("tau vector must match matrix size", GSL_EBADLEN);
}
else if (N < 3)
{
/* nothing to do */
return GSL_SUCCESS;
}
else
{
size_t i; /* looping */
gsl_vector_view c, /* matrix column */
hv; /* householder vector */
gsl_matrix_view m;
double tau_i; /* beta in algorithm 7.4.2 */
for (i = 0; i < N - 2; ++i)
{
/*
* make a copy of A(i + 1:n, i) and store it in the section
* of 'tau' that we haven't stored coefficients in yet
*/
c = gsl_matrix_subcolumn(A, i, i + 1, N - i - 1);
hv = gsl_vector_subvector(tau, i + 1, N - (i + 1));
gsl_vector_memcpy(&hv.vector, &c.vector);
/* compute householder transformation of A(i+1:n,i) */
tau_i = gsl_linalg_householder_transform(&hv.vector);
/* apply left householder matrix (I - tau_i v v') to A */
m = gsl_matrix_submatrix(A, i + 1, i, N - (i + 1), N - i);
gsl_linalg_householder_hm(tau_i, &hv.vector, &m.matrix);
/* apply right householder matrix (I - tau_i v v') to A */
m = gsl_matrix_submatrix(A, 0, i + 1, N, N - (i + 1));
gsl_linalg_householder_mh(tau_i, &hv.vector, &m.matrix);
/* save Householder coefficient */
gsl_vector_set(tau, i, tau_i);
/*
* store Householder vector below the subdiagonal in column
* i of the matrix. hv(1) does not need to be stored since
* it is always 1.
*/
c = gsl_vector_subvector(&c.vector, 1, c.vector.size - 1);
hv = gsl_vector_subvector(&hv.vector, 1, hv.vector.size - 1);
gsl_vector_memcpy(&c.vector, &hv.vector);
}
return GSL_SUCCESS;
}
} /* gsl_linalg_hessenberg_decomp() */
int average_structure(double* X, int X_dim0, int X_dim1, int X_dim2, int X_dim2_mem,
long* assignments, int assignments_dim0, long k,
double* R, int R_dim0, int R_dim1, int R_dim1_mem) {
// Compute an "average conformation" from amongst the conformations
// in xyzlist[assignments==k]
//
// Parameters (input)
// ------------------
// X : double*
// pointer to the upper left corner of the trajectoy's coordinates.
// X should be the start of a 3d matrix.
// X_dim0 : int
// number of rows of X. Corresponds to the number of frames.
// X_dim1 : int
// number of columns of X. This should be 3.
// X_dim2 : int
// size of the third dimension of X. Corresponds to the number of atoms.
// X_dim2_mem : int
// If the array on disk has "padded" atoms, then X_dim2_mem should be
// the number of atoms with padding. This is important because we
// need to skip over the right number of frames to find the n-th
// conformation on disk.
// assignments : long*
// pointer to the beginning of the assignments vector, which contains
// the index of the "state" that each conformation is assigned to.
// k : long
// this routine will only touch entries in X corresponding to frames
// whose assignment is equal to k. The other frames will be skipped.
//
// Parameters (output)
// -------------------
// R : double*
// pointer to the start of a conformation where you'd like the resulting
// average structure stored.
// R_dim0 : int
// number of rows of R. this should be 3
// R_dim1 : int
// number of columns of R. corresponds to the number of atoms
if ((X_dim1 != R_dim0) || (X_dim2 != R_dim1) || (X_dim1 != 3)){
fprintf(stderr, "X_dim1 %d\n", X_dim1);
fprintf(stderr, "R_dim0 %d\n", R_dim0);
fprintf(stderr, "X_dim2 %d\n", X_dim2);
fprintf(stderr, "R_dim1 %d\n", R_dim1);
fprintf(stderr, "average_structure called with wrong shape\n");
exit(1);
}
if (X_dim2_mem <= X_dim2) {
fprintf(stderr, "x_dim2_mem must be greater than or equal to X_dim2");
exit(1);
}
int status = 0;
// declare the workspace for the gower matrix
double B[X_dim2*X_dim2];
memset(B, 0, sizeof(double)*X_dim2*X_dim2);
status = gower_matrix(X, X_dim0, X_dim1, X_dim2, X_dim2_mem, assignments, assignments_dim0, k,
B, X_dim2, X_dim2);
if (status == -1) {
int new_seed = rand() % X_dim0;
fprintf(stderr, "Warning: No assignments for state %ld\n", k);
fprintf(stderr, "Choosing new seed structure: %d\n", new_seed);
memcpy(R, &X[new_seed*X_dim1*X_dim2_mem], X_dim2*sizeof(double));
memcpy(R + X_dim2_mem, &X[new_seed*X_dim1*X_dim2_mem + X_dim2_mem], X_dim2*sizeof(double));
memcpy(R + 2*X_dim2_mem, &X[new_seed*X_dim1*X_dim2_mem + 2*X_dim2_mem], X_dim2*sizeof(double));
return 0;
}
gsl_matrix_view mB = gsl_matrix_view_array(B, X_dim2, X_dim2);
gsl_eigen_symmv_workspace* workspace = gsl_eigen_symmv_alloc(X_dim2);
gsl_vector* eval = gsl_vector_alloc(X_dim2);
gsl_matrix* evec = gsl_matrix_alloc(X_dim2, X_dim2);
gsl_eigen_symmv(&mB.matrix, eval, evec, workspace);
gsl_eigen_symmv_free(workspace);
gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_VAL_DESC);
// printf("Eigenvectors\n");
// gsl_matrix_printf(evec);
// printf("\n");
int i;
gsl_vector_view column;
for (i = 0; i < X_dim2; i++) {
column = gsl_matrix_column(evec, i);
gsl_vector_scale(&column.vector, sqrt(gsl_vector_get(eval, i)));
}
gsl_matrix_view output = gsl_matrix_view_array_with_tda(R, R_dim0, R_dim1, R_dim1_mem);
gsl_matrix_view submatrix = gsl_matrix_submatrix(evec, 0, 0, X_dim2, 3);
gsl_matrix_transpose_memcpy(&output.matrix, &submatrix.matrix);
rectify_mirror(R, R_dim0, R_dim1, R_dim1_mem, &X[status*X_dim1*X_dim2_mem], X_dim1, X_dim2, X_dim2_mem);
gsl_vector_free(eval);
gsl_matrix_free(evec);
return 1;
}
static int
secs2d_fit(void * vstate)
{
secs2d_state_t *state = (secs2d_state_t *) vstate;
const size_t npts = 200;
/* Note: to get a reasonable current map, use tol = 3e-1 */
const double tol = 1.0e-2;
gsl_vector *reg_param = gsl_vector_alloc(npts);
gsl_vector *rho = gsl_vector_alloc(npts);
gsl_vector *eta = gsl_vector_alloc(npts);
gsl_vector *G = gsl_vector_alloc(npts);
gsl_matrix_view A = gsl_matrix_submatrix(state->X, 0, 0, state->n, state->p);
gsl_vector_view b = gsl_vector_subvector(state->rhs, 0, state->n);
gsl_vector_view wts = gsl_vector_subvector(state->wts, 0, state->n);
double lambda_gcv, lambda_l, G_gcv;
double rnorm, snorm;
size_t i;
const char *lambda_file = "lambda.dat";
FILE *fp = fopen(lambda_file, "w");
double s0; /* largest singular value */
if (state->n < state->p)
return -1;
fprintf(stderr, "\n");
fprintf(stderr, "\t n = %zu\n", state->n);
fprintf(stderr, "\t p = %zu\n", state->p);
#if 1 /* TSVD */
{
double chisq;
size_t rank;
gsl_multifit_wlinear_tsvd(&A.matrix, &wts.vector, &b.vector, tol, state->c, state->cov,
&chisq, &rank, state->multifit_p);
rnorm = sqrt(chisq);
snorm = gsl_blas_dnrm2(state->c);
fprintf(stderr, "secs2d_fit: rank = %zu/%zu\n", rank, state->p);
}
#else /* Tikhonov / L-curve */
/* convert to standard form */
gsl_multifit_linear_applyW(&A.matrix, &wts.vector, &b.vector, &A.matrix, &b.vector);
fprintf(stderr, "\t computing SVD...");
/* compute SVD of A */
gsl_multifit_linear_svd(&A.matrix, state->multifit_p);
s0 = gsl_vector_get(state->multifit_p->S, 0);
fprintf(stderr, "done\n");
/* compute GCV curve */
gsl_multifit_linear_gcv(&b.vector, reg_param, G, &lambda_gcv, &G_gcv, state->multifit_p);
/* compute L-curve */
gsl_multifit_linear_lcurve(&b.vector, reg_param, rho, eta, state->multifit_p);
fprintf(stderr, "\t secs2d_fit: writing %s...", lambda_file);
for (i = 0; i < npts; ++i)
{
fprintf(fp, "%e %e %e %e\n",
gsl_vector_get(reg_param, i),
gsl_vector_get(rho, i),
gsl_vector_get(eta, i),
gsl_vector_get(G, i));
}
fprintf(stderr, "done\n");
gsl_multifit_linear_lcorner(rho, eta, &i);
lambda_l = gsl_vector_get(reg_param, i);
/* lower bound on lambda */
lambda_l = GSL_MAX(lambda_l, tol * s0);
/* solve regularized system with lambda_l */
gsl_multifit_linear_solve(lambda_l, &A.matrix, &b.vector, state->c, &rnorm, &snorm, state->multifit_p);
fprintf(stderr, "\t s0 = %.12e\n", s0);
fprintf(stderr, "\t lambda_l = %.12e\n", lambda_l);
fprintf(stderr, "\t lambda_gcv = %.12e\n", lambda_gcv);
fprintf(stderr, "\t rnorm = %.12e\n", rnorm);
fprintf(stderr, "\t snorm = %.12e\n", snorm);
fprintf(stderr, "\t cond(X) = %.12e\n", 1.0 / gsl_multifit_linear_rcond(state->multifit_p));
#endif
gsl_vector_free(reg_param);
gsl_vector_free(rho);
gsl_vector_free(eta);
gsl_vector_free(G);
fclose(fp);
return 0;
}
static void
post_sweep_computations (linreg *l, gsl_matrix *sw)
{
gsl_matrix *xm;
gsl_matrix_view xtx;
gsl_matrix_view xmxtx;
double m;
double tmp;
size_t i;
size_t j;
int rc;
assert (sw != NULL);
assert (l != NULL);
l->sse = gsl_matrix_get (sw, l->n_indeps, l->n_indeps);
l->mse = l->sse / l->dfe;
/*
Get the intercept.
*/
m = l->depvar_mean;
for (i = 0; i < l->n_indeps; i++)
{
tmp = gsl_matrix_get (sw, i, l->n_indeps);
l->coeff[i] = tmp;
m -= tmp * linreg_get_indep_variable_mean (l, i);
}
/*
Get the covariance matrix of the parameter estimates.
Only the upper triangle is necessary.
*/
/*
The loops below do not compute the entries related
to the estimated intercept.
*/
for (i = 0; i < l->n_indeps; i++)
for (j = i; j < l->n_indeps; j++)
{
tmp = -1.0 * l->mse * gsl_matrix_get (sw, i, j);
gsl_matrix_set (l->cov, i + 1, j + 1, tmp);
}
/*
Get the covariances related to the intercept.
*/
xtx = gsl_matrix_submatrix (sw, 0, 0, l->n_indeps, l->n_indeps);
xmxtx = gsl_matrix_submatrix (l->cov, 0, 1, 1, l->n_indeps);
xm = gsl_matrix_calloc (1, l->n_indeps);
for (i = 0; i < xm->size2; i++)
{
gsl_matrix_set (xm, 0, i,
linreg_get_indep_variable_mean (l, i));
}
rc = gsl_blas_dsymm (CblasRight, CblasUpper, l->mse,
&xtx.matrix, xm, 0.0, &xmxtx.matrix);
gsl_matrix_free (xm);
if (rc == GSL_SUCCESS)
{
tmp = l->mse / l->n_obs;
for (i = 1; i < 1 + l->n_indeps; i++)
{
tmp -= gsl_matrix_get (l->cov, 0, i)
* linreg_get_indep_variable_mean (l, i - 1);
}
gsl_matrix_set (l->cov, 0, 0, tmp);
l->intercept = m;
}
else
{
fprintf (stderr, "%s:%d:gsl_blas_dsymm: %s\n",
__FILE__, __LINE__, gsl_strerror (rc));
exit (rc);
}
}
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;
}
}
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;
}
/* 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;
}
int main(){
const int max_mu_size=601;
const int zero_pad_size=pow(2,15);
FILE *in;
in= fopen("mean.chi", "r");
gsl_matrix *e = gsl_matrix_alloc(max_mu_size, 4);
gsl_vector * kvar=gsl_vector_alloc(max_mu_size);
gsl_vector * muvar=gsl_vector_alloc(max_mu_size);
gsl_vector * mu_0pad=gsl_vector_alloc(zero_pad_size);
gsl_vector * r_0pad=gsl_vector_alloc(zero_pad_size/2); //half of lenght
gsl_vector * kvar_0pad=gsl_vector_alloc(zero_pad_size);
gsl_matrix_fscanf(in, e);
fclose(in);
gsl_matrix_get_col(kvar,e,0);
gsl_matrix_get_col(muvar,e,1);
gsl_vector_set_zero(mu_0pad);
gsl_matrix_free(e);
double dk=gsl_vector_get (kvar, 1)-gsl_vector_get (kvar, 0);
double dr=M_PI/float(zero_pad_size-1)/dk;
for (int i = 0; i < zero_pad_size; i++)
{
gsl_vector_set (kvar_0pad, i, dk*i);
}
for (int i = 0; i < zero_pad_size/2; i++)
{
gsl_vector_set (r_0pad, i, dr*i);
}
for (int i = 0; i < max_mu_size; i++)
{
gsl_vector_set (mu_0pad, i, gsl_vector_get (muvar, i));
}
gsl_vector *mu_widowed=gsl_vector_alloc(zero_pad_size);
gsl_vector_memcpy (mu_widowed, mu_0pad);
double kmin=4.0, kmax=17.0, dwk=0.8;
hanning(mu_widowed, kvar_0pad, kmin, kmax, dwk);
//FFT transform
double *data = (double *) malloc(zero_pad_size*sizeof(double));
//new double [zero_pad_size] ;
memcpy(data, mu_widowed->data, zero_pad_size*sizeof(double));
gsl_fft_real_radix2_transform(data, 1, zero_pad_size);
//Unpack complex vector
gsl_vector_complex *fourier_data = gsl_vector_complex_alloc (zero_pad_size);
gsl_fft_halfcomplex_radix2_unpack(data, fourier_data->data, 1, zero_pad_size);
gsl_vector *fftR_real = gsl_vector_alloc(fourier_data->size/2);
gsl_vector *fftR_imag = gsl_vector_alloc(fourier_data->size/2);
gsl_vector *fftR_abs = gsl_vector_alloc(fourier_data->size/2);
complex_vector_parts(fourier_data, fftR_real, fftR_imag);
complex_vector_abs(fftR_abs, fftR_real, fftR_imag);
gsl_vector *first_shell=gsl_vector_alloc(fftR_abs->size);
gsl_vector_memcpy (first_shell, fftR_abs);
double rmin=0.2, rmax=3.0, dwr=0.1;
hanning(first_shell, r_0pad, rmin, rmax, dwr);
//feff0001.dat
const int path_lines=68;
e = gsl_matrix_alloc(path_lines, 7);
gsl_vector * k_p =gsl_vector_alloc(path_lines);
gsl_vector * phc_p=gsl_vector_alloc(path_lines);
gsl_vector * mag_p=gsl_vector_alloc(path_lines);
gsl_vector * pha_p=gsl_vector_alloc(path_lines);
gsl_vector * lam_p=gsl_vector_alloc(path_lines);
in= fopen("feff0001.dat", "r");
gsl_matrix_fscanf(in, e);
fclose(in);
gsl_matrix_get_col(k_p ,e,0);
gsl_matrix_get_col(phc_p,e,1);
gsl_matrix_get_col(mag_p,e,2);
gsl_matrix_get_col(pha_p,e,3);
gsl_matrix_get_col(lam_p,e,5);
gsl_matrix_free(e);
gsl_interp_accel *acc = gsl_interp_accel_alloc ();
gsl_spline *k_spline = gsl_spline_alloc (gsl_interp_cspline, path_lines);
gsl_spline *phc_spline = gsl_spline_alloc (gsl_interp_cspline, path_lines);
gsl_spline *mag_spline = gsl_spline_alloc (gsl_interp_cspline, path_lines);
gsl_spline *pha_spline = gsl_spline_alloc (gsl_interp_cspline, path_lines);
gsl_spline *lam_spline = gsl_spline_alloc (gsl_interp_cspline, path_lines);
gsl_spline_init (k_spline , k_p->data, k_p->data , path_lines);
gsl_spline_init (phc_spline, k_p->data, phc_p->data, path_lines);
gsl_spline_init (mag_spline, k_p->data, mag_p->data, path_lines);
gsl_spline_init (pha_spline, k_p->data, pha_p->data, path_lines);
gsl_spline_init (lam_spline, k_p->data, lam_p->data, path_lines);
gsl_vector * mu_p =gsl_vector_alloc(path_lines);
//struct fit_params { student_params t; double kshift; double S02; double N; inter_path splines; };
//student_params t = {2.45681867, 0.02776907, -21.28920008, 9.44741797, 0.0, 0.0, 0.0};
splines.acc=acc; splines.phc_spline=phc_spline; splines.mag_spline=mag_spline;
splines.pha_spline=pha_spline; splines.lam_spline=lam_spline;
fit_params fp = { 2.45681867, 0.02776907, -21.28920008, 9.44741797, 1.0, 0.0};
compute_itegral(k_p, &fp, mu_p);
//mu_data_fit params = { k_p, mu_p};
mu_data.k = kvar_0pad;
mu_data.mu = mu_0pad;
mu_data.mu_ft = first_shell;
mu_data.r = r_0pad;
mu_data.kmin = kmin;
mu_data.kmax = kmax;
mu_data.rmin = rmin;
mu_data.rmax = rmax;
mu_data.dwk = dwk;
mu_data.dwr = dwr;
// initialize the solver
size_t Nparams=6;
gsl_vector *guess0 = gsl_vector_alloc(Nparams);
gsl_vector_set(guess0, 0, 2.4307);
gsl_vector_set(guess0, 1, 0.040969);
gsl_vector_set(guess0, 2, 0.001314);
gsl_vector_set(guess0, 3, 7835);
gsl_vector_set(guess0, 4, 1.0);
gsl_vector_set(guess0, 5, 0.0);
gsl_vector *fit_r = gsl_vector_alloc(r_0pad->size);
compute_itegral_r(&mu_data, fp, fit_r);
gsl_matrix *plotting = gsl_matrix_calloc(r_0pad->size, 3);
gsl_matrix_set_col (plotting, 0, r_0pad);
gsl_matrix_set_col (plotting, 1, first_shell);
gsl_matrix_set_col (plotting, 2, fit_r);
plot_matplotlib(plotting);
gsl_matrix_free (plotting);
gsl_multifit_function_fdf fit_mu_k;
fit_mu_k.f = &resudial_itegral_r;
fit_mu_k.n = MAX_FIT_POINTS;
fit_mu_k.p = Nparams;
fit_mu_k.params = &mu_data;
fit_mu_k.df = NULL;
fit_mu_k.fdf = NULL;
gsl_multifit_fdfsolver *solver = gsl_multifit_fdfsolver_alloc(gsl_multifit_fdfsolver_lmsder, MAX_FIT_POINTS, Nparams);
gsl_multifit_fdfsolver_set(solver, &fit_mu_k, guess0);
size_t iter=0, status;
do{
iter++;
//cout << solver->x->data[0] << " " << solver->x->data[1] <<endl;
status = gsl_multifit_fdfsolver_iterate (solver);
//printf("%12.4f %12.4f %12.4f\n", solver->J->data[0,0], solver->J->data[1,1], solver->J->data[2,2] );
//gsl_multifit_fdfsolver_dif_df (k_p, &fit_mu_k, mu_p, solver->J);
//gsl_multifit_fdfsolver_dif_fdf (k_p, &fit_mu_k, mu_p, solver->J);
for (int i =0; i< solver->x->size; i++){
printf("%14.5f", gsl_vector_get (solver->x, i)) ;
}
printf("\n") ;
if (status) break;
status = gsl_multifit_test_delta (solver->dx, solver->x, 1e-4, 1e-4);
}while (status == GSL_CONTINUE && iter < 100);
gsl_vector * mu_fit =gsl_vector_alloc(path_lines);
fit_params fitp = { solver->x->data[0], solver->x->data[1],\
solver->x->data[2], solver->x->data[3],\
solver->x->data[4], solver->x->data[5]};
compute_itegral(k_p, &fitp, mu_fit);
fp.mu=gsl_vector_get (solver->x, 0);
fp.sig=gsl_vector_get (solver->x, 1);
fp.skew=gsl_vector_get (solver->x, 2);
fp.nu=gsl_vector_get (solver->x, 3);
fp.S02=gsl_vector_get (solver->x, 4);
fp.kshift=gsl_vector_get (solver->x, 5);
compute_itegral_r(&mu_data, fp, fit_r);
//gsl_matrix *plotting = gsl_matrix_calloc(r_0pad->size, 3);
gsl_matrix_set_col (plotting, 0, r_0pad);
gsl_matrix_set_col (plotting, 1, first_shell);
gsl_matrix_set_col (plotting, 2, fit_r);
int min_r=search_max(r_0pad, 0.);
int max_r=search_max(r_0pad, 4.);
gsl_matrix_view plotting_lim = gsl_matrix_submatrix (plotting, min_r, 0, max_r-min_r, plotting->size2);
plot_matplotlib(&plotting_lim.matrix);
gsl_matrix_free (plotting);
//cout << gsl_spline_eval (k_spline, 1.333, acc) << endl;
//cout << gsl_spline_eval (phc_spline, 1.333, acc) << endl;
//cout << data[0] << "\t" << data[1] << "\t" << data[2] << "\t" << endl;
//cout << fourier_data->data[0] << "\t" << fourier_data->data[1] << "\t" << fourier_data->data[2] << "\t" << endl;
//Plotting
/*
gsl_matrix *plotting = gsl_matrix_calloc(zero_pad_size, 3);
gsl_matrix_set_col (plotting, 0, kvar_0pad);
gsl_matrix_set_col (plotting, 1, mu_0pad);
gsl_matrix_set_col (plotting, 2, mu_widowed);
int max_k=search_max(kvar_0pad, 35.);
int min_k=search_max(kvar_0pad, 1.0);
gsl_matrix_view plotting_lim = gsl_matrix_submatrix (plotting, min_k, 0, max_k-min_k, 3);
plot_matplotlib(&plotting_lim.matrix);
gsl_matrix_free (plotting);
*/
/*
gsl_matrix *plotting = gsl_matrix_calloc(zero_pad_size, 2);
gsl_matrix_set_col (plotting, 0, r_0pad);
gsl_matrix_set_col (plotting, 1, mu_0pad);
int max_k=search_max(kvar_0pad, 35.);
int min_k=search_max(kvar_0pad, 1.0);
gsl_matrix_view plotting_lim = gsl_matrix_submatrix (plotting, min_k, 0, max_k-min_k, 3);
plot_matplotlib(&plotting_lim.matrix);
gsl_matrix_free (plotting);
*/
/*
gsl_matrix *plotting = gsl_matrix_calloc(r_0pad->size, 5);
gsl_matrix_set_col (plotting, 0, r_0pad);
gsl_matrix_set_col (plotting, 1, fftR_abs);
gsl_matrix_set_col (plotting, 2, fftR_real);
gsl_matrix_set_col (plotting, 3, fftR_imag);
gsl_matrix_set_col (plotting, 4, first_shell);
int min_r=search_max(r_0pad, 0.);
int max_r=search_max(r_0pad, 5.);
gsl_matrix_view plotting_lim = gsl_matrix_submatrix (plotting, min_r, 0, max_r-min_r, plotting->size2);
plot_matplotlib(&plotting_lim.matrix);
//plot_matplotlib(plotting);
gsl_matrix_free (plotting);
*/
//cout << "Done" << endl;
//cout << data[1] <<"\t" << data[2] << endl;
//for (int i = 0; i < kvar->size; i++)
//{
// cout << gsl_vector_get (kvar, i) <<"\t" << gsl_vector_get (muvar, i) << endl;
//}
}
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;
}
