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