Beispiel #1
0
/* 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;
}
Beispiel #2
0
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);
    }
}
Beispiel #4
0
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;
}
Beispiel #5
0
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;
}
Beispiel #6
0
    // 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;
    }
Beispiel #7
0
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;
    }
}
Beispiel #9
0
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;
}
Beispiel #10
0
 /**
  * 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 ); }
Beispiel #11
0
// Wald Test used in both summary and anova (polymophism)
int GlmTest::GeeWald(glm *Alt, gsl_matrix *LL, gsl_vector *teststat)
{
    gsl_set_error_handler_off();

    unsigned int i, j, l;
    double alpha, result, sum=0;
    unsigned int nP = Alt->nParams;
    unsigned int nDF = LL->size1;
    unsigned int nVars=tm->nVars, nRows=tm->nRows;
    int status;

    gsl_vector *LBeta = gsl_vector_alloc(nVars*nDF);
    gsl_vector_set_zero(LBeta);
    gsl_matrix *w1jX1=gsl_matrix_alloc(nRows, nP);
    gsl_matrix *XwX=gsl_matrix_alloc(nP, nP);
    gsl_matrix *Rl2 = gsl_matrix_alloc(nDF, nP);
    gsl_matrix *IinvN = gsl_matrix_alloc(nDF, nDF);
    gsl_matrix *IinvRl = gsl_matrix_alloc(nVars*nDF, nVars*nDF);
    gsl_vector *tmp = gsl_vector_alloc(nVars*nDF);
    gsl_vector_view tmp2, wj, LBj, bj; //, dj; 
    gsl_matrix_view Rl;

    gsl_matrix_set_zero(IinvRl);
    GrpMat *Z = (GrpMat*)malloc(nVars*sizeof(GrpMat));
    for (j=0; j<nVars; j++){
       Z[j].matrix = gsl_matrix_alloc(nP, nRows);
       // w1jX1 = W^1/2 * X
       wj=gsl_matrix_column(Alt->wHalf, j);
       for (i=0; i<nP; i++)
           gsl_matrix_set_col (w1jX1, i, &wj.vector);
       gsl_matrix_mul_elements (w1jX1, Alt->Xref);

       // LBeta = L*Beta       
       LBj=gsl_vector_subvector(LBeta, j*nDF, nDF);
       bj=gsl_matrix_column(Alt->Beta, j);
       gsl_blas_dgemv(CblasNoTrans,1,LL,&bj.vector,0,&LBj.vector);

       // Z = (X^T W X)^-1 * X^T W^1/2. 
       gsl_matrix_set_identity(XwX);
       gsl_blas_dsyrk (CblasLower,CblasTrans,1.0,w1jX1,0.0,XwX);
       status=gsl_linalg_cholesky_decomp (XwX);
       if (status==GSL_EDOM) {
          if (tm->warning==TRUE) 
             printf("Warning:singular matrix in wald test. An eps*I is added to the singular matrix.\n");
          gsl_matrix_set_identity(XwX);
          gsl_blas_dsyrk(CblasLower,CblasTrans,1.0,w1jX1,eps,XwX);
          gsl_linalg_cholesky_decomp(XwX);
       }
       gsl_linalg_cholesky_invert(XwX);
       gsl_blas_dgemm(CblasNoTrans,CblasTrans,1.0,XwX,w1jX1,0.0, Z[j].matrix);

       gsl_matrix_memcpy(Rl2, LL);
       gsl_blas_dtrmm (CblasRight,CblasLower,CblasNoTrans,CblasNonUnit,1.0,XwX,Rl2); // L*(X'WX)^-1
       gsl_blas_dgemm (CblasNoTrans, CblasTrans, 1.0, Rl2, LL, 0.0, IinvN); // L*(X^T*W*X)^-1*L^T 

       if ( (tm->punit!=NONE) || (tm->corr==IDENTITY) ) {
          status=gsl_linalg_cholesky_decomp (IinvN);
          if (status==GSL_EDOM) {
             if (tm->warning==TRUE) 
             printf("Warning:singular IinvN in wald test.\n");
          }
          tmp2=gsl_vector_subvector(tmp, 0, nDF);
          gsl_linalg_cholesky_solve (IinvN, &LBj.vector, &tmp2.vector);
          gsl_blas_ddot (&LBj.vector, &tmp2.vector, &result);
          gsl_vector_set(teststat, j+1, sqrt(result));
          sum = sum + result;
       }

       if (tm->corr!=IDENTITY) {
          // IinvRl=L*vSandRl*L^T 
          for (l=0; l<=j; l++) {
              Rl=gsl_matrix_submatrix(IinvRl,j*nDF,l*nDF,nDF,nDF);
              alpha = gsl_matrix_get(Rlambda, j, l);
              // borrow XwX space to store vSandRl
              gsl_blas_dgemm(CblasNoTrans,CblasTrans,alpha,Z[j].matrix,Z[l].matrix, 0.0, XwX); 
              // Rl2 = L*vSandRl*L^T
              gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,LL,XwX,0.0,Rl2);
              gsl_blas_dgemm(CblasNoTrans,CblasTrans,1.0,Rl2,LL,0.0,&Rl.matrix);
          } // end l
       }  // end if (tm->corr) 

    } // end for j=1:nVars       

    if ( tm->corr==IDENTITY ) 
        gsl_vector_set(teststat, 0, sqrt(sum));
    else {
        status=gsl_linalg_cholesky_decomp (IinvRl);
        if (status==GSL_EDOM) {
           if (tm->warning==TRUE) 
           printf("Warning:singular matrix in multivariate wald test.\n");
        }
        gsl_linalg_cholesky_solve (IinvRl, LBeta, tmp);
        gsl_blas_ddot (LBeta, tmp, &result);
        gsl_vector_set(teststat, 0, sqrt(result));
    }

    // free memory
    for (j=0; j<nVars; j++) 
        gsl_matrix_free(Z[j].matrix);
    free(Z);
    gsl_vector_free(LBeta);
    gsl_matrix_free(w1jX1);
    gsl_matrix_free(XwX);
    gsl_matrix_free(Rl2);
    gsl_matrix_free(IinvN);
    gsl_matrix_free(IinvRl);
    gsl_vector_free(tmp);

    return SUCCESS;
}
Beispiel #12
0
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);
  }
}
Beispiel #14
0
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;
    }
}
Beispiel #15
0
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;
}
Beispiel #16
0
/**
 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;
}
Beispiel #17
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;

}
Beispiel #18
0
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);
}
Beispiel #19
0
/* 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 */
Beispiel #20
0
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);
}}
Beispiel #21
0
static double slope(wrapper_t * w)
{							       /* compute gradient . direction */
	double df;
	gsl_blas_ddot(w->g_alpha, w->p, &df);
	return df;
}
Beispiel #22
0
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());
  }
}
Beispiel #23
0
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;
}
Beispiel #24
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;
}
Beispiel #25
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;    
}
Beispiel #26
0
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() */
Beispiel #27
0
    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;
}
Beispiel #29
0
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);
}
Beispiel #30
0
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;

}