void MlSldaState::InitializeAssignments(bool random_init) {
InitializeResponse();
InitializeLength();
LdawnState::InitializeAssignments(random_init);
if (FLAGS_num_seed_docs > 0) {
const gsl_vector* y = static_cast<lib_corpora::ReviewCorpus*>
(corpus_.get())->train_ratings();
boost::shared_ptr<gsl_permutation> sorted(gsl_permutation_alloc(y->size),
gsl_permutation_free);
boost::shared_ptr<gsl_permutation> rank(gsl_permutation_alloc(y->size),
gsl_permutation_free);
std::vector< std::vector<int> > num_seeds_used;
num_seeds_used.resize(corpus_->num_languages());
for (int ii = 0; ii < corpus_->num_languages(); ++ii) {
num_seeds_used[ii].resize(num_topics_);
}
gsl_sort_vector_index(sorted.get(), y);
gsl_permutation_inverse(rank.get(), sorted.get());
// We add one for padding so we don't try to set a document to be equal to
// the number of topics.
double num_train = corpus_->num_train() + 1.0;
int train_seen = 0;
int num_docs = corpus_->num_docs();
for (int dd = 0; dd < num_docs; ++dd) {
MlSeqDoc* doc = corpus_->seq_doc(dd);
int lang = doc->language();
if (!corpus_->doc(dd)->is_test()) {
// We don't assign to topic zero, so it can be stopwordy
int val = (int) floor((num_topics_ - 1) *
rank->data[train_seen] / num_train) + 1;
// Stop once we've used our limit of seed docs (too many leads to an
// overfit initial state)
if (num_seeds_used[lang][val] < FLAGS_num_seed_docs) {
cout << "Initializing doc " << lang << " " << dd << " to " << val <<
" score=" << truth_[dd] << endl;
for (int jj = 0; jj < (int)topic_assignments_[dd].size(); ++jj) {
int term = (*doc)[jj];
const topicmod_projects_ldawn::WordPaths word =
wordnet_->word(lang, term);
int num_paths = word.size();
if (num_paths > 0) {
ChangePath(dd, jj, val, rand() % num_paths);
} else {
if (use_aux_topics())
ChangeTopic(dd, jj, val);
}
}
++num_seeds_used[lang][val];
}
++train_seen;
}
}
}
}
static void
rvine_set_weight(igraph_t *graph,
dml_vine_weight_t weight,
igraph_integer_t e,
const gsl_vector *x,
const gsl_vector *y,
const gsl_permutation *x_rank,
const gsl_permutation *y_rank)
{
double value;
dml_measure_t *measure;
// The weight is minimized.
measure = dml_measure_alloc(x, y);
measure->x_rank = gsl_permutation_alloc(x_rank->size);
gsl_permutation_memcpy(measure->x_rank, x_rank);
measure->y_rank = gsl_permutation_alloc(y_rank->size);
gsl_permutation_memcpy(measure->y_rank, y_rank);
switch (weight) {
case DML_VINE_WEIGHT_TAU:
value = 1 - fabs(dml_measure_tau_coef(measure));
break;
case DML_VINE_WEIGHT_CVM:
value = measure->x->size - dml_measure_cvm_stat(measure);
break;
default:
value = 0;
break;
}
SETEAN(graph, "weight", e, value);
SETEAP(graph, "measure", e, measure);
}
static VALUE rb_gsl_permutation_get(int argc, VALUE *argv, VALUE obj)
{
gsl_permutation *b, *bnew;
gsl_index *p;
int beg, en, i, step;
size_t n, j, k;
Data_Get_Struct(obj, gsl_permutation, b);
switch (argc) {
case 0:
rb_raise(rb_eArgError, "too few arguments (%d for >= 1)", argc);
break;
case 1:
switch (TYPE(argv[0])) {
case T_FIXNUM:
i = FIX2INT(argv[0]);
if (i < 0) j = b->size + i; else j = (size_t) i;
return INT2FIX((int) b->data[j]);
break;
case T_ARRAY:
n = RARRAY(argv[0])->len;
bnew = gsl_permutation_alloc(n);
for (j = 0; j < n; j++) {
i = FIX2INT(rb_ary_entry(argv[0], j));
if (i < 0) k = b->size + i; else k = i;
bnew->data[j] = b->data[k];
}
return Data_Wrap_Struct(CLASS_OF(argv[0]), 0, gsl_permutation_free, bnew);
break;
default:
if (PERMUTATION_P(argv[0])) {
Data_Get_Struct(argv[0], gsl_index, p);
bnew = gsl_permutation_alloc(p->size);
for (j = 0; j < p->size; j++) bnew->data[j] = b->data[p->data[j]];
return Data_Wrap_Struct(CLASS_OF(argv[0]), 0, gsl_permutation_free, bnew);
} else if (CLASS_OF(argv[0]) == rb_cRange) {
get_range_int_beg_en_n(argv[0], &beg, &en, &n, &step);
bnew = gsl_permutation_alloc(n);
for (j = 0; j < n; j++)
bnew->data[j] = b->data[beg+j];
return Data_Wrap_Struct(CLASS_OF(obj), 0, gsl_permutation_free, bnew);
} else {
rb_raise(rb_eArgError, "wrong argument type %s (Fixnum, Array, or Range expected)", rb_class2name(CLASS_OF(argv[0])));
break;
}
}
break;
default:
bnew = gsl_permutation_alloc(argc);
for (j = 0; j < argc; j++) {
i = FIX2INT(argv[j]);
if (i < 0) k = b->size + i; else k = i;
bnew->data[j] = b->data[k];
}
return Data_Wrap_Struct(CLASS_OF(argv[0]), 0, gsl_permutation_free, bnew);
break;
}
return Qnil;
}
static gsl_permutation *
new_permutation(lua_State *L, int m)
{
gsl_permutation *p;
p = gsl_permutation_alloc(m);
if (p == 0) {
lua_gc(L, LUA_GCCOLLECT, 0);
p = gsl_permutation_alloc(m);
if (p == 0)
luaL_error(L, "not enough memory");
}
return p;
}
//calculate determinant of gaudin matrix for given Bethe rapidity set
double detgaudinnorm (double kEpshiftinv_gelesen[N+6])
{
double k[N];//Bethe rapidities
double determinante;
double c = kEpshiftinv_gelesen[0];//interaction strenght
for(int ll=0; ll<N; ll++)
k[ll]=kEpshiftinv_gelesen[ll+1];
//allocate and calculate gaudin matrix:
gsl_matrix * m = gsl_matrix_alloc (N, N);
for (int ii=0; ii<N; ii++)
for (int jj=0; jj<N; jj++)
gsl_matrix_set (m, ii, jj, matrixelementenorm(k, c, ii, jj));
//calculate determinant via LU decomposition
int sign_permutation;
gsl_permutation * p = gsl_permutation_alloc (N);
gsl_linalg_LU_decomp (m, p, &sign_permutation);
determinante = gsl_linalg_LU_det (m, sign_permutation);
gsl_permutation_free (p);
gsl_matrix_free (m);
//end LU decomposition and det calc
return determinante;
}
//Reads coupling matrix from file and computes its LU decomposition
void read_coupling_matrix(char *fname_in,int nbins_in,
gsl_matrix **coupling_matrix_b_out,
gsl_permutation **perm_out,
int pol1,int pol2)
{
int sig,n_cl,stat;
FILE *fi;
gsl_permutation *perm;
gsl_matrix *coupling_matrix_b;
if(pol1) {
if(pol2) n_cl=4;
else n_cl=2;
}
else {
if(pol2) n_cl=2;
else n_cl=1;
}
perm=gsl_permutation_alloc(n_cl*nbins_in);
coupling_matrix_b=gsl_matrix_alloc(n_cl*nbins_in,n_cl*nbins_in);
fi=my_fopen(fname_in,"rb");
stat=gsl_matrix_fread(fi,coupling_matrix_b);
if(stat==GSL_EFAILED)
report_error(1,"Error reading matrix from file %s\n",fname_in);
fclose(fi);
gsl_linalg_LU_decomp(coupling_matrix_b,perm,&sig);
*coupling_matrix_b_out=coupling_matrix_b;
*perm_out=perm;
}
int main ()
{
double a_data[] = { 0.18, 0.60, 0.57, 0.96,
0.41, 0.24, 0.99, 0.58,
0.14, 0.30, 0.97, 0.66,
0.51, 0.13, 0.19, 0.85 };
double b_data[] = { 1.0, 2.0, 3.0, 4.0 };
gsl_matrix_view m
= gsl_matrix_view_array (a_data, 4, 4);
gsl_vector_view b
= gsl_vector_view_array (b_data, 4);
gsl_vector *x = gsl_vector_alloc (4);
int s;
gsl_permutation * p = gsl_permutation_alloc (4);
gsl_linalg_LU_decomp (&m.matrix, p, &s);
gsl_linalg_LU_solve (&m.matrix, p, &b.vector, x);
printf ("x = \n");
gsl_vector_fprintf (stdout, x, "%g");
gsl_permutation_free (p);
gsl_vector_free (x);
return 0;
}
static gsl_matrix *augmented_param_mat_A(gsl_matrix *R11, gsl_matrix *R12)
{
gsl_matrix *t_aug_A, *aug_A, *LU, *R11_inv;
gsl_permutation *p;
int signum;
p = gsl_permutation_alloc(R11->size2);
LU = gsl_matrix_alloc(R11->size1, R11->size2);
gsl_matrix_memcpy(LU, R11);
R11_inv = gsl_matrix_alloc(R11->size1, R11->size2);
gsl_linalg_LU_decomp(LU, p, &signum);
gsl_linalg_LU_invert(LU, p, R11_inv);
aug_A = gsl_matrix_alloc(R12->size1, R12->size2);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, R11_inv, R12, 0.0, aug_A);
t_aug_A = gsl_matrix_alloc(aug_A->size2, aug_A->size1);
gsl_matrix_transpose_memcpy(t_aug_A, aug_A);
gsl_permutation_free(p);
gsl_matrix_free(LU);
gsl_matrix_free(aug_A);
gsl_matrix_free(R11_inv);
return t_aug_A;
}
void set_inverse(rgb_gaussian *gaussian)
{
gsl_matrix *cov_inv = gsl_matrix_alloc(3, 3);
// pre-compute covariance inverse
gsl_matrix *cov_cpy = gsl_matrix_alloc(3, 3);
gsl_matrix_memcpy(cov_cpy, gaussian->cov);
gsl_permutation *p = gsl_permutation_alloc(3);
int sign;
gsl_linalg_LU_decomp(cov_cpy, p, &sign);
gsl_linalg_LU_invert(cov_cpy, p, cov_inv);
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
gaussian->cov_inv[3 * i + j] = gsl_matrix_get(cov_inv, i, j);
}
}
// release memory
gsl_matrix_free(cov_inv);
gsl_matrix_free(cov_cpy);
gsl_permutation_free(p);
}
TRANSITION_MATRIX inverse(TRANSITION_MATRIX transition_matrix) {
int i, j, signum;
gsl_matrix* matrix_to_invert = gsl_matrix_alloc(transition_matrix.row_length, transition_matrix.row_length);
gsl_matrix* inversion_matrix = gsl_matrix_alloc(transition_matrix.row_length, transition_matrix.row_length);
gsl_permutation* permutation = gsl_permutation_alloc(transition_matrix.row_length);
for (i = 0; i < transition_matrix.row_length; ++i) {
for (j = 0; j < transition_matrix.row_length; ++j) {
gsl_matrix_set(matrix_to_invert, i, j, T_ROW_ORDER(transition_matrix, i, j));
}
}
gsl_linalg_LU_decomp(matrix_to_invert, permutation, &signum);
gsl_linalg_LU_invert(matrix_to_invert, permutation, inversion_matrix);
for (i = 0; i < transition_matrix.row_length; ++i) {
for (j = 0; j < transition_matrix.row_length; ++j) {
T_ROW_ORDER(transition_matrix, i, j) = gsl_matrix_get(inversion_matrix, i, j);
}
}
gsl_matrix_free(matrix_to_invert);
gsl_matrix_free(inversion_matrix);
gsl_permutation_free(permutation);
return transition_matrix;
}
double dmvt(const unsigned int n, const gsl_vector *x, const gsl_vector *location, const gsl_matrix *scale, const unsigned int dof)
{
int s;
double ax,ay,az=0.5*(dof + n);
gsl_vector *ym, *xm;
gsl_matrix *work = gsl_matrix_alloc(n,n),
*winv = gsl_matrix_alloc(n,n);
gsl_permutation *p = gsl_permutation_alloc(n);
gsl_matrix_memcpy( work, scale );
gsl_linalg_LU_decomp( work, p, &s );
gsl_linalg_LU_invert( work, p, winv );
ax = gsl_linalg_LU_det( work, s );
gsl_matrix_free( work );
gsl_permutation_free( p );
xm = gsl_vector_alloc(n);
gsl_vector_memcpy( xm, x);
gsl_vector_sub( xm, location );
ym = gsl_vector_alloc(n);
gsl_blas_dsymv(CblasUpper,1.0,winv,xm,0.0,ym);
gsl_matrix_free( winv );
gsl_blas_ddot( xm, ym, &ay);
gsl_vector_free(xm);
gsl_vector_free(ym);
ay = pow((1+ay/dof),-az)*gsl_sf_gamma(az)/(gsl_sf_gamma(0.5*dof)*sqrt( pow((dof*M_PI),double(n))*ax ));
return ay;
}
Matrix<N, M, Container> Matrix<M, N, Container>::invert() const
{
Matrix<N, M, Container> result;
gsl_matrix* Z1 = gsl_matrix_alloc(M, M);
gsl_matrix* Z = gsl_matrix_alloc(M, M);
gsl_permutation* perm = gsl_permutation_alloc(M);
int k;
for (uint i = 0; i < M; i++)
for (uint j = 0; j < M; j++)
gsl_matrix_set(Z1, i, j, (*this)(i, j));
if (gsl_linalg_LU_decomp(Z1, perm, &k))
THROW_INVALID_ARG("gsl_linalg_LU_decomp failed");
if (gsl_linalg_LU_invert(Z1, perm, Z))
THROW_INVALID_ARG("gsl_linalg_LU_invert failed");
for (uint i = 0; i < M; i++)
for (uint j = 0; j < M; j++)
result(i, j) = gsl_matrix_get(Z, i, j);
gsl_permutation_free(perm);
gsl_matrix_free(Z);
gsl_matrix_free(Z1);
return result;
}
gsl_matrix * SgFilter::pseudoInverse(gsl_matrix *A, int n_row, int n_col){
gsl_matrix * A_t = gsl_matrix_alloc (n_col, n_row); //A_t is transpose
gsl_matrix_transpose_memcpy (A_t, A);
gsl_matrix * U = gsl_matrix_alloc (n_col, n_row);
gsl_matrix * V= gsl_matrix_alloc (n_row, n_row);
gsl_vector * S = gsl_vector_alloc (n_row);
// Computing the SVD of the transpose of A
gsl_vector * work = gsl_vector_alloc (n_row);
gsl_linalg_SV_decomp (A_t, V, S, work);
gsl_vector_free(work);
gsl_matrix_memcpy (U, A_t);
//Inverting S
gsl_matrix * Sp = gsl_matrix_alloc (n_row, n_row);
gsl_matrix_set_zero (Sp);
for (int i = 0; i < n_row; i++)
gsl_matrix_set (Sp, i, i, gsl_vector_get(S, i)); // Vector 'S' to matrix 'Sp'
gsl_permutation * p = gsl_permutation_alloc (n_row);
int signum;
gsl_linalg_LU_decomp (Sp, p, &signum); // Computing the LU decomposition
// Compute the inverse
gsl_matrix * SI = gsl_matrix_calloc (n_row, n_row);
for (int i = 0; i < n_row; i++) {
if (gsl_vector_get (S, i) > 0.0000000001)
gsl_matrix_set (SI, i, i, 1.0 / gsl_vector_get (S, i));
}
gsl_matrix * VT = gsl_matrix_alloc (n_row, n_row);
gsl_matrix_transpose_memcpy (VT, V); // Tranpose of V
//THE PSEUDOINVERSE
//Computation of the pseudoinverse of trans(A) as pinv(A) = U·inv(S).trans(V) with trans(A) = U.S.trans(V)
gsl_matrix * SIpVT = gsl_matrix_alloc (n_row, n_row);
gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, // Calculating inv(S).trans(V)
1.0, SI, VT,
0.0, SIpVT);
gsl_matrix * pinv = gsl_matrix_alloc (n_col, n_row); // Calculating U·inv(S).trans(V)
gsl_blas_dgemm (CblasNoTrans, CblasNoTrans,
1.0, U, SIpVT,
0.0, pinv);
gsl_matrix_free(VT);
gsl_matrix_free(SI);
gsl_matrix_free(SIpVT);
gsl_matrix_free(A_t);
gsl_matrix_free(U);
gsl_matrix_free(A);
gsl_matrix_free(V);
gsl_vector_free(S);
return pinv;
}
int main(int ac, char** av)
{
grid_t g;
gsl_matrix* a;
gsl_vector* x;
gsl_vector* b;
gsl_permutation* p;
int s;
int i;
grid_init_once(&g);
alloc_linear_system(&g, &a, &x, &b);
generate_a(&g, a);
p = gsl_permutation_alloc(a->size1);
gsl_linalg_LU_decomp(a, p, &s);
for (i = 1; i < ac; ++i)
{
grid_load_string(&g, av[i]);
generate_b(&g, b);
gsl_linalg_LU_solve(a, p, b, x);
print_vector(x);
printf("---\n");
}
gsl_permutation_free(p);
free_linear_system(a, x, b);
return 0;
}
int lsQRPT(gsl_matrix * A, gsl_vector * b, gsl_vector * x, double * sigma)
{
int i;
gsl_vector *tau, *res;
gsl_permutation *p;
gsl_vector_view norm;
if (A->size1 < A->size2) return -1;
if (A->size1 != b->size) return -1;
if (A->size2 != x->size) return -1;
tau = gsl_vector_alloc(x->size);
res = gsl_vector_alloc(b->size);
p = gsl_permutation_alloc(x->size);
norm = gsl_vector_subvector(res, 0, x->size);
gsl_linalg_QRPT_decomp(A, tau, p, &i, &norm.vector);
gsl_linalg_QR_lssolve(A, tau, b, x, res);
gsl_permute_vector_inverse(p, x);
*sigma = gsl_blas_dnrm2(res);
gsl_vector_free(tau);
gsl_vector_free(res);
gsl_permutation_free(p);
return 0;
}
// center must be allocated to ndims vals at least.
// coord are the vertice coords
// center is the center of the resulting ndims-sphere
// function returns its radius squared
double SimplexSphere(double **coord, int ndims,double *center)
{
double a_data[ndims*ndims];
double b_data[ndims];
gsl_matrix_view m=gsl_matrix_view_array (a_data, ndims, ndims);
gsl_vector_view b=gsl_vector_view_array (b_data, ndims);
gsl_vector_view x = gsl_vector_view_array (center,ndims);
gsl_permutation * p = gsl_permutation_alloc (ndims);
int s;
int i,j,k;
double d;
for (i=0,k=0;i<ndims;i++)
{
b_data[i]=0;
for (j=0;j<ndims;j++,k++)
{
a_data[k]=((double)coord[0][j]-(double)coord[i+1][j]);
b_data[i]+=((double)coord[0][j]*(double)coord[0][j]-(double)coord[i+1][j]*(double)coord[i+1][j]);
}
b_data[i]*=0.5;
}
gsl_linalg_LU_decomp (&m.matrix, p, &s);
gsl_linalg_LU_solve (&m.matrix, p, &b.vector, &x.vector);
gsl_permutation_free (p);
for (i=0,d=0;i<ndims;i++)
d+=((double)center[i]-(double)coord[0][i])*((double)center[i]-(double)coord[0][i]);
return d;
}
void sort_mask_resolution(gsl_vector_int *resolution_array,
gsl_vector_ulong *pixnum_array, unsigned long n_mask)
{
gsl_permutation *pixel_index;
gsl_vector_ulong *tmp_pixnum_array;
gsl_vector_int *tmp_resolution_array;
unsigned long i, j;
/* Given a list of masks and resolutions, return each list sorted by the
resolution. This routine is needed to make the resolution structure. */
tmp_pixnum_array = gsl_vector_ulong_alloc(n_mask);
tmp_resolution_array = gsl_vector_int_alloc(n_mask);
pixel_index = gsl_permutation_alloc(n_mask);
gsl_sort_vector_int_index(pixel_index,resolution_array);
for (i=0;i<n_mask;i++) {
j = pixel_index->data[i];
tmp_pixnum_array->data[i] = pixnum_array->data[j];
tmp_resolution_array->data[i] = resolution_array->data[j];
}
for (i=0;i<n_mask;i++) {
pixnum_array->data[i] = tmp_pixnum_array->data[i];
resolution_array->data[i] = tmp_resolution_array->data[i];
}
gsl_vector_int_free(tmp_resolution_array);
gsl_vector_ulong_free(tmp_pixnum_array);
gsl_permutation_free(pixel_index);
}
DiscreteKalmanFilter::DiscreteKalmanFilter(const DiscreteLinearStochasticSystem &system, const gsl_vector *x0, const gsl_matrix *P0)
: system_(system), x_hat_(NULL), /*y_hat_(NULL),*/ P_(NULL), K_(NULL),
residual_(NULL), RR_(NULL), invRR_(NULL), PCt_(NULL), permutation_(NULL), v_(NULL), M_(NULL)//, M2_(NULL)
{
const size_t &stateDim = system_.getStateDim();
const size_t &inputDim = system_.getInputDim();
const size_t &outputDim = system_.getOutputDim();
if (x0 && P0 && stateDim && inputDim && outputDim &&
stateDim == x0->size && stateDim == P0->size1 && stateDim == P0->size2)
{
x_hat_ = gsl_vector_alloc(stateDim);
//y_hat_ = gsl_vector_alloc(outputDim);
P_ = gsl_matrix_alloc(stateDim, stateDim);
K_ = gsl_matrix_alloc(stateDim, outputDim);
residual_ = gsl_vector_alloc(outputDim);
RR_ = gsl_matrix_alloc(outputDim, outputDim);
invRR_ = gsl_matrix_alloc(outputDim, outputDim);
PCt_ = gsl_matrix_alloc(stateDim, outputDim);
permutation_ = gsl_permutation_alloc(outputDim);
v_ = gsl_vector_alloc(stateDim);
M_ = gsl_matrix_alloc(stateDim, stateDim);
M2_ = gsl_matrix_alloc(stateDim, stateDim);
gsl_vector_memcpy(x_hat_, x0);
//gsl_vector_set_zero(y_hat_);
gsl_matrix_memcpy(P_, P0);
//gsl_matrix_set_identity(K_);
}
}
static void *
bsimp_alloc (size_t dim)
{
bsimp_state_t *state = (bsimp_state_t *) malloc (sizeof (bsimp_state_t));
state->d = gsl_matrix_alloc (SEQUENCE_MAX, dim);
state->a_mat = gsl_matrix_alloc (dim, dim);
state->p_vec = gsl_permutation_alloc (dim);
state->yp = (double *) malloc (dim * sizeof (double));
state->y_extrap_save = (double *) malloc (dim * sizeof (double));
state->y_extrap_sequence = (double *) malloc (dim * sizeof (double));
state->extrap_work = (double *) malloc (dim * sizeof (double));
state->dfdt = (double *) malloc (dim * sizeof (double));
state->y_temp = (double *) malloc (dim * sizeof (double));
state->delta_temp = (double *) malloc (dim * sizeof(double));
state->weight = (double *) malloc (dim * sizeof(double));
state->dfdy = gsl_matrix_alloc (dim, dim);
state->rhs_temp = (double *) malloc (dim * sizeof(double));
state->delta = (double *) malloc (dim * sizeof (double));
{
size_t k_choice = bsimp_deuf_kchoice (GSL_SQRT_DBL_EPSILON, dim); /*FIXME: choice of epsilon? */
state->k_choice = k_choice;
state->k_current = k_choice;
state->order = 2 * k_choice;
}
state->h_next = -GSL_SQRT_DBL_MAX;
return state;
}
double ran_mv_normal_pdf(const gsl_vector *x, const gsl_vector *mu,
const gsl_matrix *Sigma)
{
const int k = x->size;
int s;
double det, den;
gsl_vector *y = gsl_vector_alloc(k);
gsl_vector *work_k = gsl_vector_alloc(k);
gsl_matrix *work_k_k = gsl_matrix_alloc(k, k);
gsl_matrix *Sigmainv = gsl_matrix_alloc(k, k);
gsl_permutation *p = gsl_permutation_alloc(k);
gsl_vector_memcpy(y, x);
gsl_vector_sub(y, mu);
gsl_matrix_memcpy(work_k_k, Sigma);
gsl_linalg_LU_decomp(work_k_k, p, &s);
gsl_linalg_LU_invert(work_k_k, p, Sigmainv);
det = gsl_linalg_LU_det(work_k_k, s);
gsl_blas_dgemv(CblasNoTrans, 1.0, Sigmainv, y, 0.0, work_k);
gsl_blas_ddot(y, work_k, &den);
den = exp(-0.5*den) / sqrt(pow((2*M_PI), k)*det);
gsl_vector_free(y);
gsl_vector_free(work_k);
gsl_matrix_free(work_k_k);
gsl_matrix_free(Sigmainv);
gsl_permutation_free(p);
return den;
}
crlb_so_run(const vector2 *anchor, const size_t num_anchors,
const vector2 *location)
{
int s;
float crlb;
const float v = crlb_so_sdev * crlb_so_sdev;
gsl_matrix *A = gsl_matrix_calloc(2, 2);
gsl_matrix *Ai = gsl_matrix_alloc(2, 2);
gsl_permutation *p = gsl_permutation_alloc(2);
/* See Equation (2.157) of So's Chapter. */
for (size_t i = 0; i < num_anchors; i++) {
const float dx = location->x - anchor[i].x;
const float dy = location->y - anchor[i].y;
const float d2 = dx * dx + dy * dy;
A->data[0 /* 0, 0 */] += (dx * dx) / (v * d2);
A->data[1 /* 1, 0 */] += (dx * dy) / (v * d2);
A->data[2 /* 0, 1 */] += (dy * dx) / (v * d2);
A->data[3 /* 1, 1 */] += (dy * dy) / (v * d2);
}
gsl_linalg_LU_decomp(A, p, &s);
gsl_linalg_LU_invert(A, p, Ai);
/* See Equation (2.158) of So's Chapter. */
crlb = (float) (gsl_matrix_get(Ai, 0, 0) + gsl_matrix_get(Ai, 1, 1));
gsl_permutation_free(p);
gsl_matrix_free(Ai);
gsl_matrix_free(A);
return crlb;
}
// Calculates the covariance matrix Sigma, alongside Sigma^{-1} and det(Sigma)
void TStats::get_cov_matrix(gsl_matrix* Sigma, gsl_matrix* invSigma, double* detSigma) const {
// Check that the matrices are the correct size
assert(Sigma->size1 == N);
assert(Sigma->size2 == N);
assert(invSigma->size1 == N);
assert(invSigma->size2 == N);
// Calculate the covariance matrix Sigma
double tmp;
for(unsigned int i=0; i<N; i++) {
for(unsigned int j=i; j<N; j++) {
tmp = cov(i,j);
gsl_matrix_set(Sigma, i, j, tmp);
if(i != j) { gsl_matrix_set(Sigma, j, i, tmp); }
}
}
// Get the inverse of Sigma
int s;
gsl_permutation* p = gsl_permutation_alloc(N);
gsl_matrix* LU = gsl_matrix_alloc(N, N);
gsl_matrix_memcpy(LU, Sigma);
gsl_linalg_LU_decomp(LU, p, &s);
gsl_linalg_LU_invert(LU, p, invSigma);
// Get the determinant of sigma
*detSigma = gsl_linalg_LU_det(LU, s);
// Cleanup
gsl_matrix_free(LU);
gsl_permutation_free(p);
}
ExternalForceCalculator::ExternalForceCalculator()
{
U_gsl = gsl_matrix_alloc (9, 9);
om_gsl = gsl_vector_alloc (9);
x_gsl = gsl_vector_alloc (9);
p_gsl = gsl_permutation_alloc (9);
};
/** Determinant of matrix */
double matrix<double>::determinant(){
matrix<double> m1(_matrix);
int signum;
gsl_permutation *p;
if (size_j() != size_i())
{
std::cout << "\n ** Size mismatch in matrix<double> determinant" << std::endl;
exit(EXIT_FAILURE);
}
if ((p = gsl_permutation_alloc(size_i())) == NULL)
{
std::cout << "\n ** Error in matrix<double> determinant" << std::endl;
exit(EXIT_FAILURE);
}
if(gsl_linalg_LU_decomp (m1.as_gsl_type_ptr(), p, &signum))
{
std::cout << "\n ** Error in matrix<double> determinant" << std::endl;
gsl_permutation_free(p);
exit(EXIT_FAILURE);
}
gsl_permutation_free(p);
return gsl_linalg_LU_det(m1.as_gsl_type_ptr() , signum);
}
FreeBorderCalculator::FreeBorderCalculator()
{
U_gsl = gsl_matrix_alloc (9, 9);
om_gsl = gsl_vector_alloc (9);
x_gsl = gsl_vector_alloc (9);
p_gsl = gsl_permutation_alloc (9);
};
void expm(gsl_matrix_complex * L, gsl_complex t, gsl_matrix * m)
{
int i,j,s;
gsl_vector_complex *eval = gsl_vector_complex_alloc(4);
gsl_matrix_complex *evec = gsl_matrix_complex_alloc(4, 4);
gsl_eigen_nonsymmv_workspace * w = gsl_eigen_nonsymmv_alloc(4);
gsl_matrix_complex *evalmat = gsl_matrix_complex_alloc(4, 4);
gsl_matrix_complex *vd = gsl_matrix_complex_alloc(4, 4);
gsl_complex one = gsl_complex_rect(1, 0);
gsl_complex zero = gsl_complex_rect(0, 0);
gsl_matrix_complex *K = gsl_matrix_complex_alloc(4, 4);
gsl_permutation *p = gsl_permutation_alloc(4);
gsl_vector_complex *x = gsl_vector_complex_alloc(4);
gsl_vector_complex_view bp;
gsl_complex z;
gsl_eigen_nonsymmv(m, eval, evec, w);
gsl_eigen_nonsymmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_DESC);
gsl_eigen_nonsymmv_free(w); // clear workspace
for (i = 0; i < 4; i++)
{
gsl_complex eval_i = gsl_vector_complex_get(eval, i);
gsl_complex expeval = gsl_complex_mul(eval_i,t);
expeval = gsl_complex_exp(expeval);
gsl_matrix_complex_set(evalmat, i, i, expeval);
}
gsl_vector_complex_free(eval); // clear vector for eigenvalues
// v'L'=De'v'
gsl_blas_zgemm(CblasTrans, CblasTrans, one, evalmat, evec, zero, vd);
gsl_matrix_complex_transpose(evec);//transpose v
gsl_matrix_complex_memcpy(K,evec);
for (i = 0; i < 4; i++)
{
bp = gsl_matrix_complex_column(vd, i);
gsl_linalg_complex_LU_decomp(evec, p, &s);
gsl_linalg_complex_LU_solve(evec, p, &bp.vector, x);
for (j = 0; j < 4; j++)
{
z = gsl_vector_complex_get(x, j);
gsl_matrix_complex_set(L,i,j,z); //'through the looking glass' transpose
}
gsl_matrix_complex_memcpy(evec,K);
}
gsl_permutation_free(p);
gsl_vector_complex_free(x);
gsl_matrix_complex_free(vd);
gsl_matrix_complex_free(evec);
gsl_matrix_complex_free(evalmat);
gsl_matrix_complex_free(K);
}
/**
* Adapted from: Multivariate Normal density function and random
* number generator Using GSL from Ralph dos Santos Silva
* Copyright (C) 2006
*
* multivariate normal density function
*
* @param n dimension of the random vetor
* @param mean vector of means of size n
* @param var variance matrix of dimension n x n
*/
double ssm_dmvnorm(const int n, const gsl_vector *x, const gsl_vector *mean, const gsl_matrix *var, double sd_fac)
{
int s;
double ax,ay;
gsl_vector *ym, *xm;
gsl_matrix *work = gsl_matrix_alloc(n,n),
*winv = gsl_matrix_alloc(n,n);
gsl_permutation *p = gsl_permutation_alloc(n);
gsl_matrix_memcpy( work, var );
//scale var with sd_fac^2
gsl_matrix_scale(work, sd_fac*sd_fac);
gsl_linalg_LU_decomp( work, p, &s );
gsl_linalg_LU_invert( work, p, winv );
ax = gsl_linalg_LU_det( work, s );
gsl_matrix_free( work );
gsl_permutation_free( p );
xm = gsl_vector_alloc(n);
gsl_vector_memcpy( xm, x);
gsl_vector_sub( xm, mean );
ym = gsl_vector_alloc(n);
gsl_blas_dsymv(CblasUpper,1.0,winv,xm,0.0,ym);
gsl_matrix_free( winv );
gsl_blas_ddot( xm, ym, &ay);
gsl_vector_free(xm);
gsl_vector_free(ym);
ay = exp(-0.5*ay)/sqrt( pow((2*M_PI),n)*ax );
return ay;
}
static int
test_COD_decomp_eps(const gsl_matrix * m, const double eps, const char *desc)
{
int s = 0;
size_t i, j, M = m->size1, N = m->size2;
size_t rank;
gsl_matrix * QRZT = gsl_matrix_alloc(M, N);
gsl_matrix * Q = gsl_matrix_alloc(M, M);
gsl_matrix * R = gsl_matrix_alloc(M, N);
gsl_matrix * QR = gsl_matrix_alloc(M, N);
gsl_matrix * Z = gsl_matrix_alloc(N, N);
gsl_vector * tau_Q = gsl_vector_alloc(GSL_MIN(M, N));
gsl_vector * tau_Z = gsl_vector_alloc(GSL_MIN(M, N));
gsl_vector * work = gsl_vector_alloc(N);
gsl_matrix * lhs = gsl_matrix_alloc(M, N);
gsl_matrix * rhs = gsl_matrix_alloc(M, N);
gsl_permutation * perm = gsl_permutation_alloc(N);
gsl_matrix_memcpy(QRZT, m);
s += gsl_linalg_COD_decomp(QRZT, tau_Q, tau_Z, perm, &rank, work);
s += gsl_linalg_COD_unpack(QRZT, tau_Q, tau_Z, rank, Q, R, Z);
/* compute lhs = m P */
gsl_matrix_memcpy(lhs, m);
gsl_permute_matrix(perm, lhs);
/* compute rhs = Q R Z^T */
gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, Q, R, 0.0, QR);
gsl_blas_dgemm (CblasNoTrans, CblasTrans, 1.0, QR, Z, 0.0, rhs);
for (i = 0; i < M; i++)
{
for (j = 0; j < N; j++)
{
double aij = gsl_matrix_get(rhs, i, j);
double bij = gsl_matrix_get(lhs, i, j);
gsl_test_rel(aij, bij, eps, "%s (%3lu,%3lu)[%lu,%lu]: %22.18g %22.18g\n",
desc, M, N, i, j, aij, bij);
}
}
gsl_permutation_free (perm);
gsl_vector_free(work);
gsl_vector_free(tau_Q);
gsl_vector_free(tau_Z);
gsl_matrix_free(QRZT);
gsl_matrix_free(lhs);
gsl_matrix_free(rhs);
gsl_matrix_free(QR);
gsl_matrix_free(Q);
gsl_matrix_free(R);
gsl_matrix_free(Z);
return s;
}
static VALUE rb_gsl_permutation_inverse(VALUE obj)
{
gsl_permutation *p, *inv;
Data_Get_Struct(obj, gsl_permutation, p);
inv = gsl_permutation_alloc(p->size);
gsl_permutation_inverse(inv, p);
return Data_Wrap_Struct(cgsl_permutation, 0, gsl_permutation_free, inv);
}
static VALUE rb_gsl_permutation_clone(VALUE obj)
{
gsl_permutation *p, *p2 = NULL;
Data_Get_Struct(obj, gsl_permutation, p);
p2 = gsl_permutation_alloc(p->size);
gsl_permutation_memcpy(p2, p);
return Data_Wrap_Struct(CLASS_OF(obj), 0, gsl_permutation_free, p2);
}
