/**
* Integrate SDDE one step forward for a given vector field
* and state using the Euler Maruyama scheme.
* \param[in] field Delayed vector fields to evaluate.
* \param[in] stocField stochastic vector field to evaluate.
* \param[in/out] currentState Current state to update by one time step.
*/
void
EulerMaruyamaSDDE::stepForward(vectorFieldDelay *delayedField,
vectorFieldStochastic *stocField,
gsl_matrix *currentState)
{
// Assign pointers to workspace vectors
gsl_vector_view tmp = gsl_matrix_row(work, 0);
gsl_vector_view tmp1 = gsl_matrix_row(work, 1);
gsl_vector_view presentState;
/** Evaluate drift */
delayedField->evalField(currentState, &tmp.vector);
// Scale by time step
gsl_vector_scale(&tmp.vector, dt);
/** Update historic */
updateHistoric(currentState);
// Assign pointer to present state
presentState = gsl_matrix_row(currentState, 0);
// Evaluate stochastic field at present state
stocField->evalField(&presentState.vector, &tmp1.vector);
// Scale by time step
gsl_vector_scale(&tmp1.vector, sqrt(dt));
// Add drift to present state
gsl_vector_add(&presentState.vector, &tmp.vector);
/** Add diffusion at present state */
gsl_vector_add(&presentState.vector, &tmp1.vector);
return;
}
void update_unit(unit *u, camera *cam, PLAYERS *players) {
if(is_unit_dead(u)) {
if(u->state != NULL)
unit_dead(u);
return;
}
// set the velocity to 0
gsl_vector_scale(u->velocity, 0);
if(!((state *) u->state->value)->update(players, cam, u)) {
pop_unit_state(u);
}
double norm = gsl_blas_dnrm2(u->velocity);
if(norm > 0) {
gsl_vector_scale(u->velocity, 1 / norm);
gsl_vector_memcpy(u->heading, u->velocity);
gsl_vector_scale(u->velocity, u->attributes.speed);
gsl_vector_add(u->position, u->velocity);
gsl_vector_set(u->side, 0, -y(u->heading));
gsl_vector_set(u->side, 1, x(u->heading));
}
}
void SimplexFltr::pivot(int jj, int ii) {
gsl_vector_scale(gjs[jj],1.0/gsl_vector_get(gjs[jj],ii));
for(int j=0; j<d+1; j++) {
if(j==jj) j++;
double scale = gsl_vector_get(gjs[j],ii);
if(std::abs(scale)>=signtol) { //need to subtract off this row
gsl_vector_scale(gjs[jj],scale);
gsl_vector_sub(gjs[j],gjs[jj]);
gsl_vector_scale(gjs[jj],1.0/scale);
}
}
}
static int
magcal_scale(const int dir, gsl_vector *m, magcal_workspace *w)
{
int s = 0;
gsl_vector_view v = gsl_vector_subvector(m, MAGCAL_IDX_OX, 3);
if (dir == 1) /* scale to dimensionless */
gsl_vector_scale(&v.vector, 1.0 / w->B_s);
else /* scale to nT */
gsl_vector_scale(&v.vector, w->B_s);
return s;
} /* magcal_scale() */
/**
* Integrate one step forward for a given vector field and state
* using the Runge-Kutta 4 scheme.
* \param[in] field Vector field to evaluate.
* \param[in,out] currentState Current state to update by one time step.
*/
void
RungeKutta4::stepForward(vectorField *field, gsl_vector *currentState)
{
/** Use views on a working matrix not to allocate memory
* at each time step */
gsl_vector_view k1, k2, k3, k4, tmp;
// Assign views
tmp = gsl_matrix_row(work, 0);
k1 = gsl_matrix_row(work, 1);
k2 = gsl_matrix_row(work, 2);
k3 = gsl_matrix_row(work, 3);
k4 = gsl_matrix_row(work, 4);
// First increament
field->evalField(currentState, &k1.vector);
gsl_vector_scale(&k1.vector, dt);
gsl_vector_memcpy(&tmp.vector, &k1.vector);
gsl_vector_scale(&tmp.vector, 0.5);
gsl_vector_add(&tmp.vector, currentState);
// Second increment
field->evalField(&tmp.vector, &k2.vector);
gsl_vector_scale(&k2.vector, dt);
gsl_vector_memcpy(&tmp.vector, &k2.vector);
gsl_vector_scale(&tmp.vector, 0.5);
gsl_vector_add(&tmp.vector, currentState);
// Third increment
field->evalField(&tmp.vector, &k3.vector);
gsl_vector_scale(&k3.vector, dt);
gsl_vector_memcpy(&tmp.vector, &k3.vector);
gsl_vector_add(&tmp.vector, currentState);
// Fourth increment
field->evalField(&tmp.vector, &k4.vector);
gsl_vector_scale(&k4.vector, dt);
gsl_vector_scale(&k2.vector, 2);
gsl_vector_scale(&k3.vector, 2);
gsl_vector_memcpy(&tmp.vector, &k1.vector);
gsl_vector_add(&tmp.vector, &k2.vector);
gsl_vector_add(&tmp.vector, &k3.vector);
gsl_vector_add(&tmp.vector, &k4.vector);
gsl_vector_scale(&tmp.vector, 1. / 6);
// Update state
gsl_vector_add(currentState, &tmp.vector);
return;
}
/* Returns the R matrix of a QR factorization. */
static gsl_matrix *qr_fact(struct mvar_fit *fit, gsl_vector *scale)
{
gsl_matrix *R, *K, *K_diag;
gsl_vector *tau;
double delta;
K = data_mat_K(fit);
delta = (pow(K->size2, 2) + K->size2 + 1) * GSL_DBL_EPSILON;
mvar_mat_sum_sq_sqrt(K, scale);
gsl_vector_scale(scale, sqrt(delta));
K_diag = gsl_matrix_alloc(scale->size, scale->size);
gsl_matrix_set_all(K_diag, 0.0);
mvar_mat_set_diag(K_diag, scale);
/* Combine the rows of K and K_diag into one big matrix R, which is then QR decomposed. */
R = gsl_matrix_alloc(K->size1 + scale->size, K->size2);
gsl_matrix_set_all(R, 0.0);
mvar_mat_copy(R, K, 0, 0);
mvar_mat_copy(R, K_diag, K->size1, 0);
tau = gsl_vector_alloc(R->size2);
gsl_linalg_QR_decomp(R, tau);
mvar_mat_upper_tri(R);
gsl_matrix_free(K);
gsl_vector_free(tau);
gsl_matrix_free(K_diag);
return R;
}
static void set_intercept_vec_w(struct mvar_model *model, gsl_matrix *aug_A, gsl_vector *scale)
{
gsl_vector_view vec_view = gsl_matrix_column(aug_A, 0);
gsl_vector_memcpy(model->w, &vec_view.vector);
gsl_vector_scale(model->w, scale_factor(scale));
}
gsl_matrix*
GaussianMatrix ( gsl_vector *v, float sigma ) {
gsl_vector_add_constant ( v, -1 );
gsl_vector_scale ( v, 0.5 );
int siz1 = gsl_vector_get ( v, 0 );
int siz2 = gsl_vector_get ( v, 1 );
gsl_matrix *x = gsl_matrix_alloc ( siz1*2+1, siz2*2+1 );
gsl_matrix *y = gsl_matrix_alloc ( siz1*2+1, siz2*2+1 );
for ( int i=-siz2; i<=siz2; i++ ) {
for ( int j=-siz1; j<=siz1; j++ ) {
gsl_matrix_set ( x, i+siz2, j+siz1, j );
gsl_matrix_set ( y, i+siz2, j+siz1, i );
}
}
gsl_matrix_mul_elements ( x, x );
gsl_matrix_mul_elements ( y, y );
gsl_matrix_add ( x, y );
gsl_matrix_scale ( x, -1/(2*sigma*sigma) );
float sum = 0;
for ( int i=0; i<x->size1; i++ ) {
for ( int j=0; j<x->size2; j++ ) {
gsl_matrix_set ( x, i, j, exp(gsl_matrix_get ( x, i, j )) );
sum += gsl_matrix_get ( x, i, j );
}
}
if ( sum != 0 ) gsl_matrix_scale ( x, 1/sum );
gsl_matrix_free ( y );
return x;
}
static int
pcholesky_decomp (const int copy_uplo, gsl_matrix * A, gsl_permutation * p)
{
const size_t N = A->size1;
if (N != A->size2)
{
GSL_ERROR("LDLT decomposition requires square matrix", GSL_ENOTSQR);
}
else if (p->size != N)
{
GSL_ERROR ("permutation length must match matrix size", GSL_EBADLEN);
}
else
{
gsl_vector_view diag = gsl_matrix_diagonal(A);
size_t k;
if (copy_uplo)
{
/* save a copy of A in upper triangle (for later rcond calculation) */
gsl_matrix_transpose_tricpy('L', 0, A, A);
}
gsl_permutation_init(p);
for (k = 0; k < N; ++k)
{
gsl_vector_view w;
size_t j;
/* compute j = max_idx { A_kk, ..., A_nn } */
w = gsl_vector_subvector(&diag.vector, k, N - k);
j = gsl_vector_max_index(&w.vector) + k;
gsl_permutation_swap(p, k, j);
cholesky_swap_rowcol(A, k, j);
if (k < N - 1)
{
double alpha = gsl_matrix_get(A, k, k);
double alphainv = 1.0 / alpha;
/* v = A(k+1:n, k) */
gsl_vector_view v = gsl_matrix_subcolumn(A, k, k + 1, N - k - 1);
/* m = A(k+1:n, k+1:n) */
gsl_matrix_view m = gsl_matrix_submatrix(A, k + 1, k + 1, N - k - 1, N - k - 1);
/* m = m - v v^T / alpha */
gsl_blas_dsyr(CblasLower, -alphainv, &v.vector, &m.matrix);
/* v = v / alpha */
gsl_vector_scale(&v.vector, alphainv);
}
}
return GSL_SUCCESS;
}
}
void gsl_vector_step_random(const gsl_rng* r, gsl_vector* v,
const double step_size)
{
const size_t n = v->size;
gsl_vector* vp = gsl_vector_alloc(n);
// Set normal distributed random numbers as elements of v_new and
// compute the euclidean norm of this vector.
double length = 0.;
for (size_t i = 0; i < n; ++i)
{
double* vp_i = gsl_vector_ptr(vp, i);
*vp_i = gsl_ran_ugaussian(r);
length += pow(*vp_i, 2);
}
length = sqrt(length);
// Scale vp so that the elements of vp are uniformly distributed
// within an n-sphere of radius step_size.
const double scale = pow(pow(step_size, boost::numeric_cast<int>(n))
* gsl_rng_uniform_pos(r), 1.0/n) / length;
gsl_vector_scale(vp, scale);
gsl_vector_add(v, vp);
}
void Timestep::DoInputPosActions(Group *inPos)
{
for (int i=0;i<nInPosActions; i++){
stringstream temp(inPosAction[i]);
string name;
temp>>name;
if (inPos->name==name){
string action;
temp>>action;
if (action=="scale"){
double scaleFactor=1.0;
temp >> scaleFactor;
inPos->CalcCenter();
gsl_vector *transVector=gsl_vector_alloc(3);
gsl_vector_memcpy(transVector,inPos->p->center);
gsl_vector_scale(transVector, -1.0);
inPos->TransPos(transVector);
gsl_vector *scaleVect=gsl_vector_alloc(3);
gsl_vector_set_all(scaleVect, scaleFactor);
inPos->ScalePos(scaleVect);
inPos->TransPos(inPos->p->center);
bool atomsIn=inPos->CheckAtomsInBox();
if (!atomsIn){
cout<<"Warning; new scaled position puts atoms outside box.\n --- If PBCs are used could result in overlapping atoms.\n";
}
gsl_vector_free(transVector);
gsl_vector_free(scaleVect);
}
else{
// f = (1/2) x^T Ax + b^T x
void prox_quad(gsl_vector *x, const double rho, gsl_matrix *A, gsl_matrix *b)
{
gsl_matrix *I = gsl_matrix_alloc(A->size1);
gsl_matrix_set_identity(I);
gsl_matrix_scale(I, rho);
gsl_matrix_add(I, A);
gsl_vector_scale(x, rho);
gsl_vector_scale(b, -1);
gsl_vector_add(b, x);
gsl_linalg_cholesky_decomp(I);
gsl_linalg_cholesky_solve(I, b, x);
gsl_matrix_free(I);
}
/*
* FUNCTION
* Name: stationary
* Description: Given the dissipator in Bloch form, reduce to a 3x3 problem and store
* the stationary state in the 3x1 vector *X
*
* M X = 0
*
* | 0 0 0 0 | | 1 | 0
* | M10 M11 M12 M13 | | X1 | 0
* | M20 M21 M22 M23 | | X2 | = 0
* | M30 M31 M32 M33 | | X3 | 0
*
*
* A x = b
*
* | M11 M12 M13 | | X1 | | -M10 |
* | M21 M22 M23 | | X2 | = | -M20 |
* | M31 M32 M33 | | X3 | | -M30 |
*/
int stationary ( const gsl_matrix* M, gsl_vector* stat_state )
{
/* Store space for the stationary state */
gsl_vector* req = gsl_vector_calloc ( 4 ) ;
gsl_vector_set ( req, 0, 1 ) ;
/* Copy the dissipator matrix in a temporary local matrix m
* (because the algorithm destroys it...) */
gsl_matrix* m = gsl_matrix_calloc ( 4, 4 ) ;
gsl_matrix_memcpy ( m, M ) ;
/* Create a view of the spatial part of vector req */
gsl_vector_view x = gsl_vector_subvector ( req, 1, 3 ) ;
/* Create a submatrix view of the spatial part of m and a vector view
* of the spatial part of the 0-th column, which goes into -b in the system
* A x = b */
gsl_matrix_view A = gsl_matrix_submatrix ( m, 1, 1, 3, 3 ) ;
gsl_vector_view b = gsl_matrix_subcolumn ( m, 0, 1, 3 ) ;
int status1 = gsl_vector_scale ( &b.vector, -1.0 ) ;
/* Solve the system A x = b using Householder transformations.
* Changing the view x of req => also req is changed, in the spatial part */
int status2 = gsl_linalg_HH_solve ( &A.matrix, &b.vector, &x.vector ) ;
/* Set the returning value for the state stat_state */
*stat_state = *req ;
/* Free memory */
gsl_matrix_free(m) ;
return status1 + status2 ;
} /* ----- end of function stationary ----- */
int
gsl_multifit_robust_weights(const gsl_vector *r, gsl_vector *wts,
gsl_multifit_robust_workspace *w)
{
if (r->size != wts->size)
{
GSL_ERROR("residual vector does not match weight vector size", GSL_EBADLEN);
}
else if (r->size > w->n)
{
GSL_ERROR("residual vector size larger than workspace", GSL_EBADLEN);
}
else
{
int s;
double sigma;
sigma = robust_madsigma(r, w);
/* scale residuals by sigma and tuning factor */
gsl_vector_memcpy(wts, r);
gsl_vector_scale(wts, 1.0 / (sigma * w->tune));
/* compute weights in-place */
s = w->type->wfun(wts, wts);
return s;
}
} /* gsl_multifit_robust_weights() */
static VALUE rb_gsl_rational_div(VALUE obj, VALUE other)
{
gsl_rational *r = NULL, *r2 = NULL, *rnew = NULL;
gsl_poly *p;
size_t i;
Data_Get_Struct(obj, gsl_rational, r);
if (RATIONAL_P(other)) {
Data_Get_Struct(other, gsl_rational, r2);
rnew = gsl_rational_div(r, r2);
} else if (VECTOR_P(other)) {
Data_Get_Struct(other, gsl_vector, p);
rnew = gsl_rational_div_poly(r, p);
} else {
switch (TYPE(other)) {
case T_ARRAY:
p = gsl_vector_alloc(RARRAY_LEN(other));
for (i = 0; i < p->size; i++)
gsl_vector_set(p, i, NUM2DBL(rb_ary_entry(other, i)));
rnew = gsl_rational_div_poly(r, p);
gsl_vector_free(p);
break;
case T_FLOAT:
case T_FIXNUM:
rnew = gsl_rational_new(r->pnum, r->pden);
gsl_vector_scale(rnew->pnum, 1.0/NUM2DBL(other));
break;
default:
rb_raise(rb_eTypeError, "wrong argument type %s",
rb_class2name(CLASS_OF(other)));
break;
}
}
return Data_Wrap_Struct(cgsl_rational, gsl_rational_mark, gsl_rational_free, rnew);
}
static int
fdfridge_f(const gsl_vector * x, void * params, gsl_vector * f)
{
int status;
gsl_multifit_fdfridge *w = (gsl_multifit_fdfridge *) params;
const size_t n = w->n;
const size_t p = w->p;
gsl_vector_view f_user = gsl_vector_subvector(f, 0, n);
gsl_vector_view f_tik = gsl_vector_subvector(f, n, p);
/* call user callback function to get residual vector f */
status = gsl_multifit_eval_wf(w->fdf, x, NULL, &f_user.vector);
if (status)
return status;
if (w->L_diag)
{
/* store diag(L_diag) x in Tikhonov portion of f~ */
gsl_vector_memcpy(&f_tik.vector, x);
gsl_vector_mul(&f_tik.vector, w->L_diag);
}
else if (w->L)
{
/* store Lx in Tikhonov portion of f~ */
gsl_blas_dgemv(CblasNoTrans, 1.0, w->L, x, 0.0, &f_tik.vector);
}
else
{
/* store \lambda x in Tikhonov portion of f~ */
gsl_vector_memcpy(&f_tik.vector, x);
gsl_vector_scale(&f_tik.vector, w->lambda);
}
return GSL_SUCCESS;
} /* fdfridge_f() */
static int
wnlin_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(wnlin_J, wnlin_N, wnlin_P);
double A = gsl_vector_get (x, 0);
double lambda = gsl_vector_get (x, 1);
size_t i;
for (i = 0; i < wnlin_N; i++)
{
gsl_vector_view v = gsl_matrix_row(&J.matrix, i);
double ti = i;
double swi = sqrt(wnlin_W[i]);
double e = exp(-lambda * ti);
gsl_vector_set(&v.vector, 0, e);
gsl_vector_set(&v.vector, 1, -ti * A * e);
gsl_vector_set(&v.vector, 2, 1.0);
gsl_vector_scale(&v.vector, swi);
}
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);
return GSL_SUCCESS;
}
/* singleton */
static VALUE rb_gsl_poly_make_rational(VALUE obj, VALUE other)
{
gsl_rational *rnew = NULL;
gsl_poly *p, *p2;
size_t i;
Data_Get_Struct(obj, gsl_poly, p);
if (VECTOR_P(other)) {
Data_Get_Struct(other, gsl_vector, p2);
rnew = gsl_rational_new(p, p2);
} else {
switch (TYPE(other)) {
case T_ARRAY:
p2 = gsl_vector_alloc(RARRAY_LEN(other));
for (i = 0; i < p2->size; i++)
gsl_vector_set(p2, i, NUM2DBL(rb_ary_entry(other, i)));
rnew = gsl_rational_new(p, p2);
gsl_vector_free(p2);
break;
case T_FLOAT:
case T_FIXNUM:
p2 = make_vector_clone(p);
gsl_vector_scale(p2, 1.0/NUM2DBL(other));
return Data_Wrap_Struct(cgsl_poly, 0, gsl_vector_free, p2);
break;
default:
rb_raise(rb_eTypeError, "wrong argument type %s",
rb_class2name(CLASS_OF(other)));
break;
}
}
return Data_Wrap_Struct(cgsl_rational, gsl_rational_mark, gsl_rational_free, rnew);
}
static void normalizeJacobian( gsl_matrix *jac, gsl_vector *scaling ) {
for (int i = 0; i < jac->size2; i++) {
gsl_vector jac_col = gsl_matrix_column(jac, i).vector;
gsl_vector_set(scaling, i, 1 / gsl_blas_dnrm2(&jac_col));
gsl_vector_scale(&jac_col, gsl_vector_get(scaling, i));
}
}
static double lpdf(double s) {
gsl_vector* y = gsl_vector_alloc(DIM * DIM);
gsl_vector_memcpy(y, x);
gsl_vector_scale(y, s);
double ans = mcmclib_iwishart_lpdf_compute(p, y);
gsl_vector_free(y);
return ans;
}
GslVector&
GslVector::operator*=(double a)
{
int iRC;
iRC = gsl_vector_scale(m_vec,a);
queso_require_msg(!(iRC), "failed");
return *this;
}
/*
% project v in direction of u
function p=project_vec(v,u)
p = (dot(v,u)/norm(u)^2)*u;
*/
void project_vector(gsl_vector *v, gsl_vector *u, gsl_vector *p){
double dot_product_val, vec_norm, scalar_val;
gsl_blas_ddot(v, u, &dot_product_val);
vec_norm = gsl_blas_dnrm2(u);
scalar_val = dot_product_val/(vec_norm*vec_norm);
gsl_vector_memcpy(p, u);
gsl_vector_scale (p, scalar_val);
}
static int
lmniel_gradient(void *vstate, gsl_vector * g)
{
lmniel_state_t *state = (lmniel_state_t *) vstate;
gsl_vector_memcpy(g, state->rhs);
gsl_vector_scale(g, -1.0);
return GSL_SUCCESS;
}
/** **************************************************************************************************************/
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;
}
/* Update parameters using an implicit solver for
* equation (17) of Girolami and Calderhead (2011).
* Arguments:
* state: a pointer to internal working storage for RMHMC.
* model: a pointer to the rmhmc_model structure with pointers to user defined functions.
* N: number of parameters.
* stepSize: integration step-size.
* Result:
* The method directly updates the new_x array in the state structure.
* returns 0 for success or non-zero for failure.
*/
static int parametersNewtonUpdate(rmhmc_params* state, rmhmc_model* model, int N , double stepSize){
gsl_vector_view new_x_v = gsl_vector_view_array(state->new_x, N);
gsl_vector_view new_p_v = gsl_vector_view_array(state->new_momentum, N);
gsl_matrix_view new_cholM_v = gsl_matrix_view_array(state->new_cholMx, N, N);
/* temp copy of parameters */
gsl_vector_view x0_v = gsl_vector_view_array(state->btmp, N);
gsl_vector_memcpy(&x0_v.vector, &new_x_v.vector);
/* temp copy of inverse Metric */
gsl_matrix_view new_invM_v = gsl_matrix_view_array(state->new_invMx, N, N);
gsl_matrix_view invM0_v = gsl_matrix_view_array(state->tmpM, N, N);
gsl_matrix_memcpy(&invM0_v.matrix, &new_invM_v.matrix);
gsl_vector_view a_v = gsl_vector_view_array(state->atmp, N);
/* a = invM0*pNew */
/* TODO: replace gsl_blas_dgemv with gsl_blas_dsymv since invM0_v.matrix is symetric */
gsl_blas_dgemv(CblasNoTrans, 1.0, &invM0_v.matrix, &new_p_v.vector, 0.0, &a_v.vector);
int iterations = state->fIt;
int flag = 0;
int i;
for (i = 0; i < iterations; i++) {
/* new_x = invM_new*p_new */
/* TODO: replace gsl_blas_dgemv with gsl_blas_dsymv since inew_invM_v.matrix is symetric */
gsl_blas_dgemv(CblasNoTrans, 1.0, &new_invM_v.matrix, &new_p_v.vector, 0.0, &new_x_v.vector);
/* Calculates new_x_v = x0 + 0.5*stepSize*(invM_0*newP + newInvM*newP) */
gsl_vector_add(&new_x_v.vector, &a_v.vector);
gsl_vector_scale(&new_x_v.vector, 0.5*stepSize);
gsl_vector_add(&new_x_v.vector, &x0_v.vector);
/* calculate metric at the current position or update everything if this is the last iteration */
if ( (i == iterations-1) )
/* call user defined function for updating all quantities */
model->PosteriorAll(state->new_x, model->m_params, &state->new_fx, state->new_dfx, state->new_cholMx, state->new_dMx);
else
/* call user defined function for updating only the metric ternsor */
model->Metric(state->new_x, model->m_params, state->new_cholMx);
/* calculate cholesky factor for current metric */
gsl_error_handler_t* old_handle = gsl_set_error_handler_off();
flag = gsl_linalg_cholesky_decomp( &new_cholM_v.matrix );
if (flag != 0){
fprintf(stderr,"RMHMC: matrix not positive definite in parametersNewtonUpdate.\n");
return flag;
}
gsl_set_error_handler(old_handle);
/* calculate inverse for current metric */
gsl_matrix_memcpy(&new_invM_v.matrix, &new_cholM_v.matrix );
gsl_linalg_cholesky_invert(&new_invM_v.matrix);
}
return flag;
}
int
gsl_multifit_linear_applyW(const gsl_matrix * X,
const gsl_vector * w,
const gsl_vector * y,
gsl_matrix * WX,
gsl_vector * Wy)
{
const size_t n = X->size1;
const size_t p = X->size2;
if (n != y->size)
{
GSL_ERROR("y vector does not match X", GSL_EBADLEN);
}
else if (w != NULL && n != w->size)
{
GSL_ERROR("weight vector does not match X", GSL_EBADLEN);
}
else if (n != WX->size1 || p != WX->size2)
{
GSL_ERROR("WX matrix dimensions do not match X", GSL_EBADLEN);
}
else if (n != Wy->size)
{
GSL_ERROR("Wy vector must be length n", GSL_EBADLEN);
}
else
{
size_t i;
/* copy WX = X; Wy = y if distinct pointers */
if (WX != X)
gsl_matrix_memcpy(WX, X);
if (Wy != y)
gsl_vector_memcpy(Wy, y);
if (w != NULL)
{
/* construct WX = sqrt(W) X and Wy = sqrt(W) y */
for (i = 0; i < n; ++i)
{
double wi = gsl_vector_get(w, i);
double swi;
gsl_vector_view row = gsl_matrix_row(WX, i);
double *yi = gsl_vector_ptr(Wy, i);
if (wi < 0.0)
wi = 0.0;
swi = sqrt(wi);
gsl_vector_scale(&row.vector, swi);
*yi *= swi;
}
}
return GSL_SUCCESS;
}
}
/** ***************************************************************************************/
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;
}
void Compute_Momentum(gsl_matrix * Positions, gsl_matrix * Velocities, gsl_matrix * Momentum)
{
gsl_matrix_set_zero(Momentum);
gsl_matrix_memcpy(Momentum,Velocities);
for (int i=0;i<NParticles;i++)
{
gsl_vector_view gi = gsl_matrix_row(Momentum,i);
if ((int) gsl_matrix_get(Positions,i,0) == 1 )
{
gsl_vector_scale(&gi.vector,m1);
}
else
{
gsl_vector_scale(&gi.vector,m2);
}
}
}
/** Division operator (double) */
vector<double> vector<double>::operator/(const double& a)
{
vector<double> v1(_vector);
if (gsl_vector_scale(v1.as_gsl_type_ptr(), 1./a))
{
std::cout << "\n Error in vector<double> / (double)" << std::endl;
exit(EXIT_FAILURE);
}
return v1;
}
/** Unary minus */
vector<double> vector<double>::operator-() const
{
vector<double> v1(_vector);
if (gsl_vector_scale(v1.as_gsl_type_ptr(), -1.))
{
std::cout << "\n Error in vector<double> unary -" << std::endl;
exit(EXIT_FAILURE);
}
return v1;
}
