/* compute 1-norm of original matrix, stored in upper triangle of LLT; * diagonal entries have to be reconstructed */ static double cholesky_norm1(const gsl_matrix * LLT, gsl_vector * work) { const size_t N = LLT->size1; double max = 0.0; size_t i, j; for (j = 0; j < N; ++j) { double sum = 0.0; gsl_vector_const_view lj = gsl_matrix_const_subrow(LLT, j, 0, j + 1); double Ajj; /* compute diagonal (j,j) entry of A */ gsl_blas_ddot(&lj.vector, &lj.vector, &Ajj); for (i = 0; i < j; ++i) { double *wi = gsl_vector_ptr(work, i); double Aij = gsl_matrix_get(LLT, i, j); double absAij = fabs(Aij); sum += absAij; *wi += absAij; } gsl_vector_set(work, j, sum + fabs(Ajj)); } for (i = 0; i < N; ++i) { double wi = gsl_vector_get(work, i); max = GSL_MAX(max, wi); } return max; }
static int build_cqp_data(const bundle_method_state_t *state, gsl_matrix *Q, gsl_vector *q) { size_t i,j; bundle_element *item_i; bundle_element *item_j; double r; int status; item_i = state->head; for(i=0; i<state->bundle_size; i++) { /* matrix of the object function (the matrix is symmetric)*/ item_j = item_i; for(j=i; j<state->bundle_size; j++) { status = gsl_blas_ddot(item_i->sgr, item_j->sgr, &r); gsl_matrix_set(Q, i, j, r); if(j!=i) gsl_matrix_set(Q, j, i, r); item_j = item_j->next; } /* vector of the object function */ gsl_vector_set(q, i, (item_i->lin_error)/(state->t)); item_i = item_i->next; } return GSL_SUCCESS; }
static void test_fdf_checksol(const char *sname, const char *pname, const double epsrel, gsl_multifit_nlinear_workspace *w, test_fdf_problem *problem) { gsl_multifit_nlinear_fdf *fdf = problem->fdf; const double *sigma = problem->sigma; gsl_vector *f = gsl_multifit_nlinear_residual(w); gsl_vector *x = gsl_multifit_nlinear_position(w); double sumsq; /* check solution vector x and sumsq = ||f||^2 */ gsl_blas_ddot(f, f, &sumsq); (problem->checksol)(x->data, sumsq, epsrel, sname, pname); /* check variances */ if (sigma) { const size_t n = fdf->n; const size_t p = fdf->p; size_t i; gsl_matrix * covar = gsl_matrix_alloc (p, p); gsl_matrix *J = gsl_multifit_nlinear_jac (w); gsl_multifit_nlinear_covar (J, 0.0, covar); for (i = 0; i < p; i++) { double ei = sqrt(sumsq/(n-p))*sqrt(gsl_matrix_get(covar,i,i)); gsl_test_rel (ei, sigma[i], epsrel, "%s/%s, sigma(%d)", sname, pname, i) ; } gsl_matrix_free (covar); } }
static double dogleg_beta(const double t, const double delta, const gsl_vector * diag, dogleg_state_t * state) { double beta; double a, b, c; /* compute: workp = t*dx_gn - dx_sd */ scaled_addition(t, state->dx_gn, -1.0, state->dx_sd, state->workp); /* a = || D (t*dx_gn - dx_sd) ||^2 */ a = scaled_enorm(diag, state->workp); a *= a; /* workp = D^T D (t*dx_gn - dx_sd) */ gsl_vector_mul(state->workp, diag); gsl_vector_mul(state->workp, diag); /* b = 2 dx_sd^T D^T D (t*dx_gn - dx-sd) */ gsl_blas_ddot(state->dx_sd, state->workp, &b); b *= 2.0; /* c = || D dx_sd ||^2 - delta^2 = (||D dx_sd|| + delta) (||D dx_sd|| - delta) */ c = (state->norm_Dsd + delta) * (state->norm_Dsd - delta); if (b > 0.0) { beta = (-2.0 * c) / (b + sqrt(b*b - 4.0*a*c)); } else { beta = (-b + sqrt(b*b - 4.0*a*c)) / (2.0 * a); } return beta; }
void GetMVNpdf(const gsl_matrix * mat, const double * mu, const gsl_matrix * sigmaInv, const gsl_matrix * sigmaChol, const size_t nPoints, const size_t nDim, double * returnVal) { double normConst = - log(2*M_PI)*nDim/2.0; for(size_t j = 0; j < nDim; j++) normConst -= log(gsl_matrix_get(sigmaChol, j, j)); gsl_vector_const_view vecMu = gsl_vector_const_view_array(mu, nDim); #pragma omp parallel for for(size_t i = 0; i < nPoints; i++){ gsl_vector * x1 = gsl_vector_alloc(nDim); // Note: allocating and freeing these every loop is not ideal, but needed for threadsafe. There might be a better way. gsl_vector * x2 = gsl_vector_alloc(nDim); gsl_matrix_get_row(x1, mat, i); gsl_vector_sub(x1, &vecMu.vector); gsl_blas_dsymv(CblasUpper, 1.0, sigmaInv, x1, 0.0, x2); gsl_blas_ddot(x1, x2, &returnVal[i]); returnVal[i] = exp(normConst - 0.5*returnVal[i]); gsl_vector_free(x1); gsl_vector_free(x2); } return; }
// GICP cost function double GICPOptimizer::f(const gsl_vector *x, void *params) { GICPOptData *opt_data = (GICPOptData *)params; double pt1[3]; double pt2[3]; double res[3]; // residual double temp[3]; gsl_vector_view gsl_pt1 = gsl_vector_view_array(pt1, 3); gsl_vector_view gsl_pt2 = gsl_vector_view_array(pt2, 3); gsl_vector_view gsl_res = gsl_vector_view_array(res, 3); gsl_vector_view gsl_temp = gsl_vector_view_array(temp, 3); gsl_matrix_view gsl_M; dgc_transform_t t; // initialize the temp variable; if it happens to be NaN at start, bad things will happen in blas routines below temp[0] = 0; temp[1] = 0; temp[2] = 0; // take the base transformation dgc_transform_copy(t, opt_data->base_t); // apply the current state apply_state(t, x); double f = 0; double temp_double = 0; int N = opt_data->p1->Size(); int counter = 0; for(int i = 0; i < N; i++) { int j = opt_data->nn_indecies[i]; if(j != -1) { // get point 1 pt1[0] = (*opt_data->p1)[i].x; pt1[1] = (*opt_data->p1)[i].y; pt1[2] = (*opt_data->p1)[i].z; // get point 2 pt2[0] = (*opt_data->p2)[j].x; pt2[1] = (*opt_data->p2)[j].y; pt2[2] = (*opt_data->p2)[j].z; //get M-matrix gsl_M = gsl_matrix_view_array(&opt_data->M[i][0][0], 3, 3); //transform point 1 dgc_transform_point(&pt1[0], &pt1[1], &pt1[2], t); res[0] = pt1[0] - pt2[0]; res[1] = pt1[1] - pt2[1]; res[2] = pt1[2] - pt2[2]; //cout << "res: (" << res[0] << ", " <<res[1] <<", " << res[2] << ")" << endl; // temp := M*res gsl_blas_dsymv(CblasLower, 1., &gsl_M.matrix, &gsl_res.vector, 0., &gsl_temp.vector); // temp_double := res'*temp = temp'*M*temp gsl_blas_ddot(&gsl_res.vector, &gsl_temp.vector, &temp_double); // increment total error f += temp_double/(double)opt_data->num_matches; //cout << "temp: " << temp_double << endl; //cout << "f: " << f << "\t (" << opt_data->num_matches << ")" << endl; //print_gsl_matrix(&gsl_M.matrix, "M"); counter++; } } printf("counter %d\n",counter); return f; }
int NBinGlm::nbinfit(gsl_matrix *Y, gsl_matrix *X, gsl_matrix *O, gsl_matrix *B) { gsl_set_error_handler_off(); initialGlm(Y, X, O, B); gsl_rng *rnd=gsl_rng_alloc(gsl_rng_mt19937); unsigned int i, j; //, isConv; double yij, mij, vij, hii, uij, wij, wei; double th, tol, dev_th_b_old; int status; // gsl_vector_view b0j, m0j, e0j, v0j; gsl_matrix *WX = gsl_matrix_alloc(nRows, nParams); gsl_matrix *TMP = gsl_matrix_alloc(nRows, nParams); gsl_matrix *XwX = gsl_matrix_alloc(nParams, nParams); gsl_vector_view Xwi, Xi, vj, dj, hj; for (j=0; j<nVars; j++) { betaEst(j, maxiter, &tol, maxtol); //poisson // Get initial theta estimates iterconv[j]=0.0; if (mmRef->estiMethod==CHI2) { th = getDisper(j, 1.0); while ( iterconv[j]<maxiter ) { //printf("th=%.2f, iterconv[%d]=%d\n", th, j, iterconv[j]); iterconv[j]++; dev_th_b_old = dev[j]; betaEst(j, 1.0, &tol, th); // 1-step beta th = getDisper(j, th)/th; tol = ABS((dev[j]-dev_th_b_old)/(ABS(dev[j])+0.1)); if (tol<eps) break; } } else if (mmRef->estiMethod==NEWTON) { th = thetaML(0.0, j, maxiter); while ( iterconv[j]<maxiter ) { iterconv[j]++; dev_th_b_old = dev[j]; th = thetaML(th, j, maxiter2); betaEst(j, maxiter2, &tol, th); tol=ABS((dev[j]-dev_th_b_old)/(ABS(dev[j])+0.1)); if (tol<eps) break; } } else { th = getfAfAdash(0.0, j, maxiter); /* lm=0; for (i=0; i<nRows; i++) { yij = gsl_matrix_get(Y, i, j); mij = gsl_matrix_get(Mu, i, j); lm = lm + llfunc( yij, mij, th); } */ while ( iterconv[j]<maxiter ) { iterconv[j]++; dev_th_b_old = dev[j]; betaEst(j, maxiter2, &tol, th); th = getfAfAdash(th, j, 1.0); tol=ABS((dev[j]-dev_th_b_old)/(ABS(dev[j])+0.1)); if (tol<eps) break; } } if ((iterconv[j]==maxiter)&(mmRef->warning==TRUE)) printf("Warning: reached maximum itrations - negative binomial may NOT converge in the %d-th variable (dev=%.4f, err=%.4f, theta=%.4f)!\n", j, dev[j], tol, th); // other properties based on mu and phi theta[j] = th; gsl_matrix_memcpy(WX, Xref); ll[j]=0; for (i=0; i<nRows; i++) { yij = gsl_matrix_get(Y, i, j); mij = gsl_matrix_get(Mu, i, j); vij = varfunc( mij, th); gsl_matrix_set(Var, i, j, vij); wij = sqrt(weifunc(mij, th)); gsl_matrix_set(wHalf, i, j, wij); gsl_matrix_set(Res, i, j, (yij-mij)/sqrt(vij)); ll[j] = ll[j] + llfunc( yij, mij, th); // get PIT residuals for discrete data wei = gsl_rng_uniform_pos (rnd); // wei ~ U(0, 1) uij=wei*cdf(yij, mij, th); if (yij>0) uij=uij+(1-wei)*cdf((yij-1),mij,th); gsl_matrix_set(PitRes, i, j, uij); // W^1/2 X Xwi = gsl_matrix_row (WX, i); gsl_vector_scale(&Xwi.vector, wij); } aic[j]=-ll[j]+2*(nParams+1); // X^T * W * X gsl_matrix_set_identity (XwX); gsl_blas_dsyrk (CblasLower, CblasTrans, 1.0, WX, 0.0, XwX); status=gsl_linalg_cholesky_decomp (XwX); if (status==GSL_EDOM) { if (mmRef->warning==TRUE) printf("Warning: singular matrix in calculating pit-residuals. An eps*I is added to the singular matrix.\n"); gsl_matrix_set_identity (XwX); gsl_blas_dsyrk (CblasLower, CblasTrans, 1.0, WX, mintol, XwX); gsl_linalg_cholesky_decomp (XwX); } gsl_linalg_cholesky_invert (XwX); // (X'WX)^-1 // Calc varBeta vj = gsl_matrix_column (varBeta, j); dj = gsl_matrix_diagonal (XwX); gsl_vector_memcpy (&vj.vector, &dj.vector); // hii is diagonal element of H=X*(X'WX)^-1*X'*W hj = gsl_matrix_column (sqrt1_Hii, j); gsl_blas_dsymm(CblasRight,CblasLower,1.0,XwX,Xref,0.0,TMP); // X*(X'WX)^-1 for (i=0; i<nRows; i++) { Xwi=gsl_matrix_row(TMP, i); Xi=gsl_matrix_row(Xref, i); wij=gsl_matrix_get(wHalf, i, j); gsl_blas_ddot(&Xwi.vector, &Xi.vector, &hii); gsl_vector_set(&hj.vector, i, MAX(mintol, sqrt(MAX(0, 1-wij*wij*hii)))); //printf("hii=%.4f, wij=%.4f, sqrt(1-wij*wij*hii)=%.4f\n", hii, wij, sqrt(1-wij*wij*hii)); } } // end nVar for j loop // gsl_matrix_div_elements (Res, sqrt1_Hii); // subtractMean(Res); gsl_matrix_free(XwX); gsl_matrix_free(WX); gsl_matrix_free(TMP); gsl_rng_free(rnd); return SUCCESS; }
static void minimize (gsl_multimin_function_fdf * fdf, const gsl_vector * x, const gsl_vector * p, double lambda, double stepa, double stepb, double stepc, double fa, double fb, double fc, double tol, gsl_vector * x1, gsl_vector * dx1, gsl_vector * x2, gsl_vector * dx2, gsl_vector * gradient, double * step, double * f, double * gnorm) { /* Starting at (x0, f0) move along the direction p to find a minimum f(x0 - lambda * p), returning the new point x1 = x0-lambda*p, f1=f(x1) and g1 = grad(f) at x1. */ double u = stepb; double v = stepa; double w = stepc; double fu = fb; double fv = fa; double fw = fc; double old2 = fabs(w - v); double old1 = fabs(v - u); double stepm, fm, pg, gnorm1; int iter = 0; gsl_vector_memcpy (x2, x1); gsl_vector_memcpy (dx2, dx1); *f = fb; *step = stepb; *gnorm = gsl_blas_dnrm2 (gradient); mid_trial: iter++; if (iter > 10) { return; /* MAX ITERATIONS */ } { double dw = w - u; double dv = v - u; double du = 0.0; double e1 = ((fv - fu) * dw * dw + (fu - fw) * dv * dv); double e2 = 2.0 * ((fv - fu) * dw + (fu - fw) * dv); if (e2 != 0.0) { du = e1 / e2; } if (du > 0.0 && du < (stepc - stepb) && fabs(du) < 0.5 * old2) { stepm = u + du; } else if (du < 0.0 && du > (stepa - stepb) && fabs(du) < 0.5 * old2) { stepm = u + du; } else if ((stepc - stepb) > (stepb - stepa)) { stepm = 0.38 * (stepc - stepb) + stepb; } else { stepm = stepb - 0.38 * (stepb - stepa); } } take_step (x, p, stepm, lambda, x1, dx1); fm = GSL_MULTIMIN_FN_EVAL_F (fdf, x1); #ifdef DEBUG printf ("trying stepm = %g fm = %.18e\n", stepm, fm); #endif if (fm > fb) { if (fm < fv) { w = v; v = stepm; fw = fv; fv = fm; } else if (fm < fw) { w = stepm; fw = fm; } if (stepm < stepb) { stepa = stepm; fa = fm; } else { stepc = stepm; fc = fm; } goto mid_trial; } else if (fm <= fb) { old2 = old1; old1 = fabs(u - stepm); w = v; v = u; u = stepm; fw = fv; fv = fu; fu = fm; gsl_vector_memcpy (x2, x1); gsl_vector_memcpy (dx2, dx1); GSL_MULTIMIN_FN_EVAL_DF (fdf, x1, gradient); gsl_blas_ddot (p, gradient, &pg); gnorm1 = gsl_blas_dnrm2 (gradient); #ifdef DEBUG printf ("p: "); gsl_vector_fprintf(stdout, p, "%g"); printf ("g: "); gsl_vector_fprintf(stdout, gradient, "%g"); printf ("gnorm: %.18e\n", gnorm1); printf ("pg: %.18e\n", pg); printf ("orth: %g\n", fabs (pg * lambda/ gnorm1)); #endif *f = fm; *step = stepm; *gnorm = gnorm1; if (fabs (pg * lambda / gnorm1) < tol) { #ifdef DEBUG printf("ok!\n"); #endif return; /* SUCCESS */ } if (stepm < stepb) { stepc = stepb; fc = fb; stepb = stepm; fb = fm; } else { stepa = stepb; fa = fb; stepb = stepm; fb = fm; } goto mid_trial; } }
static int vector_bfgs3_iterate(void *vstate, gsl_multimin_function_fdf * fdf, gsl_vector * x, double *f, gsl_vector * gradient, gsl_vector * dx) { vector_bfgs3_state_t *state = (vector_bfgs3_state_t *) vstate; double alpha = 0.0, alpha1; gsl_vector *x0 = state->x0; gsl_vector *g0 = state->g0; gsl_vector *p = state->p; double g0norm = state->g0norm; double pnorm = state->pnorm; double delta_f = state->delta_f; double pg, dir; int status; double f0 = *f; if (pnorm == 0.0 || g0norm == 0.0 || state->fp0 == 0) { gsl_vector_set_zero(dx); return GSL_ENOPROG; } if (delta_f < 0) { double del = GSL_MAX_DBL(-delta_f, 10 * GSL_DBL_EPSILON * fabs(f0)); alpha1 = GSL_MIN_DBL(1.0, 2.0 * del / (-state->fp0)); } else { alpha1 = fabs(state->step); } /* * line minimisation, with cubic interpolation (order = 3) */ if (debug) printf("...call minimize()\n"); status = minimize(&state->wrap.fdf_linear, state->rho, state->sigma, state->tau1, state->tau2, state->tau3, state->order, alpha1, &alpha); if (debug) printf("...end minimize()\n"); if (status != GSL_SUCCESS) { update_position(&(state->wrap), alpha, x, f, gradient); /* YES! hrue */ return status; } update_position(&(state->wrap), alpha, x, f, gradient); state->delta_f = *f - f0; /* * Choose a new direction for the next step */ { /* * This is the BFGS update: */ /* * p' = g1 - A dx - B dg */ /* * A = - (1+ dg.dg/dx.dg) B + dg.g/dx.dg */ /* * B = dx.g/dx.dg */ gsl_vector *dx0 = state->dx0; gsl_vector *dg0 = state->dg0; double dxg, dgg, dxdg, dgnorm, A, B; /* * dx0 = x - x0 */ gsl_vector_memcpy(dx0, x); gsl_blas_daxpy(-1.0, x0, dx0); gsl_vector_memcpy(dx, dx0); /* keep a copy */ /* * dg0 = g - g0 */ gsl_vector_memcpy(dg0, gradient); gsl_blas_daxpy(-1.0, g0, dg0); gsl_blas_ddot(dx0, gradient, &dxg); gsl_blas_ddot(dg0, gradient, &dgg); gsl_blas_ddot(dx0, dg0, &dxdg); dgnorm = gsl_blas_dnrm2(dg0); if (dxdg != 0) { B = dxg / dxdg; A = -(1.0 + dgnorm * dgnorm / dxdg) * B + dgg / dxdg; } else { B = 0; A = 0; } gsl_vector_memcpy(p, gradient); gsl_blas_daxpy(-A, dx0, p); gsl_blas_daxpy(-B, dg0, p); } gsl_vector_memcpy(g0, gradient); gsl_vector_memcpy(x0, x); state->g0norm = gsl_blas_dnrm2(g0); state->pnorm = gsl_blas_dnrm2(p); /* * update direction and fp0 */ gsl_blas_ddot(p, gradient, &pg); dir = (pg >= 0.0) ? -1.0 : +1.0; gsl_blas_dscal(dir / state->pnorm, p); state->pnorm = gsl_blas_dnrm2(p); gsl_blas_ddot(p, g0, &state->fp0); change_direction(&state->wrap); return GSL_SUCCESS; }
/** * C++ version of gsl_blas_ddot(). * @param X First vector * @param Y Second vector * @param result Vector product * @return Error code on failure */ int ddot( vector const& X, vector const& Y, double* result ){ return gsl_blas_ddot( X.get(), Y.get(), result ); }
// 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; }
void KF_deriv_steady_C (int *dim, double *sy, double *sZ, double *sT, double *sH, double *sR, double *sV, double *sQ, double *sa0, double *sP0, double *tol, int *maxiter, std::vector<double> *invf, std::vector<double> *vof, double *dvof, std::vector<double> *dfinvfsq, gsl_matrix *a_pred, std::vector<gsl_matrix*> *P_pred, gsl_matrix *K, std::vector<gsl_matrix*> *L, std::vector<gsl_matrix*> *da_pred, std::vector< std::vector<gsl_matrix*> > *dP_pred, std::vector<gsl_matrix*> *dK) { //int s, p = dim[1], mp1 = m + 1; int i, j, k, n = dim[0], m = dim[2], jm1, r = dim[3], rp1 = r + 1, conv = 0, counter = 0; //double v, f, fim1, df[rp1], dv, dtmp; //Kisum, Kim1sum; double v, f, fim1, dv, dtmp; //Kisum, Kim1sum; std::vector<double> df(rp1); //double mll = 0.0; // for debugging // data and state space model matrices gsl_vector_view Z = gsl_vector_view_array(sZ, m); gsl_matrix_view T = gsl_matrix_view_array(sT, m, m); gsl_matrix_view Q = gsl_matrix_view_array(sQ, m, m); // storage vectors and matrices gsl_vector *Vm = gsl_vector_alloc(m); gsl_vector *Vm_cp = gsl_vector_alloc(m); gsl_vector *Vm_cp2 = gsl_vector_alloc(m); gsl_vector *Vm_cp3 = gsl_vector_alloc(m); gsl_vector *Vm3 = gsl_vector_alloc(m); gsl_matrix *Mmm = gsl_matrix_alloc(m, m); gsl_matrix *M1m = gsl_matrix_alloc(1, m); gsl_matrix *Mm1 = gsl_matrix_alloc(m, 1); gsl_vector_view a0 = gsl_vector_view_array(sa0, m); gsl_vector *a_upd = gsl_vector_alloc(m); gsl_vector_memcpy(a_upd, &a0.vector); gsl_matrix_view P0 = gsl_matrix_view_array(sP0, m, m); gsl_matrix *P_upd = gsl_matrix_alloc(m, m); gsl_matrix_memcpy(P_upd, &P0.matrix); gsl_vector_view K_irow, m_irow, m2_irow, m3_irow, K_im1row; //Kri; gsl_matrix_view maux1; gsl_matrix_view Zm = gsl_matrix_view_array(gsl_vector_ptr(&Z.vector, 0), 1, m); gsl_vector *mZ = gsl_vector_alloc(m); gsl_vector_memcpy(mZ, &Z.vector); gsl_vector_scale(mZ, -1.0); //std::vector<std::vector<gsl_matrix*> *> *da_pred; std::vector<gsl_matrix*> dP_upd(rp1); for (j = 0; j < rp1; j++) { da_pred[0].at(j) = gsl_matrix_alloc(n, m); dP_upd.at(j) = gsl_matrix_calloc(m, m); } gsl_matrix *da_upd = gsl_matrix_calloc(rp1, m); // filtering recursions for (i = 0; i < n; i++) { m_irow = gsl_matrix_row(a_pred, i); gsl_blas_dgemv(CblasNoTrans, 1.0, &T.matrix, a_upd, 0.0, &m_irow.vector); P_pred[0].at(i) = gsl_matrix_alloc(m, m); if (conv == 0) { gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &T.matrix, P_upd, 0.0, Mmm); gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, Mmm, &T.matrix, 0.0, P_pred[0].at(i)); gsl_matrix_add(P_pred[0].at(i), &Q.matrix); } else { gsl_matrix_memcpy(P_pred[0].at(i), P_pred[0].at(i-1)); } gsl_blas_ddot(&Z.vector, &m_irow.vector, &v); v = sy[i] - v; if (conv == 0) { gsl_blas_dgemv(CblasNoTrans, 1.0, P_pred[0].at(i), &Z.vector, 0.0, Vm); gsl_blas_ddot(&Z.vector, Vm, &f); f += *sH; invf->at(i) = 1.0 / f; } else { invf->at(i) = invf->at(i-1); } gsl_vector_memcpy(Vm_cp, Vm); gsl_vector_memcpy(Vm_cp2, Vm); gsl_vector_memcpy(Vm_cp3, Vm); vof->at(i) = v * invf->at(i); // v[i]/f[i]; if (conv == 0) { maux1 = gsl_matrix_view_array(gsl_vector_ptr(Vm, 0), m, 1); gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, &maux1.matrix, &maux1.matrix, 0.0, Mmm); gsl_matrix_scale(Mmm, invf->at(i)); gsl_matrix_memcpy(P_upd, P_pred[0].at(i)); gsl_matrix_sub(P_upd, Mmm); } gsl_vector_memcpy(a_upd, &m_irow.vector); gsl_vector_scale(Vm_cp3, vof->at(i)); gsl_vector_add(a_upd, Vm_cp3); K_irow = gsl_matrix_row(K, i); gsl_vector_scale(Vm_cp, invf->at(i)); if (conv == 0) { gsl_blas_dgemv(CblasNoTrans, 1.0, &T.matrix, Vm_cp, 0.0, &K_irow.vector); } else { K_im1row = gsl_matrix_row(K, i-1); gsl_vector_memcpy(&K_irow.vector, &K_im1row.vector); } L[0].at(i) = gsl_matrix_alloc(m, m); if (conv == 0) { maux1 = gsl_matrix_view_array(gsl_vector_ptr(&K_irow.vector, 0), m, 1); gsl_matrix_memcpy(L[0].at(i), &T.matrix); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, &maux1.matrix, &Zm.matrix, 1.0, L[0].at(i)); } else { gsl_matrix_memcpy(L[0].at(i), L[0].at(i-1)); } // derivatives dK[0].at(i) = gsl_matrix_alloc(rp1, m); for (j = 0; j < rp1; j++) { k = i + j * n; m_irow = gsl_matrix_row(da_upd, j); m2_irow = gsl_matrix_row(da_pred[0].at(j), i); gsl_blas_dgemv(CblasNoTrans, 1.0, &T.matrix, &m_irow.vector, 0.0, &m2_irow.vector); gsl_blas_ddot(mZ, &m2_irow.vector, &dv); (dP_pred[0].at(i)).at(j) = gsl_matrix_alloc(m, m); if (conv == 0) { gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &T.matrix, dP_upd.at(j), 0.0, Mmm); gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, Mmm, &T.matrix, 0.0, (dP_pred[0].at(i)).at(j)); if (j != 0) { jm1 = j - 1; dtmp = gsl_matrix_get((dP_pred[0].at(i)).at(j), jm1, jm1); gsl_matrix_set((dP_pred[0].at(i)).at(j), jm1, jm1, dtmp + 1.0); } } else { gsl_matrix_memcpy((dP_pred[0].at(i)).at(j), (dP_pred[0].at(i-1)).at(j)); } if (conv == 0) { gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &Zm.matrix, (dP_pred[0].at(i)).at(j), 0.0, M1m); m_irow = gsl_matrix_row(M1m, 0); gsl_blas_ddot(&m_irow.vector, &Z.vector, &df[j]); if (j == 0) { df[j] += 1.0; } } dvof[k] = (dv * f - v * df[j]) * pow(invf->at(i), 2); m_irow = gsl_matrix_row(da_upd, j); gsl_blas_dgemv(CblasNoTrans, vof->at(i), (dP_pred[0].at(i)).at(j), &Z.vector, 0.0, &m_irow.vector); gsl_vector_add(&m_irow.vector, &m2_irow.vector); dtmp = -1.0 * df[j] * invf->at(i); gsl_blas_daxpy(dtmp, Vm_cp3, &m_irow.vector); gsl_blas_daxpy(dv, Vm_cp, &m_irow.vector); dfinvfsq->at(k) = df[j] * pow(invf->at(i), 2); if (conv == 0) { gsl_matrix_memcpy(dP_upd.at(j), (dP_pred[0].at(i)).at(j)); gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, (dP_pred[0].at(i)).at(j), &Zm.matrix, 0.0, Mm1); maux1 = gsl_matrix_view_array(gsl_vector_ptr(Vm_cp, 0), 1, m); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, Mm1, &maux1.matrix, 1.0, dP_upd.at(j)); maux1 = gsl_matrix_view_array(gsl_vector_ptr(Vm_cp2, 0), m, 1); gsl_matrix_memcpy(Mm1, &maux1.matrix); maux1 = gsl_matrix_view_array(gsl_vector_ptr(Vm_cp2, 0), 1, m); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, dfinvfsq->at(k), Mm1, &maux1.matrix, 1.0, dP_upd.at(j)); maux1 = gsl_matrix_view_array(gsl_vector_ptr(Vm_cp, 0), m, 1); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &maux1.matrix, &Zm.matrix, 0.0, Mmm); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, Mmm, (dP_pred[0].at(i)).at(j), 1.0, dP_upd.at(j)); } m3_irow = gsl_matrix_row(dK[0].at(i), j); if (conv == 0) { gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &T.matrix, (dP_pred[0].at(i)).at(j), 0.0, Mmm); gsl_blas_dgemv(CblasNoTrans, 1.0, Mmm, &Z.vector, 0.0, &m3_irow.vector); gsl_vector_scale(&m3_irow.vector, invf->at(i)); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &T.matrix, P_pred[0].at(i), 0.0, Mmm); gsl_blas_dgemv(CblasNoTrans, 1.0, Mmm, &Z.vector, 0.0, Vm3); gsl_vector_scale(Vm3, dfinvfsq->at(k)); gsl_vector_sub(&m3_irow.vector, Vm3); } else { K_im1row = gsl_matrix_row(dK[0].at(i-1), j); gsl_vector_memcpy(&m3_irow.vector, &K_im1row.vector); } } // check if convergence to the steady state has been reached if ((i > 0) & (conv == 0)) { if (i == 1) { fim1 = f + 1.0; } if (fabs(f - fim1) < *tol) { counter += 1; } fim1 = f; if (counter == *maxiter) { conv = 1; dim[5] = i; } } } // deallocate memory for (j = 0; j < rp1; j++) { gsl_matrix_free(dP_upd.at(j)); } gsl_vector_free(mZ); gsl_vector_free(a_upd); gsl_matrix_free(P_upd); gsl_vector_free(Vm); gsl_vector_free(Vm_cp); gsl_vector_free(Vm_cp2); gsl_vector_free(Vm_cp3); gsl_vector_free(Vm3); gsl_matrix_free(Mmm); gsl_matrix_free(M1m); gsl_matrix_free(Mm1); gsl_matrix_free(da_upd); }
void test_pontius () { size_t i, j; { gsl_multifit_linear_workspace * work = gsl_multifit_linear_alloc (pontius_n, pontius_p); gsl_matrix * X = gsl_matrix_alloc (pontius_n, pontius_p); gsl_vector_view y = gsl_vector_view_array (pontius_y, pontius_n); gsl_vector * c = gsl_vector_alloc (pontius_p); gsl_vector * r = gsl_vector_alloc (pontius_n); gsl_matrix * cov = gsl_matrix_alloc (pontius_p, pontius_p); gsl_vector_view diag; double chisq; double expected_c[3] = { 0.673565789473684E-03, 0.732059160401003E-06, -0.316081871345029E-14}; double expected_sd[3] = { 0.107938612033077E-03, 0.157817399981659E-09, 0.486652849992036E-16 }; double expected_chisq = 0.155761768796992E-05; for (i = 0 ; i < pontius_n; i++) { for (j = 0; j < pontius_p; j++) { gsl_matrix_set(X, i, j, pow(pontius_x[i], j)); } } gsl_multifit_linear (X, &y.vector, c, cov, &chisq, work); gsl_test_rel (gsl_vector_get(c,0), expected_c[0], 1e-10, "pontius gsl_fit_multilinear c0") ; gsl_test_rel (gsl_vector_get(c,1), expected_c[1], 1e-10, "pontius gsl_fit_multilinear c1") ; gsl_test_rel (gsl_vector_get(c,2), expected_c[2], 1e-10, "pontius gsl_fit_multilinear c2") ; diag = gsl_matrix_diagonal (cov); gsl_test_rel (gsl_vector_get(&diag.vector,0), pow(expected_sd[0],2.0), 1e-10, "pontius gsl_fit_multilinear cov00") ; gsl_test_rel (gsl_vector_get(&diag.vector,1), pow(expected_sd[1],2.0), 1e-10, "pontius gsl_fit_multilinear cov11") ; gsl_test_rel (gsl_vector_get(&diag.vector,2), pow(expected_sd[2],2.0), 1e-10, "pontius gsl_fit_multilinear cov22") ; gsl_test_rel (chisq, expected_chisq, 1e-10, "pontius gsl_fit_multilinear chisq") ; gsl_multifit_linear_residuals(X, &y.vector, c, r); gsl_blas_ddot(r, r, &chisq); gsl_test_rel (chisq, expected_chisq, 1e-10, "pontius gsl_fit_multilinear residuals") ; gsl_vector_free(c); gsl_vector_free(r); gsl_matrix_free(cov); gsl_matrix_free(X); gsl_multifit_linear_free (work); } { gsl_multifit_linear_workspace * work = gsl_multifit_linear_alloc (pontius_n, pontius_p); gsl_matrix * X = gsl_matrix_alloc (pontius_n, pontius_p); gsl_vector_view y = gsl_vector_view_array (pontius_y, pontius_n); gsl_vector * w = gsl_vector_alloc (pontius_n); gsl_vector * c = gsl_vector_alloc (pontius_p); gsl_vector * r = gsl_vector_alloc (pontius_n); gsl_matrix * cov = gsl_matrix_alloc (pontius_p, pontius_p); double chisq; double expected_c[3] = { 0.673565789473684E-03, 0.732059160401003E-06, -0.316081871345029E-14}; double expected_chisq = 0.155761768796992E-05; double expected_cov[3][3] ={ {2.76754385964916e-01 , -3.59649122807024e-07, 9.74658869395731e-14}, {-3.59649122807024e-07, 5.91630591630603e-13, -1.77210703526497e-19}, {9.74658869395731e-14, -1.77210703526497e-19, 5.62573661988878e-26} }; for (i = 0 ; i < pontius_n; i++) { for (j = 0; j < pontius_p; j++) { gsl_matrix_set(X, i, j, pow(pontius_x[i], j)); } } gsl_vector_set_all (w, 1.0); gsl_multifit_wlinear (X, w, &y.vector, c, cov, &chisq, work); gsl_test_rel (gsl_vector_get(c,0), expected_c[0], 1e-10, "pontius gsl_fit_multilinear c0") ; gsl_test_rel (gsl_vector_get(c,1), expected_c[1], 1e-10, "pontius gsl_fit_multilinear c1") ; gsl_test_rel (gsl_vector_get(c,2), expected_c[2], 1e-10, "pontius gsl_fit_multilinear c2") ; for (i = 0; i < pontius_p; i++) { for (j = 0; j < pontius_p; j++) { gsl_test_rel (gsl_matrix_get(cov,i,j), expected_cov[i][j], 1e-10, "pontius gsl_fit_wmultilinear cov(%d,%d)", i, j) ; } } gsl_test_rel (chisq, expected_chisq, 1e-10, "pontius gsl_fit_wmultilinear chisq") ; gsl_multifit_linear_residuals(X, &y.vector, c, r); gsl_blas_ddot(r, r, &chisq); gsl_test_rel (chisq, expected_chisq, 1e-10, "pontius gsl_fit_wmultilinear residuals") ; gsl_vector_free(w); gsl_vector_free(c); gsl_vector_free(r); gsl_matrix_free(cov); gsl_matrix_free(X); gsl_multifit_linear_free (work); } }
static int multifit_wlinear_svd (const gsl_matrix * X, const gsl_vector * w, const gsl_vector * y, double tol, int balance, size_t * rank, gsl_vector * c, gsl_matrix * cov, double *chisq, gsl_multifit_linear_workspace * work) { if (X->size1 != y->size) { GSL_ERROR ("number of observations in y does not match rows of matrix X", GSL_EBADLEN); } else if (X->size2 != c->size) { GSL_ERROR ("number of parameters c does not match columns of matrix X", GSL_EBADLEN); } else if (w->size != y->size) { GSL_ERROR ("number of weights does not match number of observations", GSL_EBADLEN); } else if (cov->size1 != cov->size2) { GSL_ERROR ("covariance matrix is not square", GSL_ENOTSQR); } else if (c->size != cov->size1) { GSL_ERROR ("number of parameters does not match size of covariance matrix", GSL_EBADLEN); } else if (X->size1 != work->n || X->size2 != work->p) { GSL_ERROR ("size of workspace does not match size of observation matrix", GSL_EBADLEN); } else { const size_t n = X->size1; const size_t p = X->size2; size_t i, j, p_eff; gsl_matrix *A = work->A; gsl_matrix *Q = work->Q; gsl_matrix *QSI = work->QSI; gsl_vector *S = work->S; gsl_vector *t = work->t; gsl_vector *xt = work->xt; gsl_vector *D = work->D; /* Scale X, A = sqrt(w) X */ gsl_matrix_memcpy (A, X); for (i = 0; i < n; i++) { double wi = gsl_vector_get (w, i); if (wi < 0) wi = 0; { gsl_vector_view row = gsl_matrix_row (A, i); gsl_vector_scale (&row.vector, sqrt (wi)); } } /* Balance the columns of the matrix A if requested */ if (balance) { gsl_linalg_balance_columns (A, D); } else { gsl_vector_set_all (D, 1.0); } /* Decompose A into U S Q^T */ gsl_linalg_SV_decomp_mod (A, QSI, Q, S, xt); /* Solve sqrt(w) y = A c for c, by first computing t = sqrt(w) y */ for (i = 0; i < n; i++) { double wi = gsl_vector_get (w, i); double yi = gsl_vector_get (y, i); if (wi < 0) wi = 0; gsl_vector_set (t, i, sqrt (wi) * yi); } gsl_blas_dgemv (CblasTrans, 1.0, A, t, 0.0, xt); /* Scale the matrix Q, Q' = Q S^-1 */ gsl_matrix_memcpy (QSI, Q); { double alpha0 = gsl_vector_get (S, 0); p_eff = 0; for (j = 0; j < p; j++) { gsl_vector_view column = gsl_matrix_column (QSI, j); double alpha = gsl_vector_get (S, j); if (alpha <= tol * alpha0) { alpha = 0.0; } else { alpha = 1.0 / alpha; p_eff++; } gsl_vector_scale (&column.vector, alpha); } *rank = p_eff; } gsl_vector_set_zero (c); /* Solution */ gsl_blas_dgemv (CblasNoTrans, 1.0, QSI, xt, 0.0, c); /* Unscale the balancing factors */ gsl_vector_div (c, D); /* Compute chisq, from residual r = y - X c */ { double r2 = 0; for (i = 0; i < n; i++) { double yi = gsl_vector_get (y, i); double wi = gsl_vector_get (w, i); gsl_vector_const_view row = gsl_matrix_const_row (X, i); double y_est, ri; gsl_blas_ddot (&row.vector, c, &y_est); ri = yi - y_est; r2 += wi * ri * ri; } *chisq = r2; /* Form covariance matrix cov = (X^T W X)^-1 = (Q S^-1) (Q S^-1)^T */ for (i = 0; i < p; i++) { gsl_vector_view row_i = gsl_matrix_row (QSI, i); double d_i = gsl_vector_get (D, i); for (j = i; j < p; j++) { gsl_vector_view row_j = gsl_matrix_row (QSI, j); double d_j = gsl_vector_get (D, j); double s; gsl_blas_ddot (&row_i.vector, &row_j.vector, &s); gsl_matrix_set (cov, i, j, s / (d_i * d_j)); gsl_matrix_set (cov, j, i, s / (d_i * d_j)); } } } return GSL_SUCCESS; } }
static int bundle_method_iterate (void *vstate, gsl_multimin_function_fsdf * fsdf, gsl_vector * x, double * f, gsl_vector * subgradient, gsl_vector * dx, double * eps) { bundle_method_state_t *state = (bundle_method_state_t *) vstate; bundle_element *item; size_t i, debug=0; int status; double tmp_d, t_old, t_int_l; /* local variables */ gsl_vector *y; /* a trial point (the next iteration point by the serios step) */ gsl_vector *sgr_y; /* subgradient at y */ double f_y; /* the function value at y */ gsl_vector *p; /* the aggregate subgradient */ double p_norm, lin_error_p; /* norm of p, the aggregate linear. error */ gsl_vector *tmp_v; /* data for the convex quadratic problem (for the dual problem) */ gsl_vector *q; /* elements of the array are the linearization errors */ gsl_matrix *Q; /* Q=G^T*G (G is matrix which collumns are subgradients) */ gsl_vector *lambda; /* the convex combination coefficients of the subgradients (solution of the dual problem) */ lambda = gsl_vector_alloc(state->bundle_size); if(lambda == 0) { GSL_ERROR_VAL ("failed to allocate workspace", GSL_ENOMEM, 0); } q = gsl_vector_alloc(lambda->size); if(q == 0) { gsl_vector_free(lambda); GSL_ERROR_VAL ("failed to allocate workspace", GSL_ENOMEM, 0); } y = gsl_vector_calloc(x->size); if(y == 0) { gsl_vector_free(q); gsl_vector_free(lambda); GSL_ERROR_VAL ("failed to allocate workspace", GSL_ENOMEM, 0); } sgr_y = gsl_vector_calloc(x->size); if(sgr_y == 0) { gsl_vector_free(y); gsl_vector_free(q); gsl_vector_free(lambda); GSL_ERROR_VAL ("failed to allocate workspace", GSL_ENOMEM, 0); } Q = gsl_matrix_alloc(state->bundle_size, state->bundle_size); if(Q == 0) { gsl_vector_free(sgr_y); gsl_vector_free(y); gsl_vector_free(q); gsl_vector_free(lambda); GSL_ERROR_VAL ("failed to allocate workspace", GSL_ENOMEM, 0); } p = gsl_vector_calloc(x->size); if(p == 0) { gsl_matrix_free(Q); gsl_vector_free(sgr_y); gsl_vector_free(y); gsl_vector_free(q); gsl_vector_free(lambda); GSL_ERROR_VAL ("failed to allocate workspace", GSL_ENOMEM, 0); } tmp_v = gsl_vector_calloc(x->size); if(tmp_v == 0) { gsl_vector_free(p); gsl_matrix_free(Q); gsl_vector_free(sgr_y); gsl_vector_free(y); gsl_vector_free(q); gsl_vector_free(lambda); GSL_ERROR_VAL ("failed to allocate workspace", GSL_ENOMEM, 0); } /* solve the dual problem */ status = build_cqp_data(state, Q, q); status = solve_qp_pdip(Q, q, lambda); gsl_matrix_free(Q); gsl_vector_free(q); /* compute the aggregate subgradient (it is called p in the documantation)*/ /* and the appropriated linearization error */ lin_error_p = 0.0; item = state->head; for(i=0; i<lambda->size; i++) { status = gsl_blas_daxpy(gsl_vector_get(lambda,i), item->sgr, p); lin_error_p += gsl_vector_get(lambda,i)*(item->lin_error); item = item->next; } if(debug) { printf("the dual problem solution:\n"); for(i=0;i<lambda->size;i++) printf("%7.6e ",gsl_vector_get(lambda,i)); printf("\n\n"); printf("the aggregate subgradient: \n"); for(i=0;i<p->size;i++) printf("%.6e ",gsl_vector_get(p,i)); printf("\n"); printf("lin. error for aggr subgradient = %e\n",lin_error_p); } /* the norm of the aggr subgradient */ p_norm = gsl_blas_dnrm2(p); /* search direction dx=-t*p (t is the length of step) */ status = gsl_vector_memcpy(dx,p); status = gsl_vector_scale(dx,-1.0*state->t); /* v =-t*norm(p)^2-alpha_p */ state->v = -gsl_pow_2(p_norm)*(state->t)-lin_error_p; /* the subgradient is the aggegate sungradient */ status = gsl_blas_dcopy(p,subgradient); /* iteration step */ /* y=x+dx */ status = gsl_blas_dcopy(dx,y); status = gsl_blas_daxpy(1.0,x,y); /* function value at y */ f_y = GSL_MULTIMIN_FN_EVAL_F(fsdf, y); state->f_eval++; /* for t-update */ if(!state->fixed_step_length) { t_old = state->t; if(fabs(state->v-(f_y-*f)) < state->rg || state->v-(f_y-*f) > state->rg) t_int_l = state->t_max; else t_int_l = 0.5*t_old*(state->v)/(state->v-(f_y-*f)); } else { t_old = state->t; t_int_l = state->t; } if( f_y-*f <= state->m_ss*state->v ) /* Serious-Step */ { if(debug) printf("\nSerious-Step\n"); /* the relaxation step */ if(state->relaxation) { if(f_y-*f <= state->v*state->m_rel) { double f_z; gsl_vector * z = gsl_vector_alloc(y->size); /* z = y+dx = x+2*dx */ status = gsl_blas_dcopy(x,z); status = gsl_blas_daxpy(2.0,dx,z); f_z = GSL_MULTIMIN_FN_EVAL_F(fsdf, z); state->f_eval++; if(0.5*f_z-f_y+0.5*(*f) > state->rg) state->rel_parameter = GSL_MIN_DBL(-0.5*(-0.5*f_z+2.0*f_y-1.5*(*f))/(0.5*f_z-f_y+0.5*(*f)),1.999); else if (fabs(0.5*f_z-f_y+0.5*(*f)) > state->rg) state->rel_parameter = 1.999; else /* something is wrong */ state->rel_parameter = 1.0; /* save the old iteration point */ status = gsl_blas_dcopy(y,z); /* y = (1-rel_parameter)*x+rel_parameter*y */ gsl_blas_dscal(state->rel_parameter,y); status = gsl_blas_daxpy(1.0-state->rel_parameter,x,y); /* f(y) und sgr_f(y) */ tmp_d = GSL_MULTIMIN_FN_EVAL_F(fsdf, y); state->f_eval++; if(tmp_d > f_y) { /* keep y as the current point */ status = gsl_blas_dcopy(z,y); state->rel_counter++; } else { f_y = tmp_d; /* dx = y-x */ status = gsl_blas_dcopy(y,dx); status = gsl_blas_daxpy(-1.0,x,dx); /* if iteration points bevor and after the rel. step are closly, the rel_step counte will be increased */ /* |1-rel_parameter| <= 0.1*/ if( fabs(1.0-state->rel_parameter) < 0.1) state->rel_counter++; } GSL_MULTIMIN_FN_EVAL_SDF(fsdf, y, sgr_y); state->sgr_eval++; if(state->rel_counter > state->rel_counter_max) state->relaxation = 0; /* */ status = gsl_blas_daxpy(-1.0,y,z); status = gsl_blas_ddot(p, z, &tmp_d); *eps = f_y-*f-(state->v)+tmp_d; gsl_vector_free(z); } else { *eps = f_y-(state->v)-*f; GSL_MULTIMIN_FN_EVAL_SDF(fsdf, y, sgr_y); state->sgr_eval++; } } else { *eps = f_y-(state->v)-*f; GSL_MULTIMIN_FN_EVAL_SDF(fsdf, y, sgr_y); state->sgr_eval++; } /* calculate linearization errors at new iteration point */ item = state->head; for(i=0; i<state->bundle_size; i++) { status = gsl_blas_ddot(item->sgr, dx, &tmp_d); item->lin_error += f_y-*f-tmp_d; item = item->next; } /* linearization error at new iteration point */ status = gsl_blas_ddot(p, dx, &tmp_d); lin_error_p += f_y-*f-tmp_d; /* update the bundle */ status = update_bundle(state, sgr_y, 0.0, lambda, p, lin_error_p, 1); /* adapt the step length */ if(!state->fixed_step_length) { if(f_y-*f <= state->v*state->m_t && state->step_counter > 0) state->t = t_int_l; else if(state->step_counter>3) state->t=2.0*t_old; state->t = GSL_MIN_DBL(GSL_MIN_DBL(state->t,10.0*t_old),state->t_max); /*state->eps_v = GSL_MAX_DBL(state->eps_v,-2.0*state->v);*/ state->step_counter = GSL_MAX_INT(state->step_counter+1,1); if(fabs(state->t-t_old) > state->rg) state->step_counter=1; } /* x=y, f=f(y) */ status = gsl_blas_dcopy(y,x); *f = f_y; } else /* Null-Step */ { if(debug) printf("\nNull-Step\n"); GSL_MULTIMIN_FN_EVAL_SDF(fsdf, y, sgr_y); state->sgr_eval++; /* eps for the eps_subdifferential */ *eps = lin_error_p; /*calculate the liniarization error at y */ status = gsl_blas_ddot(sgr_y,dx,&tmp_d); tmp_d += *f-f_y; /* Bundle update */ status = update_bundle(state, sgr_y, tmp_d, lambda, p, lin_error_p, 0); /* adapt the step length */ if(!state->fixed_step_length) { /*state->eps_v = GSL_MIN_DBL(state->eps_v,lin_error_p);*/ if(tmp_d > GSL_MAX_DBL(p_norm,lin_error_p) && state->step_counter < -1) state->t = t_int_l; else if(state->step_counter < -3) state->t = 0.5*t_old; state->t = GSL_MAX_DBL(GSL_MAX_DBL(0.1*t_old,state->t),state->t_min); state->step_counter = GSL_MIN_INT(state->step_counter-1,-1); if(fabs(state->t-t_old) > state->rg) state->step_counter = -1; } } state->lambda_min = p_norm * state->lm_accuracy; if(debug) { printf("\nthe new bundle:\n"); bundle_out_liste(state); printf("\n\n"); printf("the curent itarationspoint (1 x %d)\n",x->size); for(i=0;i<x->size;i++) printf("%12.6f ",gsl_vector_get(x,i)); printf("\n\n"); printf("functions value at current point: f=%.8f\n",*f); printf("\nstep length t=%.5e\n",state->t); printf("\nstep_counter sc=%d\n",state->step_counter); printf("\naccuracy: v=%.5e\n",state->v); printf("\nlambda_min=%e\n",state->lambda_min); printf("\n"); } gsl_vector_free(lambda); gsl_vector_free(y); gsl_vector_free(sgr_y); gsl_vector_free(p); return GSL_SUCCESS; }
/** main simulation loop */ int main() { // init own parameters. initDerivedParams(); // init random generator gsl_rng_env_setup(); r = gsl_rng_alloc(gsl_rng_default); gsl_rng_set(r, SEED_MAIN); // file handle for xxx file FILE *postF = fopen(FILENAME_POST, FILEPOST_FLAG); // file handle for xxx file FILE *preF = fopen(FILENAME_PRE, "wb"); // set up vectors: // to hold post synaptic potentials [unused??] gsl_vector *psp = gsl_vector_alloc(NPRE); // to hold post synaptic potentials 1st filtered gsl_vector *pspS = gsl_vector_alloc(NPRE); // to hold "excitatory" part of psp for Euler integration gsl_vector *sue = gsl_vector_alloc(NPRE); // to hold "inhibitory" part of psp for Euler integration gsl_vector *sui = gsl_vector_alloc(NPRE); // to hold psp 2nd filter gsl_vector *pspTilde = gsl_vector_alloc(NPRE); // to hold weights gsl_vector *w = gsl_vector_alloc(NPRE); // to hold xxx gsl_vector *pres = gsl_vector_alloc(NPRE); // ?? ou XXX \todo #ifdef PREDICT_OU gsl_vector *ou = gsl_vector_alloc(N_OU); gsl_vector *preU = gsl_vector_calloc(NPRE); gsl_vector *wInput = gsl_vector_alloc(N_OU); gsl_matrix *wPre = gsl_matrix_calloc(NPRE, N_OU); double *preUP = gsl_vector_ptr(preU,0); double *ouP = gsl_vector_ptr(ou,0); double *wInputP = gsl_vector_ptr(wInput,0); double *wPreP = gsl_matrix_ptr(wPre,0,0); #endif // get pointers to array within the gsl_vector data structures above. double *pspP = gsl_vector_ptr(psp,0); double *pspSP = gsl_vector_ptr(pspS,0); double *sueP = gsl_vector_ptr(sue,0); double *suiP = gsl_vector_ptr(sui,0); double *pspTildeP = gsl_vector_ptr(pspTilde,0); double *wP = gsl_vector_ptr(w,0); double *presP = gsl_vector_ptr(pres,0); for(int i=0; i<NPRE; i++) { // init pspP etc to zero *(pspP+i) = 0; *(sueP+i) = 0; *(suiP+i) = 0; #ifdef RANDI_WEIGHTS // Gaussian weights *(wP+i) = gsl_ran_gaussian(r, .1); #else *(wP+i) = 0; #endif } //! OU \todo what for? #ifdef PREDICT_OU for(int j=0; j < N_OU; j++) { *(ouP + j) = gsl_ran_gaussian(r, 1) + M_OU; *(wInputP + j) = gsl_ran_lognormal(r, 0., 2.)/N_OU/exp(2.)/2.; for(int i=0; i < NPRE; i++) *(wPreP + j*NPRE + i) = gsl_ran_lognormal(r, 0., 2.)/N_OU/exp(2.)/2.; } #endif // temp variables for the simulation yyyy double u = 0, // soma potential. uV = 0, // some potential from dendrite only (ie discounted // dendrite potential rU = 0, // instantneou rate rV = 0, // rate on dendritic potential only uI = 0, // soma potential only from somatic inputs rI = 0, // rate on somatic potential only uInput = 0; // for OU? // run simulatio TRAININGCYCLES number of times for( int s = 0; s < TRAININGCYCLES; s++) { // for all TIMEBINS for( int t = 0; t < TIMEBINS; t++) { #ifdef PREDICT_OU for(int i = 0; i < N_OU; i++) { *(ouP+i) = runOU(*(ouP+i), M_OU, GAMMA_OU, S_OU); } gsl_blas_dgemv(CblasNoTrans, 1., wPre, ou, 0., preU); #endif // update PSP of our neurons for inputs from all presynaptic neurons for( int i = 0; i < NPRE; i++) { #ifdef RAMPUPRATE /** just read in the PRE_ACT and generate a spike and store it in presP -- so PRE_ACT has inpretation of potential */ updatePre(sueP+i, suiP+i, pspP + i, pspSP + i, pspTildeP + i, *(presP + i) = spiking(PRE_ACT[t*NPRE + i], gsl_rng_uniform(r))); #elif defined PREDICT_OU //*(ouP+i) = runOU(*(ouP+i), M_OU, GAMMA_OU, S_OU); // why commented out? updatePre(sueP+i, suiP+i, pspP + i, pspSP + i, pspTildeP + i, *(presP + i) = DT * phi(*(preUP+i)));//spiking(DT * phi(*(preUP+i)), gsl_rng_uniform(r))); // why commented out? #else // PRE_ACT intepreated as spikes updatePre(sueP+i, suiP+i, pspP + i, pspSP + i, pspTildeP + i, *(presP + i) = PRE_ACT[t*NPRE + i]); #endif } // endfor NPRE #ifdef PREDICT_OU gsl_blas_ddot(wInput, ou, &uInput); GE[t] = DT * phi(uInput); #endif // now update the membrane potential. updateMembrane(&u, &uV, &uI, w, psp, GE[t], GI[t]); // now calculate rates from from potentials. #ifdef POSTSPIKING // usually switch off as learning is faster when // learning from U // with low-pass filtering of soma potential from actual // generation of spikes (back propgating dentric spikes? rU = GAMMA_POSTS*rU + (1-GAMMA_POSTS)*spiking(DT * phi(u), gsl_rng_uniform(r))/DT; #else // simpler -- direct. rU = phi(u); #endif rV = phi(uV); rI = phi(uI); // now update weights based on rU, RV, the 2nd filtered PSP and // the pspSP for(int i = 0; i < NPRE; i++) { updateWeight(wP + i, rU, *(pspTildeP+i), rV, *(pspSP+i)); } #ifdef TAUEFF /** write rU to postF, but only for the last run of the simulation and then only before the STIM_ONSET time -- ie it is the trained output without somatic drive. */ if(s == TRAININGCYCLES - 1 && t < STIM_ONSET/DT) { fwrite(&rU, sizeof(double), 1, postF); } #else /** for every 10th training cycle write all variables below to postF in order: */ if(s%(TRAININGCYCLES/10)==0) { fwrite(&rU, sizeof(double), 1, postF); fwrite(GE+t, sizeof(double), 1, postF); fwrite(&rV, sizeof(double), 1, postF); fwrite(&rI, sizeof(double), 1, postF); fwrite(&u, sizeof(double), 1, postF); } if(s == TRAININGCYCLES - 1) { #ifdef RECORD_PREACT // for the last cycle also record the activity of the // presynaptic neurons fwrite(PRE_ACT + t * NPRE, sizeof(double), 20, preF); //fwrite(ouP, sizeof(double), 20, preF); fwrite(presP, sizeof(double), 20, preF); #else // and the 1st and 2nd filtered PSP fwrite(pspSP, sizeof(double), 1, preF); fwrite(pspTildeP, sizeof(double), 1, preF); #endif } #endif } } fclose(preF); fclose(postF); return 0; }
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; }
void KF_deriv_aux_C (int *dim, double *sy, double *sZ, double *sT, double *sH, double *sR, double *sV, double *sQ, double *sa0, double *sP0, std::vector<double> *invf, std::vector<double> *vof, double *dvof, std::vector<double> *dfinvfsq, gsl_matrix *a_pred, std::vector<gsl_matrix*> *P_pred, gsl_matrix *K, std::vector<gsl_matrix*> *L, std::vector<gsl_matrix*> *da_pred, std::vector< std::vector<gsl_matrix*> > *dP_pred, std::vector<gsl_matrix*> *dK) { //int s, p = dim[1], mp1 = m + 1; int i, j, k, n = dim[0], m = dim[2], jm1, r = dim[3], rp1 = r + 1; double v, f, df, dv, dtmp; // data and state space model matrices gsl_vector_view Z = gsl_vector_view_array(sZ, m); gsl_matrix_view T = gsl_matrix_view_array(sT, m, m); gsl_matrix_view Q = gsl_matrix_view_array(sQ, m, m); // storage vectors and matrices gsl_vector *Vm = gsl_vector_alloc(m); gsl_vector *Vm_cp = gsl_vector_alloc(m); gsl_vector *Vm_cp2 = gsl_vector_alloc(m); gsl_vector *Vm3 = gsl_vector_alloc(m); gsl_matrix *Mmm = gsl_matrix_alloc(m, m); gsl_matrix *M1m = gsl_matrix_alloc(1, m); gsl_matrix *Mm1 = gsl_matrix_alloc(m, 1); gsl_vector_view a0 = gsl_vector_view_array(sa0, m); gsl_vector *a_upd = gsl_vector_alloc(m); gsl_vector_memcpy(a_upd, &a0.vector); gsl_matrix_view P0 = gsl_matrix_view_array(sP0, m, m); gsl_matrix *P_upd = gsl_matrix_alloc(m, m); gsl_matrix_memcpy(P_upd, &P0.matrix); gsl_vector_view K_irow, m_irow, m2_irow, m3_irow; gsl_matrix_view maux1; gsl_matrix_view Zm = gsl_matrix_view_array(gsl_vector_ptr(&Z.vector, 0), 1, m); gsl_vector *mZ = gsl_vector_alloc(m); gsl_vector_memcpy(mZ, &Z.vector); gsl_vector_scale(mZ, -1.0); std::vector<gsl_matrix*> dP_upd(rp1); for (j = 0; j < rp1; j++) { da_pred[0].at(j) = gsl_matrix_alloc(n, m); dP_upd.at(j) = gsl_matrix_calloc(m, m); } gsl_matrix *da_upd = gsl_matrix_calloc(rp1, m); // filtering recursions for (i = 0; i < n; i++) { m_irow = gsl_matrix_row(a_pred, i); gsl_blas_dgemv(CblasNoTrans, 1.0, &T.matrix, a_upd, 0.0, &m_irow.vector); P_pred[0].at(i) = gsl_matrix_alloc(m, m); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &T.matrix, P_upd, 0.0, Mmm); gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, Mmm, &T.matrix, 0.0, P_pred[0].at(i)); gsl_matrix_add(P_pred[0].at(i), &Q.matrix); gsl_blas_ddot(&Z.vector, &m_irow.vector, &v); v = sy[i] - v; gsl_blas_dgemv(CblasNoTrans, 1.0, P_pred[0].at(i), &Z.vector, 0.0, Vm); gsl_blas_ddot(&Z.vector, Vm, &f); f += *sH; gsl_vector_memcpy(Vm_cp, Vm); gsl_vector_memcpy(Vm_cp2, Vm); invf->at(i) = 1.0 / f; vof->at(i) = v * invf->at(i); // v[i]/f[i]; maux1 = gsl_matrix_view_array(gsl_vector_ptr(Vm, 0), m, 1); gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, &maux1.matrix, &maux1.matrix, 0.0, Mmm); gsl_matrix_scale(Mmm, invf->at(i)); gsl_vector_memcpy(a_upd, &m_irow.vector); gsl_vector_scale(Vm, vof->at(i)); gsl_vector_add(a_upd, Vm); gsl_matrix_memcpy(P_upd, P_pred[0].at(i)); gsl_matrix_sub(P_upd, Mmm); K_irow = gsl_matrix_row(K, i); gsl_vector_scale(Vm_cp, invf->at(i)); gsl_blas_dgemv(CblasNoTrans, 1.0, &T.matrix, Vm_cp, 0.0, &K_irow.vector); L[0].at(i) = gsl_matrix_alloc(m, m); maux1 = gsl_matrix_view_array(gsl_vector_ptr(&K_irow.vector, 0), m, 1); gsl_matrix_memcpy(L[0].at(i), &T.matrix); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, &maux1.matrix, &Zm.matrix, 1.0, L[0].at(i)); // derivatives dK[0].at(i) = gsl_matrix_alloc(rp1, m); for (j = 0; j < rp1; j++) { k = i + j * n; m_irow = gsl_matrix_row(da_upd, j); m2_irow = gsl_matrix_row(da_pred[0].at(j), i); gsl_blas_dgemv(CblasNoTrans, 1.0, &T.matrix, &m_irow.vector, 0.0, &m2_irow.vector); gsl_blas_ddot(mZ, &m2_irow.vector, &dv); (dP_pred[0].at(i)).at(j) = gsl_matrix_alloc(m, m); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &T.matrix, dP_upd.at(j), 0.0, Mmm); gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, Mmm, &T.matrix, 0.0, (dP_pred[0].at(i)).at(j)); if (j != 0) { jm1 = j - 1; dtmp = gsl_matrix_get((dP_pred[0].at(i)).at(j), jm1, jm1); gsl_matrix_set((dP_pred[0].at(i)).at(j), jm1, jm1, dtmp + 1.0); } gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &Zm.matrix, (dP_pred[0].at(i)).at(j), 0.0, M1m); m_irow = gsl_matrix_row(M1m, 0); gsl_blas_ddot(&m_irow.vector, &Z.vector, &df); if (j == 0) { df += 1.0; } dvof[k] = (dv * f - v * df) * pow(invf->at(i), 2); m_irow = gsl_matrix_row(da_upd, j); gsl_blas_dgemv(CblasNoTrans, vof->at(i), (dP_pred[0].at(i)).at(j), &Z.vector, 0.0, &m_irow.vector); gsl_vector_add(&m_irow.vector, &m2_irow.vector); dtmp = -1.0 * df * invf->at(i); gsl_blas_daxpy(dtmp, Vm, &m_irow.vector); gsl_blas_daxpy(dv, Vm_cp, &m_irow.vector); gsl_matrix_memcpy(dP_upd.at(j), (dP_pred[0].at(i)).at(j)); gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, (dP_pred[0].at(i)).at(j), &Zm.matrix, 0.0, Mm1); maux1 = gsl_matrix_view_array(gsl_vector_ptr(Vm_cp, 0), 1, m); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, Mm1, &maux1.matrix, 1.0, dP_upd.at(j)); maux1 = gsl_matrix_view_array(gsl_vector_ptr(Vm_cp2, 0), m, 1); gsl_matrix_memcpy(Mm1, &maux1.matrix); maux1 = gsl_matrix_view_array(gsl_vector_ptr(Vm_cp2, 0), 1, m); dfinvfsq->at(k) = df * pow(invf->at(i), 2); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, dfinvfsq->at(k), Mm1, &maux1.matrix, 1.0, dP_upd.at(j)); maux1 = gsl_matrix_view_array(gsl_vector_ptr(Vm_cp, 0), m, 1); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &maux1.matrix, &Zm.matrix, 0.0, Mmm); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, Mmm, (dP_pred[0].at(i)).at(j), 1.0, dP_upd.at(j)); m3_irow = gsl_matrix_row(dK[0].at(i), j); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &T.matrix, (dP_pred[0].at(i)).at(j), 0.0, Mmm); gsl_blas_dgemv(CblasNoTrans, 1.0, Mmm, &Z.vector, 0.0, &m3_irow.vector); gsl_vector_scale(&m3_irow.vector, invf->at(i)); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &T.matrix, P_pred[0].at(i), 0.0, Mmm); gsl_blas_dgemv(CblasNoTrans, 1.0, Mmm, &Z.vector, 0.0, Vm3); gsl_vector_scale(Vm3, dfinvfsq->at(k)); gsl_vector_sub(&m3_irow.vector, Vm3); } } // deallocate memory for (j = 0; j < rp1; j++) { gsl_matrix_free(dP_upd.at(j)); } gsl_vector_free(mZ); gsl_vector_free(a_upd); gsl_matrix_free(P_upd); gsl_vector_free(Vm); gsl_vector_free(Vm_cp); gsl_vector_free(Vm_cp2); gsl_vector_free(Vm3); gsl_matrix_free(Mmm); gsl_matrix_free(M1m); gsl_matrix_free(Mm1); gsl_matrix_free(da_upd); }
/* matrices are linearized */ double ighmm_rand_multivariate_normal_density(int length, const double *x, double *mean, double *sigmainv, double det) { # define CUR_PROC "ighmm_rand_multivariate_normal_density" /* multivariate normal density function */ /* * length dimension of the random vetor * x point at which to evaluate the pdf * mean vector of means of size n * sigmainv inverse variance matrix of dimension n x n * det determinant of covariance matrix */ #ifdef DO_WITH_GSL int i, j; double ax,ay; gsl_vector *ym, *xm, *gmean; gsl_matrix *inv = gsl_matrix_alloc(length, length); for (i=0; i<length; ++i) { for (j=0; j<length; ++j) { gsl_matrix_set(inv, i, j, sigmainv[i*length+j]); } } xm = gsl_vector_alloc(length); gmean = gsl_vector_alloc(length); /*gsl_vector_memcpy(xm, x);*/ for (i=0; i<length; ++i) { gsl_vector_set(xm, i, x[i]); gsl_vector_set(gmean, i, mean[i]); } gsl_vector_sub(xm, gmean); ym = gsl_vector_alloc(length); gsl_blas_dsymv(CblasUpper, 1.0, inv, xm, 0.0, ym); gsl_matrix_free(inv); gsl_blas_ddot(xm, ym, &ay); gsl_vector_free(xm); gsl_vector_free(ym); ay = exp(-0.5*ay) / sqrt(pow((2*M_PI), length) * det); return ay; #else /* do without GSL */ int i, j; double ay, tempv; ay = 0; for (i=0; i<length; ++i) { tempv = 0; for (j=0; j<length; ++j) { tempv += (x[j]-mean[j])*sigmainv[j*length+i]; } ay += tempv*(x[i]-mean[i]); } ay = exp(-0.5*ay) / sqrt(pow((2*PI), length) * det); return ay; #endif # undef CUR_PROC } /* double ighmm_rand_multivariate_normal_density */
void KFKSDS_deriv_C (int *dim, double *sy, double *sZ, double *sT, double *sH, double *sR, double *sV, double *sQ, double *sa0, double *sP0, double *dvof, double *epshat, double *vareps, double *etahat, double *vareta, double *r, double *N, double *dr, double *dN, double *dahat, double *dvareps) { //int s, p = dim[1], mp1 = m + 1; int i, ip1, j, k, n = dim[0], m = dim[2], ir = dim[3], rp1 = ir + 1, nrp1 = n * rp1, rp1m = rp1 * m, iaux, irp1m, irsod = ir * sizeof(double), msod = m * sizeof(double), nsod = n * sizeof(double), rp1msod = rp1 * msod; //double invf[n], vof[n], msHsq, dfinvfsq[nrp1]; double msHsq; std::vector<double> invf(n); std::vector<double> vof(n); std::vector<double> dfinvfsq(nrp1); gsl_matrix_view Q = gsl_matrix_view_array(sQ, m, m); gsl_vector_view Z = gsl_vector_view_array(sZ, m); gsl_vector * Z_cp = gsl_vector_alloc(m); gsl_matrix * ZtZ = gsl_matrix_alloc(m, m); gsl_matrix_view maux1, maux2; maux1 = gsl_matrix_view_array(gsl_vector_ptr(&Z.vector, 0), m, 1); gsl_vector_memcpy(Z_cp, &Z.vector); maux2 = gsl_matrix_view_array(gsl_vector_ptr(Z_cp, 0), 1, m); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &maux1.matrix, &maux2.matrix, 0.0, ZtZ); gsl_matrix * a_pred = gsl_matrix_alloc(n, m); std::vector<gsl_matrix*> P_pred(n); gsl_matrix * K = gsl_matrix_alloc(n, m); gsl_vector_view K_irow; std::vector<gsl_matrix*> L(n); gsl_vector_view Qdiag = gsl_matrix_diagonal(&Q.matrix); gsl_vector * Qdiag_msq = gsl_vector_alloc(m); gsl_vector_memcpy(Qdiag_msq, &Qdiag.vector); gsl_vector_mul(Qdiag_msq, &Qdiag.vector); gsl_vector_scale(Qdiag_msq, -1.0); std::vector<gsl_matrix*> da_pred(rp1); std::vector< std::vector<gsl_matrix*> > dP_pred(n, std::vector<gsl_matrix*>(rp1)); std::vector<gsl_matrix*> dK(n); // filtering KF_deriv_aux_C(dim, sy, sZ, sT, sH, sR, sV, sQ, sa0, sP0, &invf, &vof, dvof, &dfinvfsq, a_pred, &P_pred, K, &L, &da_pred, &dP_pred, &dK); // state vector smoothing and disturbances smoothing gsl_matrix_view V = gsl_matrix_view_array(sV, ir, ir); gsl_matrix_view R = gsl_matrix_view_array(sR, m, ir); gsl_vector_view vaux; gsl_vector *vaux2 = gsl_vector_alloc(m); gsl_matrix *Mmm = gsl_matrix_alloc(m, m); gsl_matrix *Mmm2 = gsl_matrix_alloc(m, m); gsl_matrix *Mrm = gsl_matrix_alloc(ir, m); gsl_vector_memcpy(Z_cp, &Z.vector); gsl_matrix *r0 = gsl_matrix_alloc(n + 1, m); gsl_vector_view r_row_t; gsl_vector_view r_row_tp1 = gsl_matrix_row(r0, n); gsl_vector_set_zero(&r_row_tp1.vector); std::vector<gsl_matrix*> N0(n + 1); N0.at(n) = gsl_matrix_calloc(m, m); gsl_vector_view Ndiag; gsl_vector *var_eps = gsl_vector_alloc(n); msHsq = -1.0 * pow(*sH, 2); //vaux = gsl_vector_view_array(invf, n); vaux = gsl_vector_view_array(&invf[0], n); gsl_vector_set_all(var_eps, msHsq); gsl_vector_mul(var_eps, &vaux.vector); gsl_vector_add_constant(var_eps, *sH); gsl_vector *vr = gsl_vector_alloc(ir); gsl_matrix *dL = gsl_matrix_alloc(m, m); std::vector<gsl_matrix*> dr0(n + 1); dr0.at(n) = gsl_matrix_calloc(rp1, m); gsl_vector_view dr_row_t, dr_row_tp1; std::vector< std::vector<gsl_matrix*> > dN0(n + 1, std::vector<gsl_matrix*>(rp1)); for (j = 0; j < rp1; j++) { (dN0.at(n)).at(j) = gsl_matrix_calloc(m, m); } for (i = n-1; i > -1; i--) { ip1 = i + 1; iaux = (i-1) * rp1m; irp1m = i * rp1m; if (i != n-1) //the case i=n-1 was initialized above r_row_tp1 = gsl_matrix_row(r0, ip1); r_row_t = gsl_matrix_row(r0, i); gsl_blas_dgemv(CblasTrans, 1.0, L.at(i), &r_row_tp1.vector, 0.0, &r_row_t.vector); gsl_vector_memcpy(Z_cp, &Z.vector); gsl_vector_scale(Z_cp, vof.at(i)); gsl_vector_add(&r_row_t.vector, Z_cp); gsl_vector_memcpy(vaux2, &r_row_tp1.vector); memcpy(&r[i * m], vaux2->data, msod); N0.at(i) = gsl_matrix_alloc(m, m); gsl_matrix_memcpy(N0.at(i), ZtZ); gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, L.at(i), N0.at(ip1), 0.0, Mmm); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Mmm, L.at(i), invf.at(i), N0.at(i)); vaux = gsl_matrix_diagonal(N0.at(ip1)); gsl_vector_memcpy(vaux2, &vaux.vector); memcpy(&N[i * m], vaux2->data, msod); K_irow = gsl_matrix_row(K, i); gsl_blas_ddot(&K_irow.vector, &r_row_tp1.vector, &epshat[i]); epshat[i] -= vof.at(i); epshat[i] *= -*sH; maux1 = gsl_matrix_view_array(gsl_vector_ptr(&K_irow.vector, 0), 1, m); maux2 = gsl_matrix_view_array(gsl_vector_ptr(Z_cp, 0), 1, m); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &maux1.matrix, N0.at(ip1), 0.0, &maux2.matrix); vaux = gsl_vector_view_array(gsl_vector_ptr(var_eps, i), 1); gsl_blas_dgemv(CblasNoTrans, msHsq, &maux2.matrix, &K_irow.vector, 1.0, &vaux.vector); gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, &V.matrix, &R.matrix, 0.0, Mrm); gsl_blas_dgemv(CblasNoTrans, 1.0, Mrm, &r_row_tp1.vector, 0.0, vr); memcpy(&etahat[i*ir], vr->data, irsod); Ndiag = gsl_matrix_diagonal(N0.at(ip1)); gsl_vector_memcpy(Z_cp, &Ndiag.vector); gsl_vector_mul(Z_cp, Qdiag_msq); gsl_vector_add(Z_cp, &Qdiag.vector); gsl_blas_dgemv(CblasTrans, 1.0, &R.matrix, Z_cp, 0.0, vr); memcpy(&vareta[i*ir], vr->data, irsod); // derivatives dr0.at(i) = gsl_matrix_alloc(rp1, m); for (j = 0; j < rp1; j++) { k = i + j * n; gsl_vector_memcpy(Z_cp, &Z.vector); gsl_vector_scale(Z_cp, dvof[k]); vaux = gsl_matrix_row(dK.at(i), j); maux1 = gsl_matrix_view_array(gsl_vector_ptr(&vaux.vector, 0), m, 1); maux2 = gsl_matrix_view_array(gsl_vector_ptr(&Z.vector, 0), 1, m); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, &maux1.matrix, &maux2.matrix, 0.0, dL); dr_row_t = gsl_matrix_row(dr0.at(i), j); dr_row_tp1 = gsl_matrix_row(dr0.at(ip1), j); gsl_blas_dgemv(CblasTrans, 1.0, dL, &r_row_tp1.vector, 0.0, &dr_row_t.vector); gsl_vector_add(&dr_row_t.vector, Z_cp); gsl_blas_dgemv(CblasTrans, 1.0, L.at(i), &dr_row_tp1.vector, 1.0, &dr_row_t.vector); (dN0.at(i)).at(j) = gsl_matrix_alloc(m, m); gsl_matrix_memcpy((dN0.at(i)).at(j), ZtZ); gsl_matrix_scale((dN0.at(i)).at(j), -1.0 * dfinvfsq.at(k)); gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, dL, N0.at(ip1), 0.0, Mmm); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Mmm, L.at(i), 1.0, (dN0.at(i)).at(j)); gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, L.at(i), (dN0.at(ip1)).at(j), 0.0, Mmm); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Mmm, L.at(i), 1.0, (dN0.at(i)).at(j)); gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, L.at(i), N0.at(ip1), 0.0, Mmm); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Mmm, dL, 1.0, (dN0.at(i)).at(j)); if (i != 0) { vaux = gsl_matrix_diagonal((dN0.at(i)).at(j)); gsl_vector_memcpy(vaux2, &vaux.vector); memcpy(&dN[iaux + j * m], vaux2->data, msod); } vaux = gsl_matrix_row(da_pred.at(j), i); gsl_blas_dgemv(CblasNoTrans, 1.0, (dP_pred.at(i)).at(j) , &r_row_t.vector, 1.0, &vaux.vector); gsl_blas_dgemv(CblasNoTrans, 1.0, P_pred.at(i), &dr_row_t.vector, 1.0, &vaux.vector); gsl_vector_memcpy(vaux2, &vaux.vector); memcpy(&dahat[irp1m + j * m], vaux2->data, msod); gsl_matrix_memcpy(Mmm, (dP_pred.at(i)).at(j)); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, (dP_pred.at(i)).at(j), N0.at(i), 0.0, Mmm2); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Mmm2, P_pred.at(i), 1.0, Mmm); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, P_pred.at(i), (dN0.at(i)).at(j), 0.0, Mmm2); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Mmm2, P_pred.at(i), 1.0, Mmm); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, P_pred.at(i), N0.at(i), 0.0, Mmm2); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Mmm2, (dP_pred.at(i)).at(j), 1.0, Mmm); gsl_matrix_mul_elements(Mmm, ZtZ); std::vector<double> vmm(Mmm->data, Mmm->data + m*m); dvareps[i*rp1 + j] = std::accumulate(vmm.begin(), vmm.end(), 0.0); gsl_matrix_free((dN0.at(ip1)).at(j)); gsl_matrix_free((dP_pred.at(i)).at(j)); } if (i != 0) { memcpy(&dr[iaux], (dr0.at(i))->data, rp1msod); } gsl_matrix_free(dr0.at(ip1)); gsl_matrix_free(dK.at(i)); gsl_matrix_free(P_pred.at(i)); gsl_matrix_free(L.at(i)); gsl_matrix_free(N0.at(ip1)); } gsl_matrix_free(N0.at(0)); gsl_matrix_free(dr0.at(0)); for (j = 0; j < rp1; j++) { gsl_matrix_free((dN0.at(0)).at(j)); gsl_matrix_free(da_pred.at(j)); } memcpy(&vareps[0], var_eps->data, nsod); gsl_matrix_free(Mmm); gsl_matrix_free(Mmm2); gsl_matrix_free(Mrm); gsl_matrix_free(r0); gsl_matrix_free(K); gsl_matrix_free(dL); gsl_matrix_free(a_pred); gsl_vector_free(Z_cp); gsl_matrix_free(ZtZ); gsl_vector_free(var_eps); gsl_vector_free(vr); gsl_vector_free(Qdiag_msq); gsl_vector_free(vaux2); }}
static double slope(wrapper_t * w) { /* compute gradient . direction */ double df; gsl_blas_ddot(w->g_alpha, w->p, &df); return df; }
VarproFunction::VarproFunction( const gsl_vector *p, Structure *s, size_t d, gsl_matrix *Phi, bool isGCD ) : myP(NULL), myD(d), myStruct(s), myReggamma(SLRA_DEF_reggamma), myIsGCD(isGCD) { if (myStruct->getNp() > p->size) { throw new Exception("Inconsistent parameter vector\n"); } if (myStruct->getN() < myStruct->getM()) { throw new Exception("Number of columns %d is less than " "the number of rows %d.", myStruct->getN(), myStruct->getM()); } if (myStruct->getNp() < myStruct->getN() * getD()) { throw new Exception("The inner minimization problem is overdetermined: " "n * (m-r) = %d, n_p = %d.\n", myStruct->getN() * getD(), myStruct->getNp()); } if (myStruct->getN() * getD() * myStruct->getM() * getD() >= 10000000L) { throw new Exception("Too much memory required: the Jacobian would have " "more than 10^8 elements. This is currently not allowed.\n"); } if (Phi != NULL) { if (Phi->size1 != myStruct->getM() || Phi->size2 > Phi->size1) { throw new Exception("Incompatible Phi matrix\n"); } myPhi = gsl_matrix_alloc(Phi->size1, Phi->size2); gsl_matrix_memcpy(myPhi, Phi); } else { myPhi = gsl_matrix_alloc(myStruct->getM(), myStruct->getM()); gsl_matrix_set_identity(myPhi); } if (d >= getNrow() || d <= 0) { throw new Exception("Incorrect rank given\n"); } myP = gsl_vector_alloc(p->size); gsl_vector_memcpy(myP, p); myRorig = gsl_matrix_alloc(getM(), getD()); myPhiPermCol = gsl_vector_alloc(getM()); myGam = myStruct->createCholesky(getD()); myDeriv = myStruct->createDGamma(getD()); myMatr = gsl_matrix_alloc(myStruct->getN(), myStruct->getM()); myTmpGradR = gsl_matrix_alloc(getM(), getD()); myTmpGradR2 = gsl_matrix_alloc(getNrow(), getD()); myTmpYr = gsl_vector_alloc(myStruct->getN() * getD()); myTmpJacobianCol = gsl_vector_alloc(myStruct->getN() * getD()); myTmpJac = gsl_matrix_alloc(myStruct->getM() * getD(), myStruct->getN() * getD()); myTmpJac2 = gsl_matrix_alloc(myStruct->getN() * getD(), getNrow() * getD()); myTmpJtJ = gsl_matrix_alloc(getNrow() * getD(), getNrow() * getD()); myTmpEye = gsl_matrix_alloc(getNrow(), getNrow()); gsl_matrix_set_identity(myTmpEye); myTmpCorr = gsl_vector_alloc(myStruct->getNp()); if (myIsGCD) { gsl_vector_memcpy(myTmpCorr, getP()); myStruct->multByWInv(myTmpCorr, 1); myStruct->fillMatrixFromP(myMatr, myTmpCorr); gsl_blas_ddot(myTmpCorr, myTmpCorr, &myPWnorm2); } else { myStruct->fillMatrixFromP(myMatr, getP()); } }
int main (void) { const gsl_multifit_nlinear_type *T = gsl_multifit_nlinear_trust; gsl_multifit_nlinear_workspace *w; gsl_multifit_nlinear_fdf fdf; gsl_multifit_nlinear_parameters fdf_params = gsl_multifit_nlinear_default_parameters(); const size_t n = N; const size_t p = 3; gsl_vector *f; gsl_matrix *J; gsl_matrix *covar = gsl_matrix_alloc (p, p); double y[N], weights[N]; struct data d = { n, y }; double x_init[3] = { 1.0, 1.0, 0.0 }; /* starting values */ gsl_vector_view x = gsl_vector_view_array (x_init, p); gsl_vector_view wts = gsl_vector_view_array(weights, n); gsl_rng * r; double chisq, chisq0; int status, info; size_t i; const double xtol = 1e-8; const double gtol = 1e-8; const double ftol = 0.0; gsl_rng_env_setup(); r = gsl_rng_alloc(gsl_rng_default); /* define the function to be minimized */ fdf.f = expb_f; fdf.df = expb_df; /* set to NULL for finite-difference Jacobian */ fdf.fvv = NULL; /* not using geodesic acceleration */ fdf.n = n; fdf.p = p; fdf.params = &d; /* this is the data to be fitted */ for (i = 0; i < n; i++) { double t = i; double yi = 1.0 + 5 * exp (-0.1 * t); double si = 0.1 * yi; double dy = gsl_ran_gaussian(r, si); weights[i] = 1.0 / (si * si); y[i] = yi + dy; printf ("data: "F_ZU" %g %g\n", i, y[i], si); }; /* allocate workspace with default parameters */ w = gsl_multifit_nlinear_alloc (T, &fdf_params, n, p); /* initialize solver with starting point and weights */ gsl_multifit_nlinear_winit (&x.vector, &wts.vector, &fdf, w); /* compute initial cost function */ f = gsl_multifit_nlinear_residual(w); gsl_blas_ddot(f, f, &chisq0); /* solve the system with a maximum of 20 iterations */ status = gsl_multifit_nlinear_driver(20, xtol, gtol, ftol, callback, NULL, &info, w); /* compute covariance of best fit parameters */ J = gsl_multifit_nlinear_jac(w); gsl_multifit_nlinear_covar (J, 0.0, covar); /* compute final cost */ gsl_blas_ddot(f, f, &chisq); #define FIT(i) gsl_vector_get(w->x, i) #define ERR(i) sqrt(gsl_matrix_get(covar,i,i)) fprintf(stderr, "summary from method '%s/%s'\n", gsl_multifit_nlinear_name(w), gsl_multifit_nlinear_trs_name(w)); fprintf(stderr, "number of iterations: "F_ZU"\n", gsl_multifit_nlinear_niter(w)); fprintf(stderr, "function evaluations: "F_ZU"\n", fdf.nevalf); fprintf(stderr, "Jacobian evaluations: "F_ZU"\n", fdf.nevaldf); fprintf(stderr, "reason for stopping: %s\n", (info == 1) ? "small step size" : "small gradient"); fprintf(stderr, "initial |f(x)| = %f\n", sqrt(chisq0)); fprintf(stderr, "final |f(x)| = %f\n", sqrt(chisq)); { double dof = n - p; double c = GSL_MAX_DBL(1, sqrt(chisq / dof)); fprintf(stderr, "chisq/dof = %g\n", chisq / dof); fprintf (stderr, "A = %.5f +/- %.5f\n", FIT(0), c*ERR(0)); fprintf (stderr, "lambda = %.5f +/- %.5f\n", FIT(1), c*ERR(1)); fprintf (stderr, "b = %.5f +/- %.5f\n", FIT(2), c*ERR(2)); } fprintf (stderr, "status = %s\n", gsl_strerror (status)); gsl_multifit_nlinear_free (w); gsl_matrix_free (covar); gsl_rng_free (r); return 0; }
int chisq_dist(int mode, int do_chisq, int nophotoerr, int ncalc, int ncol, double *covmat, double *c, double *slope, double *pivotmag, double *refmag, double *refmagerr, double *magerr, double *color, double *lupcorr, double *dist, double sigint) { //int i,j,k; int i,j,k; int nmag = ncol+1; //gsl_matrix *mcovmat; gsl_matrix_view mvcovmat; gsl_matrix *cobs; gsl_matrix *cimat; gsl_matrix *cobsmat; gsl_matrix *cobstemp; gsl_matrix *mmetric; gsl_matrix *mrotmat; gsl_vector *vdcm; gsl_permutation *pp; gsl_vector *vdc; int s; double *covmat_temp = NULL; int covmat_stride; int test; double val; double chisq,norm; int use_refmagerr=0; norm = 1.0; covmat_stride = ncol*ncol; //*sizeof(double); mrotmat = gsl_matrix_alloc(ncol,nmag); gsl_matrix_set_zero(mrotmat); // important! for (i=0;i<ncol;i++) { gsl_matrix_set(mrotmat, i, i, 1.0); gsl_matrix_set(mrotmat, i, i+1, -1.0); } cobs = gsl_matrix_alloc(nmag, nmag); cobsmat = gsl_matrix_alloc(ncol,ncol); cobstemp = gsl_matrix_alloc(ncol,nmag); pp=gsl_permutation_alloc(ncol); mmetric=gsl_matrix_alloc(ncol,ncol); vdc = gsl_vector_alloc(ncol); vdcm=gsl_vector_alloc(ncol); cimat = NULL; if (refmagerr != NULL) { use_refmagerr = 1; cimat = gsl_matrix_alloc(ncol,ncol); } if (mode == 0) { // mode = 0 // Many galaxies, one redshift // - refmag/refmagerr is an array with ncalc (==ngal) // - color is a matrix with ncol x ncalc (==ngal) // - magerr is a matrix with nmag x ncalc (==ngal) // - c is an array with ncol // - slope is an array with ncol // - pivotmag is a single value // - lupcorr is a matrix with ncol x ncalc (==ngal) // //- covmat is a matrix with ncol x ncol x ncalc (==ngal) // - covmat is a matrix with ncol x ncol // // // covmat[ncol,ncol,ncalc]: (k*ncol+j)*ncol + i // covmat[ncol,ncol]: k*ncol + j // c[ncol]: j // slope[ncol]: j // pivotmag[0] // refmag[ncalc]: i // refmagerr[ncalc]: i // magerr[nmag,ncalc]: i*nmag + j // color[ncol,ncalc]: i*ncol + j // lupcorr[ncol,ncalc]: i*ncol + j // first make a buffer for the local copy of the covmat (which gets overwritten) if ((covmat_temp = (double *)calloc(ncol*ncol, sizeof(double))) == NULL) { return -1; } for (i=0;i<ncalc;i++) { memcpy(covmat_temp,covmat,sizeof(double)*ncol*ncol); gsl_matrix_set_identity(cobs); for (j=0;j<nmag;j++) { gsl_matrix_set(cobs, j, j, magerr[i*nmag+j]*magerr[i*nmag+j]); } gsl_matrix_set_zero(cobstemp); gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, mrotmat, cobs, 0.0, cobstemp); gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, mrotmat, cobstemp, 0.0, cobsmat); //mvcovmat = gsl_matrix_view_array(&covmat[covmat_stride*i], ncol, ncol); mvcovmat = gsl_matrix_view_array(covmat_temp, ncol, ncol); // and the ci matrix if (use_refmagerr) { // make the C_i matrix for the refmag err // this is a matrix with ncol x ncol size gsl_matrix_set_zero(cimat); for (j=0;j<ncol;j++) { for (k=j;k<ncol;k++) { val = slope[j] * slope[k] * refmagerr[i] * refmagerr[i]; gsl_matrix_set(cimat, j, k, val); if (k != j) { gsl_matrix_set(cimat, k, j, val); } } } } // check sigint test = 1; for (j=0;j<ncol;j++) { if (gsl_matrix_get(&mvcovmat.matrix, j, j) < sigint*sigint) { test = 0; } } if (test == 0) { if (do_chisq) { dist[i] = 1e11; } else { dist[i] = -1e11; } } else { if (!nophotoerr) { gsl_matrix_add(&mvcovmat.matrix, cobsmat); } if (use_refmagerr) { gsl_matrix_add(&mvcovmat.matrix, cimat); } // check and fix the matrix if necessary check_and_fix_covmat(&mvcovmat.matrix); gsl_linalg_LU_decomp(&mvcovmat.matrix, pp, &s); gsl_linalg_LU_invert(&mvcovmat.matrix, pp, mmetric); if (!do_chisq) { // need the determinant norm = gsl_linalg_LU_det(&mvcovmat.matrix, s); } // now need the slope, etc. for (j=0;j<ncol;j++) { gsl_vector_set(vdc,j, (c[j]+slope[j]*(refmag[i]-pivotmag[0])) + lupcorr[i*ncol+j] - color[i*ncol+j]); } gsl_blas_dgemv(CblasNoTrans, 1.0, mmetric, vdc, 0.0, vdcm); gsl_blas_ddot(vdcm, vdc, &chisq); if (do_chisq) { dist[i] = chisq; } else { dist[i]=-0.5*chisq-0.5*log(norm); } } } free(covmat_temp); } else if (mode==1) { // mode = 1 // One galaxy, many redshifts // - refmag/refmagerr is a single value // - color is an array with ncol values // - magerr is an array with nmag=ncol+1 values // - c is a matrix with ncol x ncalc (==nz) // - slope is a matrix with ncol x ncalc (==nz) // - pivotmag is an array with ncalc (==nz) // - lupcorr is a matrix with ncol x ncalc (==nz) // - covmat is a matrix with ncol x ncol x ncalc (==nz) // // covmat[ncol,ncol,ncalc] : (k*ncol+j)*ncol+i // c[ncol,ncalc]: i*ncol+j // slope[ncol,ncalc]: i*ncol+j // pivotmag[ncalc]: i // magerr[nmag]: j // color[ncol]: j // lupcorr[ncol,ncalc]: i*ncol + j // refmag[0] // refmagerr[0] // first make a buffer for the local copy of the covmat (which gets overwritten) if ((covmat_temp = (double *)calloc(ncol*ncol*ncalc, sizeof(double))) == NULL) { return -1; } memcpy(covmat_temp,covmat,sizeof(double)*ncol*ncol*ncalc); // We can generate the C_obs matrix (cobsmat) just once gsl_matrix_set_identity(cobs); for (j=0;j<nmag;j++) { gsl_matrix_set(cobs,j,j,magerr[j]*magerr[j]); } gsl_matrix_set_zero(cobstemp); gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, mrotmat, cobs, 0.0, cobstemp); gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, mrotmat, cobstemp, 0.0, cobsmat); for (i=0;i<ncalc;i++) { mvcovmat = gsl_matrix_view_array(&covmat_temp[covmat_stride*i], ncol, ncol); if (use_refmagerr) { // make the C_i matrix for the refmag err // this is a matrix with ncol x ncol size... gsl_matrix_set_zero(cimat); for (j=0;j<ncol;j++) { for (k=j;k<ncol;k++) { val = slope[i*ncol+j] * slope[i*ncol+k] * refmagerr[0] * refmagerr[0]; gsl_matrix_set(cimat, j, k, val); if (k != j) { gsl_matrix_set(cimat, k, j, val); } } } } // check sigint test = 1; for (j=0;j<ncol;j++) { if (gsl_matrix_get(&mvcovmat.matrix, j, j) < sigint*sigint) { test = 0; } } if (test == 0) { if (do_chisq) { dist[i] = 1e11; } else { dist[i] = -1e11; } } else { if (!nophotoerr) { gsl_matrix_add(&mvcovmat.matrix, cobsmat); } if (use_refmagerr) { gsl_matrix_add(&mvcovmat.matrix, cimat); } // check and fix the matrix if necessary check_and_fix_covmat(&mvcovmat.matrix); gsl_linalg_LU_decomp(&mvcovmat.matrix, pp, &s); gsl_linalg_LU_invert(&mvcovmat.matrix, pp, mmetric); if (!do_chisq) { norm = gsl_linalg_LU_det(&mvcovmat.matrix, s); } for (j=0;j<ncol;j++) { gsl_vector_set(vdc,j, (c[i*ncol+j] + slope[i*ncol+j]*(refmag[0] - pivotmag[i])) + lupcorr[i*ncol+j] - color[j]); } gsl_blas_dgemv(CblasNoTrans, 1.0, mmetric, vdc, 0.0, vdcm); gsl_blas_ddot(vdcm,vdc, &chisq); if (do_chisq) { dist[i] = chisq; } else { dist[i]=-0.5*chisq-0.5*log(norm); } } } free(covmat_temp); } else { // mode = 2 // Many galaxies, many redshifts // - refmag/refmagerr is an array with ncalc (==ngal) // - color is a matrix with ncol x ncalc (==ngal) // - magerr is a matrix with nmag x ncalc (==ngal) // - c is a matrix with ncol x ncalc (==ngal) // - slope is a matrix with ncol x ncalc (==ngal) // - pivotmag is an array with ncalc (==ngal) // - lupcorr is a matrix with ncol x ncalc (==ngal) // - covmat is a matrix with ncol x ncol x ncalc (==ngal) // // covmat[ncol,ncol,ncalc] : (k*ncol+j)*ncol+i -> mode 0 // c[ncol,ncalc]: i*ncol+j -> mode 1 // slope[ncol,ncalc]: i*ncol+j -> mode 1 // pivotmag[ncalc]: i -> mode 1 // refmag[ncalc]: i -> mode 0 // refmagerr[ncalc]: i -> mode 0 // magerr[nmag,ncalc]: i*nmag + j -> mode 0 // color[ncol,ncalc]: i*ncol + j -> mode 0 // lupcorr[ncol,ncalc]: i*ncol + j -> mode 0, 1 if ((covmat_temp = (double *)calloc(ncol*ncol*ncalc, sizeof(double))) == NULL) { return -1; } memcpy(covmat_temp,covmat,sizeof(double)*ncol*ncol*ncalc); for (i=0;i<ncalc;i++) { // copy from mode 0 gsl_matrix_set_identity(cobs); for (j=0;j<nmag;j++) { // okay gsl_matrix_set(cobs, j, j, magerr[i*nmag+j]*magerr[i*nmag+j]); } gsl_matrix_set_zero(cobstemp); gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, mrotmat, cobs, 0.0, cobstemp); gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, mrotmat, cobstemp, 0.0, cobsmat); mvcovmat = gsl_matrix_view_array(&covmat_temp[covmat_stride*i], ncol, ncol); // and the ci matrix // copy from mode 0 if (use_refmagerr) { // make the C_i matrix for the refmag err // this is a matrix with ncol x ncol size gsl_matrix_set_zero(cimat); for (j=0;j<ncol;j++) { for (k=j;k<ncol;k++) { val = slope[j] * slope[k] * refmagerr[i] * refmagerr[i]; gsl_matrix_set(cimat, j, k, val); if (k != j) { gsl_matrix_set(cimat, k, j, val); } } } } // check sigint // copy from mode 0 test = 1; for (j=0;j<ncol;j++) { if (gsl_matrix_get(&mvcovmat.matrix, j, j) < sigint*sigint) { test = 0; } } if (test == 0) { if (do_chisq) { dist[i] = 1e11; } else { dist[i] = -1e11; } } else { // copy from mode 0 if (!nophotoerr) { gsl_matrix_add(&mvcovmat.matrix, cobsmat); } // copy from mode 0 if (use_refmagerr) { gsl_matrix_add(&mvcovmat.matrix, cimat); } // check and fix the matrix if necessary // copy from mode 0 check_and_fix_covmat(&mvcovmat.matrix); gsl_linalg_LU_decomp(&mvcovmat.matrix, pp, &s); gsl_linalg_LU_invert(&mvcovmat.matrix, pp, mmetric); if (!do_chisq) { // need the determinant norm = gsl_linalg_LU_det(&mvcovmat.matrix, s); } // vdc is a vector with ncol... // copy from mode 1 for (j=0;j<ncol;j++) { gsl_vector_set(vdc,j, (c[i*ncol+j] + slope[i*ncol+j]*(refmag[i] - pivotmag[i])) + lupcorr[i*ncol+j] - color[i*ncol+j]); } gsl_blas_dgemv(CblasNoTrans, 1.0, mmetric, vdc, 0.0, vdcm); gsl_blas_ddot(vdcm,vdc, &chisq); if (do_chisq) { dist[i] = chisq; } else { dist[i]=-0.5*chisq-0.5*log(norm); } } } free(covmat_temp); } gsl_matrix_free(mrotmat); gsl_matrix_free(cobs); gsl_matrix_free(cobsmat); gsl_matrix_free(cobstemp); if (use_refmagerr) gsl_matrix_free(cimat); gsl_permutation_free(pp); gsl_matrix_free(mmetric); gsl_vector_free(vdcm); gsl_vector_free(vdc); return 0; }
int PoissonGlm::EstIRLS(gsl_matrix *Y, gsl_matrix *X, gsl_matrix *O, gsl_matrix *B, double *a) { initialGlm(Y, X, O, B); gsl_set_error_handler_off(); gsl_rng *rnd=gsl_rng_alloc(gsl_rng_mt19937); unsigned int i, j; int status; double yij, mij, vij, wij, tol, hii, uij, wei; gsl_vector_view Xwi, Xi, vj, hj, dj; gsl_matrix *WX = gsl_matrix_alloc(nRows, nParams); gsl_matrix *TMP = gsl_matrix_alloc(nRows, nParams); gsl_matrix *XwX = gsl_matrix_alloc(nParams, nParams); for (j=0; j<nVars; j++) { if ( a!=NULL ) theta[j]=a[j]; // estimate mu and beta iterconv[j] = betaEst(j, maxiter, &tol, theta[j]); if ((mmRef->warning==TRUE)&(iterconv[j]==maxiter)) printf("Warning: EstIRLS reached max iterations, may not converge in the %d-th variable (dev=%.4f, err=%.4f)!\n", j, dev[j], tol); gsl_matrix_memcpy (WX, X); for (i=0; i<nRows; i++) { mij = gsl_matrix_get(Mu, i, j); // get variance vij = varfunc( mij, theta[j] ); gsl_matrix_set(Var, i, j, vij); // get weight wij = sqrt(weifunc(mij, theta[j])); gsl_matrix_set(wHalf, i, j, wij); // get (Pearson) residuals yij = gsl_matrix_get(Y, i, j); gsl_matrix_set(Res, i, j, (yij-mij)/sqrt(vij)); // get PIT residuals for discrete data wei = gsl_rng_uniform_pos (rnd); // wei ~ U(0, 1) uij = wei*cdf(yij, mij, theta[j]); if (yij>0) uij=uij+(1-wei)*cdf((yij-1),mij,theta[j]); gsl_matrix_set(PitRes, i, j, uij); // get elementry log-likelihood ll[j] = ll[j] + llfunc( yij, mij, theta[j]); // W^1/2 X Xwi = gsl_matrix_row (WX, i); gsl_vector_scale(&Xwi.vector, wij); } aic[j]=-ll[j]+2*(nParams); // X^T * W * X gsl_matrix_set_identity(XwX); gsl_blas_dsyrk (CblasLower, CblasTrans, 1.0, WX, 0.0, XwX); status=gsl_linalg_cholesky_decomp (XwX); if (status==GSL_EDOM) { if (mmRef->warning==TRUE) printf("Warning: singular matrix in calculating pit-residuals. An eps*I is added to the singular matrix.\n"); gsl_matrix_set_identity(XwX); gsl_blas_dsyrk (CblasLower, CblasTrans, 1.0, WX, mintol, XwX); gsl_linalg_cholesky_decomp (XwX); } gsl_linalg_cholesky_invert (XwX); // Calc varBeta dj = gsl_matrix_diagonal (XwX); vj = gsl_matrix_column (varBeta, j); gsl_vector_memcpy (&vj.vector, &dj.vector); // hii is diagonal element of H=X*(X'WX)^-1*X'*W hj = gsl_matrix_column (sqrt1_Hii, j); gsl_blas_dsymm(CblasRight,CblasLower,1.0,XwX,Xref,0.0,TMP); // X*(X'WX)^-1 for (i=0; i<nRows; i++) { Xwi=gsl_matrix_row(TMP, i); Xi=gsl_matrix_row(Xref, i); wij=gsl_matrix_get(wHalf, i, j); gsl_blas_ddot(&Xwi.vector, &Xi.vector, &hii); gsl_vector_set(&hj.vector, i, MAX(mintol, sqrt(MAX(0, 1-wij*wij*hii)))); } } // standardize perason residuals by rp/sqrt(1-hii) // gsl_matrix_div_elements (Res, sqrt1_Hii); // subtractMean(Res); // have mean subtracted gsl_matrix_free(XwX); gsl_matrix_free(WX); gsl_matrix_free(TMP); gsl_rng_free(rnd); return SUCCESS; }
void test_longley () { gsl_multifit_linear_workspace * work = gsl_multifit_linear_alloc (longley_n, longley_p); gsl_multifit_robust_workspace * work_rob = gsl_multifit_robust_alloc (gsl_multifit_robust_ols, longley_n, longley_p); gsl_matrix_view X = gsl_matrix_view_array (longley_x, longley_n, longley_p); gsl_vector_view y = gsl_vector_view_array (longley_y, longley_n); gsl_vector * c = gsl_vector_alloc (longley_p); gsl_vector * r = gsl_vector_alloc (longley_n); gsl_matrix * cov = gsl_matrix_alloc (longley_p, longley_p); double chisq, chisq_res; double expected_c[7] = { -3482258.63459582, 15.0618722713733, -0.358191792925910E-01, -2.02022980381683, -1.03322686717359, -0.511041056535807E-01, 1829.15146461355 }; double expected_sd[7] = { 890420.383607373, 84.9149257747669, 0.334910077722432E-01, 0.488399681651699, 0.214274163161675, 0.226073200069370, 455.478499142212 } ; double expected_chisq = 836424.055505915; gsl_vector_view diag = gsl_matrix_diagonal (cov); gsl_vector_view exp_c = gsl_vector_view_array(expected_c, longley_p); gsl_vector_view exp_sd = gsl_vector_view_array(expected_sd, longley_p); /* test unweighted least squares */ gsl_multifit_linear (&X.matrix, &y.vector, c, cov, &chisq, work); gsl_multifit_linear_residuals(&X.matrix, &y.vector, c, r); gsl_blas_ddot(r, r, &chisq_res); test_longley_results("longley gsl_multifit_linear", c, &exp_c.vector, &diag.vector, &exp_sd.vector, chisq, chisq_res, expected_chisq); /* test robust least squares */ gsl_multifit_robust (&X.matrix, &y.vector, c, cov, work_rob); test_longley_results("longley gsl_multifit_robust", c, &exp_c.vector, &diag.vector, &exp_sd.vector, 1.0, 1.0, 1.0); /* test weighted least squares */ { size_t i, j; gsl_vector * w = gsl_vector_alloc (longley_n); double expected_cov[7][7] = { { 8531122.56783558, -166.727799925578, 0.261873708176346, 3.91188317230983, 1.1285582054705, -0.889550869422687, -4362.58709870581}, {-166.727799925578, 0.0775861253030891, -1.98725210399982e-05, -0.000247667096727256, -6.82911920718824e-05, 0.000136160797527761, 0.0775255245956248}, {0.261873708176346, -1.98725210399982e-05, 1.20690316701888e-08, 1.66429546772984e-07, 3.61843600487847e-08, -6.78805814483582e-08, -0.00013158719037715}, {3.91188317230983, -0.000247667096727256, 1.66429546772984e-07, 2.56665052544717e-06, 6.96541409215597e-07, -9.00858307771567e-07, -0.00197260370663974}, {1.1285582054705, -6.82911920718824e-05, 3.61843600487847e-08, 6.96541409215597e-07, 4.94032602583969e-07, -9.8469143760973e-08, -0.000576921112208274}, {-0.889550869422687, 0.000136160797527761, -6.78805814483582e-08, -9.00858307771567e-07, -9.8469143760973e-08, 5.49938542664952e-07, 0.000430074434198215}, {-4362.58709870581, 0.0775255245956248, -0.00013158719037715, -0.00197260370663974, -0.000576921112208274, 0.000430074434198215, 2.23229587481535 }} ; gsl_vector_set_all (w, 1.0); gsl_multifit_wlinear (&X.matrix, w, &y.vector, c, cov, &chisq, work); gsl_multifit_linear_residuals(&X.matrix, &y.vector, c, r); gsl_blas_ddot(r, r, &chisq_res); test_longley_results("longley gsl_multifit_wlinear", c, &exp_c.vector, NULL, NULL, chisq, chisq_res, expected_chisq); for (i = 0; i < longley_p; i++) { for (j = 0; j < longley_p; j++) { gsl_test_rel (gsl_matrix_get(cov,i,j), expected_cov[i][j], 1e-7, "longley gsl_multifit_wlinear cov(%d,%d)", i, j) ; } } gsl_vector_free(w); } gsl_vector_free(c); gsl_vector_free(r); gsl_matrix_free(cov); gsl_multifit_linear_free (work); gsl_multifit_robust_free (work_rob); } /* test_longley() */
void GICPOptimizer::fdf(const gsl_vector *x, void *params, double * f, gsl_vector *g) { std::cout << ">>> fdf" << std::endl; GICPOptData *opt_data = (GICPOptData *)params; double pt1[3]; double pt2[3]; double res[3]; // residual double temp[3]; // temp local vector double temp_mat[9]; // temp matrix used for accumulating the rotation gradient gsl_vector_view gsl_pt1 = gsl_vector_view_array(pt1, 3); gsl_vector_view gsl_pt2 = gsl_vector_view_array(pt2, 3); gsl_vector_view gsl_res = gsl_vector_view_array(res, 3); gsl_vector_view gsl_temp = gsl_vector_view_array(temp, 3); gsl_vector_view gsl_gradient_t = gsl_vector_subvector(g, 0, 3); // translation comp. of gradient gsl_vector_view gsl_gradient_r = gsl_vector_subvector(g, 3, 3); // rotation comp. of gradient gsl_matrix_view gsl_temp_mat_r = gsl_matrix_view_array(temp_mat, 3, 3); gsl_matrix_view gsl_M; dgc_transform_t t; double temp_double; // take the base transformation dgc_transform_copy(t, opt_data->base_t); // apply the current state apply_state(t, x); // zero all accumulator variables *f = 0; gsl_vector_set_zero(g); gsl_vector_set_zero(&gsl_temp.vector); gsl_matrix_set_zero(&gsl_temp_mat_r.matrix); for(int i = 0; i < opt_data->p1->Size(); i++) { int j = opt_data->nn_indecies[i]; if(j != -1) { // get point 1 pt1[0] = (*opt_data->p1)[i].x; pt1[1] = (*opt_data->p1)[i].y; pt1[2] = (*opt_data->p1)[i].z; // get point 2 pt2[0] = (*opt_data->p2)[j].x; pt2[1] = (*opt_data->p2)[j].y; pt2[2] = (*opt_data->p2)[j].z; //cout << "accessing " << i << " of " << opt_data->p1->Size() << ", " << opt_data->p2->Size() << endl; //get M-matrix gsl_M = gsl_matrix_view_array(&opt_data->M[i][0][0], 3, 3); print_gsl_matrix(&gsl_M.matrix, "M"); //transform point 1 dgc_transform_point(&pt1[0], &pt1[1], &pt1[2], t); std::cout << "pt1 " << pt1[0] << "," << pt1[1] << "," << pt1[2] << std::endl; res[0] = pt1[0] - pt2[0]; res[1] = pt1[1] - pt2[1]; res[2] = pt1[2] - pt2[2]; std::cout << "res " << res[0] << "," << res[1] << "," << res[2] << std::endl; // compute the transformed residual // temp := M*res //print_gsl_matrix(&gsl_M.matrix, "gsl_m"); gsl_blas_dsymv(CblasLower, 1., &gsl_M.matrix, &gsl_res.vector, 0., &gsl_temp.vector); print_gsl_vector(&gsl_temp.vector, "temp"); // compute M-norm of the residual // temp_double := res'*temp = temp'*M*res gsl_blas_ddot(&gsl_res.vector, &gsl_temp.vector, &temp_double); // accumulate total error: f += res'*M*res *f += temp_double/(double)opt_data->num_matches; std::cout << "f " << *f << std::endl; // accumulate translation gradient: // gsl_gradient_t += 2*M*res gsl_blas_dsymv(CblasLower, 2./(double)opt_data->num_matches, &gsl_M.matrix, &gsl_res.vector, 1., &gsl_gradient_t.vector); if(opt_data->solve_rotation) { // accumulate the rotation gradient matrix // get back the original untransformed point to compute the rotation gradient pt1[0] = (*opt_data->p1)[i].x; pt1[1] = (*opt_data->p1)[i].y; pt1[2] = (*opt_data->p1)[i].z; dgc_transform_point(&pt1[0], &pt1[1], &pt1[2], opt_data->base_t); // gsl_temp_mat_r += 2*(gsl_temp).(gsl_pt1)' [ = (2*M*residual).(gsl_pt1)' ] gsl_blas_dger(2./(double)opt_data->num_matches, &gsl_pt1.vector, &gsl_temp.vector, &gsl_temp_mat_r.matrix); } } } print_gsl_vector(g, "gradient"); // the above loop sets up the gradient with respect to the translation, and the matrix derivative w.r.t. the rotation matrix // this code sets up the matrix derivatives dR/dPhi, dR/dPsi, dR/dTheta. i.e. the derivatives of the whole rotation matrix with respect to the euler angles // note that this code assumes the XYZ order of euler angles, with the Z rotation corresponding to bearing. This means the Z angle is negative of what it would be // in the regular XYZ euler-angle convention. if(opt_data->solve_rotation) { // now use the d/dR matrix to compute the derivative with respect to euler angles and put it directly into g[3], g[4], g[5]; compute_dr(x, &gsl_temp_mat_r.matrix, g); } print_gsl_matrix(&gsl_temp_mat_r.matrix, "R"); print_gsl_vector(g, "gradient"); std::cout << "<<< fdf" << std::endl; }
/// Do one iteration. bool LevenbergMarquardtMDMinimizer::iterate(size_t) { const bool debug = getProperty("Debug"); const double muMax = getProperty("MuMax"); const double absError = getProperty("AbsError"); if (!m_leastSquares) { throw std::runtime_error("Cost function isn't set up."); } size_t n = m_leastSquares->nParams(); if (n == 0) { m_errorString = "No parameters to fit."; g_log.information(m_errorString); return false; } if (m_mu > muMax) { // m_errorString = "Failed to converge, maximum mu reached"; // g_log.warning() << m_errorString << std::endl; return false; } // calculate the first and second derivatives of the cost function. if (m_mu == 0.0 || m_rho > 0) { // calculate everything first time or // if last iteration was good m_F = m_leastSquares->valDerivHessian(); } // else if m_rho < 0 last iteration was bad: reuse m_der and m_hessian // Calculate damping to hessian if (m_mu == 0) // first iteration or accidental zero { m_mu = m_tau; m_nu = 2.0; } if (debug) { g_log.warning() << "===========================================================" << std::endl; g_log.warning() << "mu=" << m_mu << std::endl << std::endl; } if (m_D.empty()) { m_D.resize(n); } // copy the hessian GSLMatrix H(m_leastSquares->getHessian()); GSLVector dd(m_leastSquares->getDeriv()); // scaling factors std::vector<double> sf(n); for (size_t i = 0; i < n; ++i) { double d = fabs(dd.get(i)); if (m_D[i] > d) d = m_D[i]; m_D[i] = d; double tmp = H.get(i, i) + m_mu * d; H.set(i, i, tmp); sf[i] = sqrt(tmp); if (tmp == 0.0) { m_errorString = "Singular matrix."; g_log.information(m_errorString); return false; } } // apply scaling for (size_t i = 0; i < n; ++i) { double d = dd.get(i); dd.set(i, d / sf[i]); for (size_t j = i; j < n; ++j) { const double f = sf[i] * sf[j]; double tmp = H.get(i, j); H.set(i, j, tmp / f); if (i != j) { tmp = H.get(j, i); H.set(j, i, tmp / f); } } } if (debug && m_rho > 0) { g_log.warning() << "Hessian:\n" << H; g_log.warning() << "Right-hand side:\n"; for (size_t j = 0; j < n; ++j) { g_log.warning() << dd.get(j) << ' '; } g_log.warning() << std::endl; g_log.warning() << "Determinant=" << H.det() << std::endl; } // Parameter corrections GSLVector dx(n); // To find dx solve the system of linear equations H * dx == -m_der dd *= -1.0; H.solve(dd, dx); if (debug) { g_log.warning() << "\nScaling factors:" << std::endl; for (size_t j = 0; j < n; ++j) { g_log.warning() << sf[j] << ' '; } g_log.warning() << std::endl; g_log.warning() << "Corrections:" << std::endl; for (size_t j = 0; j < n; ++j) { g_log.warning() << dx.get(j) << ' '; } g_log.warning() << std::endl << std::endl; } // restore scaling for (size_t i = 0; i < n; ++i) { double d = dx.get(i); dx.set(i, d / sf[i]); d = dd.get(i); dd.set(i, d * sf[i]); } // save previous state m_leastSquares->push(); // Update the parameters of the cost function. for (size_t i = 0; i < n; ++i) { double d = m_leastSquares->getParameter(i) + dx.get(i); m_leastSquares->setParameter(i, d); if (debug) { g_log.warning() << "Parameter(" << i << ")=" << d << std::endl; } } m_leastSquares->getFittingFunction()->applyTies(); // --- prepare for the next iteration --- // double dL; // der -> - der - 0.5 * hessian * dx gsl_blas_dgemv(CblasNoTrans, -0.5, m_leastSquares->getHessian().gsl(), dx.gsl(), 1., dd.gsl()); // calculate the linear part of the change in cost function // dL = - der * dx - 0.5 * dx * hessian * dx gsl_blas_ddot(dd.gsl(), dx.gsl(), &dL); double F1 = m_leastSquares->val(); if (debug) { g_log.warning() << std::endl; g_log.warning() << "Old cost function " << m_F << std::endl; g_log.warning() << "New cost function " << F1 << std::endl; g_log.warning() << "Linear part " << dL << std::endl; } // Try the stop condition if (m_rho >= 0) { GSLVector p(n); m_leastSquares->getParameters(p); double dx_norm = gsl_blas_dnrm2(dx.gsl()); if (dx_norm < absError) { if (debug) { g_log.warning() << "Successful fit, parameters changed by less than " << absError << std::endl; } return false; } if (m_rho == 0) { if (m_F != F1) { this->m_errorString = "Failed to converge, rho == 0"; g_log.warning() << m_errorString << std::endl; } if (debug) { g_log.warning() << "Successful fit, cost function didn't change." << std::endl; } return false; } } if (fabs(dL) == 0.0) { if (m_F == F1) m_rho = 1.0; else m_rho = 0; } else { m_rho = (m_F - F1) / dL; if (m_rho == 0) { return false; } } if (debug) { g_log.warning() << "rho=" << m_rho << std::endl; } if (m_rho > 0) { // good progress, decrease m_mu but no more than by 1/3 // rho = 1 - (2*rho - 1)^3 m_rho = 2.0 * m_rho - 1.0; m_rho = 1.0 - m_rho * m_rho * m_rho; const double I3 = 1.0 / 3.0; if (m_rho > I3) m_rho = I3; if (m_rho < 0.0001) m_rho = 0.1; m_mu *= m_rho; m_nu = 2.0; m_F = F1; if (debug) { g_log.warning() << "Good iteration, accept new parameters." << std::endl; g_log.warning() << "rho=" << m_rho << std::endl; } // drop saved state, accept new parameters m_leastSquares->drop(); } else { // bad iteration. increase m_mu and revert changes to parameters m_mu *= m_nu; m_nu *= 2.0; // undo parameter update m_leastSquares->pop(); m_F = m_leastSquares->val(); if (debug) { g_log.warning() << "Bad iteration, increase mu and revert changes to parameters." << std::endl; } } return true; }
void test_pontius () { size_t i, j; gsl_multifit_linear_workspace * work = gsl_multifit_linear_alloc (pontius_n, pontius_p); gsl_multifit_robust_workspace * work_rob = gsl_multifit_robust_alloc (gsl_multifit_robust_ols, pontius_n, pontius_p); gsl_matrix * X = gsl_matrix_alloc (pontius_n, pontius_p); gsl_vector_view y = gsl_vector_view_array (pontius_y, pontius_n); gsl_vector * c = gsl_vector_alloc (pontius_p); gsl_vector * r = gsl_vector_alloc (pontius_n); gsl_matrix * cov = gsl_matrix_alloc (pontius_p, pontius_p); double chisq, chisq_res; double expected_c[3] = { 0.673565789473684E-03, 0.732059160401003E-06, -0.316081871345029E-14}; double expected_sd[3] = { 0.107938612033077E-03, 0.157817399981659E-09, 0.486652849992036E-16 }; double expected_chisq = 0.155761768796992E-05; gsl_vector_view diag = gsl_matrix_diagonal (cov); gsl_vector_view exp_c = gsl_vector_view_array(expected_c, pontius_p); gsl_vector_view exp_sd = gsl_vector_view_array(expected_sd, pontius_p); for (i = 0 ; i < pontius_n; i++) { for (j = 0; j < pontius_p; j++) { gsl_matrix_set(X, i, j, pow(pontius_x[i], j)); } } /* test unweighted least squares */ gsl_multifit_linear (X, &y.vector, c, cov, &chisq, work); gsl_multifit_linear_residuals(X, &y.vector, c, r); gsl_blas_ddot(r, r, &chisq_res); test_pontius_results("pontius gsl_multifit_linear", c, &exp_c.vector, &diag.vector, &exp_sd.vector, chisq, chisq_res, expected_chisq); /* test robust least squares */ gsl_multifit_robust (X, &y.vector, c, cov, work_rob); test_pontius_results("pontius gsl_multifit_robust", c, &exp_c.vector, &diag.vector, &exp_sd.vector, 1.0, 1.0, 1.0); /* test weighted least squares */ { gsl_vector * w = gsl_vector_alloc (pontius_n); double expected_cov[3][3] ={ {2.76754385964916e-01 , -3.59649122807024e-07, 9.74658869395731e-14}, {-3.59649122807024e-07, 5.91630591630603e-13, -1.77210703526497e-19}, {9.74658869395731e-14, -1.77210703526497e-19, 5.62573661988878e-26} }; gsl_vector_set_all (w, 1.0); gsl_multifit_wlinear (X, w, &y.vector, c, cov, &chisq, work); gsl_multifit_linear_residuals(X, &y.vector, c, r); gsl_blas_ddot(r, r, &chisq_res); test_pontius_results("pontius gsl_multifit_wlinear", c, &exp_c.vector, NULL, NULL, chisq, chisq_res, expected_chisq); for (i = 0; i < pontius_p; i++) { for (j = 0; j < pontius_p; j++) { gsl_test_rel (gsl_matrix_get(cov,i,j), expected_cov[i][j], 1e-10, "pontius gsl_multifit_wlinear cov(%d,%d)", i, j) ; } } gsl_vector_free(w); } gsl_vector_free(c); gsl_vector_free(r); gsl_matrix_free(cov); gsl_matrix_free(X); gsl_multifit_linear_free (work); gsl_multifit_robust_free (work_rob); }
static int MismatchTest( LatticeTiling *tiling, gsl_matrix *metric, const double max_mismatch, const UINT8 total_ref, const double mism_hist_ref[MISM_HIST_BINS] ) { const size_t n = XLALTotalLatticeTilingDimensions(tiling); // Create lattice tiling iterator and locator LatticeTilingIterator *itr = XLALCreateLatticeTilingIterator(tiling, n); XLAL_CHECK(itr != NULL, XLAL_EFUNC); LatticeTilingLocator *loc = XLALCreateLatticeTilingLocator(tiling); XLAL_CHECK(loc != NULL, XLAL_EFUNC); // Count number of points const UINT8 total = XLALTotalLatticeTilingPoints(itr); printf("Number of lattice points: %" LAL_UINT8_FORMAT "\n", total); XLAL_CHECK(imaxabs(total - total_ref) <= 1, XLAL_EFUNC, "ERROR: |total - total_ref| = |%" LAL_UINT8_FORMAT " - %" LAL_UINT8_FORMAT "| > 1", total, total_ref); // Get all points gsl_matrix *GAMAT(points, n, total); XLAL_CHECK(XLALNextLatticeTilingPoints(itr, &points) == (int)total, XLAL_EFUNC); XLAL_CHECK(XLALNextLatticeTilingPoint(itr, NULL) == 0, XLAL_EFUNC); // Initialise mismatch histogram counts double mism_hist[MISM_HIST_BINS] = {0}; double mism_hist_total = 0, mism_hist_out_of_range = 0; // Perform 10 injections for every template { gsl_matrix *GAMAT(injections, 3, total); gsl_matrix *GAMAT(nearest, 3, total); gsl_matrix *GAMAT(temp, 3, total); RandomParams *rng = XLALCreateRandomParams(total); XLAL_CHECK(rng != NULL, XLAL_EFUNC); for (size_t i = 0; i < 10; ++i) { // Generate random injection points XLAL_CHECK(XLALRandomLatticeTilingPoints(tiling, 0.0, rng, injections) == XLAL_SUCCESS, XLAL_EFUNC); // Find nearest lattice template points XLAL_CHECK(XLALNearestLatticeTilingPoints(loc, injections, &nearest, NULL) == XLAL_SUCCESS, XLAL_EFUNC); // Compute mismatch between injections gsl_matrix_sub(nearest, injections); gsl_blas_dsymm(CblasLeft, CblasUpper, 1.0, metric, nearest, 0.0, temp); for (size_t j = 0; j < temp->size2; ++j) { gsl_vector_view temp_j = gsl_matrix_column(temp, j); gsl_vector_view nearest_j = gsl_matrix_column(nearest, j); double mismatch = 0.0; gsl_blas_ddot(&nearest_j.vector, &temp_j.vector, &mismatch); mismatch /= max_mismatch; // Increment mismatch histogram counts ++mism_hist_total; if (mismatch < 0.0 || mismatch > 1.0) { ++mism_hist_out_of_range; } else { ++mism_hist[lround(floor(mismatch * MISM_HIST_BINS))]; } } } // Cleanup GFMAT(injections, nearest, temp); XLALDestroyRandomParams(rng); } // Normalise histogram for (size_t i = 0; i < MISM_HIST_BINS; ++i) { mism_hist[i] *= MISM_HIST_BINS / mism_hist_total; } // Print mismatch histogram and its reference printf("Mismatch histogram: "); for (size_t i = 0; i < MISM_HIST_BINS; ++i) { printf(" %0.3f", mism_hist[i]); } printf("\n"); printf("Reference histogram:"); for (size_t i = 0; i < MISM_HIST_BINS; ++i) { printf(" %0.3f", mism_hist_ref[i]); } printf("\n"); // Determine error between mismatch histogram and its reference double mism_hist_error = 0.0; for (size_t i = 0; i < MISM_HIST_BINS; ++i) { mism_hist_error += fabs(mism_hist[i] - mism_hist_ref[i]); } mism_hist_error /= MISM_HIST_BINS; printf("Mismatch histogram error: %0.3e\n", mism_hist_error); const double mism_hist_error_tol = 5e-2; if (mism_hist_error >= mism_hist_error_tol) { XLAL_ERROR(XLAL_EFAILED, "ERROR: mismatch histogram error exceeds %0.3e\n", mism_hist_error_tol); } // Check fraction of injections out of histogram range const double mism_out_of_range = mism_hist_out_of_range / mism_hist_total; printf("Fraction of points out of histogram range: %0.3e\n", mism_out_of_range); const double mism_out_of_range_tol = 2e-3; if (mism_out_of_range > mism_out_of_range_tol) { XLAL_ERROR(XLAL_EFAILED, "ERROR: fraction of points out of histogram range exceeds %0.3e\n", mism_out_of_range_tol); } // Perform 10 injections outside parameter space { gsl_matrix *GAMAT(injections, 3, 10); gsl_matrix *GAMAT(nearest, n, total); RandomParams *rng = XLALCreateRandomParams(total); XLAL_CHECK(rng != NULL, XLAL_EFUNC); // Generate random injection points outside parameter space XLAL_CHECK(XLALRandomLatticeTilingPoints(tiling, 5.0, rng, injections) == XLAL_SUCCESS, XLAL_EFUNC); // Find nearest lattice template points XLAL_CHECK(XLALNearestLatticeTilingPoints(loc, injections, &nearest, NULL) == XLAL_SUCCESS, XLAL_EFUNC); // Cleanup GFMAT(injections, nearest); XLALDestroyRandomParams(rng); } // Cleanup XLALDestroyLatticeTiling(tiling); XLALDestroyLatticeTilingIterator(itr); XLALDestroyLatticeTilingLocator(loc); GFMAT(metric, points); LALCheckMemoryLeaks(); printf("\n"); fflush(stdout); return XLAL_SUCCESS; }