int crearg (gsl_vector* mtiempo, gsl_matrix* answer, int size) { gsl_vector* cte = gsl_vector_calloc( size ); gsl_vector* tt = gsl_vector_calloc( size ); gsl_vector_set_all (cte , 1.0); gsl_vector_add (tt,mtiempo); gsl_vector_mul (tt, mtiempo); gsl_vector_scale (tt, 0.5); gsl_matrix_set_col (answer, 0, cte); gsl_matrix_set_col (answer, 1, mtiempo); gsl_matrix_set_col (answer, 2, tt); return 0; }
int GMRFLib_gsl_spd_inverse(gsl_matrix * A) { /* * replace SPD matrix A with its inverse */ gsl_matrix *L; gsl_vector *x; size_t i, n; assert(A->size1 == A->size2); n = A->size1; x = gsl_vector_calloc(n); L = GMRFLib_gsl_duplicate_matrix(A); gsl_linalg_cholesky_decomp(L); for (i = 0; i < n; i++) { gsl_vector_set_basis(x, i); gsl_linalg_cholesky_svx(L, x); gsl_matrix_set_col(A, i, x); } gsl_vector_free(x); gsl_matrix_free(L); return GMRFLib_SUCCESS; }
/* compute compact QR factorization M is mxn; Q is mxk and R is kxk */ void compute_QR_compact_factorization(gsl_matrix *M, gsl_matrix *Q, gsl_matrix *R){ int i,j,m,n,k; m = M->size1; n = M->size2; k = min(m,n); //printf("QR setup..\n"); gsl_matrix *QR = gsl_matrix_calloc(M->size1, M->size2); gsl_vector *tau = gsl_vector_alloc(min(M->size1,M->size2)); gsl_matrix_memcpy (QR, M); //printf("QR decomp..\n"); gsl_linalg_QR_decomp (QR, tau); //printf("extract R..\n"); for(i=0; i<k; i++){ for(j=0; j<k; j++){ if(j>=i){ gsl_matrix_set(R,i,j,gsl_matrix_get(QR,i,j)); } } } //printf("extract Q..\n"); gsl_vector *vj = gsl_vector_calloc(m); for(j=0; j<k; j++){ gsl_vector_set(vj,j,1.0); gsl_linalg_QR_Qvec (QR, tau, vj); gsl_matrix_set_col(Q,j,vj); vj = gsl_vector_calloc(m); } }
void VarproFunction::computeJacobianOfCorrection( const gsl_vector* yr, const gsl_matrix *Rorig, const gsl_matrix *perm, gsl_matrix *jac ) { size_t nrow = perm != NULL ? perm->size2 : getNrow(); gsl_matrix_set_zero(jac); gsl_matrix_set_zero(myTmpGradR); fillZmatTmpJac(myTmpJac, yr, Rorig, 1); for (size_t i = 0; i < perm->size2; i++) { for (size_t j = 0; j < getD(); j++) { gsl_vector_view jac_col = gsl_matrix_column(jac, i * getD() + j); /* Compute first term (correction of Gam^{-1} z_{ij}) */ gsl_vector_set_zero(myTmpCorr); mulZmatPerm(myTmpJacobianCol, myTmpJac, perm, i, j); myGam->multInvGammaVector(myTmpJacobianCol); myStruct->multByGtUnweighted(myTmpCorr, Rorig, myTmpJacobianCol, -1, 1); /* Compute second term (gamma * dG_{ij} * yr) */ gsl_matrix_set_zero(myTmpGradR); setPhiPermCol(i, perm, myPhiPermCol); gsl_matrix_set_col(myTmpGradR, j, myPhiPermCol); myStruct->multByGtUnweighted(myTmpCorr, myTmpGradR, yr, -1, 1); myStruct->multByWInv(myTmpCorr, 1); gsl_vector_memcpy(&jac_col.vector, myTmpCorr); } } }
/* compute compact QR factorization and get Q M is mxn; Q is mxk and R is kxk (not computed) */ void QR_factorization_getQ(gsl_matrix *M, gsl_matrix *Q){ int i,j,m,n,k; m = M->size1; n = M->size2; k = min(m,n); gsl_matrix *QR = gsl_matrix_calloc(M->size1, M->size2); gsl_vector *tau = gsl_vector_alloc(min(M->size1,M->size2)); gsl_matrix_memcpy (QR, M); gsl_linalg_QR_decomp (QR, tau); gsl_vector *vj = gsl_vector_calloc(m); for(j=0; j<k; j++){ gsl_vector_set(vj,j,1.0); gsl_linalg_QR_Qvec (QR, tau, vj); gsl_matrix_set_col(Q,j,vj); vj = gsl_vector_calloc(m); } gsl_vector_free(vj); gsl_vector_free(tau); gsl_matrix_free(QR); }
int Schrodinger_Solve(double dx, int Dim, int barrier_L, int barrier_R, double E_well, double *Meff_ptr, double *subband_ptr, gsl_eigen_symmv_workspace *Workspace, gsl_matrix *Hamil_ptr, gsl_vector *Eigvalue_ptr, gsl_matrix *Eigvector_ptr, gsl_vector *s_eigvector_ptr, gsl_vector *Potential_ptr, gsl_matrix *Norm_Eigvector_ptr, FILE *fp_1, FILE *fp_2 ){ int n, m, N=0; double Hbar=1.054571E-34, Element; double t_0=(Hbar*Hbar)/(2*dx*dx); for (n = 0; n < Dim; n++) { for (m = 0; m < Dim; m++) { if (n == m) /*main diagonal*/ { Element = t_0/Meff_ptr[n-1] + t_0/Meff_ptr[n+1] + gsl_vector_get(Potential_ptr, n); } else if (n == m-1) /*subdiagonal*/ { Element = -1*t_0/Meff_ptr[n-1]; } else if (n == m+1) /*superdiagonal*/ { Element = -1*t_0/Meff_ptr[n+1]; } else { Element = 0; /*all other matrix elements*/ } gsl_matrix_set(Hamil_ptr, n, m, Element); /*transfer value to matrix*/ } } gsl_eigen_symmv(Hamil_ptr, Eigvalue_ptr, Eigvector_ptr, Workspace); /*solve matrix*/ gsl_eigen_symmv_sort(Eigvalue_ptr, Eigvector_ptr, GSL_EIGEN_SORT_VAL_ASC); /*re-order the results in ascending order*/ /*output eigenvalues*/ for (n = 0; n < Dim; n++){ double eigenvalue = gsl_vector_get(Eigvalue_ptr, n); if (eigenvalue>E_well && eigenvalue<0) /*select confined states*/ { fprintf(fp_1, "n%E", (eigenvalue/1.6E-22)); /*output in meV*/ subband_ptr[n] = eigenvalue; /*For Fermi-Dirac */ N = N+1; /* sum the number of eigenstates*/ } } /*output eigenvectors*/ for (n = 0; n < N; n++){ /*retrieve eigenvectors for confined states only*/ fprintf(fp_2, "nnnn"); gsl_matrix_get_col(s_eigvector_ptr, Eigvector_ptr, n); /*retrieve nth eigenvector*/ Normalise(s_eigvector_ptr, Dim, dx, fp_2); /*normalise eigenvector*/ gsl_matrix_set_col(Norm_Eigvector_ptr, n, s_eigvector_ptr); } return N; }
static void set_data_mat_K_intercept_col(struct mvar_fit *fit, gsl_matrix *K) { gsl_vector *vec; vec = gsl_vector_alloc(fit->nr_eqs); gsl_vector_set_all(vec, 1.0); gsl_matrix_set_col(K, 0, vec); gsl_vector_free(vec); }
/* copy the first k columns of M into M_out where k = M_out->ncols (M_out pre-initialized) */ void matrix_copy_first_columns(gsl_matrix *M_out, gsl_matrix *M){ int i,k; k = M_out->size2; gsl_vector * col_vec; for(i=0; i<k; i++){ col_vec = gsl_vector_calloc(M->size1); gsl_matrix_get_col(col_vec,M,i); gsl_matrix_set_col(M_out,i,col_vec); gsl_vector_free(col_vec); } }
void HLayeredBlWStructure::fillMatrixFromP( gsl_matrix* c, const gsl_vector* p ) { size_t sum_np = 0, sum_nl = 0, l_1, j; gsl_vector psub; for (l_1 = 0; l_1 < getQ(); sum_np += getLayerNp(l_1), sum_nl += getLayerLag(l_1), ++l_1) { for (j = 0; j < getLayerLag(l_1); ++j) { psub = gsl_vector_const_subvector(p, sum_np + j, getN()).vector; gsl_matrix_set_col(c, j + sum_nl, &psub); } } }
static void set_data_mat_K_observations(struct mvar_fit *fit, gsl_matrix *K) { gsl_matrix_view mat_view; gsl_vector_view vec_view; size_t i, j; mat_view = gsl_matrix_submatrix(fit->v, fit->p, 0, fit->n - fit->p, fit->m); for (i = fit->nr_params, j = 0; i < K->size2; i++, j++) { vec_view = gsl_matrix_column(&mat_view.matrix, j); gsl_matrix_set_col(K, i, &vec_view.vector); } }
void ccl_mat_transpose(const double* mat,int i, int j,double* mat_T){ gsl_matrix * mat_ = gsl_matrix_alloc(i,j); memcpy(mat_->data,mat,i*j*sizeof(double)); gsl_matrix mat_T_ = gsl_matrix_view_array(mat_T,j,i).matrix; gsl_vector * vec = gsl_vector_alloc(j); int row, col; for (row=0;row<i;row++){ gsl_matrix_get_row(vec,mat_,row); gsl_matrix_set_col(&mat_T_,row,vec); } gsl_vector_free(vec); gsl_matrix_free(mat_); }
void ccl_get_sub_mat_cols(const double * mat, const int row, const int col,const int * ind, int size, double * ret){ gsl_matrix * mat_ = gsl_matrix_alloc(row,col); memcpy(mat_->data,mat,row*col*sizeof(double)); gsl_matrix_view ret_ = gsl_matrix_view_array(ret,row,size); int i; for (i=0;i<size;i++){ gsl_vector* vec = gsl_vector_alloc(row); gsl_matrix_get_col(vec,mat_,ind[i]); gsl_matrix_set_col(&ret_.matrix,i,vec); gsl_vector_free(vec); } gsl_matrix_free(mat_); }
void VarproFunction::computePseudoJacobianLsFromYr( const gsl_vector* yr, const gsl_matrix *Rorig, const gsl_matrix *perm, gsl_matrix *pjac, double factor ) { size_t nrow = perm != NULL ? perm->size2 : getNrow(); fillZmatTmpJac(myTmpJac, yr, Rorig, factor); for (size_t i = 0; i < nrow; i++) { for (size_t j = 0; j < getD(); j++) { mulZmatPerm(myTmpJacobianCol, myTmpJac, perm, i, j); myGam->multInvCholeskyVector(myTmpJacobianCol, 1); gsl_matrix_set_col(pjac, i * getD() + j, myTmpJacobianCol); } } }
static void set_data_mat_K_predictors(struct mvar_fit *fit, gsl_matrix *K) { gsl_matrix_view mat_view; gsl_vector_view vec_view; size_t i, j, k; for (i = 1; i <= fit->p; i++) { mat_view = gsl_matrix_submatrix(fit->v, fit->p - i, 0, fit->n - fit->p, fit->m); for (j = 1 + fit->m * (i - 1), k = 0; j < 1 + fit->m * i; j++, k++) { vec_view = gsl_matrix_column(&mat_view.matrix, k); gsl_matrix_set_col(K, j, &vec_view.vector); } } }
int semirmvnorm(const gsl_rng *rnd, const unsigned int n, const gsl_matrix *Sigma, gsl_vector *randeffect) { unsigned int k, r=0; double lambda; gsl_matrix *work = gsl_matrix_alloc(n,n); gsl_matrix_memcpy(work, Sigma); // replace cholesky with eigen decomposition gsl_eigen_symmv_workspace * w = gsl_eigen_symmv_alloc (n); gsl_vector *eval=gsl_vector_alloc (n); gsl_matrix *evec=gsl_matrix_alloc (n, n); // work = evec*diag(eval)*t(evec) gsl_eigen_symmv (work, eval, evec, w); // displayvector (eval, "eigen values of work"); // displaymatrix (evec, "eigen vector of work"); for (k=0; k<n; k++) { gsl_vector_view evec_i=gsl_matrix_column(evec, k); lambda=gsl_vector_get(eval, k); if (lambda>10e-10){ // non-zero variables // U = t(eval(r)*evec(:, r)) gsl_vector_scale (&evec_i.vector, sqrt(lambda)); // copy U to work gsl_matrix_set_col(work, r, &evec_i.vector); r++; } } // printf("r=%d.\n", r); gsl_matrix_view U=gsl_matrix_submatrix (work, 0, 0, n, r); // displaymatrix (&U.matrix, "partial eigen vectors"); // generate standard normal vector gsl_vector *z=gsl_vector_alloc(r); for(k=0; k<r; k++) gsl_vector_set( z, k, gsl_ran_ugaussian(rnd) ); // displayvector (z, "z"); // X_i = mu_i + t(U)*z gsl_blas_dgemv (CblasNoTrans, 1.0, &U.matrix, z, 0.0, randeffect); // displayvector (randeffect, "randeffect"); gsl_matrix_free(work); gsl_eigen_symmv_free(w); gsl_matrix_free(evec); gsl_vector_free(eval); gsl_vector_free(z); return 0; }
int juxtapose_matrix_array(gsl_matrix ** pieces, size_t N, gsl_matrix * m_assembled) { /* First pieces fix the height */ size_t height = pieces[0]->size1; gsl_vector * v_T = gsl_vector_calloc(height); size_t i; size_t offset = 0; for(i = 0; i < N; i++) { size_t j; size_t width = pieces[i]->size2; for(j = 0; j < width; j++) { gsl_matrix_get_col(v_T,pieces[i],j); gsl_matrix_set_col(m_assembled,offset+j,v_T); } offset += width; } gsl_vector_free(v_T); return 0; }
static void diagonalize_covariance(void) { gsl_vector *vec_dum=gsl_vector_alloc(glob_n_nu); gsl_matrix *evec_dum=gsl_matrix_alloc(glob_n_nu,glob_n_nu); gsl_vector *eval_dum=gsl_vector_alloc(glob_n_nu); eigenvals=gsl_vector_alloc(glob_n_nu); eigenvecs=gsl_matrix_alloc(glob_n_nu,glob_n_nu); //Diagonalize gsl_eigen_symmv_workspace *w=gsl_eigen_symmv_alloc(glob_n_nu); gsl_eigen_symmv(covariance,eval_dum,evec_dum,w); gsl_eigen_symmv_free(w); //Sort eigenvalues gsl_permutation *p=gsl_permutation_alloc(glob_n_nu); gsl_sort_vector_index(p,eval_dum); int ii; for(ii=0;ii<glob_n_nu;ii++) { int inew=gsl_permutation_get(p,ii); gsl_vector_set(eigenvals,ii,gsl_vector_get(eval_dum,inew)); gsl_matrix_get_col(vec_dum,evec_dum,inew); gsl_matrix_set_col(eigenvecs,ii,vec_dum); } gsl_permutation_free(p); gsl_vector_free(vec_dum); gsl_vector_free(eval_dum); gsl_matrix_free(evec_dum); FILE *fo; char fname[256]; sprintf(fname,"%s_pca_eigvals.dat",glob_prefix_out); fo=my_fopen(fname,"w"); for(ii=0;ii<glob_n_nu;ii++) { double lambda=gsl_vector_get(eigenvals,ii); fprintf(fo,"%d %lE\n",ii,lambda); } fclose(fo); }
/* build orthonormal basis matrix Q = Y; for j=1:k vj = Q(:,j); for i=1:(j-1) vi = Q(:,i); vj = vj - project_vec(vj,vi); end vj = vj/norm(vj); Q(:,j) = vj; end */ void build_orthonormal_basis_from_mat(gsl_matrix *A, gsl_matrix *Q){ int m,n,i,j,ind,num_ortos=2; double vec_norm; gsl_vector *vi,*vj,*p; m = A->size1; n = A->size2; vi = gsl_vector_calloc(m); vj = gsl_vector_calloc(m); p = gsl_vector_calloc(m); gsl_matrix_memcpy(Q, A); for(ind=0; ind<num_ortos; ind++){ for(j=0; j<n; j++){ gsl_matrix_get_col(vj, Q, j); for(i=0; i<j; i++){ gsl_matrix_get_col(vi, Q, i); project_vector(vj, vi, p); gsl_vector_sub(vj, p); } vec_norm = gsl_blas_dnrm2(vj); gsl_vector_scale(vj, 1.0/vec_norm); gsl_matrix_set_col (Q, j, vj); } } }
void update_position(state_t *state, double left_ticks, double right_ticks){ printf("updating position\n"); double dl, dr, delta_x, delta_y, delta_theta; // IMPLEMENT ME gsl_matrix *J_plus = gsl_matrix_calloc(3,6); gsl_vector *old_pose = gsl_vector_calloc(3); gsl_vector *change_pose = gsl_vector_calloc(3); gsl_vector *new_pose = gsl_vector_calloc(3); gsl_matrix *old_sigma = gsl_matrix_calloc(3,3); gsl_matrix *change_sigma = gsl_matrix_calloc(3,3); gsl_matrix *new_sigma = gsl_matrix_calloc(3,3); memcpy(old_pose->data, state->xyt, sizeof state->xyt); memcpy (old_sigma->data, state->Sigma, sizeof state->Sigma); static unsigned char first_feedback = 0; if(first_feedback == 0){ state->left_ticks = left_ticks; state->right_ticks = right_ticks; first_feedback = 1; } // calculate new expected value dl = METERS_PER_TICK * (left_ticks - state->left_ticks); dr = METERS_PER_TICK * (right_ticks - state->right_ticks); delta_x = (dl + dr)/2.0; delta_y = 0; delta_theta = gslu_math_mod2pi((dr - dl)/BASELINE); gsl_vector_set(old_pose,2,gslu_math_mod2pi(old_pose->data[2])); gsl_vector_set(change_pose,0,delta_x); gsl_vector_set(change_pose,1,delta_y); gsl_vector_set(change_pose,2,delta_theta); //printf("delx,dely,delT = %f,%f,%f\n",delta_x,delta_y,delta_theta); xyt_head2tail_gsl(new_pose, J_plus, old_pose, change_pose); gsl_vector_set(new_pose,2,gslu_math_mod2pi(new_pose->data[2])); //printf("dl,dr,theta = %f,%f,%f\n",dl,dr,new_pose->data[2]); //printf("x,y,theta = %f,%f,%f\n",new_pose->data[0],new_pose->data[1],new_pose->data[2]); // calculate new covariance gsl_matrix_set(change_sigma, 0,0, (ALPHA/4.0)*(fabs(dl) + fabs(dr))); gsl_matrix_set(change_sigma, 0,2, (ALPHA/(2*BASELINE))*(fabs(dr) - fabs(dl))); gsl_matrix_set(change_sigma, 1,1, BETA * fabs(dl + dr)); gsl_matrix_set(change_sigma, 2,0, (ALPHA/(2*BASELINE))*(fabs(dr) - fabs(dl))); gsl_matrix_set(change_sigma, 2,2, (ALPHA/(BASELINE*BASELINE))*(fabs(dl) + fabs(dr))); //gsl_matrix_set(change_sigma, 2,2, 0); //printf("sig theta = %f\n",(ALPHA/(BASELINE*BASELINE))*(fabs(dl) + fabs(dr))); gsl_matrix *J_plus1 = gsl_matrix_calloc(3,3); gsl_matrix *J_plus2 = gsl_matrix_calloc(3,3); gsl_vector *column = gsl_vector_calloc(3); // set up J_plus1 and Jplus2 gsl_matrix_get_col(column, J_plus, 0); gsl_matrix_set_col(J_plus1, 0 ,column); gsl_matrix_get_col(column, J_plus, 1); gsl_matrix_set_col(J_plus1, 1 ,column); gsl_matrix_get_col(column, J_plus, 2); gsl_matrix_set_col(J_plus1, 2 ,column); gsl_matrix_get_col(column, J_plus, 3); gsl_matrix_set_col(J_plus2, 0 ,column); gsl_matrix_get_col(column, J_plus, 4); gsl_matrix_set_col(J_plus2, 1 ,column); gsl_matrix_get_col(column, J_plus, 5); gsl_matrix_set_col(J_plus2, 2 ,column); // get transposes gsl_matrix *J_plus1_T = gsl_matrix_calloc(3,3); gsl_matrix *J_plus2_T = gsl_matrix_calloc(3,3); gsl_matrix_transpose_memcpy(J_plus1_T, J_plus1); gsl_matrix_transpose_memcpy(J_plus2_T, J_plus2); // multiply to get the covariances gsl_matrix *temp1 = gsl_matrix_calloc(3,3); gsl_matrix *temp2 = gsl_matrix_calloc(3,3); gsl_matrix *sigma_temp = gsl_matrix_calloc(3,3); temp1 = matrix_multiply(J_plus1, old_sigma); new_sigma = matrix_multiply(temp1, J_plus1_T); gsl_blas_dgemm (CblasNoTrans, CblasNoTrans,1.0, J_plus1, old_sigma,0.0, temp1); gsl_blas_dgemm (CblasNoTrans, CblasTrans,1.0, temp1, J_plus1,0.0, new_sigma); gsl_blas_dgemm (CblasNoTrans, CblasNoTrans,1.0, J_plus2, change_sigma,0.0, temp2); gsl_blas_dgemm (CblasNoTrans, CblasTrans,1.0, temp2, J_plus2,0.0, sigma_temp); printf("J1EJ1:\n"); print_matrix(new_sigma); //temp3 = matrix_multiply(J_plus2, change_sigma); //temp4 = matrix_multiply(temp3, J_plus2_T); printf("J2EJ2\n"); print_matrix(sigma_temp); gsl_matrix_add(new_sigma, sigma_temp); // gsl_matrix_set(new_sigma,2,2,fabs(gslu_math_mod2pi(new_sigma->data[8]))); state->left_ticks = left_ticks; state->right_ticks = right_ticks; // update state memcpy(state->xyt, new_pose->data, sizeof state->xyt); memcpy(state->Sigma, new_sigma->data, sizeof state->Sigma); // prints for testing //printf("Old Position:\n"); //print_vector(old_pose); //printf("Change Position:\n"); //print_vector(change_pose); printf("Jacobian Plus:\n"); print_matrix(J_plus); printf("Old Sigma:\n"); print_matrix(old_sigma); printf("Change Sigma:\n"); print_matrix(change_sigma); printf("New Sigma:\n"); print_matrix(new_sigma); printf("New Position:\n"); print_vector(new_pose); gsl_matrix_free(J_plus); gsl_matrix_free(J_plus1); gsl_matrix_free(J_plus2); gsl_matrix_free(temp1); gsl_matrix_free(temp2); gsl_matrix_free(sigma_temp); gsl_matrix_free(old_sigma); gsl_matrix_free(change_sigma); gsl_matrix_free(new_sigma); gsl_vector_free(old_pose); gsl_vector_free(change_pose); gsl_vector_free(new_pose); return; }
int main(int argc, char **argv){ int row = atoi(argv[2]); int col = atoi(argv[3]); printf("%d %d\n", row, col); gsl_matrix* data = gsl_matrix_alloc(row, col); //gsl_matrix* data = gsl_matrix_alloc(col, row); FILE* f = fopen(argv[1], "r"); gsl_matrix_fscanf(f, data); //gsl_matrix_fread(f, data); //gsl_matrix_transpose_memcpy(data, data_raw); fclose(f); //printf("%f %f", gsl_matrix_get(data,0,0), gsl_matrix_get(data,0,1)); //f = fopen("test.dat", "w"); //gsl_matrix_fprintf(f, data, "%f"); //fclose(f); // data centering, subtract the mean in each dimension (col.-wise) int i, j; double mean, sum, std; gsl_vector_view col_vector; for (i = 0; i < col; ++i){ col_vector = gsl_matrix_column(data, i); mean = gsl_stats_mean((&col_vector.vector)->data, 1, (&col_vector.vector)->size); gsl_vector_add_constant(&col_vector.vector, -mean); gsl_matrix_set_col(data, i, &col_vector.vector); } char filename[50]; //sprintf(filename, "%s.zscore", argv[1]); //print2file(filename, data); gsl_matrix* u; if (col > row) { u = gsl_matrix_alloc(data->size2, data->size1); gsl_matrix_transpose_memcpy(u, data); } else { u = gsl_matrix_alloc(data->size1, data->size2); gsl_matrix_memcpy(u, data); } // svd gsl_matrix* X = gsl_matrix_alloc(col, col); gsl_matrix* V = gsl_matrix_alloc(u->size2, u->size2); gsl_vector* S = gsl_vector_alloc(u->size2); gsl_vector* work = gsl_vector_alloc(u->size2); gsl_linalg_SV_decomp(u, V, S, work); //gsl_linalg_SV_decomp_jacobi(u, V, S); // mode coefficient //print2file("u.dat", u); /* // characteristic mode gsl_matrix* diag = diag_alloc(S); gsl_matrix* mode = gsl_matrix_alloc(diag->size1, V->size1); gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, diag, V, 0.0, mode); gsl_matrix_transpose(mode); print2file("mode.dat", mode); gsl_matrix_transpose(mode); */ // reconstruction gsl_matrix *recons = gsl_matrix_alloc(u->size2, data->size1); if (col > row) { gsl_matrix_view data_sub = gsl_matrix_submatrix(data, 0, 0, u->size2, u->size2); gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, V, &data_sub.matrix, 0.0, recons); } else gsl_blas_dgemm(CblasTrans, CblasTrans, 1.0, V, data, 0.0, recons); //gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, u, mode, 0.0, // recons); gsl_matrix *recons_trans = gsl_matrix_alloc(recons->size2, recons->size1); gsl_matrix_transpose_memcpy(recons_trans, recons); // take the first two eigenvectors gsl_matrix_view final = gsl_matrix_submatrix(recons_trans, 0, 0, recons_trans->size1, 2); print2file(argv[4], &final.matrix); // eigenvalue gsl_vector_mul(S, S); f = fopen("eigenvalue.dat", "w"); //gsl_vector_fprintf(f, S, "%f"); fclose(f); gsl_matrix_free(data); gsl_matrix_free(X); gsl_matrix_free(V); //gsl_matrix_free(diag); //gsl_matrix_free(mode); gsl_matrix_free(recons); gsl_matrix_free(recons_trans); gsl_matrix_free(u); gsl_vector_free(S); gsl_vector_free(work); //gsl_vector_free(zero); //gsl_vector_free(corrcoef); //gsl_vector_free(corrcoef_mean); return 0; }
// Wald Test used in both summary and anova (polymophism) int GlmTest::GeeWald(glm *Alt, gsl_matrix *LL, gsl_vector *teststat) { gsl_set_error_handler_off(); unsigned int i, j, l; double alpha, result, sum=0; unsigned int nP = Alt->nParams; unsigned int nDF = LL->size1; unsigned int nVars=tm->nVars, nRows=tm->nRows; int status; gsl_vector *LBeta = gsl_vector_alloc(nVars*nDF); gsl_vector_set_zero(LBeta); gsl_matrix *w1jX1=gsl_matrix_alloc(nRows, nP); gsl_matrix *XwX=gsl_matrix_alloc(nP, nP); gsl_matrix *Rl2 = gsl_matrix_alloc(nDF, nP); gsl_matrix *IinvN = gsl_matrix_alloc(nDF, nDF); gsl_matrix *IinvRl = gsl_matrix_alloc(nVars*nDF, nVars*nDF); gsl_vector *tmp = gsl_vector_alloc(nVars*nDF); gsl_vector_view tmp2, wj, LBj, bj; //, dj; gsl_matrix_view Rl; gsl_matrix_set_zero(IinvRl); GrpMat *Z = (GrpMat*)malloc(nVars*sizeof(GrpMat)); for (j=0; j<nVars; j++){ Z[j].matrix = gsl_matrix_alloc(nP, nRows); // w1jX1 = W^1/2 * X wj=gsl_matrix_column(Alt->wHalf, j); for (i=0; i<nP; i++) gsl_matrix_set_col (w1jX1, i, &wj.vector); gsl_matrix_mul_elements (w1jX1, Alt->Xref); // LBeta = L*Beta LBj=gsl_vector_subvector(LBeta, j*nDF, nDF); bj=gsl_matrix_column(Alt->Beta, j); gsl_blas_dgemv(CblasNoTrans,1,LL,&bj.vector,0,&LBj.vector); // Z = (X^T W X)^-1 * X^T W^1/2. gsl_matrix_set_identity(XwX); gsl_blas_dsyrk (CblasLower,CblasTrans,1.0,w1jX1,0.0,XwX); status=gsl_linalg_cholesky_decomp (XwX); if (status==GSL_EDOM) { if (tm->warning==TRUE) printf("Warning:singular matrix in wald test. An eps*I is added to the singular matrix.\n"); gsl_matrix_set_identity(XwX); gsl_blas_dsyrk(CblasLower,CblasTrans,1.0,w1jX1,eps,XwX); gsl_linalg_cholesky_decomp(XwX); } gsl_linalg_cholesky_invert(XwX); gsl_blas_dgemm(CblasNoTrans,CblasTrans,1.0,XwX,w1jX1,0.0, Z[j].matrix); gsl_matrix_memcpy(Rl2, LL); gsl_blas_dtrmm (CblasRight,CblasLower,CblasNoTrans,CblasNonUnit,1.0,XwX,Rl2); // L*(X'WX)^-1 gsl_blas_dgemm (CblasNoTrans, CblasTrans, 1.0, Rl2, LL, 0.0, IinvN); // L*(X^T*W*X)^-1*L^T if ( (tm->punit!=NONE) || (tm->corr==IDENTITY) ) { status=gsl_linalg_cholesky_decomp (IinvN); if (status==GSL_EDOM) { if (tm->warning==TRUE) printf("Warning:singular IinvN in wald test.\n"); } tmp2=gsl_vector_subvector(tmp, 0, nDF); gsl_linalg_cholesky_solve (IinvN, &LBj.vector, &tmp2.vector); gsl_blas_ddot (&LBj.vector, &tmp2.vector, &result); gsl_vector_set(teststat, j+1, sqrt(result)); sum = sum + result; } if (tm->corr!=IDENTITY) { // IinvRl=L*vSandRl*L^T for (l=0; l<=j; l++) { Rl=gsl_matrix_submatrix(IinvRl,j*nDF,l*nDF,nDF,nDF); alpha = gsl_matrix_get(Rlambda, j, l); // borrow XwX space to store vSandRl gsl_blas_dgemm(CblasNoTrans,CblasTrans,alpha,Z[j].matrix,Z[l].matrix, 0.0, XwX); // Rl2 = L*vSandRl*L^T gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,LL,XwX,0.0,Rl2); gsl_blas_dgemm(CblasNoTrans,CblasTrans,1.0,Rl2,LL,0.0,&Rl.matrix); } // end l } // end if (tm->corr) } // end for j=1:nVars if ( tm->corr==IDENTITY ) gsl_vector_set(teststat, 0, sqrt(sum)); else { status=gsl_linalg_cholesky_decomp (IinvRl); if (status==GSL_EDOM) { if (tm->warning==TRUE) printf("Warning:singular matrix in multivariate wald test.\n"); } gsl_linalg_cholesky_solve (IinvRl, LBeta, tmp); gsl_blas_ddot (LBeta, tmp, &result); gsl_vector_set(teststat, 0, sqrt(result)); } // free memory for (j=0; j<nVars; j++) gsl_matrix_free(Z[j].matrix); free(Z); gsl_vector_free(LBeta); gsl_matrix_free(w1jX1); gsl_matrix_free(XwX); gsl_matrix_free(Rl2); gsl_matrix_free(IinvN); gsl_matrix_free(IinvRl); gsl_vector_free(tmp); return SUCCESS; }
int GlmTest::GeeScore(gsl_matrix *X1, glm *PtrNull, gsl_vector *teststat) { gsl_set_error_handler_off(); double result, alpha, sum=0; unsigned int i, j, l, nP = X1->size2; unsigned int nVars=tm->nVars, nRows=tm->nRows; int status; gsl_vector *U = gsl_vector_alloc(nVars*nP); gsl_matrix *kRlNull = gsl_matrix_alloc(nVars*nP, nVars*nP); gsl_matrix_set_zero (kRlNull); gsl_matrix *XwX = gsl_matrix_alloc(nP, nP); gsl_vector *tmp=gsl_vector_alloc(nVars*nP); gsl_vector_view wj, uj, rj, tmp2; //, dj; gsl_matrix_view Rl; GrpMat *Z = (GrpMat*)malloc(nVars*sizeof(GrpMat)); for (j=0; j<nVars; j++) { Z[j].matrix = gsl_matrix_alloc(nRows, nP); // get W^1/2 * X wj = gsl_matrix_column (PtrNull->wHalf, j); for (i=0; i<nP; i++) gsl_matrix_set_col (Z[j].matrix, i, &wj.vector); gsl_matrix_mul_elements (Z[j].matrix, X1); uj=gsl_vector_subvector(U, j*nP, nP); rj=gsl_matrix_column(PtrNull->Res, j); gsl_blas_dgemv(CblasTrans, 1, Z[j].matrix, &rj.vector, 0, &uj.vector); if ( (tm->punit!=NONE) || (tm->corr==IDENTITY) ) { gsl_matrix_set_identity(XwX); gsl_blas_dsyrk(CblasLower, CblasTrans, 1, Z[j].matrix, 0, XwX); status=gsl_linalg_cholesky_decomp(XwX); if (status==GSL_EDOM) { if (tm->warning==TRUE) printf("Warning: singular matrix in score test. An eps*I is added to the singular matrix.\n"); gsl_matrix_set_identity(XwX); gsl_blas_dsyrk(CblasLower,CblasTrans,1,Z[j].matrix,eps,XwX); gsl_linalg_cholesky_decomp(XwX); } tmp2=gsl_vector_subvector(tmp, 0, nP); gsl_linalg_cholesky_solve(XwX, &uj.vector, &tmp2.vector); gsl_blas_ddot(&uj.vector, &tmp2.vector, &result); gsl_vector_set(teststat, j+1, result); sum = sum+result; } if ( tm->corr!=IDENTITY) { for (l=0; l<=j; l++) { // lower half alpha = gsl_matrix_get(Rlambda, j, l); Rl=gsl_matrix_submatrix(kRlNull,j*nP,l*nP,nP,nP); gsl_blas_dgemm(CblasTrans, CblasNoTrans, alpha, Z[j].matrix, Z[l].matrix, 0, &Rl.matrix); } } } // end for j=1:nVars // multivariate test stat if ( tm->corr==IDENTITY ) gsl_vector_set(teststat, 0, sum); else { status=gsl_linalg_cholesky_decomp (kRlNull); if (status==GSL_EDOM) { if (tm->warning==TRUE) printf("Warning:singular kRlNull in multivariate score test.\n"); } gsl_linalg_cholesky_solve (kRlNull, U, tmp); gsl_blas_ddot (U, tmp, &result); gsl_vector_set(teststat, 0, result); } // clear memory gsl_vector_free(U); gsl_vector_free(tmp); gsl_matrix_free(XwX); gsl_matrix_free(kRlNull); for (j=0; j<nVars; j++) gsl_matrix_free(Z[j].matrix); free(Z); return SUCCESS; }
static void vine_ran_rvine(const dml_vine_t *vine, const gsl_rng *rng, gsl_matrix *data) { size_t n, m; gsl_vector ***vdirect, ***vindirect; gsl_vector *z1 = NULL, *z2 = NULL, *hinv = NULL; // Initialized to avoid GCC warnings. size_t **M; n = vine->dim; m = data->size1; vdirect = g_malloc_n(n, sizeof(gsl_vector **)); vindirect = g_malloc_n(n, sizeof(gsl_vector **)); for (size_t i = 0; i < n; i++) { vdirect[i] = g_malloc0_n(n, sizeof(gsl_vector *)); vindirect[i] = g_malloc0_n(n, sizeof(gsl_vector *)); } M = g_malloc_n(n, sizeof(size_t *)); for (size_t i = 0; i < n; i++) { M[i] = g_malloc0_n(i + 1, sizeof(size_t)); } // Line 4. for (size_t k = 0; k < n; k++) { vdirect[n - 1][k] = gsl_vector_alloc(m); for (size_t i = 0; i < m; i++) { gsl_vector_set(vdirect[n - 1][k], i, gsl_rng_uniform(rng)); } } // Line 5. for (size_t k = 0; k < n; k++) { M[k][k] = vine->matrix[k][k]; M[n - 1][k] = vine->matrix[n - 1][k]; for (size_t i = n - 2; i > k; i--) { if (vine->matrix[i][k] > M[i + 1][k]) { M[i][k] = vine->matrix[i][k]; } else { M[i][k] = M[i + 1][k]; } } } // Line 6. gsl_matrix_set_col(data, vine->order[0], vdirect[n - 1][n - 1]); // for loop in line 7. for (size_t k = n - 2; /* See break call. */; k--) { // for loop in line 8. for (size_t i = k + 1; i < n; i++) { // Line 14. if (vine->matrix[i][k] != 0 && dml_copula_type(vine->copulas[i][k]) != DML_COPULA_INDEP) { if (M[i][k] == vine->matrix[i][k]) { z2 = vdirect[i][n - M[i][k]]; } else { z2 = vindirect[i][n - M[i][k]]; } hinv = gsl_vector_alloc(m); dml_copula_hinv(vine->copulas[i][k], vdirect[n - 1][k], z2, hinv); gsl_vector_free(vdirect[n - 1][k]); vdirect[n - 1][k] = hinv; } } // Line 16. gsl_matrix_set_col(data, vine->order[n - k - 1], vdirect[n - 1][k]); if (k == 0) break; // Avoid problems decrementing the unsigned k if k is 0. // for loop in line 17. for (size_t i = n - 1; i > k; i--) { // Line 18. z1 = vdirect[i][k]; // Line 19. if (vdirect[i - 1][k] == NULL) { vdirect[i - 1][k] = gsl_vector_alloc(m); } if (vine->matrix[i][k] == 0 || dml_copula_type(vine->copulas[i][k]) == DML_COPULA_INDEP) { // Vine truncated or independence copula. gsl_vector_memcpy(vdirect[i - 1][k], z1); } else { dml_copula_h(vine->copulas[i][k], z1, z2, vdirect[i - 1][k]); } if (vindirect[i - 1][k] == NULL) { vindirect[i - 1][k] = gsl_vector_alloc(m); } gsl_vector_memcpy(vindirect[i - 1][k], vdirect[i - 1][k]); } } // Freeing memory. for (size_t i = 0; i < n; i++) { g_free(M[i]); } g_free(M); for (size_t i = 0; i < n; i++) { for (size_t j = 0; j < n; j++) { if (vdirect[i][j] != NULL) { gsl_vector_free(vdirect[i][j]); } if (vindirect[i][j] != NULL) { gsl_vector_free(vindirect[i][j]); } } g_free(vdirect[i]); g_free(vindirect[i]); } g_free(vdirect); g_free(vindirect); }
void vec_to_mat(const gsl_vector * vec, gsl_matrix *mat){ gsl_matrix_set_col(mat,0,vec); }
int chol(gsl_matrix *A, gsl_vector *b, gsl_vector *x, gsl_vector **x_bar, gsl_vector **x_error, double *max_error) { gsl_matrix *ML, *MU, *L; gsl_vector *vec, *y_bar; size_t j; int status; if (b->size != A->size1) { return MATRIX_VECTOR_UNEQUAL_ROW_DIM; } /* if ((status = isPositiveDefinite(A))) { */ /* return status; */ /* } */ if ((status = cholcrout(A, &L))) { return status; } /* prepare the (L|b) matrix */ ML = gsl_matrix_alloc(L->size1, L->size2 + 1); vec = gsl_vector_alloc(L->size2); for (j = 0; j < L->size2; ++j) { gsl_matrix_get_col(vec, L, j); gsl_matrix_set_col(ML, j, vec); } gsl_matrix_set_col(ML, ML->size2 - 1, b); /* solve the (L|b) matrix using forwards substitution */ if ((status = subst_forwards(ML, &y_bar))) { return status; } /* prepare the (L'|b) matrix */ gsl_matrix_transpose(L); MU = gsl_matrix_alloc(L->size1, L->size2 + 1); for (j = 0; j < L->size2; ++j) { gsl_matrix_get_col(vec, L, j); gsl_matrix_set_col(MU, j, vec); } gsl_vector_free(vec); gsl_matrix_set_col(MU, MU->size2 - 1, y_bar); /* solve the (U|b) matrix using backwards substitution */ if ((status = subst_backwards(MU, x_bar))) { return status; } gsl_vector_free(y_bar); gsl_matrix_free(ML); gsl_matrix_free(MU); gsl_matrix_free(L); /* get the error statistics */ gatherErrors(*x_bar, x, x_error, max_error); return 0; }
void ccl_learn_lambda(const double * Un,const double *X,void (*J_func)(const double*,const int,double*),const int dim_b,const int dim_r,const int dim_n,const int dim_x,const int dim_u,LEARN_A_MODEL optimal){ LEARN_A_MODEL model; double * centres,*var_tmp,*vec, *BX, *RnVn,*Rn,*Vn; double variance; int i,lambda_id; gsl_matrix *Un_,*X_; // J_fun = &Jacobian; model.dim_b = dim_b; model.dim_r = dim_r; model.dim_x = dim_x; model.dim_n = dim_n; model.dim_t = dim_r-1; model.dim_u = dim_u; // calculate model.var var_tmp = malloc(dim_u*sizeof(double)); ccl_mat_var(Un,dim_u,dim_n,0,var_tmp); model.var = ccl_vec_sum(var_tmp,dim_u); free(var_tmp); Un_ = gsl_matrix_alloc(dim_u,dim_n); memcpy(Un_->data,Un,dim_u*dim_n*sizeof(double)); X_ = gsl_matrix_alloc(dim_x,dim_n); memcpy(X_->data,X,dim_x*dim_n*sizeof(double)); optimal.nmse = 1000000; gsl_vector* X_col = gsl_vector_alloc(dim_x); gsl_vector* Un_col = gsl_vector_alloc(dim_u); gsl_matrix* Vn_container = gsl_matrix_alloc(dim_r,dim_n); gsl_matrix_set_zero(Vn_container); double* norm = malloc(dim_n*sizeof(double)); double* J_x = malloc(dim_r*dim_u*sizeof(double)); gsl_vector* Jx_Un = gsl_vector_alloc(dim_r); int* id_keep = malloc(dim_n*sizeof(double)); for (i=0;i<dim_n;i++){ gsl_matrix_get_col(X_col,X_,i); gsl_matrix_get_col(Un_col,Un_,i); J_func(X_col->data,dim_x,J_x); ccl_dot_product(J_x,dim_r,dim_u,Un_col->data,dim_u,1,Jx_Un->data); gsl_matrix_set_col(Vn_container,i,Jx_Un); norm[i] = gsl_blas_dnrm2(Jx_Un); } // here dim_n maybe change. int new_dim_n = ccl_find_index_double(norm,dim_n,2,1E-3,id_keep); model.dim_n = new_dim_n; Vn = malloc(dim_r*model.dim_n*sizeof(double)); gsl_matrix Vn_ = gsl_matrix_view_array(Vn,dim_r,model.dim_n).matrix; gsl_matrix*X_new = gsl_matrix_alloc(dim_x,model.dim_n); gsl_matrix*Un_new = gsl_matrix_alloc(dim_u,model.dim_n); for (i=0;i<new_dim_n;i++){ gsl_vector* Vn_tmp = gsl_vector_alloc(dim_r); gsl_matrix_get_col(Vn_tmp,Vn_container,id_keep[i]); gsl_matrix_set_col(&Vn_,i,Vn_tmp); gsl_vector_free(Vn_tmp); Vn_tmp = gsl_vector_alloc(dim_x); gsl_matrix_get_col(Vn_tmp,X_,id_keep[i]); gsl_matrix_set_col(X_new,i,Vn_tmp); gsl_vector_free(Vn_tmp); Vn_tmp = gsl_vector_alloc(dim_u); gsl_matrix_get_col(Vn_tmp,Un_,id_keep[i]); gsl_matrix_set_col(Un_new,i,Vn_tmp); gsl_vector_free(Vn_tmp); } gsl_vector_free(Jx_Un); gsl_vector_free(X_col); gsl_vector_free(Un_col); gsl_matrix_free(Vn_container); gsl_matrix_free(Un_); free(J_x); free(norm); free(id_keep); // prepare for BX centres = malloc(dim_x*dim_b*sizeof(double)); generate_kmeans_centres(X_new->data,dim_x,model.dim_n,dim_b,centres); var_tmp = malloc(dim_b*dim_b*sizeof(double)); vec = malloc(dim_b*sizeof(double)); ccl_mat_distance(centres,dim_x,dim_b,centres,dim_x,dim_b,var_tmp); for (i=0;i<dim_b*dim_b;i++){ var_tmp[i] = sqrt(var_tmp[i]); } ccl_mat_mean(var_tmp,dim_b,dim_b,1,vec); variance = pow(gsl_stats_mean(vec,1,dim_b),2); BX = malloc(dim_b*model.dim_n*sizeof(double)); ccl_gaussian_rbf(X_new->data,dim_x,model.dim_n,centres,dim_x,dim_b,variance,BX); gsl_matrix_free(X_new); gsl_matrix_free(Un_new); free(var_tmp); free(vec); Rn = malloc(dim_r*dim_r*sizeof(double)); gsl_matrix Rn_ = gsl_matrix_view_array(Rn,dim_r,dim_r).matrix; gsl_matrix_set_identity(&Rn_); RnVn = malloc(dim_r*model.dim_n*sizeof(double)); memcpy(RnVn,Vn,dim_r*model.dim_n*sizeof(double)); model.dim_u = model.dim_r; ccl_learn_alpha_model_alloc(&model); memcpy(model.c,centres,model.dim_x*model.dim_b*sizeof(double)); model.s2 = variance; clock_t t = clock(); for(lambda_id=0;lambda_id<dim_r;lambda_id++){ model.dim_k = lambda_id+1; // model.w[lambda_id] = malloc((dim_u-model.dim_k)*dim_b*sizeof(double*)); if(dim_r-model.dim_k==0){ model.dim_k = lambda_id; break; } else{ search_learn_alpha(BX,RnVn,&model); double* theta = malloc(model.dim_n*model.dim_t*sizeof(double)); double *W_BX = malloc((dim_r-model.dim_k)*model.dim_n*sizeof(double)); double *W_BX_T = malloc(model.dim_n*(dim_r-model.dim_k)*sizeof(double)); ccl_dot_product(model.w[lambda_id],dim_r-model.dim_k,dim_b,BX,dim_b,model.dim_n,W_BX); ccl_mat_transpose(W_BX,dim_r-model.dim_k,model.dim_n,W_BX_T); if (model.dim_k ==1){ memcpy(theta,W_BX_T,model.dim_n*(dim_r-model.dim_k)*sizeof(double)); } else{ gsl_matrix* ones = gsl_matrix_alloc(model.dim_n,model.dim_k-1); gsl_matrix_set_all(ones,1); gsl_matrix_scale(ones,M_PI/2); mat_hotz_app(ones->data,model.dim_n,model.dim_k-1,W_BX_T,model.dim_n,dim_r-model.dim_k,theta); gsl_matrix_free(ones); } for(i=0;i<model.dim_n;i++){ gsl_matrix theta_mat = gsl_matrix_view_array(theta,model.dim_n,model.dim_t).matrix; gsl_vector *vec = gsl_vector_alloc(model.dim_t); gsl_matrix_get_row(vec,&theta_mat,i); ccl_get_rotation_matrix(vec->data,Rn,&model,lambda_id,Rn); gsl_vector_free(vec); vec = gsl_vector_alloc(dim_r); gsl_matrix_get_col(vec,&Vn_,i); ccl_dot_product(Rn,dim_r,dim_r,vec->data,dim_r,1,vec->data); gsl_matrix RnVn_ = gsl_matrix_view_array(RnVn,dim_r,model.dim_n).matrix; gsl_matrix_set_col(&RnVn_,i,vec); gsl_vector_free(vec); } if(model.nmse > optimal.nmse && model.nmse > 1E-5){ model.dim_k = lambda_id; printf("\n I am out...\n");//optimal; break; } else{ printf("\n copy model -> optimal\n");//optimal; } free(W_BX); free(W_BX_T); free(theta); } } t = clock()-t; printf("\n learning lambda used: %f second \n",((float)t)/CLOCKS_PER_SEC); double* A = malloc(model.dim_k*model.dim_r*sizeof(double)); gsl_matrix* Iu = gsl_matrix_alloc(model.dim_r,model.dim_r); gsl_matrix_set_identity(Iu); gsl_vector* x = gsl_vector_alloc(model.dim_x); gsl_matrix_get_col(x,X_,5); // predict_proj_lambda(x->data, model,Jacobian,centres,variance,Iu->data,A); // print_mat_d(A,model.dim_k,dim_r); ccl_write_learn_lambda_model("/home/yuchen/Desktop/ccl-1.1.0/data/learn_lambda_model_l.txt", &model); free(A); gsl_matrix_free(Iu); gsl_vector_free(x); free(Vn); free(Rn); free(BX); free(RnVn); free(centres); gsl_matrix_free(X_); ccl_learn_alpha_model_free(&model); }
static void motor_feedback_handler (const lcm_recv_buf_t *rbuf, const char *channel, const maebot_motor_feedback_t *msg, void *user) { state_t *state = user; double dl, dr, delta_x, delta_y, delta_theta; // IMPLEMENT ME gsl_matrix *J_plus = gsl_matrix_calloc(3,6); gsl_vector *old_pose = gsl_vector_calloc(3); gsl_vector *change_pose = gsl_vector_calloc(3); gsl_vector *new_pose = gsl_vector_calloc(3); gsl_matrix *old_sigma = gsl_matrix_calloc(3,3); gsl_matrix *change_sigma = gsl_matrix_calloc(3,3); gsl_matrix *new_sigma = gsl_matrix_calloc(3,3); memcpy(old_pose->data, state->xyt, sizeof state->xyt); memcpy (old_sigma->data, state->Sigma, sizeof state->Sigma); static unsigned char first_feedback = 0; if(first_feedback == 0){ state->left_ticks = msg->encoder_left_ticks; state->right_ticks = msg->encoder_right_ticks; first_feedback = 1; } // calculate new expected value dl = METERS_PER_TICK * (msg->encoder_left_ticks - state->left_ticks); dr = METERS_PER_TICK * (msg->encoder_right_ticks - state->right_ticks); delta_x = (dl + dr)/2.0; delta_y = 0; if(state->yaw_calibrated) delta_theta = (state->yaw - state->yaw_old)*M_PI/180; else delta_theta = gslu_math_mod2pi((dr - dl)/BASELINE); gsl_vector_set(old_pose,2,gslu_math_mod2pi(old_pose->data[2])); gsl_vector_set(change_pose,0,delta_x); gsl_vector_set(change_pose,1,delta_y); gsl_vector_set(change_pose,2,delta_theta); xyt_head2tail_gsl(new_pose, J_plus, old_pose, change_pose); gsl_vector_set(new_pose,2,gslu_math_mod2pi(new_pose->data[2])); // calculate new covariance gsl_matrix_set(change_sigma, 0,0, (ALPHA/4.0)*(fabs(dl) + fabs(dr))); gsl_matrix_set(change_sigma, 1,1, BETA * fabs(dl + dr)); if (state->yaw_calibrated){ gsl_matrix_set(change_sigma, 2,2, GYRO_COV); } else{ gsl_matrix_set(change_sigma, 2,2, (ALPHA/(BASELINE*BASELINE))*(fabs(dl) + fabs(dr))); gsl_matrix_set(change_sigma, 0,2, (ALPHA/(2*BASELINE))*(fabs(dr) - fabs(dl))); gsl_matrix_set(change_sigma, 2,0, (ALPHA/(2*BASELINE))*(fabs(dr) - fabs(dl))); } gsl_matrix *J_plus1 = gsl_matrix_calloc(3,3); gsl_matrix *J_plus2 = gsl_matrix_calloc(3,3); gsl_vector *column = gsl_vector_calloc(3); // set up J_plus1 and Jplus2 gsl_matrix_get_col(column, J_plus, 0); gsl_matrix_set_col(J_plus1, 0 ,column); gsl_matrix_get_col(column, J_plus, 1); gsl_matrix_set_col(J_plus1, 1 ,column); gsl_matrix_get_col(column, J_plus, 2); gsl_matrix_set_col(J_plus1, 2 ,column); gsl_matrix_get_col(column, J_plus, 3); gsl_matrix_set_col(J_plus2, 0 ,column); gsl_matrix_get_col(column, J_plus, 4); gsl_matrix_set_col(J_plus2, 1 ,column); gsl_matrix_get_col(column, J_plus, 5); gsl_matrix_set_col(J_plus2, 2 ,column); // multiply to get the covariances gsl_matrix *temp1 = gsl_matrix_calloc(3,3); gsl_matrix *temp2 = gsl_matrix_calloc(3,3); gsl_matrix *sigma_temp = gsl_matrix_calloc(3,3); gsl_blas_dgemm (CblasNoTrans, CblasNoTrans,1.0, J_plus1, old_sigma,0.0, temp1); gsl_blas_dgemm (CblasNoTrans, CblasTrans,1.0, temp1, J_plus1,0.0, new_sigma); gsl_blas_dgemm (CblasNoTrans, CblasNoTrans,1.0, J_plus2, change_sigma,0.0, temp2); gsl_blas_dgemm (CblasNoTrans, CblasTrans,1.0, temp2, J_plus2,0.0, sigma_temp); gsl_matrix_add(new_sigma, sigma_temp); // update state memcpy(state->xyt, new_pose->data, sizeof state->xyt); memcpy(state->Sigma, new_sigma->data, sizeof state->Sigma); // update state memcpy(state->xyt, new_pose->data, sizeof state->xyt); memcpy(state->Sigma, new_sigma->data, sizeof state->Sigma); // print_matrix(new_sigma); state->left_ticks = msg->encoder_left_ticks; state->right_ticks = msg->encoder_right_ticks; // publish pose_xyt_t odo = { .utime = msg->utime }; memcpy (odo.xyt, state->xyt, sizeof state->xyt); memcpy (odo.Sigma, state->Sigma, sizeof state->Sigma); pose_xyt_t_publish (state->lcm, state->odometry_channel, &odo); gsl_matrix_free(J_plus); gsl_matrix_free(J_plus1); gsl_matrix_free(J_plus2); gsl_matrix_free(temp1); gsl_matrix_free(temp2); gsl_matrix_free(sigma_temp); gsl_matrix_free(old_sigma); gsl_matrix_free(change_sigma); gsl_matrix_free(new_sigma); gsl_vector_free(old_pose); gsl_vector_free(change_pose); gsl_vector_free(new_pose); } int64_t find_gyro_bias(state_t *state){ // used linear least squares approximation to get slope gsl_matrix *A = gsl_matrix_calloc(100,2); gsl_matrix *AT = gsl_matrix_calloc(2,100); gsl_matrix *ATA; gsl_matrix *ATA_inv = gsl_matrix_calloc(2,2); gsl_matrix *ATA_inv_AT; gsl_matrix *x; gsl_matrix *b = gsl_matrix_calloc(100,1); gsl_permutation *perm = gsl_permutation_alloc(2); int i, s; for (i=0;i<100;i++){ gsl_matrix_set(A,i,0,1); gsl_matrix_set(A,i,1,i); gsl_matrix_set(b,i,0,state->yaw_cal_array[i]); } print_matrix(A); gsl_matrix_transpose_memcpy(AT, A); ATA = matrix_multiply(AT,A); gsl_linalg_LU_decomp(ATA, perm, &s); print_matrix(ATA); gsl_linalg_LU_invert(ATA, perm, ATA_inv); ATA_inv_AT = matrix_multiply(ATA_inv, AT); print_matrix(ATA_inv_AT); x = matrix_multiply(ATA_inv_AT,b); print_matrix(x); print_matrix(b); return (int64_t)gsl_matrix_get(x,1,0); }
void ccl_mat_set_col(double * mat,int i, int j, int c,double* vec){ gsl_matrix mat_ = gsl_matrix_view_array(mat,i,j).matrix; gsl_vector vec_ = gsl_vector_view_array(vec,i).vector; gsl_matrix_set_col(&mat_,c,&vec_); }
void estimate_rank_and_buildQ(gsl_matrix *M, double frac_of_max_rank, double TOL, gsl_matrix **Q, int *good_rank){ int m,n,i,j,ind,maxdim; double vec_norm; gsl_matrix *RN,*Y,*Qbig,*Qsmall; gsl_vector *vi,*vj,*p,*p1; m = M->size1; n = M->size2; maxdim = round(min(m,n)*frac_of_max_rank); vi = gsl_vector_calloc(m); vj = gsl_vector_calloc(m); p = gsl_vector_calloc(m); p1 = gsl_vector_calloc(m); // build random matrix printf("form RN..\n"); RN = gsl_matrix_calloc(n, maxdim); initialize_random_matrix(RN); // multiply to get matrix of random samples Y printf("form Y: %d x %d..\n",m,maxdim); Y = gsl_matrix_calloc(m, maxdim); matrix_matrix_mult(M, RN, Y); // estimate rank k and build Q from Y printf("form Qbig..\n"); Qbig = gsl_matrix_calloc(m, maxdim); gsl_matrix_memcpy(Qbig, Y); printf("estimate rank with TOL = %f..\n", TOL); *good_rank = maxdim; int forbreak = 0; for(j=0; !forbreak && j<maxdim; j++){ gsl_matrix_get_col(vj, Qbig, j); for(i=0; i<j; i++){ gsl_matrix_get_col(vi, Qbig, i); project_vector(vj, vi, p); gsl_vector_sub(vj, p); if(gsl_blas_dnrm2(p) < TOL && gsl_blas_dnrm2(p1) < TOL){ *good_rank = j; forbreak = 1; break; } gsl_vector_memcpy(p1,p); } vec_norm = gsl_blas_dnrm2(vj); gsl_vector_scale(vj, 1.0/vec_norm); gsl_matrix_set_col(Qbig, j, vj); } printf("estimated rank = %d\n", *good_rank); Qsmall = gsl_matrix_calloc(m, *good_rank); *Q = gsl_matrix_calloc(m, *good_rank); matrix_copy_first_columns(Qsmall, Qbig); QR_factorization_getQ(Qsmall, *Q); gsl_matrix_free(RN); gsl_matrix_free(Y); gsl_matrix_free(Qbig); gsl_matrix_free(Qsmall); gsl_vector_free(p); gsl_vector_free(p1); gsl_vector_free(vi); gsl_vector_free(vj); }
int main(){ const int max_mu_size=601; const int zero_pad_size=pow(2,15); FILE *in; in= fopen("mean.chi", "r"); gsl_matrix *e = gsl_matrix_alloc(max_mu_size, 4); gsl_vector * kvar=gsl_vector_alloc(max_mu_size); gsl_vector * muvar=gsl_vector_alloc(max_mu_size); gsl_vector * mu_0pad=gsl_vector_alloc(zero_pad_size); gsl_vector * r_0pad=gsl_vector_alloc(zero_pad_size/2); //half of lenght gsl_vector * kvar_0pad=gsl_vector_alloc(zero_pad_size); gsl_matrix_fscanf(in, e); fclose(in); gsl_matrix_get_col(kvar,e,0); gsl_matrix_get_col(muvar,e,1); gsl_vector_set_zero(mu_0pad); gsl_matrix_free(e); double dk=gsl_vector_get (kvar, 1)-gsl_vector_get (kvar, 0); double dr=M_PI/float(zero_pad_size-1)/dk; for (int i = 0; i < zero_pad_size; i++) { gsl_vector_set (kvar_0pad, i, dk*i); } for (int i = 0; i < zero_pad_size/2; i++) { gsl_vector_set (r_0pad, i, dr*i); } for (int i = 0; i < max_mu_size; i++) { gsl_vector_set (mu_0pad, i, gsl_vector_get (muvar, i)); } gsl_vector *mu_widowed=gsl_vector_alloc(zero_pad_size); gsl_vector_memcpy (mu_widowed, mu_0pad); double kmin=4.0, kmax=17.0, dwk=0.8; hanning(mu_widowed, kvar_0pad, kmin, kmax, dwk); //FFT transform double *data = (double *) malloc(zero_pad_size*sizeof(double)); //new double [zero_pad_size] ; memcpy(data, mu_widowed->data, zero_pad_size*sizeof(double)); gsl_fft_real_radix2_transform(data, 1, zero_pad_size); //Unpack complex vector gsl_vector_complex *fourier_data = gsl_vector_complex_alloc (zero_pad_size); gsl_fft_halfcomplex_radix2_unpack(data, fourier_data->data, 1, zero_pad_size); gsl_vector *fftR_real = gsl_vector_alloc(fourier_data->size/2); gsl_vector *fftR_imag = gsl_vector_alloc(fourier_data->size/2); gsl_vector *fftR_abs = gsl_vector_alloc(fourier_data->size/2); complex_vector_parts(fourier_data, fftR_real, fftR_imag); complex_vector_abs(fftR_abs, fftR_real, fftR_imag); gsl_vector *first_shell=gsl_vector_alloc(fftR_abs->size); gsl_vector_memcpy (first_shell, fftR_abs); double rmin=0.2, rmax=3.0, dwr=0.1; hanning(first_shell, r_0pad, rmin, rmax, dwr); //feff0001.dat const int path_lines=68; e = gsl_matrix_alloc(path_lines, 7); gsl_vector * k_p =gsl_vector_alloc(path_lines); gsl_vector * phc_p=gsl_vector_alloc(path_lines); gsl_vector * mag_p=gsl_vector_alloc(path_lines); gsl_vector * pha_p=gsl_vector_alloc(path_lines); gsl_vector * lam_p=gsl_vector_alloc(path_lines); in= fopen("feff0001.dat", "r"); gsl_matrix_fscanf(in, e); fclose(in); gsl_matrix_get_col(k_p ,e,0); gsl_matrix_get_col(phc_p,e,1); gsl_matrix_get_col(mag_p,e,2); gsl_matrix_get_col(pha_p,e,3); gsl_matrix_get_col(lam_p,e,5); gsl_matrix_free(e); gsl_interp_accel *acc = gsl_interp_accel_alloc (); gsl_spline *k_spline = gsl_spline_alloc (gsl_interp_cspline, path_lines); gsl_spline *phc_spline = gsl_spline_alloc (gsl_interp_cspline, path_lines); gsl_spline *mag_spline = gsl_spline_alloc (gsl_interp_cspline, path_lines); gsl_spline *pha_spline = gsl_spline_alloc (gsl_interp_cspline, path_lines); gsl_spline *lam_spline = gsl_spline_alloc (gsl_interp_cspline, path_lines); gsl_spline_init (k_spline , k_p->data, k_p->data , path_lines); gsl_spline_init (phc_spline, k_p->data, phc_p->data, path_lines); gsl_spline_init (mag_spline, k_p->data, mag_p->data, path_lines); gsl_spline_init (pha_spline, k_p->data, pha_p->data, path_lines); gsl_spline_init (lam_spline, k_p->data, lam_p->data, path_lines); gsl_vector * mu_p =gsl_vector_alloc(path_lines); //struct fit_params { student_params t; double kshift; double S02; double N; inter_path splines; }; //student_params t = {2.45681867, 0.02776907, -21.28920008, 9.44741797, 0.0, 0.0, 0.0}; splines.acc=acc; splines.phc_spline=phc_spline; splines.mag_spline=mag_spline; splines.pha_spline=pha_spline; splines.lam_spline=lam_spline; fit_params fp = { 2.45681867, 0.02776907, -21.28920008, 9.44741797, 1.0, 0.0}; compute_itegral(k_p, &fp, mu_p); //mu_data_fit params = { k_p, mu_p}; mu_data.k = kvar_0pad; mu_data.mu = mu_0pad; mu_data.mu_ft = first_shell; mu_data.r = r_0pad; mu_data.kmin = kmin; mu_data.kmax = kmax; mu_data.rmin = rmin; mu_data.rmax = rmax; mu_data.dwk = dwk; mu_data.dwr = dwr; // initialize the solver size_t Nparams=6; gsl_vector *guess0 = gsl_vector_alloc(Nparams); gsl_vector_set(guess0, 0, 2.4307); gsl_vector_set(guess0, 1, 0.040969); gsl_vector_set(guess0, 2, 0.001314); gsl_vector_set(guess0, 3, 7835); gsl_vector_set(guess0, 4, 1.0); gsl_vector_set(guess0, 5, 0.0); gsl_vector *fit_r = gsl_vector_alloc(r_0pad->size); compute_itegral_r(&mu_data, fp, fit_r); gsl_matrix *plotting = gsl_matrix_calloc(r_0pad->size, 3); gsl_matrix_set_col (plotting, 0, r_0pad); gsl_matrix_set_col (plotting, 1, first_shell); gsl_matrix_set_col (plotting, 2, fit_r); plot_matplotlib(plotting); gsl_matrix_free (plotting); gsl_multifit_function_fdf fit_mu_k; fit_mu_k.f = &resudial_itegral_r; fit_mu_k.n = MAX_FIT_POINTS; fit_mu_k.p = Nparams; fit_mu_k.params = &mu_data; fit_mu_k.df = NULL; fit_mu_k.fdf = NULL; gsl_multifit_fdfsolver *solver = gsl_multifit_fdfsolver_alloc(gsl_multifit_fdfsolver_lmsder, MAX_FIT_POINTS, Nparams); gsl_multifit_fdfsolver_set(solver, &fit_mu_k, guess0); size_t iter=0, status; do{ iter++; //cout << solver->x->data[0] << " " << solver->x->data[1] <<endl; status = gsl_multifit_fdfsolver_iterate (solver); //printf("%12.4f %12.4f %12.4f\n", solver->J->data[0,0], solver->J->data[1,1], solver->J->data[2,2] ); //gsl_multifit_fdfsolver_dif_df (k_p, &fit_mu_k, mu_p, solver->J); //gsl_multifit_fdfsolver_dif_fdf (k_p, &fit_mu_k, mu_p, solver->J); for (int i =0; i< solver->x->size; i++){ printf("%14.5f", gsl_vector_get (solver->x, i)) ; } printf("\n") ; if (status) break; status = gsl_multifit_test_delta (solver->dx, solver->x, 1e-4, 1e-4); }while (status == GSL_CONTINUE && iter < 100); gsl_vector * mu_fit =gsl_vector_alloc(path_lines); fit_params fitp = { solver->x->data[0], solver->x->data[1],\ solver->x->data[2], solver->x->data[3],\ solver->x->data[4], solver->x->data[5]}; compute_itegral(k_p, &fitp, mu_fit); fp.mu=gsl_vector_get (solver->x, 0); fp.sig=gsl_vector_get (solver->x, 1); fp.skew=gsl_vector_get (solver->x, 2); fp.nu=gsl_vector_get (solver->x, 3); fp.S02=gsl_vector_get (solver->x, 4); fp.kshift=gsl_vector_get (solver->x, 5); compute_itegral_r(&mu_data, fp, fit_r); //gsl_matrix *plotting = gsl_matrix_calloc(r_0pad->size, 3); gsl_matrix_set_col (plotting, 0, r_0pad); gsl_matrix_set_col (plotting, 1, first_shell); gsl_matrix_set_col (plotting, 2, fit_r); int min_r=search_max(r_0pad, 0.); int max_r=search_max(r_0pad, 4.); gsl_matrix_view plotting_lim = gsl_matrix_submatrix (plotting, min_r, 0, max_r-min_r, plotting->size2); plot_matplotlib(&plotting_lim.matrix); gsl_matrix_free (plotting); //cout << gsl_spline_eval (k_spline, 1.333, acc) << endl; //cout << gsl_spline_eval (phc_spline, 1.333, acc) << endl; //cout << data[0] << "\t" << data[1] << "\t" << data[2] << "\t" << endl; //cout << fourier_data->data[0] << "\t" << fourier_data->data[1] << "\t" << fourier_data->data[2] << "\t" << endl; //Plotting /* gsl_matrix *plotting = gsl_matrix_calloc(zero_pad_size, 3); gsl_matrix_set_col (plotting, 0, kvar_0pad); gsl_matrix_set_col (plotting, 1, mu_0pad); gsl_matrix_set_col (plotting, 2, mu_widowed); int max_k=search_max(kvar_0pad, 35.); int min_k=search_max(kvar_0pad, 1.0); gsl_matrix_view plotting_lim = gsl_matrix_submatrix (plotting, min_k, 0, max_k-min_k, 3); plot_matplotlib(&plotting_lim.matrix); gsl_matrix_free (plotting); */ /* gsl_matrix *plotting = gsl_matrix_calloc(zero_pad_size, 2); gsl_matrix_set_col (plotting, 0, r_0pad); gsl_matrix_set_col (plotting, 1, mu_0pad); int max_k=search_max(kvar_0pad, 35.); int min_k=search_max(kvar_0pad, 1.0); gsl_matrix_view plotting_lim = gsl_matrix_submatrix (plotting, min_k, 0, max_k-min_k, 3); plot_matplotlib(&plotting_lim.matrix); gsl_matrix_free (plotting); */ /* gsl_matrix *plotting = gsl_matrix_calloc(r_0pad->size, 5); gsl_matrix_set_col (plotting, 0, r_0pad); gsl_matrix_set_col (plotting, 1, fftR_abs); gsl_matrix_set_col (plotting, 2, fftR_real); gsl_matrix_set_col (plotting, 3, fftR_imag); gsl_matrix_set_col (plotting, 4, first_shell); int min_r=search_max(r_0pad, 0.); int max_r=search_max(r_0pad, 5.); gsl_matrix_view plotting_lim = gsl_matrix_submatrix (plotting, min_r, 0, max_r-min_r, plotting->size2); plot_matplotlib(&plotting_lim.matrix); //plot_matplotlib(plotting); gsl_matrix_free (plotting); */ //cout << "Done" << endl; //cout << data[1] <<"\t" << data[2] << endl; //for (int i = 0; i < kvar->size; i++) //{ // cout << gsl_vector_get (kvar, i) <<"\t" << gsl_vector_get (muvar, i) << endl; //} }