static int
exp1_df (CBLAS_TRANSPOSE_t TransJ, const gsl_vector * x,
const gsl_vector * u, void * params, gsl_vector * v,
gsl_matrix * JTJ)
{
gsl_matrix_view J = gsl_matrix_view_array(exp1_J, exp1_N, exp1_P);
double x1 = gsl_vector_get(x, 0);
double x2 = gsl_vector_get(x, 1);
double x3 = gsl_vector_get(x, 2);
double x4 = gsl_vector_get(x, 3);
size_t i;
for (i = 0; i < exp1_N; ++i)
{
double ti = 0.02*(i + 1.0);
double term1 = exp(x1*ti);
double term2 = exp(x2*ti);
gsl_matrix_set(&J.matrix, i, 0, -x3*ti*term1);
gsl_matrix_set(&J.matrix, i, 1, -x4*ti*term2);
gsl_matrix_set(&J.matrix, i, 2, -term1);
gsl_matrix_set(&J.matrix, i, 3, -term2);
}
if (v)
gsl_blas_dgemv(TransJ, 1.0, &J.matrix, u, 0.0, v);
if (JTJ)
gsl_blas_dsyrk(CblasLower, CblasTrans, 1.0, &J.matrix, 0.0, JTJ);
(void)params; /* avoid unused parameter warning */
return GSL_SUCCESS;
}
int
gsl_linalg_QR_QRsolve (gsl_matrix * Q, gsl_matrix * R, const gsl_vector * b, gsl_vector * x)
{
const size_t M = R->size1;
const size_t N = R->size2;
if (M != N)
{
return GSL_ENOTSQR;
}
else if (Q->size1 != M || b->size != M || x->size != M)
{
return GSL_EBADLEN;
}
else
{
/* compute sol = Q^T b */
gsl_blas_dgemv (CblasTrans, 1.0, Q, b, 0.0, x);
/* Solve R x = sol, storing x in-place */
gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, R, x);
return GSL_SUCCESS;
}
}
static int
cod_householder_mh(const double tau, const gsl_vector * v, gsl_matrix * A,
gsl_vector * work)
{
if (tau == 0)
{
return GSL_SUCCESS; /* H = I */
}
else
{
const size_t M = A->size1;
const size_t N = A->size2;
const size_t L = v->size;
gsl_vector_view A1 = gsl_matrix_subcolumn(A, 0, 0, M);
gsl_matrix_view C = gsl_matrix_submatrix(A, 0, N - L, M, L);
/* work(1:M) = A(1:M,1) */
gsl_vector_memcpy(work, &A1.vector);
/* work(1:M) = work(1:M) + A(1:M,M+1:N) * v(1:N-M) */
gsl_blas_dgemv(CblasNoTrans, 1.0, &C.matrix, v, 1.0, work);
/* A(1:M,1) = A(1:M,1) - tau * work(1:M) */
gsl_blas_daxpy(-tau, work, &A1.vector);
/* A(1:M,M+1:N) = A(1:M,M+1:N) - tau * work(1:M) * v(1:N-M)' */
gsl_blas_dger(-tau, work, v, &C.matrix);
return GSL_SUCCESS;
}
}
static int
vardim_df (CBLAS_TRANSPOSE_t TransJ, const gsl_vector * x,
const gsl_vector * u, void * params, gsl_vector * v,
gsl_matrix * JTJ)
{
gsl_matrix_view J = gsl_matrix_view_array(vardim_J, vardim_N, vardim_P);
size_t i;
double sum = 0.0;
gsl_matrix_view m = gsl_matrix_submatrix(&J.matrix, 0, 0, vardim_P, vardim_P);
gsl_matrix_set_identity(&m.matrix);
for (i = 0; i < vardim_P; ++i)
{
double xi = gsl_vector_get(x, i);
sum += (i + 1.0) * (xi - 1.0);
}
for (i = 0; i < vardim_P; ++i)
{
gsl_matrix_set(&J.matrix, vardim_P, i, i + 1.0);
gsl_matrix_set(&J.matrix, vardim_P + 1, i, 2*(i + 1.0)*sum);
}
if (v)
gsl_blas_dgemv(TransJ, 1.0, &J.matrix, u, 0.0, v);
if (JTJ)
gsl_blas_dsyrk(CblasLower, CblasTrans, 1.0, &J.matrix, 0.0, JTJ);
(void)params; /* avoid unused parameter warning */
return GSL_SUCCESS;
}
void writeTMeans(char *szOutFile, t_Cluster *ptCluster, t_Data *ptData)
{
int nK = ptCluster->nK, nD = ptCluster->nD, nT = ptData->nT, i = 0, j = 0;
FILE* ofp = fopen(szOutFile,"w");
gsl_vector* ptVector = gsl_vector_alloc(nD);
gsl_vector* ptTVector = gsl_vector_alloc(nT);
if(ofp){
for(i = 0; i < nK; i++){
for(j = 0; j < nD; j++){
gsl_vector_set(ptVector,j,ptCluster->aadMu[i][j]);
}
gsl_blas_dgemv (CblasNoTrans, 1.0,ptData->ptTMatrix,ptVector,0.0, ptTVector);
for(j = 0; j < nT - 1; j++){
fprintf(ofp,"%f,",gsl_vector_get(ptTVector,j));
}
fprintf(ofp,"%f\n",gsl_vector_get(ptTVector,nD - 1));
}
}
else{
fprintf(stderr,"Failed to open %s for writing in writeMeanss\n", szOutFile);
fflush(stderr);
}
gsl_vector_free(ptVector);
gsl_vector_free(ptTVector);
}
/*============================================================================*/
int ighmm_rand_multivariate_normal (int dim, double *x, double *mue, double *sigmacd, int seed)
{
# define CUR_PROC "ighmm_rand_multivariate_normal"
/* generate random vector of multivariate normal
*
* dim number of dimensions
* x space to store resulting vector in
* mue vector of means
* sigmacd linearized cholesky decomposition of cov matrix
* seed RNG seed
*
* see Barr & Slezak, A Comparison of Multivariate Normal Generators */
int i, j;
#ifdef DO_WITH_GSL
gsl_vector *y = gsl_vector_alloc(dim);
gsl_vector *xgsl = gsl_vector_alloc(dim);
gsl_matrix *cd = gsl_matrix_alloc(dim, dim);
#endif
if (seed != 0) {
GHMM_RNG_SET (RNG, seed);
/* do something here */
return 0;
}
else {
#ifdef DO_WITH_GSL
/* cholesky decomposition matrix */
for (i=0;i<dim;i++) {
for (j=0;j<dim;j++) {
gsl_matrix_set(cd, i, j, sigmacd[i*dim+j]);
}
}
/* generate a random vector N(O,I) */
for (i=0;i<dim;i++) {
gsl_vector_set(y, i, ighmm_rand_std_normal(seed));
}
/* multiply cd with y */
gsl_blas_dgemv(CblasNoTrans, 1.0, cd, y, 0.0, xgsl);
for (i=0;i<dim;i++) {
x[i] = gsl_vector_get(xgsl, i) + mue[i];
}
gsl_vector_free(y);
gsl_vector_free(xgsl);
gsl_matrix_free(cd);
#else
/* multivariate random numbers without gsl */
double randuni;
for (i=0;i<dim;i++) {
randuni = ighmm_rand_std_normal(seed);
for (j=0;j<dim;j++) {
if (i==0)
x[j] = mue[j];
x[j] += randuni * sigmacd[j*dim+i];
}
}
#endif
return 0;
}
# undef CUR_PROC
} /* ighmm_rand_multivariate_normal */
/**
* Evaluate the linear vector field at a given state.
* \param[in] state State at which to evaluate the vector field.
* \param[out] field Vector resulting from the evaluation of the vector field.
*/
void
linearField::evalField(gsl_vector *state, gsl_vector *field)
{
// Linear field: apply operator A to state
gsl_blas_dgemv(CblasNoTrans, 1., A, state, 0., field);
return;
}
void VarproFunction::computeJtJmulE( const gsl_matrix* R, const gsl_matrix* E, gsl_matrix *out, int useJtJ ) {
gsl_vector vecE = gsl_vector_const_view_array(E->data, E->size1 * E->size2).vector;
gsl_vector vecOut = gsl_vector_const_view_array(out->data, out->size1 * out->size2).vector;
if (R->size1 * R->size2 != 0) {
computeFuncAndPseudoJacobianLs(R, myTmpEye, NULL, myTmpJac2);
} /* Otherwise use the precomputed Jacobian */
if (useJtJ) {
if (R->size1 * R->size2 != 0) {
gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, myTmpJac2, myTmpJac2, 0, myTmpJtJ);
} /* Otherwise use the precomputed JtJ */
gsl_blas_dgemv(CblasNoTrans, 2.0, myTmpJtJ, &vecE, 0.0, &vecOut);
} else {
gsl_blas_dgemv(CblasNoTrans, 1.0, myTmpJac2, &vecE, 0.0, myTmpJacobianCol);
gsl_blas_dgemv(CblasTrans, 2.0, myTmpJac2, myTmpJacobianCol, 0.0, &vecOut);
}
}
void BAFT_LNsurv_update_sigSq(gsl_vector *yL,
gsl_vector *yU,
gsl_vector *yU_posinf,
gsl_vector *c0,
gsl_vector *c0_neginf,
gsl_matrix *X,
gsl_vector *y,
gsl_vector *beta,
double beta0,
double *sigSq,
double a_sigSq,
double b_sigSq,
double sigSq_prop_var,
int *accept_sigSq)
{
int i, u;
double eta, loglh, loglh_prop, logR, gamma_prop, sigSq_prop;
double logprior, logprior_prop;
int n = X -> size1;
gsl_vector *xbeta = gsl_vector_calloc(n);
loglh = 0;
loglh_prop = 0;
gamma_prop = rnorm(log(*sigSq), sqrt(sigSq_prop_var));
sigSq_prop = exp(gamma_prop);
gsl_blas_dgemv(CblasNoTrans, 1, X, beta, 0, xbeta);
for(i=0;i<n;i++)
{
eta = beta0 + gsl_vector_get(xbeta, i);
if(gsl_vector_get(c0_neginf, i) == 0)
{
loglh += dnorm(gsl_vector_get(y, i), eta, sqrt(*sigSq), 1) - pnorm(gsl_vector_get(c0, i), eta, sqrt(*sigSq), 0, 1);
loglh_prop += dnorm(gsl_vector_get(y, i), eta, sqrt(sigSq_prop), 1) - pnorm(gsl_vector_get(c0, i), eta, sqrt(sigSq_prop), 0, 1);
}else
{
loglh += dnorm(gsl_vector_get(y, i), eta, sqrt(*sigSq), 1);
loglh_prop += dnorm(gsl_vector_get(y, i), eta, sqrt(sigSq_prop), 1);
}
}
logprior = (-a_sigSq-1)*log(*sigSq)-b_sigSq /(*sigSq);
logprior_prop = (-a_sigSq-1)*log(sigSq_prop)-b_sigSq/sigSq_prop;
logR = loglh_prop - loglh + logprior_prop - logprior + gamma_prop - log(*sigSq);
u = log(runif(0, 1)) < logR;
if(u == 1)
{
*sigSq = sigSq_prop;
*accept_sigSq += 1;
}
gsl_vector_free(xbeta);
return;
}
/** **************************************************************************************************************/
int rv_g_inner_gaus (const gsl_vector *epsilonvec, void *params, double *gvalue)
{
double epsilon=gsl_vector_get(epsilonvec,0);
const gsl_vector *Y = ((struct fnparams *) params)->Y;/** response variable **/
const gsl_matrix *X = ((struct fnparams *) params)->X;/** design matrix INC epsilon col **/
const gsl_vector *beta = ((struct fnparams *) params)->beta;/** fixed covariate and precision terms **/
gsl_vector *vectmp1 = ((struct fnparams *) params)->vectmp1;
gsl_vector *vectmp1long = ((struct fnparams *) params)->vectmp1long;
gsl_vector *vectmp2long = ((struct fnparams *) params)->vectmp2long;
double tau_rv = gsl_vector_get(beta,beta->size-2);/** inc the precision terms - second last entries */
double tau_resid = gsl_vector_get(beta,beta->size-1);/** last entry - residual precision */
double n = (double)(Y->size);/** number of observations */
int i;
double term1,term2;
/** easy terms collected together - no Y,X, or betas **/
term1 = (n/2.0)*log(tau_resid/(2.0*M_PI)) - (tau_rv/2.0)*epsilon*epsilon + 0.5*log(tau_rv/(2.0*M_PI));
/** now for the more complex term */
/** the design matrix does not include precisions but does include epsilon, beta includes precisions but not epsilon. To use matrix operations
we make a copy of beta and replace one precision value with value for epsilon - copy into vectmp1 */
for(i=0;i<beta->size-2;i++){gsl_vector_set(vectmp1,i,gsl_vector_get(beta,i));} /** copy **/
gsl_vector_set(vectmp1,beta->size-2,epsilon); /** last entry in vectmp1 is not precision but epsilon **/
/*for(i=0;i<vectmp1->size;i++){Rprintf("=>%f\n",gsl_vector_get(vectmp1,i));} */
/** get X%*%beta where beta = (b0,b1,...,epsilon) and so we get a vector of b0*1+b1*x1i+b2*x2i+epsilon*1 for each obs i */
gsl_blas_dgemv (CblasNoTrans, 1.0, X, vectmp1, 0.0, vectmp1long);/** vectmp1long hold X%*%vectmp1 = X%*%mybeta **/
/*for(i=0;i<vectmp1long->size;i++){Rprintf("=%f\n",gsl_vector_get(vectmp1long,i));}*/
/*Rprintf("---\n");for(i=0;i<X->size1;i++){for(j=0;j<X->size2;j++){Rprintf("%f ",gsl_matrix_get(X,i,j));}Rprintf("\n");}Rprintf("---\n");*/
/*for(i=0;i<vectmp2long->size;i++){Rprintf(">%f\n",gsl_vector_get(vectmp2long,i));}*/
gsl_vector_scale(vectmp1long,-1.0);/** multiple each entry by -1 **/
gsl_vector_memcpy(vectmp2long,Y);/** vectmp2long becomes Y **/
gsl_vector_add(vectmp2long,vectmp1long);/** vectmp2long becomes Y-XB **/
/*for(i=0;i<vectmp2long->size;i++){Rprintf("> %f\n",gsl_vector_get(vectmp2long,i));}*/
/** need sum of (Y-XB)^2 so just do a dot product **/
gsl_vector_memcpy(vectmp1long,vectmp2long);/** copy vectmp2long into vectmp1long */
gsl_blas_ddot (vectmp2long, vectmp1long, &term2);/** just to get the sum of (Y-XB)^2 */
term2 *= -(tau_resid/2.0);
/*Rprintf("term2=%f epsilon=%f tau_resid=%f\n",term2,epsilon,tau_resid);*/
*gvalue = (-1.0/n)*(term1 + term2);
/*Rprintf("\n----value of term1 %f %f %f----\n",((storedbl1+storedbl2)*(-1/n)),term2,term3); */
if(gsl_isnan(*gvalue)){error("\n oops - got an NAN! in g_rv_g_inner_gaus-----\n");}
return GSL_SUCCESS;
}
/* Compute Gibbs sampling pieces X'*y, and X'*X */
void olsg(gsl_vector *y, gsl_matrix *X, gsl_vector *XTy, gsl_matrix *XTX)
{
gsl_vector_set_all(XTy,0.0);
gsl_blas_dgemv (CblasTrans,1.0,X,y, 0.0 , XTy);
gsl_matrix_set_all(XTX,0.0);
/* XTX stored in lower triangle only */
gsl_blas_dsyrk (CblasLower, CblasTrans,1.0,X,0.0,XTX);
}
static int
lmniel_set(void *vstate, const gsl_vector *swts,
gsl_multifit_function_fdf *fdf, gsl_vector *x,
gsl_vector *f, gsl_vector *dx)
{
int status;
lmniel_state_t *state = (lmniel_state_t *) vstate;
const size_t p = x->size;
size_t i;
/* initialize counters for function and Jacobian evaluations */
fdf->nevalf = 0;
fdf->nevaldf = 0;
/* evaluate function and Jacobian at x and apply weight transform */
status = gsl_multifit_eval_wf(fdf, x, swts, f);
if (status)
return status;
if (fdf->df)
status = gsl_multifit_eval_wdf(fdf, x, swts, state->J);
else
status = gsl_multifit_fdfsolver_dif_df(x, swts, fdf, f, state->J);
if (status)
return status;
/* compute rhs = -J^T f */
gsl_blas_dgemv(CblasTrans, -1.0, state->J, f, 0.0, state->rhs);
#if SCALE
gsl_vector_set_zero(state->diag);
#else
gsl_vector_set_all(state->diag, 1.0);
#endif
/* set default parameters */
state->nu = 2;
#if SCALE
state->mu = state->tau;
#else
/* compute mu_0 = tau * max(diag(J^T J)) */
state->mu = -1.0;
for (i = 0; i < p; ++i)
{
gsl_vector_view c = gsl_matrix_column(state->J, i);
double result; /* (J^T J)_{ii} */
gsl_blas_ddot(&c.vector, &c.vector, &result);
state->mu = GSL_MAX(state->mu, result);
}
state->mu *= state->tau;
#endif
return GSL_SUCCESS;
} /* lmniel_set() */
/** ***************************************************************************************/
int rv_dg_inner_gaus (const gsl_vector *epsilonvec, void *params, gsl_vector *dgvalues)
{
/*double epsilon=0.3; */
double epsilon=gsl_vector_get(epsilonvec,0);
const gsl_vector *Y = ((struct fnparams *) params)->Y;/** response variable **/
const gsl_matrix *X = ((struct fnparams *) params)->X;/** design matrix INC epsilon col **/
const gsl_vector *beta = ((struct fnparams *) params)->beta;/** fixed covariate and precision terms **/
gsl_vector *vectmp1 = ((struct fnparams *) params)->vectmp1;
gsl_vector *vectmp1long = ((struct fnparams *) params)->vectmp1long;
gsl_vector *vectmp2long = ((struct fnparams *) params)->vectmp2long;
double tau_rv = gsl_vector_get(beta,beta->size-2);/** inc the precision terms - second last entries */
double tau_resid = gsl_vector_get(beta,beta->size-1);/** last entry - residual precision */
double n = (double)(Y->size);/** number of observations */
int i;
double term3,term2;
term3 = (tau_rv*epsilon)/n;/** correct sign */
/** now for the more complex term */
/** the design matrix does not include precisions but does include epsilon, beta includes precisions but not epsilon. To use matrix operations
we make a copy of beta and replace one precision value with value for epsilon - copy into vectmp1 */
for(i=0;i<beta->size-2;i++){gsl_vector_set(vectmp1,i,gsl_vector_get(beta,i));} /** copy **/
gsl_vector_set(vectmp1,beta->size-2,epsilon); /** last entry in vectmp1 is not precision but epsilon **/
/*for(i=0;i<vectmp1->size;i++){Rprintf("=>%f\n",gsl_vector_get(vectmp1,i));} */
/** get X%*%beta where beta = (b0,b1,...,epsilon) and so we get a vector of b0*1+b1*x1i+b2*x2i+epsilon*1 for each obs i */
gsl_blas_dgemv (CblasNoTrans, 1.0, X, vectmp1, 0.0, vectmp1long);/** vectmp1long hold X%*%vectmp1 = X%*%mybeta **/
/*for(i=0;i<vectmp1long->size;i++){Rprintf("=%f\n",gsl_vector_get(vectmp1long,i));}*/
/*Rprintf("---\n");for(i=0;i<X->size1;i++){for(j=0;j<X->size2;j++){Rprintf("%f ",gsl_matrix_get(X,i,j));}Rprintf("\n");}Rprintf("---\n");*/
/*for(i=0;i<vectmp2long->size;i++){Rprintf(">%f\n",gsl_vector_get(vectmp2long,i));}*/
gsl_vector_scale(vectmp1long,-1.0);/** multiple each entry by -1 **/
gsl_vector_memcpy(vectmp2long,Y);/** vectmp2long becomes Y **/
gsl_vector_add(vectmp2long,vectmp1long);/** vectmp2long becomes Y-XB **/
gsl_vector_set_all(vectmp1long,1.0);/** reset each value to unity **/
gsl_blas_ddot (vectmp2long, vectmp1long, &term2);/** just to get the sum of vectmp2long */
/* Rprintf("analytical solution=%f\n",(tau_resid*term2)/(tau_resid*n + tau_rv));*/
/** This derivative can be solved analytically so no need to return value of d_g/d_epsilon
term2 *= -tau_resid/n;
gsl_vector_set(dgvalues,0,term2+term3);
if(gsl_isnan(gsl_vector_get(dgvalues,0))){error("rv_dg_inner is nan %f %f %f\n",term2,term3);}
**/
gsl_vector_set(dgvalues,0,(tau_resid*term2)/(tau_resid*n + tau_rv)); /** solves dg/d_epsilon=0 */
return GSL_SUCCESS;
}
int
gsl_linalg_cholesky_invert(gsl_matrix * LLT)
{
if (LLT->size1 != LLT->size2)
{
GSL_ERROR ("cholesky matrix must be square", GSL_ENOTSQR);
}
else
{
const size_t N = LLT->size1;
size_t i;
gsl_vector_view v1, v2;
/* invert the lower triangle of LLT */
gsl_linalg_tri_lower_invert(LLT);
/*
* The lower triangle of LLT now contains L^{-1}. Now compute
* A^{-1} = L^{-T} L^{-1}
*/
for (i = 0; i < N; ++i)
{
double aii = gsl_matrix_get(LLT, i, i);
if (i < N - 1)
{
double tmp;
v1 = gsl_matrix_subcolumn(LLT, i, i, N - i);
gsl_blas_ddot(&v1.vector, &v1.vector, &tmp);
gsl_matrix_set(LLT, i, i, tmp);
if (i > 0)
{
gsl_matrix_view m = gsl_matrix_submatrix(LLT, i + 1, 0, N - i - 1, i);
v1 = gsl_matrix_subcolumn(LLT, i, i + 1, N - i - 1);
v2 = gsl_matrix_subrow(LLT, i, 0, i);
gsl_blas_dgemv(CblasTrans, 1.0, &m.matrix, &v1.vector, aii, &v2.vector);
}
}
else
{
v1 = gsl_matrix_row(LLT, N - 1);
gsl_blas_dscal(aii, &v1.vector);
}
}
/* copy lower triangle to upper */
gsl_matrix_transpose_tricpy('L', 0, LLT, LLT);
return GSL_SUCCESS;
}
} /* gsl_linalg_cholesky_invert() */
void mvn_sample(gsl_vector *mean_cand, gsl_matrix *var)
{
/* Takes a mean vec, mean and var matrix,
* var and gives vector of MVN(mean,var) realisations, x
*/
int i, j;
int dimen = var -> size1;
double value;
gsl_matrix *disp;
gsl_vector *ran;
gsl_matrix *fast_species;
fast_species = gsl_matrix_alloc(2, 2);
gsl_matrix_set_identity(fast_species);
for(i=0;i<dimen; i++) {
if(MGET(var, i, i) <0.00000000001) {
MSET(var, i, i, 1.0);
MSET(fast_species, i, i, 0.0);
}
}
disp = gsl_matrix_alloc(2, 2);
ran = gsl_vector_alloc(2);
gsl_matrix_memcpy(disp, var);
if(postive_definite == 1) {
gsl_linalg_cholesky_decomp(disp);
for(i=0;i<dimen;i++) {
for (j=i+1;j<dimen;j++) {
MSET(disp,i,j,0.0);
}
}
}else{
value = pow(MGET(disp, 0 ,0), 0.5);
gsl_matrix_set_identity(disp);
MSET(disp, 0,0, value);
MSET(disp, 1,1, value);
}
for (j=0;j<dimen;j++) {
VSET(ran,j,gsl_ran_gaussian(r,1.0));
}
/*remove update from slow species*/
gsl_matrix_mul_elements(disp, fast_species);
/*Add noise to mean cand*/
gsl_blas_dgemv(CblasNoTrans,1.0, disp, ran, 1.0, mean_cand);
for(i=0; i<2; i++) {
if(VGET(mean_cand,i)<=0.0001 && MGET(fast_species, i, i) > 0.000001)
VSET(mean_cand,i,0.0001);
}
gsl_vector_free(ran);
gsl_matrix_free(disp);
gsl_matrix_free(fast_species);
}
template<typename Type> void process(Type* aInputs, Type* aOutputs)
{
for(int j = 0; j < m_number_of_inputs; j++)
gsl_vector_set(m_input_vector, j, aInputs[j]);
gsl_blas_dgemv(CblasTrans,1.0, m_recompMicCoefsSet[(int)round(m_fishEyeFactor * (NUMBEROFCIRCLEPOINTS-1))], m_input_vector, 0.0, m_output_vector);
for(int j = 0; j < m_number_of_outputs; j++)
aOutputs[j] = gsl_vector_get(m_output_vector, j);
}
void VarproFunction::setPhiPermCol( size_t i, const gsl_matrix *perm,
gsl_vector *phiPermCol ) {
if (perm != NULL) {
gsl_vector permCol = gsl_matrix_const_column(perm, i).vector;
gsl_blas_dgemv(CblasNoTrans, 1.0, myPhi, &permCol, 0.0, phiPermCol);
} else {
gsl_vector phiCol = gsl_matrix_column(myPhi, i).vector;
gsl_vector_memcpy(phiPermCol, &phiCol);
}
}
int
lls_fold(gsl_matrix *A, gsl_vector *b,
gsl_vector *wts, lls_workspace *w)
{
const size_t n = A->size1;
if (A->size2 != w->p)
{
GSL_ERROR("A has wrong size2", GSL_EBADLEN);
}
else if (n != b->size)
{
GSL_ERROR("b has wrong size", GSL_EBADLEN);
}
else if (n != wts->size)
{
GSL_ERROR("wts has wrong size", GSL_EBADLEN);
}
else
{
int s = 0;
size_t i;
double bnorm;
for (i = 0; i < n; ++i)
{
gsl_vector_view rv = gsl_matrix_row(A, i);
double *bi = gsl_vector_ptr(b, i);
double wi = gsl_vector_get(wts, i);
double swi = sqrt(wi);
/* A <- sqrt(W) A */
gsl_vector_scale(&rv.vector, swi);
/* b <- sqrt(W) b */
*bi *= swi;
}
/* ATA += A^T W A, using only the upper half of the matrix */
s = gsl_blas_dsyrk(CblasUpper, CblasTrans, 1.0, A, 1.0, w->ATA);
if (s)
return s;
/* ATb += A^T W b */
s = gsl_blas_dgemv(CblasTrans, 1.0, A, b, 1.0, w->ATb);
if (s)
return s;
/* bTb += b^T W b */
bnorm = gsl_blas_dnrm2(b);
w->bTb += bnorm * bnorm;
return s;
}
} /* lls_fold() */
static int
dogleg_preloop(const void * vtrust_state, void * vstate)
{
int status;
const gsl_multifit_nlinear_trust_state *trust_state =
(const gsl_multifit_nlinear_trust_state *) vtrust_state;
dogleg_state_t *state = (dogleg_state_t *) vstate;
const gsl_multifit_nlinear_parameters *params = trust_state->params;
double u;
double alpha; /* ||g||^2 / ||Jg||^2 */
/* initialize linear least squares solver */
status = (params->solver->init)(trust_state, trust_state->solver_state);
if (status)
return status;
/* prepare the linear solver to compute Gauss-Newton step */
status = (params->solver->presolve)(0.0, trust_state, trust_state->solver_state);
if (status)
return status;
/* solve: J dx_gn = -f for Gauss-Newton step */
status = (params->solver->solve)(trust_state->f,
state->dx_gn,
trust_state,
trust_state->solver_state);
if (status)
return status;
/* now calculate the steepest descent step */
/* compute workp = D^{-1} g and its norm */
gsl_vector_memcpy(state->workp, trust_state->g);
gsl_vector_div(state->workp, trust_state->diag);
state->norm_Dinvg = gsl_blas_dnrm2(state->workp);
/* compute workp = D^{-2} g */
gsl_vector_div(state->workp, trust_state->diag);
/* compute: workn = J D^{-2} g */
gsl_blas_dgemv(CblasNoTrans, 1.0, trust_state->J, state->workp, 0.0, state->workn);
state->norm_JDinv2g = gsl_blas_dnrm2(state->workn);
u = state->norm_Dinvg / state->norm_JDinv2g;
alpha = u * u;
/* dx_sd = -alpha D^{-2} g */
gsl_vector_memcpy(state->dx_sd, state->workp);
gsl_vector_scale(state->dx_sd, -alpha);
state->norm_Dgn = scaled_enorm(trust_state->diag, state->dx_gn);
state->norm_Dsd = scaled_enorm(trust_state->diag, state->dx_sd);
return GSL_SUCCESS;
}
/*! \brief Discrete Cepstrum Transform
*
* method for computing cepstrum aenalysis from a discrete
* set of partial peaks (frequency and amplitude)
*
* This implementation is owed to the help of Jordi Janer (thanks!) from the MTG,
* along with the following paper:
* "Regularization Techniques for Discrete Cepstrum Estimation"
* Olivier Cappe and Eric Moulines, IEEE Signal Processing Letters, Vol. 3
* No.4, April 1996
*
* \todo add anchor point add at frequency = 0 with the same magnitude as the first
* peak in pMag. This does not change the size of the cepstrum, only helps to smoothen it
* at the very beginning.
*
* \param sizeCepstrum order+1 of the discrete cepstrum
* \param pCepstrum pointer to output array of cepstrum coefficients
* \param sizeFreq number of partials peaks (the size of pFreq should be the same as pMag
* \param pFreq pointer to partial peak frequencies (hertz)
* \param pMag pointer to partial peak magnitudes (linear)
* \param fLambda regularization factor
* \param iMaxFreq maximum frequency of cepstrum
*/
void sms_dCepstrum( int sizeCepstrum, sfloat *pCepstrum, int sizeFreq, sfloat *pFreq, sfloat *pMag,
sfloat fLambda, int iMaxFreq)
{
int i, k;
sfloat factor;
sfloat fNorm = PI / (float)iMaxFreq; /* value to normalize frequencies to 0:0.5 */
//static sizeCepstrumStatic
static CepstrumMatrices m;
//printf("nPoints: %d, nCoeff: %d \n", m.nPoints, m.nCoeff);
if(m.nPoints != sizeCepstrum || m.nCoeff != sizeFreq)
AllocateDCepstrum(sizeFreq, sizeCepstrum, &m);
int s; /* signum: "(-1)^n, where n is the number of interchanges in the permutation." */
/* compute matrix M (eq. 4)*/
for (i=0; i<sizeFreq; i++)
{
gsl_matrix_set (m.pM, i, 0, 1.); // first colum is all 1
for (k=1; k <sizeCepstrum; k++)
gsl_matrix_set (m.pM, i, k , 2.*sms_sine(PI_2 + fNorm * k * pFreq[i]) );
}
/* compute transpose of M */
gsl_matrix_transpose_memcpy (m.pMt, m.pM);
/* compute R diagonal matrix (for eq. 7)*/
factor = COEF * (fLambda / (1.-fLambda)); /* \todo why is this divided like this again? */
for (k=0; k<sizeCepstrum; k++)
gsl_matrix_set(m.pR, k, k, factor * powf((sfloat) k,2.));
/* MtM = Mt * M, later will add R */
gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1., m.pMt, m.pM, 0.0, m.pMtMR);
/* add R to make MtMR */
gsl_matrix_add (m.pMtMR, m.pR);
/* set pMag in X and multiply with Mt to get pMtXk */
for(k = 0; k <sizeFreq; k++)
gsl_vector_set(m.pXk, k, log(pMag[k]));
gsl_blas_dgemv (CblasNoTrans, 1., m.pMt, m.pXk, 0., m.pMtXk);
/* solve x (the cepstrum) in Ax = b, where A=MtMR and b=pMtXk */
/* ==== the Cholesky Decomposition way ==== */
/* MtM is 'symmetric and positive definite?' */
//gsl_linalg_cholesky_decomp (m.pMtMR);
//gsl_linalg_cholesky_solve (m.pMtMR, m.pMtXk, m.pC);
/* ==== the LU decomposition way ==== */
gsl_linalg_LU_decomp (m.pMtMR, m.pPerm, &s);
gsl_linalg_LU_solve (m.pMtMR, m.pPerm, m.pMtXk, m.pC);
/* copy pC to pCepstrum */
for(i = 0; i < sizeCepstrum; i++)
pCepstrum[i] = gsl_vector_get (m.pC, i);
}
void BAFT_LNsurv_update_beta0(gsl_vector *yL,
gsl_vector *yU,
gsl_vector *yU_posinf,
gsl_vector *c0,
gsl_vector *c0_neginf,
gsl_matrix *X,
gsl_vector *y,
gsl_vector *beta,
double *beta0,
double sigSq,
double beta0_prop_var,
int *accept_beta0)
{
int i, u;
double eta, eta_prop, loglh, loglh_prop, logR, beta0_prop, logprior, logprior_prop;
int n = X -> size1;
gsl_vector *xbeta = gsl_vector_calloc(n);
loglh = 0;
loglh_prop = 0;
beta0_prop = rnorm(*beta0, sqrt(beta0_prop_var));
gsl_blas_dgemv(CblasNoTrans, 1, X, beta, 0, xbeta);
for(i=0;i<n;i++)
{
eta = *beta0 + gsl_vector_get(xbeta, i);
eta_prop = beta0_prop + gsl_vector_get(xbeta, i);
if(gsl_vector_get(c0_neginf, i) == 0)
{
loglh += dnorm(gsl_vector_get(y, i), eta, sqrt(sigSq), 1) - pnorm(gsl_vector_get(c0, i), eta, sqrt(sigSq), 0, 1);
loglh_prop += dnorm(gsl_vector_get(y, i), eta_prop, sqrt(sigSq), 1) - pnorm(gsl_vector_get(c0, i), eta_prop, sqrt(sigSq), 0, 1);
}else
{
loglh += dnorm(gsl_vector_get(y, i), eta, sqrt(sigSq), 1);
loglh_prop += dnorm(gsl_vector_get(y, i), eta_prop, sqrt(sigSq), 1);
}
}
logprior = dnorm(*beta0, 0, pow(10,6)*sqrt(sigSq), 1);
logprior_prop = dnorm(beta0_prop, 0, pow(10,6)*sqrt(sigSq), 1);
logR = loglh_prop - loglh;
u = log(runif(0, 1)) < logR;
if(u == 1)
{
*beta0 = beta0_prop;
*accept_beta0 += 1;
}
gsl_vector_free(xbeta);
return;
}
double objective(gsl_matrix *A, gsl_vector *b, double lambda, gsl_vector *z) {
double obj = 0;
gsl_vector *Azb = gsl_vector_calloc(A->size1);
gsl_blas_dgemv(CblasNoTrans, 1, A, z, 0, Azb);
gsl_vector_sub(Azb, b);
double Azb_nrm2;
gsl_blas_ddot(Azb, Azb, &Azb_nrm2);
obj = 0.5 * Azb_nrm2 + lambda * gsl_blas_dasum(z);
gsl_vector_free(Azb);
return obj;
}
/* construct design matrix and rhs vector for Shaw problem */
static int
shaw_system(gsl_matrix * X, gsl_vector * y)
{
int s = GSL_SUCCESS;
const size_t n = X->size1;
const size_t p = X->size2;
const double dtheta = M_PI / (double) p;
size_t i, j;
gsl_vector *m = gsl_vector_alloc(p);
/* build the design matrix */
for (i = 0; i < n; ++i)
{
double si = (i + 0.5) * M_PI / n - M_PI / 2.0;
double csi = cos(si);
double sni = sin(si);
for (j = 0; j < p; ++j)
{
double thetaj = (j + 0.5) * M_PI / p - M_PI / 2.0;
double term1 = csi + cos(thetaj);
double term2 = gsl_sf_sinc(sni + sin(thetaj));
double Xij = term1 * term1 * term2 * term2 * dtheta;
gsl_matrix_set(X, i, j, Xij);
}
}
/* construct coefficient vector */
{
const double a1 = 2.0;
const double a2 = 1.0;
const double c1 = 6.0;
const double c2 = 2.0;
const double t1 = 0.8;
const double t2 = -0.5;
for (j = 0; j < p; ++j)
{
double tj = -M_PI / 2.0 + (j + 0.5) * dtheta;
double mj = a1 * exp(-c1 * (tj - t1) * (tj - t1)) +
a2 * exp(-c2 * (tj - t2) * (tj - t2));
gsl_vector_set(m, j, mj);
}
}
/* construct rhs vector */
gsl_blas_dgemv(CblasNoTrans, 1.0, X, m, 0.0, y);
gsl_vector_free(m);
return s;
}
/* imaginary part */
void bath_js03_Qt_i(gsl_vector *qi, double t)
{
int i;
/* C(t) */
i=0;
while(BATH_JS03OpBra[i]>0) {
gsl_vector_set(BATH_JS03Ct,i,bath_js03_ct_i_cached(BATH_JS03BathFunc+i,t));
i++;
}
/* Q(t) = OpQ * C(t) */
gsl_blas_dgemv (CblasNoTrans, 1.0, BATH_JS03OpQ, BATH_JS03Ct, 0.0, qi);
}
/* Returns the value: At_i * S * A_i */
static double
calc_den (const gsl_matrix *S, const gsl_vector *A_i, gsl_vector *v_aux)
{
double res = 0.0;
/* v_aux = S * A_i */
gsl_blas_dgemv (CblasNoTrans, 1.0, S, A_i, 0.0, v_aux);
/* res = At_i * v_aux */
gsl_blas_ddot (A_i, v_aux, &res);
return res;
}
/* compute b^T A b */
static double
compute_sigmasq(const gsl_vector *b, const gsl_matrix *A, gsl_vector *work)
{
double result;
/* compute work = A b */
gsl_blas_dgemv(CblasNoTrans, 1.0, A, b, 0.0, work);
/* compute result = b . work */
gsl_blas_ddot(b, work, &result);
return result;
}
int
kalman_meas (Kalman * k, const double * z, int M, double dt,
KalmanMeasFunc meas_func, KalmanMeasJacobFunc meas_jacob_func,
KalmanMeasCovFunc meas_cov_func)
{
kalman_pred (k, dt);
double K[k->N * M];
double PHt[k->N * M];
double H[M * k->N];
double R[M * M];
double I[M * M];
double h[M];
gsl_matrix_view Kv = gsl_matrix_view_array (K, k->N, M);
gsl_matrix_view PHtv = gsl_matrix_view_array (PHt, k->N, M);
gsl_matrix_view Hv = gsl_matrix_view_array (H, M, k->N);
gsl_matrix_view Rv = gsl_matrix_view_array (R, M, M);
gsl_matrix_view Iv = gsl_matrix_view_array (I, M, M);
gsl_vector_view hv = gsl_vector_view_array (h, M);
meas_jacob_func (H, M, k->x, k->N, k->user);
/* K = P_*H'*inv(H*P_*H' + R) */
gsl_blas_dgemm (CblasNoTrans, CblasTrans, 1.0, &k->Pv.matrix,
&Hv.matrix, 0.0, &PHtv.matrix);
meas_cov_func (R, M, k->user);
gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, &Hv.matrix,
&PHtv.matrix, 1.0, &Rv.matrix);
size_t permv[M];
gsl_permutation perm = { M, permv };
int signum;
gsl_linalg_LU_decomp (&Rv.matrix, &perm, &signum);
gsl_linalg_LU_invert (&Rv.matrix, &perm, &Iv.matrix);
gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, &PHtv.matrix,
&Iv.matrix, 0.0, &Kv.matrix);
/* x = x + K*(z - h(x)) */
meas_func (h, M, k->x, k->N, k->user);
vector_sub_nd (z, h, M, h);
gsl_blas_dgemv (CblasNoTrans, 1.0, &Kv.matrix, &hv.vector, 1.0,
&k->xv.vector);
/* P = P_ - K*H*P_ */
gsl_blas_dgemm (CblasNoTrans, CblasTrans, -1.0, &Kv.matrix,
&PHtv.matrix, 1.0, &k->Pv.matrix);
return 0;
}
/* compute draw from sigma^2 | beta ~ IG( (T+v)/2 , (s0 +
(y-X*beta)'(y-X*beta))/2 ) **/
double linear_gibbs_sigma2(const gsl_vector *y, const gsl_matrix *X,
const gsl_vector *beta, const double v0,
const double s0)
{
size_t nobs=y->size;
gsl_vector *u=gsl_vector_alloc(nobs);
double ss;
/* form u=y-X*beta */
gsl_vector_memcpy(u,y);
gsl_blas_dgemv (CblasNoTrans,-1.0,X,beta,1.0,u);
gsl_blas_ddot (u,u,&ss);
gsl_vector_free(u);
return ran_invgamma( rng, .5*(nobs+v0), .5*(s0+ss) );
}
static int
rm_mul_rv(lua_State *L)
{
mMatReal *m = qlua_checkMatReal(L, 1);
mVecReal *v = qlua_checkVecReal(L, 2);
mVecReal *r = qlua_newVecReal(L, m->l_size);
gsl_vector_view vv = gsl_vector_view_array(v->val, v->size);
gsl_vector_view vr = gsl_vector_view_array(r->val, r->size);
if (m->r_size != v->size)
return luaL_error(L, "matrix size mismatch in m * v");
gsl_blas_dgemv(CblasNoTrans, 1.0, m->m, &vv.vector, 0.0, &vr.vector);
return 1;
}
/// Matrix by vector multiplication
/// @param v :: A vector to multiply by. Must have the same size as size2().
/// @returns A vector - the result of the multiplication. Size of the returned
/// vector equals size1().
/// @throws std::invalid_argument if the input vector has a wrong size.
/// @throws std::runtime_error if the underlying GSL routine fails.
GSLVector GSLMatrix::operator*(const GSLVector &v) const {
if (v.size() != size2()) {
throw std::invalid_argument(
"Matrix by vector multiplication: wrong size of vector.");
}
GSLVector res(size1());
auto status =
gsl_blas_dgemv(CblasNoTrans, 1.0, gsl(), v.gsl(), 0.0, res.gsl());
if (status != GSL_SUCCESS) {
std::string message = "Failed to multiply matrix by a vector.\n"
"Error message returned by the GSL:\n" +
std::string(gsl_strerror(status));
throw std::runtime_error(message);
}
return res;
}
