void Testiwishart(CuTest* tc) { /*set a non-trivial location matrix*/ gsl_matrix* V = gsl_matrix_alloc(DIM, DIM); gsl_matrix_set_identity(V); gsl_matrix_add_constant(V, 1.0); p = mcmclib_iwishart_lpdf_alloc(V, P); x = gsl_vector_alloc(DIM * DIM); gsl_matrix_view X_v = gsl_matrix_view_vector(x, DIM, DIM); gsl_matrix* X = &(X_v.matrix); gsl_matrix_set_identity(X); gsl_matrix_add_constant(X, 1.0); /*check for side-effects*/ double tmp = lpdf(1.0); CuAssertTrue(tc, tmp == lpdf(1.0)); CuAssertDblEquals(tc, -18.188424, lpdf(1.0), TOL); CuAssertDblEquals(tc, 49.59203, lpdf(0.5), TOL); CuAssertDblEquals(tc, -88.468878, lpdf(2.0), TOL); mcmclib_iwishart_lpdf_free(p); gsl_vector_free(x); gsl_matrix_free(V); }
void mvn_sample(gsl_vector *mean_cand, gsl_matrix *var) { /* Takes a mean vec, mean and var matrix, * var and gives vector of MVN(mean,var) realisations, x */ int i, j; int dimen = var -> size1; double value; gsl_matrix *disp; gsl_vector *ran; gsl_matrix *fast_species; fast_species = gsl_matrix_alloc(2, 2); gsl_matrix_set_identity(fast_species); for(i=0;i<dimen; i++) { if(MGET(var, i, i) <0.00000000001) { MSET(var, i, i, 1.0); MSET(fast_species, i, i, 0.0); } } disp = gsl_matrix_alloc(2, 2); ran = gsl_vector_alloc(2); gsl_matrix_memcpy(disp, var); if(postive_definite == 1) { gsl_linalg_cholesky_decomp(disp); for(i=0;i<dimen;i++) { for (j=i+1;j<dimen;j++) { MSET(disp,i,j,0.0); } } }else{ value = pow(MGET(disp, 0 ,0), 0.5); gsl_matrix_set_identity(disp); MSET(disp, 0,0, value); MSET(disp, 1,1, value); } for (j=0;j<dimen;j++) { VSET(ran,j,gsl_ran_gaussian(r,1.0)); } /*remove update from slow species*/ gsl_matrix_mul_elements(disp, fast_species); /*Add noise to mean cand*/ gsl_blas_dgemv(CblasNoTrans,1.0, disp, ran, 1.0, mean_cand); for(i=0; i<2; i++) { if(VGET(mean_cand,i)<=0.0001 && MGET(fast_species, i, i) > 0.000001) VSET(mean_cand,i,0.0001); } gsl_vector_free(ran); gsl_matrix_free(disp); gsl_matrix_free(fast_species); }
/*! \brief Input: _. Output: A matrix * * Generates a unimodular matrix. */ gsl_matrix* genU(){ int z, j ,display = 1; /* Initialize and allocate memory for necessary matrices */ gsl_matrix* result = gsl_matrix_alloc(SIZE,SIZE); gsl_matrix* result2 = gsl_matrix_alloc(SIZE,SIZE); gsl_matrix* base = gsl_matrix_alloc(SIZE,SIZE); gsl_matrix_set_identity(result); gsl_matrix_set_identity(result2); gsl_matrix_set_identity(base); /* Display progress of generating a unimodular to the user */ printf("Generating Unimodular\n"); printf("|..................................................|\r\033[01;35m|"); fflush(stdout); if(SIZE/50 > 1) display = SIZE/50; for(z = 0; z<DEPTH; z++){ if(z%2 == 0){ for(j=0; j < SIZE ; j++) { if(j%2 == 0){ randomLine(base,j); gsl_linalg_matmult(result,base,result2); gsl_matrix_memcpy(result,result2); clearLine(base,j); if(j % display == 0){ printf("-"); fflush(stdout); } } } }else{ printf("| 50%%\r\033[01;32m|"); for(j=SIZE-1; j >= 0 ; j--) { if(j%2 == 1){ randomLine(base,j); gsl_linalg_matmult(result,base,result2); gsl_matrix_memcpy(result,result2); clearLine(base,j); if(j % display == 1){ printf("="); fflush(stdout); } } } } } printf("| 100%% DONE\rUnimodular Generated\033[0m\n"); free(result2); free(base); return result; }
static int vardim_df (CBLAS_TRANSPOSE_t TransJ, const gsl_vector * x, const gsl_vector * u, void * params, gsl_vector * v, gsl_matrix * JTJ) { gsl_matrix_view J = gsl_matrix_view_array(vardim_J, vardim_N, vardim_P); size_t i; double sum = 0.0; gsl_matrix_view m = gsl_matrix_submatrix(&J.matrix, 0, 0, vardim_P, vardim_P); gsl_matrix_set_identity(&m.matrix); for (i = 0; i < vardim_P; ++i) { double xi = gsl_vector_get(x, i); sum += (i + 1.0) * (xi - 1.0); } for (i = 0; i < vardim_P; ++i) { gsl_matrix_set(&J.matrix, vardim_P, i, i + 1.0); gsl_matrix_set(&J.matrix, vardim_P + 1, i, 2*(i + 1.0)*sum); } if (v) gsl_blas_dgemv(TransJ, 1.0, &J.matrix, u, 0.0, v); if (JTJ) gsl_blas_dsyrk(CblasLower, CblasTrans, 1.0, &J.matrix, 0.0, JTJ); (void)params; /* avoid unused parameter warning */ return GSL_SUCCESS; }
static int vardim_df (const gsl_vector * x, void *params, gsl_matrix * J) { size_t i; double sum = 0.0; gsl_matrix_view m = gsl_matrix_submatrix(J, 0, 0, vardim_P, vardim_P); gsl_matrix_set_identity(&m.matrix); for (i = 0; i < vardim_P; ++i) { double xi = gsl_vector_get(x, i); sum += (i + 1.0) * (xi - 1.0); } for (i = 0; i < vardim_P; ++i) { gsl_matrix_set(J, vardim_P, i, i + 1.0); gsl_matrix_set(J, vardim_P + 1, i, 2*(i + 1.0)*sum); } (void)params; /* avoid unused parameter warning */ return GSL_SUCCESS; }
/** alloc a new at7_gamma object. Input arguments are copied @internal */ static at7_gamma* at7_gamma_alloc(const gsl_vector* beta, gsl_vector** mu, gsl_matrix** Sigma) { at7_gamma* ans = (at7_gamma*) malloc(sizeof(at7_gamma)); const size_t K = beta->size; const size_t dim = mu[0]->size; ans->beta = gsl_vector_alloc(K); gsl_vector_memcpy(ans->beta, beta); ans->mu = (gsl_vector**) malloc(K * sizeof(gsl_vector*)); ans->Sigma = (gsl_matrix**) malloc(K * sizeof(gsl_matrix*)); ans->pik = (mcmclib_mvnorm_lpdf**) malloc(K * sizeof(mcmclib_mvnorm_lpdf*)); ans->tmpMean = gsl_vector_alloc(dim); ans->qVariances = (gsl_matrix**) malloc(K * sizeof(gsl_matrix*)); ans->qdk = (mcmclib_mvnorm_lpdf**) malloc(K * sizeof(mcmclib_mvnorm_lpdf*)); for(size_t k=0; k < K; k++) { ans->mu[k] = gsl_vector_alloc(dim); gsl_vector_memcpy(ans->mu[k], mu[k]); ans->Sigma[k] = gsl_matrix_alloc(dim, dim); gsl_matrix_memcpy(ans->Sigma[k], Sigma[k]); ans->pik[k] = mcmclib_mvnorm_lpdf_alloc(ans->mu[k], ans->Sigma[k]->data); ans->qVariances[k] = gsl_matrix_alloc(dim, dim); ans->qdk[k] = mcmclib_mvnorm_lpdf_alloc(ans->tmpMean, ans->qVariances[k]->data); } gsl_vector_memcpy(ans->beta, beta); ans->pi = mcmclib_mixnorm_lpdf_alloc(ans->beta, ans->pik); ans->Sigma_eps = gsl_matrix_alloc(dim, dim); gsl_matrix_set_identity(ans->Sigma_eps); gsl_matrix_scale(ans->Sigma_eps, 0.001); ans->scaling_factors = gsl_vector_alloc(K); gsl_vector_set_all(ans->scaling_factors, 2.38*2.38 / (double) dim); ans->weights = gsl_vector_alloc(K); gsl_vector_set_all(ans->weights, 0.0); at7_gamma_update_Sigma(ans); return ans; }
int main(int argc, char *argv[]){ int args; int m, n; double pdf, dof; args = 1; m = atoi(argv[args++]); n = atoi(argv[args++]); // allocate space for matrix X, test_copy, and Scale. gsl_matrix *X = gsl_matrix_alloc(m, n); gsl_matrix *inv = gsl_matrix_alloc(m,n); gsl_matrix *Scale = gsl_matrix_alloc(m, n); gsl_matrix_set_identity(Scale); fill_matrix(X); dof = m + 1; pdf = iwishpdf(X, Scale, inv, dof); // Print the data printf("\n%s", "Inverse Wishart matrix"); print_matrix(X); printf("\n%s", "Scale matrix"); print_matrix(Scale); printf("\n%s", "Wishart matrix"); print_matrix(inv); printf("\n%s", "Density"); printf("\n%f\n", pdf); return(0); }
static int _equalf(CMATRIX *a, double f) { bool result; if (COMPLEX(a)) { if (f == 0.0) return gsl_matrix_complex_isnull(CMAT(a)); gsl_matrix_complex *m = gsl_matrix_complex_alloc(WIDTH(a), HEIGHT(a)); gsl_matrix_complex_set_identity(m); gsl_matrix_complex_scale(m, gsl_complex_rect(f, 0)); result = gsl_matrix_complex_equal(CMAT(a), m); gsl_matrix_complex_free(m); } else { if (f == 0.0) return gsl_matrix_isnull(MAT(a)); gsl_matrix *m = gsl_matrix_alloc(WIDTH(a), HEIGHT(a)); gsl_matrix_set_identity(m); gsl_matrix_scale(m, f); result = gsl_matrix_equal(MAT(a), m); gsl_matrix_free(m); } return result; }
static void Givens_set_Shij(gsl_matrix* S, size_t i, size_t j, double alpha_ij) { gsl_matrix_set_identity(S); gsl_matrix_set(S, i, i, cos(alpha_ij)); gsl_matrix_set(S, j, j, cos(alpha_ij)); gsl_matrix_set(S, i, j, sin(alpha_ij)); gsl_matrix_set(S, j, i, -sin(alpha_ij)); }
gsl_vector* linear_ols_beta(gsl_vector *v_y, gsl_matrix *m_X){ size_t i_k = m_X->size2; gsl_vector *v_XTy = gsl_vector_alloc(i_k); gsl_vector *v_betahat = gsl_vector_alloc(i_k); gsl_matrix *m_XTX = gsl_matrix_alloc(i_k,i_k); gsl_matrix *m_invXTX = gsl_matrix_alloc(i_k,i_k); gsl_vector_set_all(v_XTy,0); gsl_vector_set_all(v_betahat,0); gsl_matrix_set_all(m_XTX,0); olsg(v_y,m_X,v_XTy,m_XTX); gsl_linalg_cholesky_decomp (m_XTX); gsl_matrix_set_identity(m_invXTX); gsl_blas_dtrsm (CblasLeft, CblasLower,CblasNoTrans,CblasNonUnit,1.0,m_XTX,m_invXTX); gsl_blas_dtrsm (CblasLeft, CblasLower,CblasTrans,CblasNonUnit,1.0,m_XTX,m_invXTX); gsl_vector_set_all(v_betahat,0.0); gsl_blas_dsymv (CblasLower,1.0,m_invXTX,v_XTy,0.0,v_betahat); gsl_vector_free(v_XTy); gsl_matrix_free(m_XTX); gsl_matrix_free(m_invXTX); return v_betahat; }
void Testwishart(CuTest* tc) { /*set a non-trivial location matrix*/ gsl_matrix* V = gsl_matrix_alloc(DIM, DIM); gsl_matrix_set_identity(V); gsl_matrix_scale(V, V0); for(size_t i=0; i<DIM; i++) for(size_t j=i+1; j < DIM; j++) { gsl_matrix_set(V, i, j, 1.0); gsl_matrix_set(V, j, i, 1.0); } p = mcmclib_wishart_lpdf_alloc(V, P); x = gsl_vector_alloc(DIM * DIM); gsl_matrix_view X_v = gsl_matrix_view_vector(x, DIM, DIM); gsl_matrix* X = &(X_v.matrix); gsl_matrix_set_all(X, 0.2); for(size_t i=0; i<DIM; i++) gsl_matrix_set(X, i, i, 1.0); CuAssertDblEquals(tc, -7.152627, lpdf(1.0), TOL); CuAssertDblEquals(tc, -29.549142, lpdf(0.5), TOL); CuAssertDblEquals(tc, 13.380252, lpdf(2.0), TOL); mcmclib_wishart_lpdf_free(p); gsl_vector_free(x); gsl_matrix_free(V); }
/** * Returns a GSL-indentity matrix. * * This free function returns a GSL-matrix with the provided dimensions. * It allocates the memory using gsl_matrix_alloc and the caller of the function * is responsible for freeing the memory again. * * @param rows :: Number of rows in the matrix. * @param cols :: Number of columns in the matrix. * @return Identity matrix with dimensions (rows, columns). */ gsl_matrix *getGSLIdentityMatrix(size_t rows, size_t cols) { gsl_matrix *gslMatrix = gsl_matrix_alloc(rows, cols); gsl_matrix_set_identity(gslMatrix); return gslMatrix; }
void kalmanFilterPosition0(double latitude, double longitude, gsl_matrix *x, gsl_matrix *P) { /* Wektor pomiarów */ gsl_matrix *z = gsl_matrix_alloc(2, 1); gsl_matrix_set(z, 0, 0, latitude); gsl_matrix_set(z, 1, 0, longitude); /* Macierz przejœcia ze stanu do pomiaru */ gsl_matrix *H = gsl_matrix_alloc(2,2); gsl_matrix_set_identity(H); /* Macierz kowariancji szumu procesu */ gsl_matrix *Q = gsl_matrix_alloc(2,2); gsl_matrix_set_identity(Q); double q = 0.00001; gsl_matrix_scale(Q, q); /* Macierz kowariancji szumu pomiaru */ double wrong_latitude = pow(WRONG_LATI, 2); double wrong_longitude = pow(WRONG_LONGI,2); gsl_matrix *R = gsl_matrix_calloc(2,2); gsl_matrix_set(R, 0, 0, wrong_latitude); gsl_matrix_set(R, 1, 1, wrong_longitude); /* Macierz jednostkowa */ gsl_matrix *I = gsl_matrix_alloc(2,2); gsl_matrix_set_identity(I); /* P = P(k-1) + Q */ gsl_matrix_add(P, Q); /* K = P/(P + R) */ gsl_matrix *K = gsl_matrix_calloc(2,2); (*K) = (*P); gsl_matrix_add(R, P); gsl_matrix_div_elements(K, R); /* P(k) = (I-K)*P */ gsl_matrix_sub(I, K); gsl_matrix_mul_elements(I, P); (*P) = (*I); /* x(k) = x + K(z-x) */ gsl_matrix_sub(z, x); gsl_matrix_mul_elements(K, z); gsl_matrix_add(x, K); //zaktualizowana wartoœc gsl_matrix_free(z); gsl_matrix_free(H); gsl_matrix_free(Q); gsl_matrix_free(R); gsl_matrix_free(I); gsl_matrix_free(K); }
int gsl_multifit_linear_Lk(const size_t p, const size_t k, gsl_matrix *L) { if (p <= k) { GSL_ERROR("p must be larger than derivative order", GSL_EBADLEN); } else if (k >= GSL_MULTIFIT_MAXK - 1) { GSL_ERROR("derivative order k too large", GSL_EBADLEN); } else if (p - k != L->size1 || p != L->size2) { GSL_ERROR("L matrix must be (p-k)-by-p", GSL_EBADLEN); } else { double c_data[GSL_MULTIFIT_MAXK]; gsl_vector_view cv = gsl_vector_view_array(c_data, k + 1); size_t i, j; /* zeroth derivative */ if (k == 0) { gsl_matrix_set_identity(L); return GSL_SUCCESS; } gsl_matrix_set_zero(L); gsl_vector_set_zero(&cv.vector); gsl_vector_set(&cv.vector, 0, -1.0); gsl_vector_set(&cv.vector, 1, 1.0); for (i = 1; i < k; ++i) { double cjm1 = 0.0; for (j = 0; j < k + 1; ++j) { double cj = gsl_vector_get(&cv.vector, j); gsl_vector_set(&cv.vector, j, cjm1 - cj); cjm1 = cj; } } /* build L, the c_i are the entries on the diagonals */ for (i = 0; i < k + 1; ++i) { gsl_vector_view v = gsl_matrix_superdiagonal(L, i); double ci = gsl_vector_get(&cv.vector, i); gsl_vector_set_all(&v.vector, ci); } return GSL_SUCCESS; } } /* gsl_multifit_linear_Lk() */
static void matrix_add_identity(gsl_matrix *m, double f) { gsl_matrix *id = gsl_matrix_alloc(m->size1, m->size2); gsl_matrix_set_identity(id); gsl_matrix_scale(id, f); gsl_matrix_add(m, id); gsl_matrix_free(id); }
int Matrix::initAsIdentity() { if((row==0)||(col==0)) { cout << "Warning!! Row or col = 0" << endl; } gsl_matrix_set_identity (matrix); return 0; }
int GlmTest::resampSmryCase(glm *model, gsl_matrix *bT, GrpMat *GrpXs, gsl_matrix *bO, unsigned int i ) { gsl_set_error_handler_off(); int status, isValid=TRUE; unsigned int j, k, id; gsl_vector_view yj, oj, xj; gsl_matrix *tXX = NULL; unsigned int nRows=tm->nRows, nParam=tm->nParam; if (bootID == NULL) { tXX = gsl_matrix_alloc(nParam, nParam); while (isValid==TRUE) { // if all isSingular==TRUE if (tm->reprand!=TRUE) GetRNGstate(); for (j=0; j<nRows; j++) { // resample Y, X, offsets accordingly if (tm->reprand==TRUE) id = (unsigned int) gsl_rng_uniform_int(rnd, nRows); else id = (unsigned int) nRows * Rf_runif(0, 1); xj = gsl_matrix_row(model->Xref, id); gsl_matrix_set_row(GrpXs[0].matrix, j, &xj.vector); yj = gsl_matrix_row(model->Yref, id); gsl_matrix_set_row(bT, j, &yj.vector); oj = gsl_matrix_row(model->Eta, id); gsl_matrix_set_row(bO, j, &oj.vector); } if (tm->reprand!=TRUE) PutRNGstate(); gsl_matrix_set_identity(tXX); gsl_blas_dsyrk(CblasLower,CblasTrans,1.0,GrpXs[0].matrix,0.0,tXX); status=gsl_linalg_cholesky_decomp(tXX); if (status!=GSL_EDOM) break; // if (calcDet(tXX)>eps) break; } for (k=2; k<nParam+2; k++) subX2(GrpXs[0].matrix, k-2, GrpXs[k].matrix); } else { for (j=0; j<nRows; j++) { id = (unsigned int) gsl_matrix_get(bootID, i, j); // resample Y and X and offset yj=gsl_matrix_row(model->Yref, id); gsl_matrix_set_row (bT, j, &yj.vector); oj = gsl_matrix_row(model->Eta, id); gsl_matrix_set_row(bO, j, &oj.vector); xj = gsl_matrix_row(model->Xref, id); gsl_matrix_set_row(GrpXs[0].matrix, j, &xj.vector); } for (k=2; k<nParam+2; k++) subX2(GrpXs[0].matrix, k-2, GrpXs[k].matrix); } gsl_matrix_free(tXX); return SUCCESS; }
static CMATRIX *MATRIX_identity(int width, int height, bool complex) { CMATRIX *m = MATRIX_create(width, height, complex, FALSE); if (complex) gsl_matrix_complex_set_identity(CMAT(m)); else gsl_matrix_set_identity(MAT(m)); return m; }
mcmclib_mcar_model* mcmclib_mcar_model_alloc(mcmclib_mcar_tilde_lpdf* m, gsl_vector* e) { mcmclib_mcar_model* a = (mcmclib_mcar_model*) malloc(sizeof(mcmclib_mcar_model)); a->lpdf = m; a->e = e; size_t p = m->p; gsl_matrix* V = gsl_matrix_alloc(p, p); gsl_matrix_set_identity(V); a->w = mcmclib_iwishart_lpdf_alloc(V, (unsigned int) p); gsl_matrix_free(V); return a; }
int gsl_linalg_hessenberg_unpack(gsl_matrix * H, gsl_vector * tau, gsl_matrix * U) { int s; gsl_matrix_set_identity(U); s = gsl_linalg_hessenberg_unpack_accum(H, tau, U); return s; } /* gsl_linalg_hessenberg_unpack() */
void fred2_solver(double a, double b, gsl_vector* t, gsl_vector* f, gsl_vector* w) { gsl_integration_glfixed_table* glgrid = gsl_integration_glfixed_table_alloc(MAX); gsl_permutation* p = gsl_permutation_alloc(MAX); gsl_matrix* lhs = gsl_matrix_alloc(MAX, MAX); gsl_matrix* ktilde = gsl_matrix_alloc(MAX, MAX); gsl_matrix* ak = gsl_matrix_alloc(MAX, MAX); gsl_vector* g = gsl_vector_alloc(MAX); int i, j, error, s; double ptsi, wghtsi; // set Gauss-Legendre integration points and weights for (i = 0; i < MAX; i++) { error = gsl_integration_glfixed_point(a, b, i, &ptsi, &wghtsi, glgrid); gsl_vector_set(t, i, ptsi); gsl_vector_set(w, i, wghtsi); } kernel(ak, t, t); aux_g(g, t); // fill in unit matrix first gsl_matrix_set_identity(lhs); for (i = 0; i < MAX; i++) { for (j = 0; j < MAX; j++) { gsl_matrix_set(ktilde, i, j, gsl_matrix_get(ak, i, j) * gsl_vector_get(w, j)); } } // set up LHS matrix error = gsl_matrix_sub(lhs, ktilde); gsl_linalg_LU_decomp(lhs, p, &s); gsl_linalg_LU_solve(lhs, p, g, f); gsl_integration_glfixed_table_free(glgrid); gsl_permutation_free(p); gsl_matrix_free(lhs); gsl_matrix_free(ktilde); gsl_vector_free(g); gsl_matrix_free(ak); return; }
void Nav_MotionEstimator::Nav_PKPropagator_Unicycle( double * Uk, double T) { double thPose = gsl_matrix_get(MeanPose, 2, 0); /** todo: move uncertanty values to configuration file */ double Sig1VtError = 0.01; double Sig2VthError = 0.01; double Sig3VtError = 0.01; double Sig4VthError = 0.01; double M11 = Sig1VtError * fabs(Uk[0]) + Sig2VthError * fabs(Uk[1]); double M22 = Sig3VtError * fabs(Uk[0]) + Sig4VthError * fabs(Uk[1]); //on the ekf use systematic error gsl_matrix_set(Nk, 0, 0, pow(M11, 2)); gsl_matrix_set(Nk, 0, 1, 0); gsl_matrix_set(Nk, 1, 0, 0); gsl_matrix_set(Nk, 1, 1, pow(M22, 2)); /** position gradient Fx =[[ 1 0 -vt*T*sin(theta) ] [ 0 1 vt*T*cos(theta) ] [ 0 0 1 ]] */ gsl_matrix_set_identity(Fx); gsl_matrix_set(Fx, 0, 2, -Uk[0] * T * sin(thPose)); gsl_matrix_set(Fx, 1, 2, Uk[0] * T * cos(thPose)); gsl_matrix_transpose_memcpy(FxT, Fx); //F transpose /** velocities gradient Fu = [ [ T*cos(theta) 0 ] [ T*sin(theta) 0 ] [ 0 T ]] */ gsl_matrix_set(Fu, 0, 0, T * cos(thPose)); gsl_matrix_set(Fu, 0, 1, 0); gsl_matrix_set(Fu, 1, 0, T * sin(thPose)); gsl_matrix_set(Fu, 1, 1, 0); gsl_matrix_set(Fu, 2, 0, 0); gsl_matrix_set(Fu, 2, 1, T); gsl_matrix_transpose_memcpy(FuT, Fu); //F transpose gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Fu, Nk, 0.0, Qk_temp); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Qk_temp, FuT, 0.0, Qk); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Fx, CovPose, 0.0, Pk_temp); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Pk_temp, FxT, 0.0, res3x3); gsl_matrix_set_zero(CovPose); gsl_matrix_add(CovPose, Qk); gsl_matrix_add(CovPose, res3x3); }
int gsl_linalg_LQ_unpack (const gsl_matrix * LQ, const gsl_vector * tau, gsl_matrix * Q, gsl_matrix * L) { const size_t N = LQ->size1; const size_t M = LQ->size2; if (Q->size1 != M || Q->size2 != M) { GSL_ERROR ("Q matrix must be M x M", GSL_ENOTSQR); } else if (L->size1 != N || L->size2 != M) { GSL_ERROR ("R matrix must be N x M", GSL_ENOTSQR); } else if (tau->size != GSL_MIN (M, N)) { GSL_ERROR ("size of tau must be MIN(M,N)", GSL_EBADLEN); } else { size_t i, j, l_border; /* Initialize Q to the identity */ gsl_matrix_set_identity (Q); for (i = GSL_MIN (M, N); i-- > 0;) { gsl_vector_const_view c = gsl_matrix_const_row (LQ, i); gsl_vector_const_view h = gsl_vector_const_subvector (&c.vector, i, M - i); gsl_matrix_view m = gsl_matrix_submatrix (Q, i, i, M - i, M - i); double ti = gsl_vector_get (tau, i); gsl_linalg_householder_mh (ti, &h.vector, &m.matrix); } /* Form the lower triangular matrix L from a packed LQ matrix */ for (i = 0; i < N; i++) { l_border=GSL_MIN(i,M-1); for (j = 0; j <= l_border ; j++) gsl_matrix_set (L, i, j, gsl_matrix_get (LQ, i, j)); for (j = l_border+1; j < M; j++) gsl_matrix_set (L, i, j, 0.0); } return GSL_SUCCESS; } }
int gsl_linalg_QR_unpack (const gsl_matrix * QR, const gsl_vector * tau, gsl_matrix * Q, gsl_matrix * R) { const size_t M = QR->size1; const size_t N = QR->size2; if (Q->size1 != M || Q->size2 != M) { GSL_ERROR ("Q matrix must be M x M", GSL_ENOTSQR); } else if (R->size1 != M || R->size2 != N) { GSL_ERROR ("R matrix must be M x N", GSL_ENOTSQR); } else if (tau->size != GSL_MIN (M, N)) { GSL_ERROR ("size of tau must be MIN(M,N)", GSL_EBADLEN); } else { size_t i, j; /* Initialize Q to the identity */ gsl_matrix_set_identity (Q); for (i = GSL_MIN (M, N); i-- > 0;) { gsl_vector_const_view c = gsl_matrix_const_column (QR, i); gsl_vector_const_view h = gsl_vector_const_subvector (&c.vector, i, M - i); gsl_matrix_view m = gsl_matrix_submatrix (Q, i, i, M - i, M - i); double ti = gsl_vector_get (tau, i); gsl_linalg_householder_hm (ti, &h.vector, &m.matrix); } /* Form the right triangular matrix R from a packed QR matrix */ for (i = 0; i < M; i++) { for (j = 0; j < i && j < N; j++) gsl_matrix_set (R, i, j, 0.0); for (j = i; j < N; j++) gsl_matrix_set (R, i, j, gsl_matrix_get (QR, i, j)); } return GSL_SUCCESS; } }
void getPosition0(Position0 *pos, GpsDate *gps, CircBuffer *gps_buffer) { gsl_matrix *x = gsl_matrix_calloc(2,1); gsl_matrix *P = gsl_matrix_alloc(2,2); gsl_matrix_set_identity(P); double previous_latitude0; double previous_longitude0; double latitude0; double longitude0; int i = 0; while (i < 5) { if((gpsUpdate(gps, gps_buffer)) && ((*gps).status == 'A')) { kalmanFilterPosition0((*gps).latitude, (*gps).longitude, x, P); i++; printf("Aktualizacja w filtrze KALMANA poraz %i", i+1); //potrzebne do debugowania } } i = 0; while(i<5) { previous_latitude0 = gsl_matrix_get(x, 0, 0); previous_longitude0 = gsl_matrix_get(x, 1, 0); if((gpsUpdate(gps, gps_buffer)) && ((*gps).status == 'A')) { kalmanFilterPosition0((*gps).latitude, (*gps).longitude, x, P); latitude0 = gsl_matrix_get(x, 0, 0); longitude0 = gsl_matrix_get(x, 1, 0); /* Blok danych do wyœwietlenia przy debugowaniu */ printf("Wartoœc previous_latitude0 = %f", previous_latitude0); printf("Wartoœc previous_longitude0 = %f", previous_longitude0); printf("Wartoœc latitude0 = %f", latitude0); printf("Wartoœc longitude0 = %f", longitude0); if((previous_latitude0 == latitude0) && (previous_longitude0 == longitude0)) { i++; //Kolejny wektor x identyczny z poprzednim wektorem x if(i == 5) { pos->latitude0 = latitude0; pos->longitude0 = longitude0; gsl_matrix_free(x); gsl_matrix_free(P); } } else { i = 0; } } } }
//int GlmTest::resampAnovaCase(glm *model, gsl_matrix *Onull, gsl_matrix *bT, gsl_matrix *bX, gsl_matrix *bO, gsl_matrix *bOnull, unsigned int i) int GlmTest::resampAnovaCase(glm *model, gsl_matrix *bT, gsl_matrix *bX, gsl_matrix *bO, unsigned int i) { gsl_set_error_handler_off(); int status, isValid=TRUE; unsigned int j, id, nP; gsl_vector_view yj, xj, oj; nP = model->Xref->size2; gsl_matrix *tXX = gsl_matrix_alloc(nP, nP); unsigned int nRows=tm->nRows; if (bootID == NULL) { while (isValid==TRUE) { if (tm->reprand!=TRUE) GetRNGstate(); for (j=0; j<nRows; j++) { if (tm->reprand==TRUE) id=(unsigned int)gsl_rng_uniform_int(rnd, nRows); else id=(unsigned int) nRows*Rf_runif(0, 1); // resample Y and X and offset yj=gsl_matrix_row(model->Yref, id); xj = gsl_matrix_row(model->Xref, id); oj = gsl_matrix_row(model->Eta, id); gsl_matrix_set_row (bT, j, &yj.vector); gsl_matrix_set_row(bX, j, &xj.vector); gsl_matrix_set_row(bO, j, &oj.vector); } if (tm->reprand!=TRUE) PutRNGstate(); gsl_matrix_set_identity(tXX); gsl_blas_dsyrk(CblasLower,CblasTrans,1.0,bX,0.0,tXX); status=gsl_linalg_cholesky_decomp(tXX); if (status!=GSL_EDOM) break; } } else { for (j=0; j<nRows; j++) { id = (unsigned int) gsl_matrix_get(bootID, i, j); // resample Y and X and offset yj=gsl_matrix_row(model->Yref, id); xj = gsl_matrix_row(model->Xref, id); oj = gsl_matrix_row(model->Oref, id); gsl_matrix_set_row (bT, j, &yj.vector); gsl_matrix_set_row(bX, j, &xj.vector); gsl_matrix_set_row(bO, j, &oj.vector); } } gsl_matrix_free(tXX); return SUCCESS; }
// constructor GlmTest::GlmTest(const mv_Method *tm) : tm(tm) { eps = tm->tol; // eps = 1e-6; smryStat = NULL; Psmry = NULL; anovaStat = NULL; Panova = NULL; Xin = NULL; XBeta = NULL; Sigma = NULL; bootID = NULL; bootStore = NULL; // Prepared for geeCalc L = gsl_matrix_alloc(tm->nParam, tm->nParam); gsl_matrix_set_identity(L); Rlambda = gsl_matrix_alloc(tm->nVars, tm->nVars); Wj = gsl_matrix_alloc(tm->nRows, tm->nRows); // Initialize GSL rnd environment variables const gsl_rng_type *T; gsl_rng_env_setup(); T = gsl_rng_default; // an mt19937 generator with a seed of 0 rnd = gsl_rng_alloc(T); if (tm->reprand != TRUE) { struct timeval tv; // seed generation based on time gettimeofday(&tv, 0); unsigned long mySeed = tv.tv_sec + tv.tv_usec; gsl_rng_set(rnd, mySeed); // reset seed } if (tm->resamp == PERMUTE) { permid = (size_t *)malloc(tm->nRows * sizeof(size_t)); for (size_t i = 0; i < tm->nRows; i++) permid[i] = i; } else permid = NULL; if (tm->resamp == MONTECARLO) { XBeta = gsl_matrix_alloc(tm->nRows, tm->nVars); Sigma = gsl_matrix_alloc(tm->nVars, tm->nVars); } aic = new double[tm->nVars]; }
// f = (1/2) x^T Ax + b^T x void prox_quad(gsl_vector *x, const double rho, gsl_matrix *A, gsl_matrix *b) { gsl_matrix *I = gsl_matrix_alloc(A->size1); gsl_matrix_set_identity(I); gsl_matrix_scale(I, rho); gsl_matrix_add(I, A); gsl_vector_scale(x, rho); gsl_vector_scale(b, -1); gsl_vector_add(b, x); gsl_linalg_cholesky_decomp(I); gsl_linalg_cholesky_solve(I, b, x); gsl_matrix_free(I); }
match_result algorithm_iden::match(graph& g, graph& h,gsl_matrix* gm_P_i, gsl_matrix* gm_ldh,double dalpha_ldh) { if (bverbose) *gout<<"Identity matching"<<std::endl; //some duplicate variables match_result mres; mres.gm_P=gsl_matrix_alloc(N,N); gsl_matrix_set_identity(mres.gm_P); mres.gm_P_exact=NULL; //initial score mres.vd_trace.push_back(graph_dist(g,h,cscore_matrix)); //final score mres.vd_trace.push_back(graph_dist(g,h,mres.gm_P,cscore_matrix)); mres.dres=mres.vd_trace.at(1); mres.inum_iteration=2; return mres; }
gsl_matrix* calc_Ws(double s, double tres, double* Qxx_m, double* Qyy_m, double* Qxy_m, double* Qyx_m, int kx, int ky){ gsl_matrix *ws; ws=gsl_matrix_alloc(kx,kx); gsl_matrix_set_identity(ws); gsl_matrix_scale(ws,s); //Calculate H(s) gsl_matrix *hs = calc_Hs(s,tres,Qxx_m,Qyy_m,Qxy_m,Qyx_m, kx, ky); gsl_matrix_sub(ws,hs); //cleanup gsl_matrix_free(hs); return ws; }