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; }