int curve_fit_cubic(uint8_t count, FittingData f_data[], double *aa, double *bb, double *cc, double *dd)
{
int i, n;
double xi, yi, chisq;
gsl_matrix *X, *cov;
gsl_vector *y, *c;
n = count;
X = gsl_matrix_alloc (n, 4);
y = gsl_vector_alloc (n);
c = gsl_vector_alloc (4);
cov = gsl_matrix_alloc (4, 4);
for (i = 0; i < n; i++)
{
xi = f_data[i].x;
yi = f_data[i].y;
////printf ("%g %g\n", xi, yi);
gsl_matrix_set (X, i, 0, 1.0);
gsl_matrix_set (X, i, 1, xi);
gsl_matrix_set (X, i, 2, xi*xi);
gsl_matrix_set (X, i, 3, xi*xi*xi);
gsl_vector_set (y, i, yi);
}
gsl_multifit_linear_workspace * work
= gsl_multifit_linear_alloc (n, 4);
gsl_multifit_linear (X, y, c, cov,
&chisq, work);
gsl_multifit_linear_free (work);
//printf ("# best fit: Y = %g + %g X + %g X^2 + %g X^3\n",
// C(0), C(1), C(2), C(3));
//printf ("# covariance matrix:\n");
//printf ("[ %+.5e, %+.5e, %+.5e, %+.5e \n",
// COV(0,0), COV(0,1), COV(0,2), COV(0,3));
//printf (" %+.5e, %+.5e, %+.5e, %+.5e \n",
// COV(1,0), COV(1,1), COV(1,2), COV(1,3));
//printf (" %+.5e, %+.5e, %+.5e, %+.5e ]\n",
// COV(2,0), COV(2,1), COV(2,2), COV(2,3));
//printf (" %+.5e, %+.5e, %+.5e, %+.5e ]\n",
// COV(3,0), COV(3,1), COV(3,2), COV(3,3));
//printf ("# chisq = %g\n", chisq);
*aa = C(0);
*bb = C(1);
*cc = C(2);
*dd = C(3);
gsl_matrix_free (X);
gsl_vector_free (y);
gsl_vector_free (c);
gsl_matrix_free (cov);
return 0;
}
/*
* Create the matrix and vector for the circuit elements
*/
int create_mna(LIST *list , gsl_matrix **matrix , gsl_vector** vector, int transient, gsl_matrix **c_matrix){
LIST_NODE* curr;
gsl_matrix* tmp_matrix;
gsl_vector* tmp_vector;
gsl_matrix* tmp_c_matrix;
//int group1 = list->len - list->m2;
//int group2 = list->m2;
int m2_elements_found = 0;
int rows;
int columns;
if( !matrix || !vector || !list)
return 0;
if(transient && !c_matrix)
return 0;
/* allocate matrix and vector */
rows = list->hashtable->num_nodes + list->m2;
columns = list->hashtable->num_nodes + list->m2;
printf("Creating matrix: rows = %d columns = %d\n",rows,columns);
tmp_matrix = gsl_matrix_calloc(rows , columns);
if( !tmp_matrix )
return 0;
tmp_c_matrix = gsl_matrix_calloc(rows , columns);
if( !tmp_c_matrix )
return 0;
tmp_vector = gsl_vector_calloc( rows);
if( !tmp_vector )
return 0;
/* compute mna */
for( curr = list->head ; curr ; curr = curr->next){
/*
* RESISTANCE ELEMENT
*/
if( curr->type == NODE_RESISTANCE_TYPE ){
double conductance = 1 / curr->node.resistance.value ;
int plus_node = curr->node.resistance.node1 - 1 ;
int minus_node = curr->node.resistance.node2 - 1;
/* <+> is ground */
if( plus_node == -1 ){
double value = gsl_matrix_get(tmp_matrix , minus_node , minus_node);
value += conductance ;
gsl_matrix_set( tmp_matrix , minus_node , minus_node , value );
//printf("Adding to matrix element (%d,%d) value:%f\n\n",minus_node,minus_node,value);
}
else if( minus_node == -1 ){
/* <-> is ground */
double value = gsl_matrix_get(tmp_matrix , plus_node , plus_node);
value += conductance;
gsl_matrix_set( tmp_matrix , plus_node ,plus_node , value );
//printf("Adding to matrix element (%d,%d) value:%f\n\n",plus_node,plus_node,value);
}
else {
/* set <+> <+> matrix element */
double value;
value = gsl_matrix_get(tmp_matrix , plus_node , plus_node);
value += conductance ;
gsl_matrix_set(tmp_matrix , plus_node , plus_node , value);
//printf("Adding to matrix element (%d,%d) value:%f\n",plus_node,plus_node,value);
/* set <+> <-> */
value = gsl_matrix_get(tmp_matrix , plus_node , minus_node);
value -= conductance ;
gsl_matrix_set(tmp_matrix , plus_node , minus_node , value);
//printf("Adding to matrix element (%d,%d) value:%f\n",plus_node,minus_node,value);
/* set <-> <+> */
value = gsl_matrix_get(tmp_matrix , minus_node , plus_node);
value -= conductance ;
gsl_matrix_set(tmp_matrix , minus_node , plus_node , value);
//printf("Adding to matrix element (%d,%d) value:%f\n",minus_node,plus_node,value);
/* set <-> <-> */
value = gsl_matrix_get(tmp_matrix , minus_node , minus_node);
value += conductance ;
gsl_matrix_set(tmp_matrix , minus_node , minus_node , value);
//printf("Adding to matrix element (%d,%d) value:%f\n\n",minus_node,minus_node,value);
}
}
/*
* CAPACITY ELEMENT
*/
else if( curr->type == NODE_CAPACITY_TYPE && transient){
double capacity = curr->node.capacity.value ;
int plus_node = curr->node.capacity.node1 - 1 ;
int minus_node = curr->node.capacity.node2 - 1;
/* <+> is ground */
if( plus_node == -1 ){
double value = gsl_matrix_get(tmp_c_matrix , minus_node , minus_node);
value += capacity ;
gsl_matrix_set( tmp_c_matrix , minus_node , minus_node , value );
//printf("Adding to matrix element (%d,%d) value:%f\n\n",minus_node,minus_node,value);
}
else if( minus_node == -1 ){
/* <-> is ground */
double value = gsl_matrix_get(tmp_c_matrix , plus_node , plus_node);
value += capacity;
gsl_matrix_set( tmp_c_matrix , plus_node ,plus_node , value );
//printf("Adding to matrix element (%d,%d) value:%f\n\n",plus_node,plus_node,value);
}
else {
/* set <+> <+> matrix element */
double value;
value = gsl_matrix_get(tmp_c_matrix , plus_node , plus_node);
value += capacity ;
gsl_matrix_set(tmp_c_matrix , plus_node , plus_node , value);
//printf("Adding to matrix element (%d,%d) value:%f\n",plus_node,plus_node,value);
/* set <+> <-> */
value = gsl_matrix_get(tmp_c_matrix , plus_node , minus_node);
value -= capacity ;
gsl_matrix_set(tmp_c_matrix , plus_node , minus_node , value);
//printf("Adding to matrix element (%d,%d) value:%f\n",plus_node,minus_node,value);
/* set <-> <+> */
value = gsl_matrix_get(tmp_c_matrix , minus_node , plus_node);
value -= capacity ;
gsl_matrix_set(tmp_c_matrix , minus_node , plus_node , value);
//printf("Adding to matrix element (%d,%d) value:%f\n",minus_node,plus_node,value);
/* set <-> <-> */
value = gsl_matrix_get(tmp_c_matrix , minus_node , minus_node);
value += capacity ;
gsl_matrix_set(tmp_c_matrix , minus_node , minus_node , value);
//printf("Adding to matrix element (%d,%d) value:%f\n\n",minus_node,minus_node,value);
}
}
/*
* CURRENT SOURCE
*/
else if( curr->type == NODE_SOURCE_I_TYPE ){
/* change only the vector */
double current = curr->node.source_i.value;
double value;
if( curr->node.source_i.node1 != 0 ){
/* ste <+> */
value = gsl_vector_get(tmp_vector , curr->node.source_i.node1 - 1 );
value -= current;
gsl_vector_set(tmp_vector , curr->node.source_i.node1 -1 , value );
}
if( curr->node.source_i.node2 != 0 ){
/* <-> */
value = gsl_vector_get(tmp_vector , curr->node.source_i.node2 - 1 );
value += current;
gsl_vector_set(tmp_vector , curr->node.source_i.node2 - 1 , value);
}
}
/*
* VOLTAGE SOURCE
*/
else if ( curr->type == NODE_SOURCE_V_TYPE ){
m2_elements_found++;
int matrix_row = list->hashtable->num_nodes + m2_elements_found - 1 ;
curr->node.source_v.mna_row = matrix_row;
double value;
double c_value;
/* set vector value */
value = gsl_vector_get(tmp_vector , matrix_row );
value += curr->node.source_v.value;
c_value = value;
gsl_vector_set(tmp_vector, matrix_row , value);
/* Change the matrix */
int plus_node = curr->node.source_v.node1 - 1 ;
int minus_node = curr->node.source_v.node2 - 1;
/* <+> */
if( plus_node != -1 ){
value = gsl_matrix_get(tmp_matrix , matrix_row , plus_node);
//value++;
gsl_matrix_set(tmp_matrix , matrix_row , plus_node , 1);
//printf("VOLTAGE SOURCE : (%d,%d) +1\n",matrix_row,plus_node);
value = gsl_matrix_get(tmp_matrix , plus_node , matrix_row);
//value++;
gsl_matrix_set(tmp_matrix , plus_node , matrix_row , 1);
//printf("VOLTAGE SOURCE : (%d,%d) + 1 \n",plus_node,matrix_row);
}
/* <->*/
if( minus_node != -1 ){
//value = gsl_matrix_get(tmp_matrix , matrix_row , minus_node);
//value++;
gsl_matrix_set(tmp_matrix , matrix_row , minus_node , -1);
//value = gsl_matrix_get(tmp_matrix , minus_node , matrix_row);
//value--;
gsl_matrix_set(tmp_matrix , minus_node , matrix_row , -1);
}
}
/*
* Inductance
*/
else if ( curr->type == NODE_INDUCTANCE_TYPE ){
m2_elements_found++;
int matrix_row = list->hashtable->num_nodes + m2_elements_found - 1 ;
double value;
double c_value = curr->node.inductance.value;
/* Change the matrix */
int plus_node = curr->node.inductance.node1 - 1 ;
int minus_node = curr->node.inductance.node2 - 1;
/* <+> */
if( plus_node != -1 ){
value = gsl_matrix_get(tmp_matrix , matrix_row , plus_node);
//value++;
gsl_matrix_set(tmp_matrix , matrix_row , plus_node , 1);
//printf("VOLTAGE SOURCE : (%d,%d) +1\n",matrix_row,plus_node);
value = gsl_matrix_get(tmp_matrix , plus_node , matrix_row);
//value++;
gsl_matrix_set(tmp_matrix , plus_node , matrix_row , 1);
//printf("VOLTAGE SOURCE : (%d,%d) + 1 \n",plus_node,matrix_row);
}
/* <->*/
if( minus_node != -1 ){
//value = gsl_matrix_get(tmp_matrix , matrix_row , minus_node);
//value++;
gsl_matrix_set(tmp_matrix , matrix_row , minus_node , -1);
//value = gsl_matrix_get(tmp_matrix , minus_node , matrix_row);
//value--;
gsl_matrix_set(tmp_matrix , minus_node , matrix_row , -1);
}
if(transient)
{
//value = gsl_matrix_get(tmp_matrix , matrix_row , minus_node);
c_value = c_value * (-1);
gsl_matrix_set(tmp_matrix , matrix_row , matrix_row , c_value);
}
}
}
*matrix = tmp_matrix;
*c_matrix = tmp_c_matrix;
*vector = tmp_vector;
/* return */
return 1;
}
static void
vine_ran_dvine(const dml_vine_t *vine,
const gsl_rng *rng,
gsl_matrix *data)
{
size_t n;
gsl_matrix *v;
gsl_vector *w;
gsl_vector *x, *y, *r;
n = vine->dim;
v = gsl_matrix_alloc(n+1, GSL_MAX(n, 2*n-4) + 1);
w = gsl_vector_alloc(n);
x = gsl_vector_alloc(1);
y = gsl_vector_alloc(1);
r = gsl_vector_alloc(1);
for (size_t s = 0; s < data->size1; s++) { // Loop over samples.
for (size_t i = 0; i < n; i++) {
gsl_vector_set(w, i, gsl_rng_uniform(rng));
}
gsl_matrix_set(v, 1, 1, gsl_vector_get(w, 0));
gsl_matrix_set(data, s, vine->order[0], gsl_vector_get(w, 0));
gsl_vector_set(x, 0, gsl_vector_get(w, 1));
gsl_vector_set(y, 0, gsl_matrix_get(v, 1, 1));
dml_copula_hinv(vine->copulas[0][0], x, y, r);
gsl_matrix_set(v, 2, 1, gsl_vector_get(r, 0));
gsl_matrix_set(data, s, vine->order[1], gsl_vector_get(r, 0));
gsl_vector_set(x, 0, gsl_matrix_get(v, 1, 1));
gsl_vector_set(y, 0, gsl_matrix_get(v, 2, 1));
dml_copula_h(vine->copulas[0][0], x, y, r);
gsl_matrix_set(v, 2, 2, gsl_vector_get(w, 1));
for (size_t i = 3; i <= n; i++) { // Loop over the rest of the variables.
gsl_matrix_set(v, i, 1, gsl_vector_get(w, i-1));
if (vine->trees >= 2) {
for (size_t k = GSL_MIN(vine->trees, i - 1); k >= 2; k--) {
gsl_vector_set(x, 0, gsl_matrix_get(v, i, 1));
gsl_vector_set(y, 0, gsl_matrix_get(v, i-1, 2*k-2));
dml_copula_hinv(vine->copulas[k-1][i-k-1], x, y, r);
gsl_matrix_set(v, i, 1, gsl_vector_get(r, 0));
}
}
gsl_vector_set(x, 0, gsl_matrix_get(v, i, 1));
gsl_vector_set(y, 0, gsl_matrix_get(v, i-1, 1));
dml_copula_hinv(vine->copulas[0][i-2], x, y, r);
gsl_matrix_set(v, i, 1, gsl_vector_get(r, 0));
gsl_matrix_set(data, s, vine->order[i-1], gsl_vector_get(r, 0));
if (i == n) break;
if (vine->trees >= 2) {
gsl_vector_set(x, 0, gsl_matrix_get(v, i-1, 1));
gsl_vector_set(y, 0, gsl_matrix_get(v, i, 1));
dml_copula_h(vine->copulas[0][i-2], x, y, r);
gsl_matrix_set(v, i, 2, gsl_vector_get(r, 0));
}
if (vine->trees >= 3) {
gsl_vector_set(x, 0, gsl_matrix_get(v, i, 1));
gsl_vector_set(y, 0, gsl_matrix_get(v, i-1, 1));
dml_copula_h(vine->copulas[0][i-2], x, y, r);
gsl_matrix_set(v, i, 3, gsl_vector_get(r, 0));
}
if (vine->trees >= 3 && i > 3) {
for (size_t j = 2; j <= GSL_MIN(vine->trees - 1, i - 2); j++) {
gsl_vector_set(x, 0, gsl_matrix_get(v, i-1, 2*j-2));
gsl_vector_set(y, 0, gsl_matrix_get(v, i, 2*j-1));
dml_copula_h(vine->copulas[j-1][i-j-1], x, y, r);
gsl_matrix_set(v, i, 2*j, gsl_vector_get(r, 0));
gsl_vector_set(x, 0, gsl_matrix_get(v, i, 2*j-1));
gsl_vector_set(y, 0, gsl_matrix_get(v, i-1, 2*j-2));
dml_copula_h(vine->copulas[j-1][i-j-1], x, y, r);
gsl_matrix_set(v, i, 2*j+1, gsl_vector_get(r, 0));
}
}
if (vine->trees >= i) {
gsl_vector_set(x, 0, gsl_matrix_get(v, i-1, 2*i-4));
gsl_vector_set(y, 0, gsl_matrix_get(v, i, 2*i-3));
dml_copula_h(vine->copulas[i-2][0], x, y, r);
gsl_matrix_set(v, i, 2*i-2, gsl_vector_get(r, 0));
}
}
}
gsl_matrix_free(v);
gsl_vector_free(w);
gsl_vector_free(x);
gsl_vector_free(y);
gsl_vector_free(r);
}
static VALUE rb_gsl_interp_evaluate(VALUE obj, VALUE xxa, VALUE yya, VALUE xx,
double (*eval)(const gsl_interp *, const double [],
const double [], double,
gsl_interp_accel *))
{
rb_gsl_interp *rgi = NULL;
double *ptrx = NULL, *ptry = NULL;
gsl_vector *v = NULL, *vnew = NULL;
gsl_matrix *m = NULL, *mnew = NULL;
VALUE ary, x;
double val;
size_t n, i, j, size, stridex, stridey;
Data_Get_Struct(obj, rb_gsl_interp, rgi);
ptrx = get_vector_ptr(xxa, &stridex, &size);
if (size != rgi->p->size ) {
rb_raise(rb_eTypeError, "size mismatch (xa:%d != %d)", (int) size, (int) rgi->p->size);
}
ptry = get_vector_ptr(yya, &stridey, &size);
if (size != rgi->p->size ) {
rb_raise(rb_eTypeError, "size mismatch (ya:%d != %d)", (int) size, (int) rgi->p->size);
}
if (CLASS_OF(xx) == rb_cRange) xx = rb_gsl_range2ary(xx);
switch (TYPE(xx)) {
case T_FIXNUM: case T_BIGNUM: case T_FLOAT:
Need_Float(xx);
return rb_float_new((*eval)(rgi->p, ptrx, ptry, NUM2DBL(xx), rgi->a));
break;
case T_ARRAY:
// n = RARRAY(xx)->len;
n = RARRAY_LEN(xx);
ary = rb_ary_new2(n);
for (i = 0; i < n; i++) {
x = rb_ary_entry(xx, i);
Need_Float(x);
val = (*eval)(rgi->p, ptrx, ptry, NUM2DBL(x), rgi->a);
rb_ary_store(ary, i, rb_float_new(val));
}
return ary;
break;
default:
#ifdef HAVE_NARRAY_H
if (NA_IsNArray(xx)) {
struct NARRAY *na = NULL;
double *ptrz = NULL, *ptr = NULL;
GetNArray(xx, na);
ptrz = (double*) na->ptr;
ary = na_make_object(NA_DFLOAT, na->rank, na->shape, CLASS_OF(xx));
ptr = NA_PTR_TYPE(ary, double*);
for (i = 0; (int) i < na->total; i++)
ptr[i] = (*eval)(rgi->p, ptrx, ptry, ptrz[i], rgi->a);
return ary;
}
#endif
if (VECTOR_P(xx)) {
Data_Get_Struct(xx, gsl_vector, v);
vnew = gsl_vector_alloc(v->size);
for (i = 0; i < v->size; i++) {
val = (*eval)(rgi->p, ptrx, ptry, gsl_vector_get(v, i), rgi->a);
gsl_vector_set(vnew, i, val);
}
return Data_Wrap_Struct(cgsl_vector, 0, gsl_vector_free, vnew);
} else if (MATRIX_P(xx)) {
Data_Get_Struct(xx, gsl_matrix, m);
mnew = gsl_matrix_alloc(m->size1, m->size2);
for (i = 0; i < m->size1; i++) {
for (j = 0; j < m->size2; j++) {
val = (*eval)(rgi->p, ptrx, ptry, gsl_matrix_get(m, i, j), rgi->a);
gsl_matrix_set(mnew, i, j, val);
}
}
return Data_Wrap_Struct(cgsl_matrix, 0, gsl_matrix_free, mnew);
} else {
rb_raise(rb_eTypeError, "wrong argument type %s", rb_class2name(CLASS_OF(xx)));
}
break;
}
/* never reach here */
return Qnil;
}
static int
bsimp_step_local (void *vstate,
size_t dim,
const double t0,
const double h_total,
const unsigned int n_step,
const double y[],
const double yp[],
const double dfdt[],
const gsl_matrix * dfdy,
double y_out[],
const gsl_odeiv_system * sys)
{
bsimp_state_t *state = (bsimp_state_t *) vstate;
gsl_matrix *const a_mat = state->a_mat;
gsl_permutation *const p_vec = state->p_vec;
double *const delta = state->delta;
double *const y_temp = state->y_temp;
double *const delta_temp = state->delta_temp;
double *const rhs_temp = state->rhs_temp;
double *const w = state->weight;
gsl_vector_view y_temp_vec = gsl_vector_view_array (y_temp, dim);
gsl_vector_view delta_temp_vec = gsl_vector_view_array (delta_temp, dim);
gsl_vector_view rhs_temp_vec = gsl_vector_view_array (rhs_temp, dim);
const double h = h_total / n_step;
double t = t0 + h;
double sum;
/* This is the factor sigma referred to in equation 3.4 of the
paper. A relative change in y exceeding sigma indicates a
runaway behavior. According to the authors suitable values for
sigma are >>1. I have chosen a value of 100*dim. BJG */
const double max_sum = 100.0 * dim;
int signum;
size_t i, j;
size_t n_inter;
/* Calculate the matrix for the linear system. */
for (i = 0; i < dim; i++)
{
for (j = 0; j < dim; j++)
{
gsl_matrix_set (a_mat, i, j, -h * gsl_matrix_get (dfdy, i, j));
}
gsl_matrix_set (a_mat, i, i, gsl_matrix_get (a_mat, i, i) + 1.0);
}
/* LU decomposition for the linear system. */
gsl_linalg_LU_decomp (a_mat, p_vec, &signum);
/* Compute weighting factors */
compute_weights (y, w, dim);
/* Initial step. */
for (i = 0; i < dim; i++)
{
y_temp[i] = h * (yp[i] + h * dfdt[i]);
}
gsl_linalg_LU_solve (a_mat, p_vec, &y_temp_vec.vector, &delta_temp_vec.vector);
sum = 0.0;
for (i = 0; i < dim; i++)
{
const double di = delta_temp[i];
delta[i] = di;
y_temp[i] = y[i] + di;
sum += fabs(di) / w[i];
}
if (sum > max_sum)
{
return GSL_EFAILED ;
}
/* Intermediate steps. */
GSL_ODEIV_FN_EVAL (sys, t, y_temp, y_out);
for (n_inter = 1; n_inter < n_step; n_inter++)
{
for (i = 0; i < dim; i++)
{
rhs_temp[i] = h * y_out[i] - delta[i];
}
gsl_linalg_LU_solve (a_mat, p_vec, &rhs_temp_vec.vector, &delta_temp_vec.vector);
sum = 0.0;
for (i = 0; i < dim; i++)
{
delta[i] += 2.0 * delta_temp[i];
y_temp[i] += delta[i];
sum += fabs(delta[i]) / w[i];
}
if (sum > max_sum)
{
return GSL_EFAILED ;
}
t += h;
GSL_ODEIV_FN_EVAL (sys, t, y_temp, y_out);
}
/* Final step. */
for (i = 0; i < dim; i++)
{
rhs_temp[i] = h * y_out[i] - delta[i];
}
gsl_linalg_LU_solve (a_mat, p_vec, &rhs_temp_vec.vector, &delta_temp_vec.vector);
sum = 0.0;
for (i = 0; i < dim; i++)
{
y_out[i] = y_temp[i] + delta_temp[i];
sum += fabs(delta_temp[i]) / w[i];
}
if (sum > max_sum)
{
return GSL_EFAILED ;
}
return GSL_SUCCESS;
}
int
gsl_linalg_QR_update (gsl_matrix * Q, gsl_matrix * R,
gsl_vector * w, const gsl_vector * v)
{
const size_t M = R->size1;
const size_t N = R->size2;
if (Q->size1 != M || Q->size2 != M)
{
GSL_ERROR ("Q matrix must be M x M if R is M x N", GSL_ENOTSQR);
}
else if (w->size != M)
{
GSL_ERROR ("w must be length M if R is M x N", GSL_EBADLEN);
}
else if (v->size != N)
{
GSL_ERROR ("v must be length N if R is M x N", GSL_EBADLEN);
}
else
{
size_t j, k;
double w0;
/* Apply Given's rotations to reduce w to (|w|, 0, 0, ... , 0)
J_1^T .... J_(n-1)^T w = +/- |w| e_1
simultaneously applied to R, H = J_1^T ... J^T_(n-1) R
so that H is upper Hessenberg. (12.5.2) */
for (k = M - 1; k > 0; k--)
{
double c, s;
double wk = gsl_vector_get (w, k);
double wkm1 = gsl_vector_get (w, k - 1);
create_givens (wkm1, wk, &c, &s);
apply_givens_vec (w, k - 1, k, c, s);
apply_givens_qr (M, N, Q, R, k - 1, k, c, s);
}
w0 = gsl_vector_get (w, 0);
/* Add in w v^T (Equation 12.5.3) */
for (j = 0; j < N; j++)
{
double r0j = gsl_matrix_get (R, 0, j);
double vj = gsl_vector_get (v, j);
gsl_matrix_set (R, 0, j, r0j + w0 * vj);
}
/* Apply Givens transformations R' = G_(n-1)^T ... G_1^T H
Equation 12.5.4 */
for (k = 1; k < GSL_MIN(M,N+1); k++)
{
double c, s;
double diag = gsl_matrix_get (R, k - 1, k - 1);
double offdiag = gsl_matrix_get (R, k, k - 1);
create_givens (diag, offdiag, &c, &s);
apply_givens_qr (M, N, Q, R, k - 1, k, c, s);
gsl_matrix_set (R, k, k - 1, 0.0); /* exact zero of G^T */
}
return GSL_SUCCESS;
}
}
int model::predict(const dataset &tds, gsl_matrix **pp)
{
int ret = -1;
gsl_matrix *mat = NULL;
gsl_matrix *ptv = NULL;
gsl_matrix *km1 = NULL;
gsl_matrix *km2 = NULL;
gsl_matrix *res = NULL;
gsl_matrix *stm = NULL;
gsl_vector_view avg_col;
gsl_vector_view dv;
if (tds.ins_num() <= 0 || tds.fea_num() != (int)_col_mean->size) {
ULIB_FATAL("invalid test dimensions, (ins_num=%d,fea_num=%d)",
tds.ins_num(), tds.fea_num());
goto done;
}
mat = gsl_matrix_alloc(tds.ins_num(), tds.fea_num());
if (mat == NULL) {
ULIB_FATAL("couldn't allocate test feature matrix");
goto done;
}
ptv = gsl_matrix_alloc(tds.ins_num(), 2);
if (ptv == NULL) {
ULIB_FATAL("couldn't allocate prediction matrix");
goto done;
}
if (tds.get_matrix(mat)) {
ULIB_FATAL("couldn't get test matrix");
goto done;
}
dbg_print_mat(mat, "Test Matrix:");
zero_out_mat(mat);
norm_mat(mat);
dbg_print_mat(mat, "Normalized Test Matrix:");
km1 = comp_kern_mat(mat, _fm, _kern);
if (km1 == NULL) {
ULIB_FATAL("couldn't compute test1 kernel matrix");
goto done;
}
dbg_print_mat(km1, "Test Kernel Matrix:");
km2 = comp_kern_mat(mat, mat, _kern);
if (km2 == NULL) {
ULIB_FATAL("couldn't compute test2 kernel matrix");
goto done;
}
dbg_print_mat(km1, "Test Kernel Matrix:");
dv = gsl_matrix_diagonal(km2);
res = gsl_matrix_alloc(km1->size1, _ikm->size2);
if (res == NULL) {
ULIB_FATAL("couldn't allocate temporary matrix");
goto done;
}
stm = gsl_matrix_alloc(km2->size1, km2->size2);
if (stm == NULL) {
ULIB_FATAL("couldn't allocate std matrix");
goto done;
}
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, km1, _ikm, 0.0, res);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, res, km1, 0.0, stm);
gsl_matrix_sub(km2, stm);
dbg_print_mat(res, "Predictive Matrix:");
avg_col = gsl_matrix_column(ptv, 0);
gsl_blas_dgemv(CblasNoTrans, 1.0, res, _tv, 0.0, &avg_col.vector);
gsl_vector_add_constant(&avg_col.vector, _t_avg);
gsl_matrix_scale(km2, _t_std*_t_std);
gsl_vector_add_constant(&dv.vector, _noise_var);
for (size_t i = 0; i < km2->size1; ++i)
gsl_matrix_set(ptv, i, 1, sqrt(gsl_vector_get(&dv.vector, i)));
*pp = ptv;
ptv = NULL;
ret = 0;
done:
gsl_matrix_free(mat);
gsl_matrix_free(ptv);
gsl_matrix_free(km1);
gsl_matrix_free(km2);
gsl_matrix_free(res);
gsl_matrix_free(stm);
return ret;
}
int vertex(struct track trk[4],double * x,double * y,double * z, double * chi2, double init[3]) {
gsl_vector * v_vertex = gsl_vector_calloc(3);
gsl_vector * va_tracks[TRACK_NBR];
unsigned int i;
for(i = 0; i < TRACK_NBR; i++) {
va_tracks[i] = gsl_vector_calloc(6);
}
/*
* pos. [mm]
* mom. [MeV/c]
*/
/* Initial conditions */
gsl_vector_set(v_vertex,0,43.0);
gsl_vector_set(v_vertex,1,0.0);
gsl_vector_set(v_vertex,2,137728.0);
unsigned int j;
for(i=0;i<TRACK_NBR;i++) {
for(j=0;j<6;j++) {
gsl_vector_set(va_tracks[i],j,trk[i].param[j]);
}
}
/* Covariance matricies */
gsl_matrix * m_V_alpha_zero = gsl_matrix_calloc(6*TRACK_NBR+3,6*TRACK_NBR+3);
//VTX
gsl_matrix_set(m_V_alpha_zero, 0, 0, init[0]);
gsl_matrix_set(m_V_alpha_zero, 1, 1, init[1]);
gsl_matrix_set(m_V_alpha_zero, 2, 2, init[2]);
unsigned int k;
for(i=0;i < TRACK_NBR; i++) {
for(j=0;j<6;j++) {
for(k=0;k<6;k++) {
gsl_matrix_set(m_V_alpha_zero, 3+i*6+j, 3+i*6+k, trk[i].cov[j][k]);
//printf("%i,%i,%g\n",3+i*6+j,3+i*6+k,trk[i].cov[j][k]);
}
}
}
/* Prepare inputs */
gsl_vector * v_alpha_zero = gsl_vector_calloc(6*TRACK_NBR+3);
/* Ntracks + 1 : vector + tracks */
gsl_vector * va_inputs[1+TRACK_NBR];
va_inputs[0] = v_vertex;
/* ! Merge this loop ! */
for(i = 0; i < TRACK_NBR; i++) {
va_inputs[i+1] = va_tracks[i];
va_inputs[i+1] = va_tracks[i];
}
stack_vector_array(va_inputs,1+TRACK_NBR,v_alpha_zero);
gsl_vector * v_d = gsl_vector_calloc(CONSTRAINTS*TRACK_NBR);
gsl_matrix * m_D = gsl_matrix_calloc(CONSTRAINTS*TRACK_NBR,6*TRACK_NBR+3);
/* LOOP START HERE */
prepare(TRACK_NBR,v_alpha_zero,v_d,m_D);
/* Ok now minimise ! */
/* Allocate memory for the updated results */
gsl_vector * v_alpha = gsl_vector_calloc(6*TRACK_NBR+3);
gsl_matrix * m_V_alpha = gsl_matrix_calloc(6*TRACK_NBR+3,6*TRACK_NBR+3);
minimise(v_alpha_zero,v_alpha_zero,m_V_alpha_zero,v_d,m_D,v_alpha,m_V_alpha,chi2);
/* Compute D (back proj. to GTK) */
double gtk_pos = 102400.0;
double xp = -1*(gsl_vector_get(v_alpha,2) - gtk_pos)*(gsl_vector_get(v_alpha,6)/gsl_vector_get(v_alpha,8)) + gsl_vector_get(v_alpha,0);
double yp = -1*(gsl_vector_get(v_alpha,2) - gtk_pos)*(gsl_vector_get(v_alpha,7)/gsl_vector_get(v_alpha,8)) + gsl_vector_get(v_alpha,1);
//printf("%g %g %g %g\n",chi2,gsl_vector_get(v_alpha,0),gsl_vector_get(v_alpha,1),gsl_vector_get(v_alpha,2));
*x = gsl_vector_get(v_alpha,0);
*y = gsl_vector_get(v_alpha,1);
*z = gsl_vector_get(v_alpha,2);
//printf("*** %g %g %g\n",*x,*y,*z);
/* LOOP END HERE */
/* Clear the memory */
gsl_matrix_free(m_V_alpha_zero);
gsl_vector_free(v_alpha_zero);
gsl_matrix_free(m_V_alpha);
gsl_vector_free(v_alpha);
gsl_vector_free(v_d);
gsl_matrix_free(m_D);
gsl_vector_free(v_vertex);
for(i=0;i<TRACK_NBR;i++) {
gsl_vector_free(va_tracks[i]);
}
return 0;
}
static void
linreg_fit_qr (const gsl_matrix *cov, linreg *l)
{
double intcpt_coef = 0.0;
double intercept_variance = 0.0;
gsl_matrix *xtx;
gsl_matrix *q;
gsl_matrix *r;
gsl_vector *xty;
gsl_vector *tau;
gsl_vector *params;
double tmp = 0.0;
size_t i;
size_t j;
xtx = gsl_matrix_alloc (cov->size1 - 1, cov->size2 - 1);
xty = gsl_vector_alloc (cov->size1 - 1);
tau = gsl_vector_alloc (cov->size1 - 1);
params = gsl_vector_alloc (cov->size1 - 1);
for (i = 0; i < xtx->size1; i++)
{
gsl_vector_set (xty, i, gsl_matrix_get (cov, cov->size2 - 1, i));
for (j = 0; j < xtx->size2; j++)
{
gsl_matrix_set (xtx, i, j, gsl_matrix_get (cov, i, j));
}
}
gsl_linalg_QR_decomp (xtx, tau);
q = gsl_matrix_alloc (xtx->size1, xtx->size2);
r = gsl_matrix_alloc (xtx->size1, xtx->size2);
gsl_linalg_QR_unpack (xtx, tau, q, r);
gsl_linalg_QR_solve (xtx, tau, xty, params);
for (i = 0; i < params->size; i++)
{
l->coeff[i] = gsl_vector_get (params, i);
}
l->sst = gsl_matrix_get (cov, cov->size1 - 1, cov->size2 - 1);
l->ssm = 0.0;
for (i = 0; i < l->n_indeps; i++)
{
l->ssm += gsl_vector_get (xty, i) * l->coeff[i];
}
l->sse = l->sst - l->ssm;
gsl_blas_dtrsm (CblasLeft, CblasLower, CblasNoTrans, CblasNonUnit, linreg_mse (l),
r, q);
/* Copy the lower triangle into the upper triangle. */
for (i = 0; i < q->size1; i++)
{
gsl_matrix_set (l->cov, i + 1, i + 1, gsl_matrix_get (q, i, i));
for (j = i + 1; j < q->size2; j++)
{
intercept_variance -= 2.0 * gsl_matrix_get (q, i, j) *
linreg_get_indep_variable_mean (l, i) *
linreg_get_indep_variable_mean (l, j);
gsl_matrix_set (q, i, j, gsl_matrix_get (q, j, i));
}
}
l->intercept = linreg_get_depvar_mean (l);
tmp = 0.0;
for (i = 0; i < l->n_indeps; i++)
{
tmp = linreg_get_indep_variable_mean (l, i);
l->intercept -= l->coeff[i] * tmp;
intercept_variance += tmp * tmp * gsl_matrix_get (q, i, i);
}
/* Covariances related to the intercept. */
intercept_variance += linreg_mse (l) / linreg_n_obs (l);
gsl_matrix_set (l->cov, 0, 0, intercept_variance);
for (i = 0; i < q->size1; i++)
{
for (j = 0; j < q->size2; j++)
{
intcpt_coef -= gsl_matrix_get (q, i, j)
* linreg_get_indep_variable_mean (l, j);
}
gsl_matrix_set (l->cov, 0, i + 1, intcpt_coef);
gsl_matrix_set (l->cov, i + 1, 0, intcpt_coef);
intcpt_coef = 0.0;
}
gsl_matrix_free (q);
gsl_matrix_free (r);
gsl_vector_free (xty);
gsl_vector_free (tau);
gsl_matrix_free (xtx);
gsl_vector_free (params);
}
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);
}
}
/* Perform one cycle of post refinement on 'image' against 'full' */
static double pr_iterate(Crystal *cr, const RefList *full,
PartialityModel pmodel, int *n_filtered)
{
gsl_matrix *M;
gsl_vector *v;
gsl_vector *shifts;
int param;
Reflection *refl;
RefListIterator *iter;
RefList *reflections;
double max_shift;
int nref = 0;
const int verbose = 0;
int num_params = 0;
enum gparam rv[32];
*n_filtered = 0;
/* If partiality model is anything other than "unity", refine all the
* geometrical parameters */
if ( pmodel != PMODEL_UNITY ) {
rv[num_params++] = GPARAM_ASX;
rv[num_params++] = GPARAM_ASY;
rv[num_params++] = GPARAM_ASZ;
rv[num_params++] = GPARAM_BSX;
rv[num_params++] = GPARAM_BSY;
rv[num_params++] = GPARAM_BSZ;
rv[num_params++] = GPARAM_CSX;
rv[num_params++] = GPARAM_CSY;
rv[num_params++] = GPARAM_CSZ;
}
STATUS("Refining %i parameters.\n", num_params);
reflections = crystal_get_reflections(cr);
M = gsl_matrix_calloc(num_params, num_params);
v = gsl_vector_calloc(num_params);
/* Construct the equations, one per reflection in this image */
for ( refl = first_refl(reflections, &iter);
refl != NULL;
refl = next_refl(refl, iter) )
{
signed int ha, ka, la;
double I_full, delta_I;
double w;
double I_partial;
int k;
double p, l;
Reflection *match;
double gradients[num_params];
/* Find the full version */
get_indices(refl, &ha, &ka, &la);
match = find_refl(full, ha, ka, la);
if ( match == NULL ) continue;
if ( (get_intensity(refl) < 3.0*get_esd_intensity(refl))
|| (get_partiality(refl) < MIN_PART_REFINE)
|| (get_redundancy(match) < 2) ) continue;
I_full = get_intensity(match);
/* Actual measurement of this reflection from this pattern? */
I_partial = get_intensity(refl) / crystal_get_osf(cr);
p = get_partiality(refl);
l = get_lorentz(refl);
/* Calculate the weight for this reflection */
w = pow(get_esd_intensity(refl), 2.0);
w += l * p * I_full * pow(get_esd_intensity(match), 2.0);
w = pow(w, -1.0);
/* Calculate all gradients for this reflection */
for ( k=0; k<num_params; k++ ) {
gradients[k] = p_gradient(cr, rv[k], refl, pmodel) * l;
}
for ( k=0; k<num_params; k++ ) {
int g;
double v_c, v_curr;
for ( g=0; g<num_params; g++ ) {
double M_c, M_curr;
/* Matrix is symmetric */
if ( g > k ) continue;
M_c = gradients[g] * gradients[k];
M_c *= w * pow(I_full, 2.0);
M_curr = gsl_matrix_get(M, k, g);
gsl_matrix_set(M, k, g, M_curr + M_c);
gsl_matrix_set(M, g, k, M_curr + M_c);
}
delta_I = I_partial - (l * p * I_full);
v_c = w * delta_I * I_full * gradients[k];
v_curr = gsl_vector_get(v, k);
gsl_vector_set(v, k, v_curr + v_c);
}
nref++;
}
if ( verbose ) {
STATUS("The original equation:\n");
show_matrix_eqn(M, v);
}
//STATUS("%i reflections went into the equations.\n", nref);
if ( nref == 0 ) {
crystal_set_user_flag(cr, 2);
gsl_matrix_free(M);
gsl_vector_free(v);
return 0.0;
}
max_shift = 0.0;
shifts = solve_svd(v, M, n_filtered, verbose);
if ( shifts != NULL ) {
for ( param=0; param<num_params; param++ ) {
double shift = gsl_vector_get(shifts, param);
apply_shift(cr, rv[param], shift);
//STATUS("Shift %i: %e\n", param, shift);
if ( fabs(shift) > max_shift ) max_shift = fabs(shift);
}
} else {
crystal_set_user_flag(cr, 3);
}
gsl_matrix_free(M);
gsl_vector_free(v);
gsl_vector_free(shifts);
return max_shift;
}
/** **************************************************************************************************************/
void build_designmatrix_gaus_rv(network *dag,datamatrix *obsdata, double priormean, double priorsd,const double priorgamshape, const double priorgamscale,datamatrix *designmatrix, int nodeid, int storeModes)
{
int i,j,k;
int numparents=0;
gsl_vector_int *parentindexes=0;
int num_unq_grps=0;
int *groupcnts;
int *curindex;
gsl_matrix **array_of_designs;
gsl_vector **array_of_Y;
if(dag->maxparents>0){
parentindexes=gsl_vector_int_alloc(dag->maxparents);
/** collect parents of this node **/
for(j=0;j<dag->numNodes;j++){
if( dag->defn[nodeid][j]==1 /** got a parent so get its index **/
&& numparents<dag->maxparents /** if numparents==dag->maxparents then we are done **/
){
gsl_vector_int_set(parentindexes,numparents++,j);/** store index of parent **/
}
}
} /** check for maxparent=0 */
/** this part is new and just for posterior param est - it does not affect Laplace approx in any way****/
/** setup matrix where each non DBL_MAX entry in a row is for a parameter to be estimated and the col is which param
first col is for the intercept */
if(storeModes){
for(k=0;k<dag->numNodes+3;k++){gsl_matrix_set(dag->modes,nodeid,k,DBL_MAX);} /** initialise row to DBL_MAX n.b. +2 here is need in fitabn.R part**/
gsl_matrix_set(dag->modes,nodeid,0,1);/** the intercept term - always have an intercept - but not in dag.m definition */
for(k=0;k<numparents;k++){gsl_matrix_set(dag->modes,nodeid,gsl_vector_int_get(parentindexes,k)+1,1);} /** offset is 1 due to intercept */
gsl_matrix_set(dag->modes,nodeid,dag->numNodes+1,1);/** the residual precision **/
gsl_matrix_set(dag->modes,nodeid,dag->numNodes+2,1);/** the group level precision term put at end of other params */
}
/** ****************************************************************************************************/
designmatrix->datamatrix=gsl_matrix_alloc(obsdata->numDataPts,numparents+1+1);/** +1=intercept +1=rv_precision - note this is just for the mean so no extra term for gaussian node **/
designmatrix->Y=gsl_vector_alloc(obsdata->numDataPts);
designmatrix->priormean=gsl_vector_alloc(numparents+1);
designmatrix->priorsd=gsl_vector_alloc(numparents+1);
designmatrix->priorgamshape=gsl_vector_alloc(1); /** only 1 of these per node - NOTE: use same prior for group precision and overall precision */
designmatrix->priorgamscale=gsl_vector_alloc(1); /** only 1 of these per node - NOTE: use same prior for group precision and overall precision */
designmatrix->datamatrix_noRV=gsl_matrix_alloc(obsdata->numDataPts,numparents+1);/** drop the last col - used for initial value estimation only**/
/** create design matrix - ALL DATA POINTS - copy relevant cols from the observed data **/
/** int** designmatrix is just used as storage space, fill up from left cols across until as far as needed */
for(i=0;i<obsdata->numDataPts;i++){/** for each observed data point **/
gsl_matrix_set(designmatrix->datamatrix,i,0,1.0); /** set first column - intercept - to 1's **/
gsl_matrix_set(designmatrix->datamatrix_noRV,i,0,1.0);/** build matrix same as datamatrix just without the last col (which contains 1's for epsilon rv term */
gsl_matrix_set(designmatrix->datamatrix,i,(designmatrix->datamatrix)->size2-1,1.0);/** set last column - rv precision to 1.0 **/
gsl_vector_set(designmatrix->Y,i,obsdata->defn[i][nodeid]);/** copy values at node - response values - into vector Y */
for(k=0;k<numparents;k++){/** now build design matrix of explanatories other than intercept*/
gsl_matrix_set(designmatrix->datamatrix,i,k+1,obsdata->defn[i][gsl_vector_int_get(parentindexes,k)]);
gsl_matrix_set(designmatrix->datamatrix_noRV,i,k+1,obsdata->defn[i][gsl_vector_int_get(parentindexes,k)]);
} /** end of explanatories **/
} /** end of data point loop */
designmatrix->numparams=numparents+1;/** +1 for intercept - excludes precisions **/
/** now set the priormean and priorsd vector */
for(k=0;k<designmatrix->numparams;k++){/** num params does NOT include precision term **/
gsl_vector_set(designmatrix->priormean,k,priormean);
gsl_vector_set(designmatrix->priorsd,k,priorsd);
}
/** set prior for precision **/
gsl_vector_set(designmatrix->priorgamshape,0,priorgamshape);/** prior for precision term */
gsl_vector_set(designmatrix->priorgamscale,0,priorgamscale);/** prior for precision term */
gsl_vector_int_free(parentindexes);/** finished with this **/
/** ***********************************************************************************************************************/
/** ***********************************************************************************************************************/
/** DOWN HERE is splitting the single single design matrix and Y into separate chunks *************************************/
/** we now want to split designmatrix->datamatrix and designmatrix->Y into grouped blocks **/
/** ***********************************************************************************************************************/
/** get number of unique groups - equal to max int since using R factors **/
num_unq_grps=0;for(i=0;i<obsdata->numDataPts;i++){if(obsdata->groupIDs[i]>num_unq_grps){num_unq_grps=obsdata->groupIDs[i];}}
groupcnts=(int *)R_alloc(num_unq_grps,sizeof(int));/** will hold n_j's e.g. counts of how many obs in each group **/
curindex=(int *)R_alloc(num_unq_grps,sizeof(int));
for(i=0;i<num_unq_grps;i++){groupcnts[i]=0;curindex[i]=0;}
for(i=0;i<num_unq_grps;i++){/** for each unique group of data **/
for(j=0;j<obsdata->numDataPts;j++){/** for each observation **/
if( (obsdata->groupIDs[j]-1)==i){groupcnts[i]++;/** increment count **/}
}
}
/** create an array of gsl_matrix where each one is the design matrix for a single group of data **/
array_of_designs=(gsl_matrix **)R_alloc(num_unq_grps,sizeof(gsl_matrix*));/** a list of design matrix, one for each group */
array_of_Y=(gsl_vector **)R_alloc(num_unq_grps,sizeof(gsl_vector*)); /** a list of Y vectors,, one for each group */
for(i=0;i<num_unq_grps;i++){array_of_designs[i]=gsl_matrix_alloc(groupcnts[i],(designmatrix->datamatrix)->size2);
array_of_Y[i]=gsl_vector_alloc(groupcnts[i]);}
/** now loop through group j; for fixed j loop through each record in total design matrix and copying group members into new group design matrix **/
for(j=0;j<num_unq_grps;j++){/** for each group **/
for(i=0;i<obsdata->numDataPts;i++){/** for each data point **/
if( (obsdata->groupIDs[i]-1)==j){/** if current data point is for group j then store **/
for(k=0;k<(designmatrix->datamatrix)->size2;k++){/** for each member of the row in design matrix **/
gsl_matrix_set(array_of_designs[j],curindex[j],k,gsl_matrix_get(designmatrix->datamatrix,i,k));}
gsl_vector_set(array_of_Y[j],curindex[j],gsl_vector_get(designmatrix->Y,i));/** copy relevant Ys for fixed j */
curindex[j]++;
}
}
}
/** uncomment to print out array of design matrices - one for each data group **/
/* Rprintf("no cols=%d\n",array_of_designs[0]->size2);
for(j=0;j<num_unq_grps;j++){Rprintf("-------group %d------\n",j);
for(i=0;i<array_of_designs[j]->size1;i++){
Rprintf("Y=%f\t",gsl_vector_get(array_of_Y[j],i));
for(k=0;k<array_of_designs[j]->size2;k++){
Rprintf("%f ",gsl_matrix_get(array_of_designs[j],i,k));
}
Rprintf("\n");
}
}
*/
/** down to here we now have a split the design matrix and Y up into separate matrices and vectors, one for each observational group */
/** so we can free the previous datamatrix as this is not needed, also the previous Y **/
gsl_matrix_free(designmatrix->datamatrix);
/*gsl_vector_free(designmatrix->Y);*/ /** need to keep y*/
/** copy addresses */
designmatrix->numUnqGrps=num_unq_grps;
designmatrix->array_of_designs=array_of_designs;
designmatrix->array_of_Y=array_of_Y;
}
/*
Diese Funktion ordnet an Hand einer Fressmatrix von S Spezies diesen Spezies trophische Level und Massen zu.
Sie gibt eine 2x(Rnum+S) große Matrix zurück.
Die erste Zeile enthält die Masse der Rnum+S Spezies.
Dabei haben basale Spezies die Masse 0 und nicht basale Spezies erhalten ihren Nischenwert als Masse.
Die trophischen Level der Spezies werden in der zweiten Zeile der Matrix gespeichert. Basale Spezies haben TL 0.
*/
gsl_matrix *SetMasses(struct foodweb nicheweb, gsl_matrix* NV, gsl_matrix* A, double Rsize){ //Ohne x für Allometrie
//printf("SetMasses");
int S = nicheweb.S;
int Rnum = nicheweb.Rnum;
gsl_matrix* mas = gsl_matrix_calloc(2, Rnum + S); // nullte Zeile Masse, erste Zeile trophisches Level
//double x = nicheweb.x;
int check = 0;
int i, j = 0;
int tlgesucht = 1; // TL 0 darf es nicht geben, sonst keine Beute für diese Spezies
//--TL = 0: Ressource--------------------------------------------------------------------------------------------------------
for(i=0; i< Rnum; i++) gsl_matrix_set(mas, 1, i, 0); // Ressource hat TL = 0;
//--TL = 1 Spezies bestimmen (Link zur Ressource)-------------------------------------------------------------------------------
for(i=Rnum; i < S+Rnum; i++)
{
for(j=0; j<Rnum; j++)
{
if(gsl_matrix_get(A, i, j)!=0)
{
gsl_matrix_set(mas, 1, i, 1);
//printf("Spezies %i hat trophisches Level 1\n", i-Rnum);
}
}
}
//--TL>1 Spezies bestimmen--------------------------------------------------------------------------------
tlgesucht = 2;
while(tlgesucht <= S)
{
check = 0;
for(i=Rnum; i< (S+Rnum); i++)
{
if(gsl_matrix_get(mas, 1, i) == 0) //noch kein TL zugewiesen -> suche mögliche Beuten
{
for(j=Rnum; j< (S+Rnum); j++)
{
//printf("check=%i\n", check);
//printf("i=%i,\tj=%i\n", i, j);
//Spezies hat trophisches Level, das genau 1 kleiner ist und ist auch Beute laut A
if((gsl_matrix_get(mas, 1, j) == (tlgesucht-1)) && (gsl_matrix_get(A, i-Rnum, j-Rnum)!=0))
{
gsl_matrix_set(mas, 1, i, tlgesucht);
//printf("Spezies %i hat trophisches Level %i\n", i-Rnum, tlgesucht);
check++;
}
}
}
}
if(check == 0) // Keine Zuweisung in der letzten Runde, aufhören
{
tlgesucht = S+1;
}
else tlgesucht++;
} // Frage mit TL=5 als Maximalwert?
int TL0 = 0;
int TL1 = 0;
int TL2 = 0;
int TL3 = 0;
int TL4 = 0;
int TL5 = 0;
for(i=Rnum; i<S+Rnum; i++)
{
int TL = (int)gsl_matrix_get(mas, 1, i);
switch(TL){
case 0: TL0++;
break;
case 1: TL1++;
break;
case 2: TL2++;
break;
case 3: TL3++;
break;
case 4: TL4++;
break;
default: TL5++;
}
}
printf("Trophische Level-Verteilung: Ressourcen: %i TL1: %i, TL2: %i, TL3: %i, TL4: %i, TL>4: %i\n", TL0, TL1, TL2, TL3, TL4, TL5);
//--Massen eintragen (0 oder Nischenwert, Rsize bei Ressource)----------------------------------------------------------------------------------
gsl_matrix_set(mas, 0, 0, Rsize); // Nur für eine Ressource gültig! mas[0][0] Größe der Ressource
for(i = Rnum; i< Rnum+S; i++)
{
/*if(nicheweb.x == 0.0){
if(gsl_matrix_get(mas, 1, i) == 1) gsl_matrix_set(mas, 0, i, 0); // Basale Sp. Masse = 0
else if(gsl_matrix_get(mas, 1, i) > 1) gsl_matrix_set(mas, 0, i, gsl_matrix_get(NV, 0, i-Rnum)); // Sonst Masse = Nischenwert
}
else{*/
gsl_matrix_set(mas, 0, i, pow(10, -0.25*(nicheweb.x*4))); // Allometrie
//}
}
return mas;
}
int
powellsing_df (const gsl_vector * x, void *params, gsl_matrix * df)
{
double x0 = gsl_vector_get (x, 0);
double x1 = gsl_vector_get (x, 1);
double x2 = gsl_vector_get (x, 2);
double x3 = gsl_vector_get (x, 3);
double df00 = 1, df01 = 10, df02 = 0, df03 = 0;
double df10 = 0, df11 = 0, df12 = sqrt (5.0), df13 = -df12;
double df20 = 0, df21 = 2 * (x1 - 2 * x2), df22 = -2 * df21, df23 = 0;
double df30 = 2 * sqrt (10.0) * (x0 - x3), df31 = 0, df32 = 0, df33 = -df30;
gsl_matrix_set (df, 0, 0, df00);
gsl_matrix_set (df, 0, 1, df01);
gsl_matrix_set (df, 0, 2, df02);
gsl_matrix_set (df, 0, 3, df03);
gsl_matrix_set (df, 1, 0, df10);
gsl_matrix_set (df, 1, 1, df11);
gsl_matrix_set (df, 1, 2, df12);
gsl_matrix_set (df, 1, 3, df13);
gsl_matrix_set (df, 2, 0, df20);
gsl_matrix_set (df, 2, 1, df21);
gsl_matrix_set (df, 2, 2, df22);
gsl_matrix_set (df, 2, 3, df23);
gsl_matrix_set (df, 3, 0, df30);
gsl_matrix_set (df, 3, 1, df31);
gsl_matrix_set (df, 3, 2, df32);
gsl_matrix_set (df, 3, 3, df33);
params = 0; /* avoid warning about unused parameters */
return GSL_SUCCESS;
}
int
wood_df (const gsl_vector * x, void *params, gsl_matrix * df)
{
double x0 = gsl_vector_get (x, 0);
double x1 = gsl_vector_get (x, 1);
double x2 = gsl_vector_get (x, 2);
double x3 = gsl_vector_get (x, 3);
double t1 = x1 - 3 * x0 * x0;
double t2 = x3 - 3 * x2 * x2;
double df00 = -200.0 * t1 + 1, df01 = -200.0 * x0, df02 = 0, df03 = 0;
double df10 = -400.0*x0, df11 = 200.0 + 20.2, df12 = 0, df13 = 19.8;
double df20 = 0, df21 = 0, df22 = -180.0 * t2 + 1, df23 = -180.0 * x2;
double df30 = 0, df31 = 19.8, df32 = -2 * 180 * x2, df33 = 180.0 + 20.2;
gsl_matrix_set (df, 0, 0, df00);
gsl_matrix_set (df, 0, 1, df01);
gsl_matrix_set (df, 0, 2, df02);
gsl_matrix_set (df, 0, 3, df03);
gsl_matrix_set (df, 1, 0, df10);
gsl_matrix_set (df, 1, 1, df11);
gsl_matrix_set (df, 1, 2, df12);
gsl_matrix_set (df, 1, 3, df13);
gsl_matrix_set (df, 2, 0, df20);
gsl_matrix_set (df, 2, 1, df21);
gsl_matrix_set (df, 2, 2, df22);
gsl_matrix_set (df, 2, 3, df23);
gsl_matrix_set (df, 3, 0, df30);
gsl_matrix_set (df, 3, 1, df31);
gsl_matrix_set (df, 3, 2, df32);
gsl_matrix_set (df, 3, 3, df33);
params = 0; /* avoid warning about unused parameters */
return GSL_SUCCESS;
}
static void update_doppler(int in_np, int out_np, float *gr2sr, meta_parameters *meta)
{
double d1 = meta->sar->range_doppler_coefficients[1];
double d2 = meta->sar->range_doppler_coefficients[2];
// least squares fit
const int N=1000;
double chisq, xi[N], yi[N];
int ii;
for (ii=0; ii<N; ++ii) {
xi[ii] = (double)ii/(double)N * (double)(out_np-1);
// the gr2sr array maps a slant range index to a ground range index
double g = gr2sr[(int)xi[ii]];
yi[ii] = d1*g + d2*g*g;
}
gsl_matrix *X, *cov;
gsl_vector *y, *w, *c;
X = gsl_matrix_alloc(N,3);
y = gsl_vector_alloc(N);
w = gsl_vector_alloc(N);
c = gsl_vector_alloc(3);
cov = gsl_matrix_alloc(3, 3);
for (ii=0; ii<N; ++ii) {
gsl_matrix_set(X, ii, 0, 1.0);
gsl_matrix_set(X, ii, 1, xi[ii]);
gsl_matrix_set(X, ii, 2, xi[ii]*xi[ii]);
gsl_vector_set(y, ii, yi[ii]);
gsl_vector_set(w, ii, 1.0);
}
gsl_multifit_linear_workspace *work = gsl_multifit_linear_alloc(N, 3);
gsl_multifit_wlinear(X, w, y, c, cov, &chisq, work);
gsl_multifit_linear_free(work);
double c0 = gsl_vector_get(c, 0);
double c1 = gsl_vector_get(c, 1);
double c2 = gsl_vector_get(c, 2);
gsl_matrix_free(X);
gsl_vector_free(y);
gsl_vector_free(w);
gsl_vector_free(c);
gsl_matrix_free(cov);
// now the x and y vectors are the desired doppler polynomial
double ee2=0;
for (ii=0; ii<out_np; ii+=100) {
// ii: index in slant range, g: index in ground range
double g = gr2sr[ii];
// dop1: doppler in slant, dop2: doppler in ground (should agree)
double dop1 = d1*g + d2*g*g;
double dop3 = c0 + c1*ii + c2*ii*ii;
double e2 = fabs(dop1-dop3);
ee2 += e2;
if (ii % 1000 == 0)
printf("%5d -> %8.3f %8.3f %5d -> %5d %7.4f\n",
ii, dop1, dop3, (int)g, ii, e2);
}
printf("Original: %14.9f %14.9f %14.9f\n", 0., d1, d2);
printf("Modified: %14.9f %14.9f %14.9f\n\n", c0, c1, c2);
printf("Overall errors: %8.4f\n", ee2);
meta->sar->range_doppler_coefficients[0] += c0;
meta->sar->range_doppler_coefficients[1] = c1;
meta->sar->range_doppler_coefficients[2] = c2;
}
FouPack::FouPack(const QVector< double >* T, const QVector< double >* Y, int FouDegree, double FouShortestPeriod_h) :
MinPack(T, Y),
fouDegree(FouDegree),
fouShortestPeriod_h(FouShortestPeriod_h),
Pyy(0),
s(fouDegree+1, QVector<double>(t->size(), 1)),
c(fouDegree+1, QVector<double>(t->size(), 1)),
mat_LU (gsl_matrix_alloc (2*fouDegree+1, 2*fouDegree+1)),
per (gsl_permutation_alloc(2*fouDegree+1)),
Dy (gsl_vector_alloc (2*fouDegree+1))
{
gsl_vector *wy = gsl_vector_calloc (2*fouDegree+1); // set to 0
for (int tNr = 0; tNr < y->size(); tNr++) wy->data[0] += y->at(tNr);
gsl_matrix_set(mat_LU, 0, 0, t->size());
vector<double> sSum(2*fouDegree+1);
vector<double> cSum(2*fouDegree+1);
sSum[0] = 0;
cSum[0] = t->size();
for (int tNr = 0; tNr < t->size(); tNr++)
{
const double tPha = 2*M_PI * 24.0 * t->at(tNr) / fouDegree / fouShortestPeriod_h;
for (int deg = 1; deg <= fouDegree; deg++)
{
s[deg][tNr] = sin(deg*tPha);
c[deg][tNr] = cos(deg*tPha);
sSum[deg] += s[deg][tNr];
cSum[deg] += c[deg][tNr];
wy->data[deg] += y->at(tNr)*s[deg][tNr];
wy->data[deg+fouDegree] += y->at(tNr)*c[deg][tNr];
}
for (int deg = fouDegree+1; deg <= 2*fouDegree; deg++)
{
sSum[deg] += sin(deg*tPha);
cSum[deg] += cos(deg*tPha);
}
}
for (int deg = 1; deg <= fouDegree; deg++)
{
gsl_matrix_set(mat_LU, 0, deg, sSum[deg]);
gsl_matrix_set(mat_LU, 0, deg+fouDegree, cSum[deg]);
gsl_matrix_set(mat_LU, deg, 0, sSum[deg]);
gsl_matrix_set(mat_LU, deg+fouDegree, 0, cSum[deg]);
for (int deg2 = 1; deg2 < deg; deg2++)
{
gsl_matrix_set(mat_LU, deg, deg2, 0.5*(cSum[deg-deg2]-cSum[deg+deg2]));
gsl_matrix_set(mat_LU, deg+fouDegree, deg2, 0.5*(sSum[deg+deg2]-sSum[deg-deg2])); // sin changes sign
gsl_matrix_set(mat_LU, deg, deg2+fouDegree, 0.5*(sSum[deg+deg2]+sSum[deg-deg2])); // sin changes sign
gsl_matrix_set(mat_LU, deg+fouDegree, deg2+fouDegree, 0.5*(cSum[deg-deg2]+cSum[deg+deg2]));
}
for (int deg2 = deg; deg2 <= fouDegree; deg2++)
{
gsl_matrix_set(mat_LU, deg, deg2, 0.5*(cSum[deg2-deg]-cSum[deg2+deg]));
gsl_matrix_set(mat_LU, deg+fouDegree, deg2, 0.5*(sSum[deg2+deg]+sSum[deg2-deg])); // sin changes sign
gsl_matrix_set(mat_LU, deg, deg2+fouDegree, 0.5*(sSum[deg2+deg]-sSum[deg2-deg])); // sin changes sign
gsl_matrix_set(mat_LU, deg+fouDegree, deg2+fouDegree, 0.5*(cSum[deg2-deg]+cSum[deg2+deg]));
}
}
int sign;
gsl_linalg_LU_decomp (mat_LU, per, &sign);
gsl_linalg_LU_solve(mat_LU, per, wy, Dy);
const double NMyy = Dy->data[0] * wy->data[0];
double Psyys = 0, Pcyyc = 0;
for (int deg = 1; deg <= fouDegree; deg++)
{
Psyys += Dy->data[deg] * wy->data[deg];
Pcyyc += Dy->data[deg+fouDegree] * wy->data[deg+fouDegree];
}
Pyy = yy - NMyy - Psyys - Pcyyc;
gsl_vector_free(wy);
func.n = 2;
func.f = f;
func.df = df;
func.fdf = fdf;
func.params = reinterpret_cast<void*>(this);
}
void TGaussianMixture::expectation_maximization(const double *x, const double *w_n, unsigned int N, unsigned int iterations) {
double *p_kn = new double[nclusters*N];
// Determine total weight
double sum_w = 0.;
for(unsigned int n=0; n<N; n++) { sum_w += w_n[n]; }
// Choose means randomly from given points
unsigned int *index = new unsigned int[nclusters];
bool repeated_index;
for(unsigned int k=0; k<nclusters; k++) {
repeated_index = true;
while(repeated_index) {
index[k] = gsl_rng_uniform_int(r, N);
repeated_index = false;
for(unsigned int i=0; i<k; i++) {
if(index[i] == index[k]) { repeated_index = true; break; }
}
}
for(unsigned int i=0; i<ndim; i++) { mu[k*ndim + i] = x[index[k]*ndim + i]; }
}
delete[] index;
// Assign points to nearest cluster center
double sum, tmp, min_dist;
unsigned int nearest_cluster;
for(unsigned int n=0; n<N; n++) {
// Find nearest cluster center
min_dist = std::numeric_limits<double>::infinity();
for(unsigned int k=0; k<nclusters; k++) {
sum = 0.;
for(unsigned int i=0; i<ndim; i++) {
tmp = x[n*ndim + i] - mu[k*ndim + i];
sum += tmp*tmp;
}
if(sum < min_dist) {
min_dist = sum;
nearest_cluster = k;
}
}
for(unsigned int k=0; k<nclusters; k++) { p_kn[n*nclusters + k] = 0.; }
p_kn[n*nclusters + nearest_cluster] = 1.;
w[nearest_cluster] += w_n[n];
}
sum = 0.;
for(unsigned int k=0; k<nclusters; k++) { sum += w[k]; }
for(unsigned int k=0; k<nclusters; k++) { w[k] /= sum; }
// Iterate
for(unsigned int count=0; count<=iterations; count++) {
// Assign probability for each point to be in each cluster
if(count != 0) {
density(x, N, p_kn);
for(unsigned int n=0; n<N; n++) {
// Normalize probability for point to be in some cluster to unity
sum = 0.;
for(unsigned int k=0; k<nclusters; k++) { sum += p_kn[n*nclusters + k]; }
for(unsigned int k=0; k<nclusters; k++) { p_kn[n*nclusters + k] /= sum; }
}
}
// Determine cluster properties from members
if(count != 0) {
for(unsigned int k=0; k<nclusters; k++) { // Strength of Gaussian
w[k] = 0.;
for(unsigned int n=0; n<N; n++) {
w[k] += w_n[n] * p_kn[n*nclusters + k];
}
w[k] /= sum_w;
}
}
for(unsigned int k=0; k<nclusters; k++) { // Mean of Gaussian
for(unsigned int j=0; j<ndim; j++) {
mu[k*ndim + j] = 0.;
for(unsigned int n=0; n<N; n++) {
mu[k*ndim + j] += w_n[n] * p_kn[n*nclusters + k] * x[n*ndim + j] / w[k];
}
mu[k*ndim + j] /= sum_w;
}
}
for(unsigned int k=0; k<nclusters; k++) { // Covariance
for(unsigned int i=0; i<ndim; i++) {
for(unsigned int j=i; j<ndim; j++) {
sum = 0.;
tmp = 0.;
for(unsigned int n=0; n<N; n++) {
sum += p_kn[n*nclusters + k] * w_n[n] * (x[n*ndim + i] - mu[k*ndim + i]) * (x[n*ndim + j] - mu[k*ndim + j]);
tmp += w_n[n] * p_kn[n*nclusters + k];
}
sum /= tmp;
if(i == j) {
gsl_matrix_set(cov[k], i, j, 1.01*sum + 0.01);
} else {
gsl_matrix_set(cov[k], i, j, sum);
gsl_matrix_set(cov[k], j, i, sum);
}
}
}
}
invert_covariance();
/*std::cout << "Iteration #" << count << std::endl;
std::cout << "=======================================" << std::endl;
for(unsigned int k=0; k<nclusters; k++) {
std::cout << "Cluster #" << k+1 << std::endl;
std::cout << "w = " << w[k] << std::endl;
std::cout << "mu =";
for(unsigned int i=0; i<ndim; i++) {
std::cout << " " << mu[k*ndim + i];
}
std::cout << std::endl;
std::cout << "Covariance:" << std::endl;
for(unsigned int i=0; i<ndim; i++) {
for(unsigned int j=0; j<ndim; j++) {
std::cout << " " << gsl_matrix_get(cov[k], i, j);
}
std::cout << std::endl;
}
std::cout << std::endl;
}*/
//w_k = np.einsum('n,kn->k', w, p_kn) / N
//mu = np.einsum('k,n,kn,nj->kj', 1./w_k, w, p_kn, x) / N
//for j in xrange(k):
// Delta[j] = x - mu[j]
//cov = np.einsum('kn,n,kni,knj->kij', p_kn, w, Delta, Delta)
//cov = np.einsum('kij,k->kij', cov, 1./np.sum(p_kn, axis=1))
}
// Find the vector sqrt_cov s.t. sqrt_cov sqrt_cov^T = cov.
for(unsigned int k=0; k<nclusters; k++) {
sqrt_matrix(cov[k], sqrt_cov[k], esv, eival, eivec, sqrt_eival);
}
// Cleanup
delete[] p_kn;
}
/* Trains the network using the 'P' training samples provided
* Input of every sample element should have the correct dimension
* P should overcome the dimension of the input.
*
* NOTE: batch-like update mode is used
* WARNING: this routine is EXTREMELY SLOW & CPU BOUND
*
* PRE: net != NULL
* s != NULL
* length_of (s) == P
* length_of (s[k].in) == network input dimension k ∈ {1,..,P}
* P > (network # of branches) * (network input dimension + 1)
*
* POS: result == ANFIS_OK && net's parameters have been updated
* sample was not modified
* or
* result == ANFIS_ERR
*/
int
anfis_train (anfis_t net, t_sample *s, unsigned int P)
{
int res = ANFIS_OK;
long k = 0, i = 0;
size_t n = 0, t = 0, M = 0;
gsl_matrix *MF = NULL, /* Membership values for input */
*b_tau = NULL, /* Barred taus for input */
*A = NULL; /* Predictor variables for LSE */
gsl_vector *ccp = NULL; /* Copy of new consequent parameters */
double value = 0.0;
assert (net != NULL);
assert (s != NULL);
t = (size_t) net->t;
n = (size_t) net->n;
M = t * (n+1);
assert (P > M);
/* Membership values for all the P inputs */
MF = gsl_matrix_alloc (P, t*n);
handle_error_2 (MF);
/* Barred taus for all the P inputs */
b_tau = gsl_matrix_alloc (P, t);
handle_error_2 (b_tau);
/* Predictor variables matrix for LSE. See below 'A' layout */
A = gsl_matrix_alloc (P, M);
handle_error_2 (A);
/* Performing P partial propagations to compute A matrix */
for (k=0 ; k < P ; k++) {
gsl_vector_view MF_k = gsl_matrix_row (MF, k),
b_tau_k = gsl_matrix_row (b_tau, k);
/* Filling MF matrix k-th row */
res = anfis_compute_membership (net, s[k].in, &(MF_k.vector));
handle_error_1 (res);
/* Filling b_tau matrix k-th row */
res = anfis_partial_fwd_prop (net, s[k].in, &(MF_k.vector),
&(b_tau_k.vector));
handle_error_1 (res);
/* Filling A matrix k-th row (see below) */
#pragma omp parallel for default(shared) private(i,value)
for (i=0 ; i < M ; i++) {
value = gsl_matrix_get (b_tau, k, i/(n+1));
if (i % (n+1)) {
value *= gsl_vector_get (s[k].in, (i%(n+1))-1);
}
gsl_matrix_set (A, k, i, value);
}
}
/* Computing best consequent parameters with LSE */
ccp = anfis_lse (net, A, s, P);
if (ccp == NULL) {
res = ANFIS_ERR;
} else {
/* Updating premise parameters with gradient descent method */
res = anfis_grad_desc (net, s, A, MF, b_tau, ccp, P);
handle_error_1 (res);
}
gsl_matrix_free (A); A = NULL;
gsl_matrix_free (MF); MF = NULL;
gsl_matrix_free (b_tau); b_tau = NULL;
gsl_vector_free (ccp); ccp = NULL;
return res;
}
static void generate_a(const grid_t* g, gsl_matrix* a)
{
/* generate the coefficient matrix */
const unsigned int adim = g->dim - 2;
const unsigned int nrows = g->dim - 2;
const unsigned int ncols = g->dim - 2;
size_t index;
size_t i;
size_t j;
size_t k;
ij_pair_t ijp;
size_t nis[4];
gsl_matrix_set_zero(a);
/* border columns */
for (i = 1; i < (nrows - 1); ++i)
{
/* first col, north, east, south */
index = ij_to_index(adim, make_ij_pair(&ijp, i, 0));
gsl_matrix_set(a, index, index, 4.f);
get_neigh_indices(adim, index, nis);
gsl_matrix_set(a, index, nis[NEIGH_NORTH_SIDE], -1.f);
gsl_matrix_set(a, index, nis[NEIGH_EAST_SIDE], -1.f);
gsl_matrix_set(a, index, nis[NEIGH_SOUTH_SIDE], -1.f);
/* last col, north, west, south */
index = ij_to_index(adim, make_ij_pair(&ijp, i, ncols - 1));
gsl_matrix_set(a, index, index, 4.f);
get_neigh_indices(adim, index, nis);
gsl_matrix_set(a, index, nis[NEIGH_NORTH_SIDE], -1.f);
gsl_matrix_set(a, index, nis[NEIGH_WEST_SIDE], -1.f);
gsl_matrix_set(a, index, nis[NEIGH_SOUTH_SIDE], -1.f);
}
/* border rows */
for (j = 1; j < (ncols - 1); ++j)
{
/* first row, east, south, west */
index = ij_to_index(adim, make_ij_pair(&ijp, 0, j));
gsl_matrix_set(a, index, index, 4.f);
get_neigh_indices(adim, index, nis);
gsl_matrix_set(a, index, nis[NEIGH_EAST_SIDE], -1.f);
gsl_matrix_set(a, index, nis[NEIGH_SOUTH_SIDE], -1.f);
gsl_matrix_set(a, index, nis[NEIGH_WEST_SIDE], -1.f);
/* last row, north, east, west */
index = ij_to_index(adim, make_ij_pair(&ijp, nrows - 1, j));
gsl_matrix_set(a, index, index, 4.f);
get_neigh_indices(adim, index, nis);
gsl_matrix_set(a, index, nis[NEIGH_NORTH_SIDE], -1.f);
gsl_matrix_set(a, index, nis[NEIGH_EAST_SIDE], -1.f);
gsl_matrix_set(a, index, nis[NEIGH_WEST_SIDE], -1.f);
}
/* corner cases, north west first, clockwise */
index = ij_to_index(adim, make_ij_pair(&ijp, 0, 0));
get_neigh_indices(adim, index, nis);
gsl_matrix_set(a, index, index, 4.f);
gsl_matrix_set(a, index, nis[NEIGH_EAST_SIDE], -1.f);
gsl_matrix_set(a, index, nis[NEIGH_SOUTH_SIDE], -1.f);
index = ij_to_index(adim, make_ij_pair(&ijp, 0, ncols - 1));
get_neigh_indices(adim, index, nis);
gsl_matrix_set(a, index, index, 4.f);
gsl_matrix_set(a, index, nis[NEIGH_WEST_SIDE], -1.f);
gsl_matrix_set(a, index, nis[NEIGH_SOUTH_SIDE], -1.f);
index = ij_to_index(adim, make_ij_pair(&ijp, nrows - 1, ncols - 1));
get_neigh_indices(adim, index, nis);
gsl_matrix_set(a, index, index, 4.f);
gsl_matrix_set(a, index, nis[NEIGH_WEST_SIDE], -1.f);
gsl_matrix_set(a, index, nis[NEIGH_NORTH_SIDE], -1.f);
index = ij_to_index(adim, make_ij_pair(&ijp, nrows - 1, 0));
get_neigh_indices(adim, index, nis);
gsl_matrix_set(a, index, index, 4.f);
gsl_matrix_set(a, index, nis[NEIGH_EAST_SIDE], -1.f);
gsl_matrix_set(a, index, nis[NEIGH_NORTH_SIDE], -1.f);
/* inner points */
for (i = 1; i < nrows - 1; ++i)
{
for (j = 1; j < ncols - 1; ++j)
{
index = ij_to_index(adim, make_ij_pair(&ijp, i, j));
gsl_matrix_set(a, index, index, 4.f);
get_neigh_indices(adim, ij_to_index(adim, &ijp), nis);
for (k = 0; k < 4; ++k)
gsl_matrix_set(a, index, nis[k], -1.f);
}
}
}
gsl_matrix * bootstrap_and_calc_adj_matrix(
gsl_matrix * A,
size_t num_steps,
decimal threshold,
CoexpressionMeasure filter,
void (* interaction_matrix_generator_fnc)(gsl_matrix *, gsl_matrix *, CoexpressionMeasure),
bool use_tmp_file = false
)
{
size_t num_elems = A->size1;
//num_obs = A->size2;
gsl_matrix * interaction = gsl_matrix_calloc(num_elems, num_elems);
interaction_matrix_generator_fnc(A, interaction, filter);
// Bootstrapping
std::list<gsl_matrix *> boot_matrix;
{
detail::write_matrix_3d write_boot_matrix;
if (use_tmp_file)
write_boot_matrix.set_filename("tmp_file.out");
boost::mt19937 rng(time(NULL) - num_steps - int(threshold * 100));
for (size_t i = 0; i < num_steps; ++i)
{
std::cout << "step = " << i << std::endl; // DEBUG
bootstrap_and_calc_adj_matrix_helper(A, boot_matrix, write_boot_matrix, filter, rng, interaction_matrix_generator_fnc, use_tmp_file);
}
}
// Compute adjacency matrix
detail::read_matrix_3d read_boot_matrix(num_elems, num_elems, num_steps);
if (use_tmp_file)
read_boot_matrix.set_filename("tmp_file.out");
gsl_matrix * adj_matrix = gsl_matrix_alloc(num_elems, num_elems);
for (size_t r = 0; r < num_elems; ++r)
{
for (size_t c = 0; c < num_elems; ++c)
{
double value = gsl_matrix_get(interaction, r, c);
gsl_vector * boot_z_vec = gsl_vector_calloc(num_steps);
if (use_tmp_file)
{
read_boot_matrix.read(r, c, boot_z_vec);
}
else
{
size_t i = 0;
for (std::list<gsl_matrix *>::iterator li = boot_matrix.begin(); li != boot_matrix.end(); ++li, ++i)
gsl_vector_set(boot_z_vec, i, gsl_matrix_get(*li, r, c));
}
double mean = gsl_stats_mean(boot_z_vec->data, boot_z_vec->stride, num_steps),
sd = gsl_stats_sd(boot_z_vec->data, boot_z_vec->stride, num_steps);
// DEBUG
std::string filename = "boot_" + to_string(r) + "_" + to_string(c) + ".out";
FILE * outfile = fopen(filename.c_str(), "wb");
gsl_vector_fwrite(outfile, boot_z_vec);
fclose(outfile);
// END DEBUG
if (value < (mean - threshold * sd) || value > (mean + threshold * sd))
gsl_matrix_set(adj_matrix, r, c, value);
else
gsl_matrix_set(adj_matrix, r, c, 0.0);
gsl_vector_free(boot_z_vec);
}
}
// Free interaction matrices
gsl_matrix_free(interaction);
if (!use_tmp_file)
for (std::list<gsl_matrix *>::iterator li = boot_matrix.begin(); li != boot_matrix.end(); ++li)
gsl_matrix_free(*li);
return adj_matrix;
}
SEXP rint_flmm(SEXP pexplan_sexp, SEXP presp_sexp, SEXP pn_sexp, SEXP pp_sexp, SEXP pcovar_sexp, SEXP pp_covar_sexp, SEXP pVar2_sexp, SEXP nu_naught_sexp, SEXP gamma_naught_sexp)
{
double *pexplan, *presp, *pnu_naught, *pgamma_naught, *pcovar, *pVar2;
double* pchisq;
double* pherit;
unsigned int *pn, *pp_covar, *pp;
char pret_names[][100]={"coefs", "chi.sq", "herit", "null.herit"};
SEXP preturn_list_SEXP, preturn_names_SEXP, paname_SEXP;
SEXP pchisq_SEXP;
SEXP pherit_SEXP;
SEXP pbeta_SEXP;
SEXP pnullherit_SEXP;
gsl_matrix* pvar1_mat, *pvar2_mat;
gsl_matrix* pcovar_mat;
gsl_vector* presponse_vec;
double* pnullherit;
double* pbeta;
// really must check all gsl returns
gsl_set_error_handler_off();
// C side pointers to R objects
pexplan=(double*) REAL(pexplan_sexp);
presp=(double*) REAL(presp_sexp);
pn=(unsigned int*) INTEGER(pn_sexp);
pp=(unsigned int*) INTEGER(pp_sexp);
pcovar=(double*) REAL(pcovar_sexp);
pp_covar=(unsigned int*) INTEGER(pp_covar_sexp);
pVar2=(double*) REAL(pVar2_sexp);
pnu_naught=(double*) REAL(nu_naught_sexp);
pgamma_naught=(double*) REAL(gamma_naught_sexp);
/* gsl_vector_view response_vecview=gsl_vector_view_array(presp, *pn);
presponse_vec=&(response_vecview.vector);*/
gsl_matrix_view var2_matview=gsl_matrix_view_array(pVar2, *pn, *pn);
pvar2_mat=&(var2_matview.matrix);
// freed
pvar1_mat=gsl_matrix_alloc(*pn, *pn);
/* gsl_matrix_view covar_matview=gsl_matrix_view_array(pcovar, *pn, *pp_covar);
pcovar_mat=&(covar_matview.matrix);*/
// sort out missing in response
// better to bulk copy then iterate?
unsigned int it;
unsigned int nonzerocount=0;
// freed
presponse_vec=gsl_vector_alloc(*pn);
double meanval=0.0;
for(it=0;it<*pn;it++)
{
if(!ISNA(presp[it]))
{
meanval+=presp[it];
nonzerocount+=1;
}
}
meanval/=(double) nonzerocount;
for(it=0;it<*pn;it++)
{
if(ISNA(presp[it]))
gsl_vector_set(presponse_vec, it, meanval);
else
gsl_vector_set(presponse_vec, it, presp[it]);
}
// freed
pcovar_mat=gsl_matrix_alloc( *pn, *pp_covar);
unsigned it2;
for(it2=0;it2<*pp_covar;it2++)
{
meanval=0.0;
nonzerocount=0;
for(it=0;it<*pn;it++)
{
if(!ISNA(pcovar[it*(*pp_covar)+it2]))
{
meanval+=pcovar[it*(*pp_covar)+it2];
nonzerocount+=1;
}
}
meanval/=(double) nonzerocount;
for(it=0;it<*pn;it++)
{
if(ISNA(pcovar[it*(*pp_covar)+it2]))
gsl_matrix_set(pcovar_mat, it, it2, meanval);
else
gsl_matrix_set(pcovar_mat, it, it2, pcovar[it*(*pp_covar)+it2]);
}
}
/*std::cout<<"cov="<<pcovar[0]<<","<<pcovar[1]<<","<<pcovar[2]<<std::endl;
std::cout<<"pcovar_mat";
gslprint(pcovar_mat);*/
gsl_matrix* pincid1_mat, *pincid2_mat;
// freed
pincid1_mat=gsl_matrix_alloc(*pn,*pn);
//freed
pincid2_mat=gsl_matrix_alloc(*pn,*pn);
gsl_matrix_set_identity(pvar1_mat);
// gsl_matrix_set_identity(pvar2_mat);
gsl_matrix_set_identity(pincid1_mat);
gsl_matrix_set_identity(pincid2_mat);
PROTECT(pbeta_SEXP=NEW_NUMERIC(*pp));
pbeta=NUMERIC_POINTER(pbeta_SEXP);
PROTECT(pchisq_SEXP=NEW_NUMERIC(*pp));
pchisq=NUMERIC_POINTER(pchisq_SEXP);
PROTECT(pherit_SEXP=NEW_NUMERIC(*pp));
pherit=NUMERIC_POINTER(pherit_SEXP);
PROTECT(pnullherit_SEXP=NEW_NUMERIC(1));
pnullherit=NUMERIC_POINTER(pnullherit_SEXP);
TwoVarCompModel DaddyTwoVarCompModel(presponse_vec, pcovar_mat, pvar1_mat, pvar2_mat, pincid1_mat, pincid2_mat, pnu_naught, pgamma_naught);
double nullminimand=0.5;
double altminimand;
double nulldev=DaddyTwoVarCompModel.MinimiseNullDeviance(&nullminimand);
*pnullherit=nullminimand;
/*std::cout<<"si==0.2"<<std::endl<<DaddyTwoVarCompModel.NullDeviance(0.2)<<std::endl;
std::cout<<"si==0.4"<<std::endl<<DaddyTwoVarCompModel.NullDeviance(0.4)<<std::endl;
std::cout<<"si==0.6"<<std::endl<<DaddyTwoVarCompModel.NullDeviance(0.6)<<std::endl;
std::cout<<"si==0.8"<<std::endl<<DaddyTwoVarCompModel.NullDeviance(0.8)<<std::endl;
*/
pgsl_vector* ppexplantemp_vec = new pgsl_vector[OMP_GET_MAX_THREADS];
pgsl_vector* ppbeta_vec=new pgsl_vector[OMP_GET_MAX_THREADS];
for(it=0;it<OMP_GET_MAX_THREADS;it++)
{
ppexplantemp_vec[it]=gsl_vector_alloc(*pn);
ppbeta_vec[it]=gsl_vector_alloc(1);
}
#pragma omp parallel for shared(pexplan, pp, pn, pchisq, pherit, nulldev, nullminimand, ppexplantemp_vec, pbeta, ppbeta_vec) private(altminimand, it2, meanval, nonzerocount)
for(it=0;it<*pp;it++)
{
TwoVarCompModel ChildTwoVarCompModel(DaddyTwoVarCompModel);
// std::cout<<".";
meanval=0.0;
nonzerocount=0;
for(it2=0;it2<*pn;it2++)
{
if(!ISNA(pexplan[it2+(*pn)*it]))
{
meanval+=pexplan[it2+(*pn)*it];
nonzerocount+=1;
}
}
meanval/=(double) nonzerocount;
for(it2=0;it2<*pn;it2++)
{
if(ISNA(pexplan[it2+(*pn)*it]))
gsl_vector_set(ppexplantemp_vec[OMP_GET_THREAD_NUM], it2, meanval);
else
gsl_vector_set(ppexplantemp_vec[OMP_GET_THREAD_NUM], it2, pexplan[it2+(*pn)*it]);
}
ChildTwoVarCompModel.SetExplan(ppexplantemp_vec[OMP_GET_THREAD_NUM]);
altminimand=nullminimand;
pchisq[it]=nulldev-ChildTwoVarCompModel.MinimiseDeviance(&altminimand);
pherit[it]=altminimand;
ChildTwoVarCompModel.GetBeta(ppbeta_vec[OMP_GET_THREAD_NUM], altminimand);
pbeta[it]=gsl_vector_get(ppbeta_vec[OMP_GET_THREAD_NUM], 0);
}
for(it=0;it<OMP_GET_MAX_THREADS;it++)
{
gsl_vector_free(ppexplantemp_vec[it]);
gsl_vector_free(ppbeta_vec[it]);
}
delete[] ppexplantemp_vec;
delete[] ppbeta_vec;
gsl_matrix_free(pvar1_mat);
gsl_vector_free(presponse_vec);
gsl_matrix_free(pcovar_mat);
gsl_matrix_free(pincid1_mat);
gsl_matrix_free(pincid2_mat);
PROTECT(preturn_list_SEXP=allocVector(VECSXP,4));
SET_VECTOR_ELT(preturn_list_SEXP, 0,pbeta_SEXP);
SET_VECTOR_ELT(preturn_list_SEXP, 1,pchisq_SEXP);
SET_VECTOR_ELT(preturn_list_SEXP, 2,pherit_SEXP);
SET_VECTOR_ELT(preturn_list_SEXP, 3,pnullherit_SEXP);
PROTECT(preturn_names_SEXP=allocVector(STRSXP,4));
for(int it=0;it<4;it++)
{
PROTECT(paname_SEXP=Rf_mkChar(pret_names[it]));
SET_STRING_ELT(preturn_names_SEXP,it,paname_SEXP);
}
setAttrib(preturn_list_SEXP, R_NamesSymbol,preturn_names_SEXP);
UNPROTECT(10);
return preturn_list_SEXP;
}
//INICIO DE LA FUNCION JACOBIANA
int pv_df (const gsl_vector * x, void *data, gsl_matrix * J)
{
//parametros fijos del fiteo (salen de los datos experimentales así como del para_fit2d.dat)
int n = ((struct data *)data) -> n;
int numrings = ((struct data *)data) -> numrings;
gsl_vector * ttheta = ((struct data *)data) -> ttheta;
//gsl_vector * y = ((struct data *)data) -> y;
//gsl_vector * sigma = ((struct data *) data) -> sigma;
gsl_matrix * bg_pos = ((struct data *) data) -> bg_pos;
int i, j = 0, k = 0;
//parametros del fiteo (para el programa representan las variables independientes)
double H;
double eta;
double I0[numrings];
double t0[numrings];
double shift_H[numrings];
double shift_eta[numrings];
//double bg_int[numrings][2];
double H_i[numrings];
double eta_i[numrings];
//inicializo los parametros
H = gsl_vector_get (x, j); j++;
eta = gsl_vector_get (x, j); j++;
for(i = 0; i < numrings; i++)
{
I0[i] = gsl_vector_get(x, j); j++;
t0[i] = gsl_vector_get(x, j); j++;
shift_H[i] = gsl_vector_get(x, j); j++;
shift_eta[i] = gsl_vector_get(x, j); j++;
H_i[i] = H + shift_H[i];
eta_i[i] = eta + shift_eta[i];
j++;
j++;
//bg_int[i][0] = gsl_vector_get(x, j); j++;
//bg_int[i][1] = gsl_vector_get(x, j); j++;
}
//evaluo el jacobiano
for (i = 0; i < n; i++)
{//recorro todos los puntos experimentales (en 2\theta)
/* Jacobian matrix J(i,j) = dfi / dxj, */
/* where fi = (Yi - yi)/sigma[i], */
/* Yi = pseudo_voigt */
/* and the xj are the parameters (I0, t0, H, eta, shift_H, shift_eta, Bg_Int) */
k = 0; //reinicio el indice de la columna del jacobiano
//double s = gsl_vector_get(sigma, i);
double s = S;
//Derivadas respecto a los parámetros globales
//Derivada respecto de H
double dH = dpv_dH(numrings, I0, gsl_vector_get(ttheta, i), t0, eta_i, H_i);
gsl_matrix_set (J, i, k, dH/s); k++;
//Derivada respecto de eta
double deta = dpv_deta(numrings, I0, gsl_vector_get(ttheta, i), t0, H_i);
gsl_matrix_set (J, i, k, deta/s); k++;
//Derivada respecto a los parametros de los picos
double dI0[numrings], dt0[numrings], dshift_H[numrings], dshift_eta[numrings], dbg[numrings][2];
for(j = 0; j < numrings; j++)
{
//Derivada respecto a la Intensidad máxima
dI0[j] = dpv_dI0(gsl_vector_get(ttheta, i), t0[j], eta_i[j], H_i[j]);
gsl_matrix_set (J, i, k, dI0[j] / s); k++;
//Derivada respecto a la posición del centro
dt0[j] = dpv_dt0 (I0[j], gsl_vector_get(ttheta, i), t0[j], eta_i[j], H_i[j]);
gsl_matrix_set (J, i, k, dt0[j] / s); k++;
//Derivada respecto al corrimiento en el ancho de pico
dshift_H[j] = dpv_dshift_H(I0[j], gsl_vector_get(ttheta, i), t0[j], eta_i[j], H_i[j]);
gsl_matrix_set (J, i, k, dshift_H[j] / s); k++;
//Derivada respecto al corrimiento del eta
dshift_eta[j] = dpv_dshift_eta(I0[j], gsl_vector_get(ttheta, i), t0[j], H_i[j]);
gsl_matrix_set (J, i, k, dshift_eta[j] / s); k++;
//Derivada respecto a la intensidad del background a los lados de cada pico
dbg[j][0] = dpv_dbg_left(numrings, gsl_vector_get(ttheta, i), bg_pos);
gsl_matrix_set (J, i, k, dbg[j][0] / s); k++;
dbg[j][1] = dpv_dbg_right(numrings, gsl_vector_get(ttheta, i), bg_pos);
gsl_matrix_set (J, i, k, dbg[j][1] / s); k++;
}
}
return GSL_SUCCESS;
}
int
main()
{
const size_t n = N;
const size_t p = 2;
size_t i;
gsl_rng *r = gsl_rng_alloc(gsl_rng_default);
gsl_matrix *X = gsl_matrix_alloc(n, p);
gsl_vector *y = gsl_vector_alloc(n);
for (i = 0; i < n; ++i)
{
/* generate first random variable u */
double ui = gsl_ran_gaussian(r, 1.0);
/* set v = u + noise */
double vi = ui + gsl_ran_gaussian(r, 0.001);
/* set y = u + v + noise */
double yi = ui + vi + gsl_ran_gaussian(r, 1.0);
/* since u =~ v, the matrix X is ill-conditioned */
gsl_matrix_set(X, i, 0, ui);
gsl_matrix_set(X, i, 1, vi);
/* rhs vector */
gsl_vector_set(y, i, yi);
}
{
gsl_multifit_linear_workspace *w =
gsl_multifit_linear_alloc(n, p);
gsl_vector *c = gsl_vector_alloc(p);
gsl_vector *c_ridge = gsl_vector_alloc(p);
gsl_matrix *cov = gsl_matrix_alloc(p, p);
double chisq;
/* unregularized (standard) least squares fit, lambda = 0 */
gsl_multifit_linear_ridge(0.0, X, y, c, cov, &chisq, w);
fprintf(stderr, "=== Unregularized fit ===\n");
fprintf(stderr, "best fit: y = %g u + %g v\n",
gsl_vector_get(c, 0), gsl_vector_get(c, 1));
fprintf(stderr, "chisq/dof = %g\n", chisq / (n - p));
/* regularize with lambda = 1 */
gsl_multifit_linear_ridge(1.0, X, y, c_ridge, cov, &chisq, w);
fprintf(stderr, "=== Regularized fit ===\n");
fprintf(stderr, "best fit: y = %g u + %g v\n",
gsl_vector_get(c_ridge, 0), gsl_vector_get(c_ridge, 1));
fprintf(stderr, "chisq/dof = %g\n", chisq / (n - p));
gsl_multifit_linear_free(w);
gsl_matrix_free(cov);
gsl_vector_free(c);
gsl_vector_free(c_ridge);
}
gsl_rng_free(r);
gsl_matrix_free(X);
gsl_vector_free(y);
return 0;
}
void matrix<double>::assign(const size_t& i, const size_t& j, const double& a)
{
gsl_matrix_set(_matrix, i, j, a);
}
void Cornea::createJacobian(const gsl_vector *gx,
gsl_matrix *J) const { // x x y Jacobian matrix
const double RHO = trackerSettings.RHO;
int ind_J = 0;
const int nLEDs = data.size();
const size_t rows = 3*pairsOfTwo(nLEDs);
// zero the Jacobian matrix
for(size_t row = 0; row < rows; ++row) {
for(size_t col = 0; col < nLEDs; ++col) {
gsl_matrix_set(J, row, col, 0.0);
}
}
for(size_t x = 0; x < nLEDs - 1; ++x) {
const DATA_FOR_CORNEA_COMPUTATION &data1 = data[x];
const double gx_guess1 = gsl_vector_get(gx, x);
const double tan_a1 = tan(data1.alpha_aux);
const double a1 = gx_guess1 * tan_a1 / (data1.l_aux - gx_guess1);
const double atan_res1 = atan2(gx_guess1 * tan_a1, data1.l_aux - gx_guess1);
double dA_plus_dB[3];
for(int i = 0; i < 3; ++i) {
/*******************************************************
* Get the gxi component
*******************************************************/
const double rx0 = data1.R(i, 0);
// d/dgxi A(gxi)
const double dA =
rx0 + rx0 * RHO * cos((data1.alpha_aux - atan_res1) / 2.) *
(tan_a1 * (1.0 / (data1.l_aux - gx_guess1)) +
(1.0 / POW2(data1.l_aux - gx_guess1)) * gx_guess1 * tan_a1) / (2. * (1. + POW2(a1)));
const double rx1 = data1.R(i, 2);
// d/dgxi B(gxi)
const double dB =
rx1 * tan_a1 + rx1 * RHO * sin((data1.alpha_aux - atan_res1) / 2.) *
(tan_a1 * (1.0 / (data1.l_aux - gx_guess1)) +
(1.0 / POW2(data1.l_aux - gx_guess1)) * gx_guess1 * tan_a1) / (2. * (1. + POW2(a1)));
dA_plus_dB[i] = dA + dB;
}
for(size_t y = x + 1; y < nLEDs; ++y) {
const DATA_FOR_CORNEA_COMPUTATION &data2 = data[y];
const double gx_guess2 = gsl_vector_get(gx, y);
const double tan_a2 = tan(data2.alpha_aux);
const double a2 = gx_guess2 * tan_a2 / (data2.l_aux - gx_guess2);
const double atan_res2 = atan2(gx_guess2 * tan_a2, data2.l_aux - gx_guess2);
for(int i = 0; i < 3; ++i) {
/******************************************************************************************
* Get the gxj component
******************************************************************************************/
const double rx0 = data2.R(i, 0);
// d/dgxj C(gxj)
const double dC =
rx0 + rx0 * RHO * cos((data2.alpha_aux - atan_res2) / 2.) *
(tan_a2 * (1.0 / (data2.l_aux - gx_guess2)) +
(1.0 / POW2(data2.l_aux - gx_guess2)) * gx_guess2 * tan_a2) / (2. * (1. + POW2(a2)));
const double rx1 = data2.R(i, 2);
// d/dgxj D(gxj)
const double dD =
rx1 * tan_a2 + rx1 * RHO * sin((data2.alpha_aux - atan_res2) / 2.) *
(tan_a2 * (1.0 / (data2.l_aux - gx_guess2)) +
(1.0 / POW2(data2.l_aux - gx_guess2)) * gx_guess2 * tan_a2) / (2. * (1. + POW2(a2)));
/*
* Set only 2 values per row, since we consider LED pairs,
* i.e. no other LED affects the jacobian in this row than
* one of the the LEDs in the pair in question.
*/
gsl_matrix_set(J, ind_J, x, dA_plus_dB[i]);
gsl_matrix_set(J, ind_J, y, -dC - dD);
++ind_J;
}
}
}
}
bool LanePiece::interpretHeadingCurvature()
{
// Recalculate theta, k and dk
if(this->numOfPoints < NUM_FIT_FRONT + NUM_FIT_BACK + 1) {
int i = 0;
for( i = 0; i < this->numOfPoints; i++) {
if(i == this->numOfPoints - 1) {
this->theta[i] = this->theta[i-1];
this->k[i] = 0;
break;
}
this->theta[i] = atan2(this->y[i+1] - this->y[i], this->x[i+1] - this->x[i]);
this->k[i] = 0;
}
}
else {
for(int i = 0; i < this->numOfPoints; i++) {
int id0 = (i-NUM_FIT_FRONT >= 0) ? (i-NUM_FIT_FRONT):(0);
int id1 = (i+NUM_FIT_BACK < this->numOfPoints) ? (i+NUM_FIT_BACK):(this->numOfPoints-1);
double si, xi, yi, /*ei,*/ chisq;
gsl_matrix *S, *cov;
gsl_vector *x, *y, *w, *cx, *cy;
int n = id1-id0+1;
S = gsl_matrix_alloc (n, 3);
x = gsl_vector_alloc (n);
y = gsl_vector_alloc (n);
w = gsl_vector_alloc (n);
cx = gsl_vector_alloc (3);
cy = gsl_vector_alloc (3);
cov = gsl_matrix_alloc (3, 3);
for (int j = 0; j < n; j++)
{
si = this->s[id0+j];
xi = this->x[id0+j];
yi = this->y[id0+j];
gsl_matrix_set (S, j, 0, 1.0);
gsl_matrix_set (S, j, 1, si);
gsl_matrix_set (S, j, 2, si*si);
gsl_vector_set (x, j, xi);
gsl_vector_set (y, j, yi);
gsl_vector_set (w, j, 1.0);
}
gsl_multifit_linear_workspace * work = gsl_multifit_linear_alloc (n, 3);
gsl_multifit_wlinear (S, w, x, cx, cov, &chisq, work);
gsl_multifit_linear_free (work);
work = gsl_multifit_linear_alloc (n, 3);
gsl_multifit_wlinear (S, w, y, cy, cov, &chisq, work);
gsl_multifit_linear_free (work);
#define Cx(i) (gsl_vector_get(cx,(i)))
#define Cy(i) (gsl_vector_get(cy,(i)))
double s_ = this->s[i];
// double x_ = Cx(2) * s_ * s_ + Cx(1) * s_ + Cx(0);
double xd_ = 2 * Cx(2) * s_ + Cx(1);
double xdd_ = 2 * Cx(2);
// double y_ = Cy(2) * s_ * s_ + Cy(1) * s_ + Cy(0);
double yd_ = 2 * Cy(2) * s_ + Cy(1);
double ydd_ = 2 * Cy(2);
this->theta[i] = atan2(yd_, xd_);
this->k[i] = (xd_ * ydd_ - yd_ * xdd_)
/ ( sqrt( (xd_*xd_ + yd_*yd_)*(xd_*xd_ + yd_*yd_)*(xd_*xd_ + yd_*yd_) ) );
gsl_matrix_free (S);
gsl_vector_free (x);
gsl_vector_free (y);
gsl_vector_free (w);
gsl_vector_free (cx);
gsl_vector_free (cy);
gsl_matrix_free (cov);
}
}
return true;
}
void do_mytest() //double X[PROBDIM], int N, int IORD)
{
double GRAD[data.Nth]; // 1st derivatives
double sGRAD[data.Nth]; // 1st derivatives
double HES[data.Nth][data.Nth]; // hessian
double sHES[data.Nth][data.Nth]; // hessian
double X[data.Nth];
int i, j;
double t1, t2;
const int reps = 1; //200;
double *theta = data.theta;
double *XL = data.lowerbound;
double *XU = data.upperbound;
double *UH = data.diffstep;
int N = data.Nth;
int IORD = data.iord;
double FEPS = 1e-6;
int IPRINT = 0;
int NOC;
int IERR;
for (i = 0; i < data.Nth; i++) X[i] = theta[i];
reset_nfc();
#if 1 // GRADIENT
printf("\n================================================\n");
t1 = torc_gettime();
{
for (i = 0; i < data.Nth; i++) sGRAD[i] = 0.0;
int t;
for (t = 0; t < reps; t++) {
if (t > 0) {
double c = 1e-5;
double delta;
if (drand48() < 0.5) delta = -1; else delta = 1;
for (i = 0; i < data.Nth; i++) X[i] = theta[i] + c*delta;
}
c_pndlga(F,X,&N,XL,XU,UH,&FEPS,&IORD,&IPRINT,GRAD,&NOC,&IERR);
//printf("gradient -> %d\n", t);
for (i = 0; i < data.Nth; i++) sGRAD[i] += GRAD[i];
}
for (i = 0; i < data.Nth; i++) GRAD[i] = sGRAD[i]/reps;
}
t2 = torc_gettime();
printf("t2-t1 = %lf seconds\n", t2-t1);
#if 1 //VERBOSE
printf("IORD = %d\n", IORD);
printf("NOC = %d\n", NOC);
printf("GRADIENT VECTOR :\n");
for (i = 0; i < N; i++) {
printf("%12.4lf ", GRAD[i]);
}
printf("\n"); fflush(0);
#endif
#endif
#if 1 // HESSIAN WITH FUNCTION CALLS
if (IORD == 4) IORD = 2;
printf("\n================================================\n");
t1 = torc_gettime();
{
for (i = 0; i < N; i++)
for (j = 0; j < N; j++)
sHES[i][j] = 0.0;
int t;
for (t = 0; t < reps; t++) {
if (t > 0) {
double c = 1e-5;
double delta;
if (drand48() < 0.5) delta = -1; else delta = 1;
for (i = 0; i < data.Nth; i++) X[i] = theta[i] + c*delta;
}
c_pndlhfa(F,X,&N,XL,XU,UH,&FEPS,&IORD,&IPRINT,(double *)HES,&N,&NOC,&IERR);
//printf("hessian -> %d\n", t);
for (i = 0; i < N; i++)
for (j = i+1; j < N; j++)
HES[j][i] = HES[i][j];
for (i = 0; i < N; i++)
for (j = 0; j < N; j++)
sHES[i][j] += HES[i][j];
}
for (i = 0; i < N; i++)
for (j = 0; j < N; j++)
HES[j][i] = sHES[i][j]/reps;
}
t2 = torc_gettime();
printf("t2-t1 = %lf seconds\n", t2-t1);
#if VERBOSE
printf("IORD = %d\n", IORD);
printf("NOC = %d\n", NOC);
printf("HESSIAN MATRIX :\n");
for (i = 0; i < N; i++) {
for (j = 0; j < N; j++)
printf("%12.4lf ", HES[i][j]);
printf("\n");
}
printf("\n"); fflush(0);
#endif
#endif
get_nfc();
printf("total function calls = %d\n", get_tfc());
printf("GRADIENT VECTOR :\n");
for (i = 0; i < N; i++) {
printf("%15.8lf ", GRAD[i]);
}
printf("\n"); fflush(0);
printf("HESSIAN MATRIX :\n");
for (i = 0; i < N; i++) {
for (j = 0; j < N; j++)
printf("%15.8lf ", HES[i][j]);
printf("\n");
}
printf("\n"); fflush(0);
gsl_matrix *hessian_mat = gsl_matrix_alloc(data.Nth, data.Nth);
for(i=0; i<data.Nth; i++){
for(j=0; j<data.Nth; j++){
gsl_matrix_set(hessian_mat, i, j, HES[i][j]);
}
}
eigs(hessian_mat, data.Nth);
if (data.posdef == -1) return; // -1 or 0 to 3
int res = check_mat_pos_def(hessian_mat, data.Nth);
if (res == 0) {
int m;
//for (m = 0; m <=3; m++) {
for (m = data.posdef; m <=data.posdef; m++) {
printf(">>> METHOD %d <<<<\n", m);
for(i=0; i<data.Nth; i++){
for(j=0; j<data.Nth; j++){
gsl_matrix_set(hessian_mat, i, j, HES[i][j]);
}
}
gsl_matrix *hessian_mat2 = gsl_matrix_alloc(data.Nth, data.Nth);
force_pos_def(hessian_mat, hessian_mat2, m, data.Nth);
//gsl_matrix_fprintf(stdout, hessian_mat2, "%lf");
printf("mat2 = \n");
for(i=0; i<data.Nth; i++){
for(j=0; j<data.Nth; j++){
printf("%15.8lf ", gsl_matrix_get(hessian_mat2, i, j));
}
printf("\n");
}
eigs(hessian_mat2, data.Nth);
//int res = check_mat_pos_def(hessian_mat2);
}
}
}
/**
* Add signal s_mu = M_mu_nu A^nu within the given transient-window
* to given atoms.
*
* RETURN: SNR^2 of the injected signal
* and the effective AntennaPatternMatrix M_mu_nu for this signal.
*/
REAL8
XLALAddSignalToFstatAtomVector ( FstatAtomVector* atoms, /**< [in/out] atoms vectors containing antenna-functions and possibly noise {Fa,Fb} */
AntennaPatternMatrix *M_mu_nu, /**< [out] effective antenna-pattern matrix for the injected signal */
const PulsarAmplitudeVect A_Mu, /**< [in] input canonical amplitude vector A^mu = {A1,A2,A3,A4} */
transientWindow_t transientWindow /**< transient signal window */
)
{
int gslstat;
/* check input consistency */
if ( !atoms || !atoms->data ) {
XLALPrintError ( "%s: Invalid NULL input 'atoms'\n", __func__ );
XLAL_ERROR_REAL8 ( XLAL_EINVAL );
}
if ( !M_mu_nu ) {
XLALPrintError ( "%s: Invalid NULL input 'M_mu_nu'\n", __func__ );
XLAL_ERROR_REAL8 ( XLAL_EINVAL );
}
/* prepare transient-window support */
UINT4 t0, t1;
if ( XLALGetTransientWindowTimespan ( &t0, &t1, transientWindow ) != XLAL_SUCCESS ) {
XLALPrintError ("%s: XLALGetTransientWindowTimespan() failed.\n", __func__ );
XLAL_ERROR_REAL8 ( XLAL_EFUNC );
}
/* prepare gsl-matrix for Mh_mu_nu = [ a^2, a*b ; a*b , b^2 ] */
gsl_matrix *Mh_mu_nu;
if ( (Mh_mu_nu = gsl_matrix_calloc ( 4, 4 )) == NULL ) {
XLALPrintError ("%s: gsl_matrix_calloc ( 4, 4 ) failed.\n", __func__ );
XLAL_ERROR_REAL8 ( XLAL_ENOMEM );
}
gsl_vector_const_view A_Mu_view = gsl_vector_const_view_array ( A_Mu, 4 );
REAL8 TAtom = atoms->TAtom;
UINT4 numAtoms = atoms->length;
UINT4 alpha;
REAL8 Ad = 0, Bd = 0, Cd = 0; // usual non-transient antenna-pattern functions
REAL8 Ap = 0, Bp = 0, Cp = 0; // "effective" transient-CW antenna-pattern matrix M'_mu_nu
for ( alpha=0; alpha < numAtoms; alpha ++ )
{
UINT4 ti = atoms->data[alpha].timestamp;
REAL8 win = XLALGetTransientWindowValue ( ti, t0, t1, transientWindow.tau, transientWindow.type );
if ( win == 0 )
continue;
/* compute sh_mu = sqrt(gamma/2) * Mh_mu_nu A^nu * win, where Mh_mu_nu is now just
* the per-atom block matrix [a^2, ab; ab, b^2 ]
* where Sn=1, so gamma = Sinv*TAtom = TAtom
*/
// NOTE: for sh_mu: only LINEAR in window-function, NOT quadratic! -> see notes
REAL8 a2 = win * atoms->data[alpha].a2_alpha;
REAL8 b2 = win * atoms->data[alpha].b2_alpha;
REAL8 ab = win * atoms->data[alpha].ab_alpha;
Ad += a2;
Bd += b2;
Cd += ab;
// we also compute M'_mu_nu, which will be used to estimate optimal SNR
// NOTE: M'_mu_nu is QUADRATIC in window-function!, so we multiply by win again
Ap += win * a2;
Bp += win * b2;
Cp += win * ab;
/* upper-left block */
gsl_matrix_set ( Mh_mu_nu, 0, 0, a2 );
gsl_matrix_set ( Mh_mu_nu, 1, 1, b2 );
gsl_matrix_set ( Mh_mu_nu, 0, 1, ab );
gsl_matrix_set ( Mh_mu_nu, 1, 0, ab );
/* lower-right block: +2 on all components */
gsl_matrix_set ( Mh_mu_nu, 2, 2, a2 );
gsl_matrix_set ( Mh_mu_nu, 3, 3, b2 );
gsl_matrix_set ( Mh_mu_nu, 2, 3, ab );
gsl_matrix_set ( Mh_mu_nu, 3, 2, ab );
/* placeholder for 4-vector sh_mu */
PulsarAmplitudeVect sh_mu = {0,0,0,0};
gsl_vector_view sh_mu_view = gsl_vector_view_array ( sh_mu, 4 );
/* int gsl_blas_dgemv (CBLAS_TRANSPOSE_t TransA, double alpha, const gsl_matrix * A, const gsl_vector * x, double beta, gsl_vector * y)
* compute the matrix-vector product and sum y = \alpha op(A) x + \beta y, where op(A) = A, A^T, A^H
* for TransA = CblasNoTrans, CblasTrans, CblasConjTrans.
*
* sh_mu = sqrt(gamma/2) * Mh_mu_nu A^nu, where here gamma = TAtom, as Sinv=1 for multi-IFO value, and weights for SinvX!=1 have already been absorbed in atoms through XLALComputeMultiAMCoeffs()
*/
REAL8 norm_s = sqrt(TAtom / 2.0);
if ( (gslstat = gsl_blas_dgemv (CblasNoTrans, norm_s, Mh_mu_nu, &A_Mu_view.vector, 0.0, &sh_mu_view.vector)) != 0 ) {
XLALPrintError ( "%s: gsl_blas_dgemv(L * norm) failed: %s\n", __func__, gsl_strerror (gslstat) );
XLAL_ERROR_REAL8 ( XLAL_EFAILED );
}
/* add this signal to the atoms, using the relation Fa,Fb <--> x_mu: see Eq.(72) in CFSv2-LIGO-T0900149-v2.pdf */
REAL8 s1,s2,s3,s4;
s1 = sh_mu[0];
s2 = sh_mu[1];
s3 = sh_mu[2];
s4 = sh_mu[3];
atoms->data[alpha].Fa_alpha += crectf( s1, - s3 );
atoms->data[alpha].Fb_alpha += crectf( s2, - s4 );
} /* for alpha < numAtoms */
/* compute optimal SNR^2 expected for this signal,
* using rho2 = A^mu M'_mu_nu A^nu = T/Sn( A' [A1^2+A3^2] + 2C' [A1A2 +A3A4] + B' [A2^2+A4^2])
* NOTE: here we use the "effective" transient-CW antenna-pattern matrix M'_mu_nu
*/
REAL8 A1 = A_Mu[0];
REAL8 A2 = A_Mu[1];
REAL8 A3 = A_Mu[2];
REAL8 A4 = A_Mu[3];
REAL8 rho2 = TAtom * ( Ap * ( SQ(A1) + SQ(A3) ) + 2.0*Cp * ( A1*A2 + A3*A4 ) + Bp * ( SQ(A2) + SQ(A4) ) );
/* return "effective" transient antenna-pattern matrix */
M_mu_nu->Sinv_Tsft = TAtom; /* everything here in units of Sn, so effectively Sn=1 */
M_mu_nu->Ad = Ap;
M_mu_nu->Bd = Bp;
M_mu_nu->Cd = Cp;
M_mu_nu->Dd = Ap * Bp - Cp * Cp;
/* free memory */
gsl_matrix_free ( Mh_mu_nu );
/* return SNR^2 */
return rho2;
} /* XLALAddSignalToFstatAtomVector() */
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;
}
