示例#1
0
文件: kinfit.c 项目: bvelghe/kinfit
int minimise(gsl_vector * v_alpha_zero, gsl_vector * v_alpha_A, gsl_matrix * m_V_alpha_zero, gsl_vector * v_d, gsl_matrix * m_D, gsl_vector * v_alpha, gsl_matrix * m_V_alpha, double * chi2) {
  size_t height = v_alpha_zero->size;

  gsl_vector * v_delta_alpha = gsl_vector_calloc(height);
  gsl_vector_memcpy(v_delta_alpha,v_alpha_zero); /* delta_alpha = alpha_zero - alpha_A*/
  gsl_vector_sub(v_delta_alpha,v_alpha_A);  


  gsl_matrix * m_T1 = gsl_matrix_calloc(TRACK_NBR*CONSTRAINTS,height); /* Temporary matrix */
  gsl_matrix * m_T2 = gsl_matrix_calloc(TRACK_NBR*CONSTRAINTS,TRACK_NBR*CONSTRAINTS); /* Temporary matrix */
  gsl_vector * v_T3 = gsl_vector_calloc(TRACK_NBR*CONSTRAINTS); /* Temporary vector */

  gsl_matrix * m_V_D = gsl_matrix_calloc(TRACK_NBR*CONSTRAINTS,TRACK_NBR*CONSTRAINTS);

  /* V_D */
  gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,m_D,m_V_alpha_zero,0.0,m_T1); /* T1 = D*V_alpha_zero */
  gsl_blas_dgemm(CblasNoTrans,CblasTrans,1.0,m_T1,m_D,0.0,m_T2); /* T2 = T1*D' */
 
  /* Ok */

  gsl_blas_dgemv(CblasNoTrans,1.0,m_D,v_delta_alpha,0.0,v_T3); /* T3 = D*delta_alpha */
  gsl_vector_add(v_T3,v_d); /* T3 = T3 + d */

  /* Ok */

  gsl_vector * v_lambda = gsl_vector_calloc(TRACK_NBR*CONSTRAINTS);
 
  /* lambda = inv(T2)*T3 */
  int s;
  gsl_permutation * p = gsl_permutation_alloc (TRACK_NBR*CONSTRAINTS);
  gsl_linalg_LU_decomp (m_T2, p, &s); 
  gsl_linalg_LU_solve(m_T2,p,v_T3,v_lambda); 

  /* Ok */

  gsl_matrix * m_T4 = gsl_matrix_calloc(height,TRACK_NBR*CONSTRAINTS);
  gsl_vector * v_T5 = gsl_vector_calloc(height);
  gsl_blas_dgemm(CblasNoTrans,CblasTrans,1.0,m_V_alpha_zero,m_D,0.0,m_T4); /* T4 = V_alpha_zero*m_D' */
  gsl_blas_dgemv(CblasNoTrans,1.0,m_T4,v_lambda,0.0,v_T5); /* T5 = T4*lambda */ 
  
  
  gsl_vector_memcpy(v_alpha,v_alpha_A); /* alpha = alpha_A */  
  gsl_vector_sub(v_alpha,v_T5); /* alpha = alpha_A - T5*/

  /* Ok */

  /* Compute V_alpha */
  gsl_matrix * m_T6 = gsl_matrix_calloc(TRACK_NBR*CONSTRAINTS,TRACK_NBR*CONSTRAINTS);  
  gsl_matrix * m_T7 = gsl_matrix_calloc(height,TRACK_NBR*CONSTRAINTS);
  gsl_matrix * m_T8 = gsl_matrix_calloc(height,height);
  gsl_matrix * m_T9 = gsl_matrix_calloc(height,height);

  gsl_matrix_memcpy(m_V_alpha,m_V_alpha_zero); /* V_alpha = V_alpha_zero */
  /* T6 = inv(T2) */  
  gsl_linalg_LU_invert(m_T2,p,m_T6); /* See before for LU_decomp(m_T2,...)*/ 
  /* T7 = T4 * T6 */  
  gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,m_T4,m_T6,0.0,m_T7); 
  /* T8 = T7 * D */
  gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,m_T7,m_D,0.0,m_T8); 
  /* T9 = T8 * V_alpha_zero */
  gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,m_T8,m_V_alpha_zero,0.0,m_T9); 
  gsl_matrix_sub(m_V_alpha,m_T9);

  /* Ok */

  /* Compute chi2 */ 
  /* chi2 = lambda' * T3 */
  gsl_blas_ddot(v_lambda,v_T3,chi2);

  gsl_vector_free(v_T5);

  /* Clean the mess */
  gsl_matrix_free(m_T1);
  gsl_matrix_free(m_T2);
  gsl_vector_free(v_T3);
  gsl_matrix_free(m_T4);
  gsl_matrix_free(m_T6);
  gsl_matrix_free(m_T7);
  gsl_matrix_free(m_T8);
  gsl_matrix_free(m_T9);
  
  gsl_permutation_free(p);

  gsl_matrix_free(m_V_D);
  gsl_vector_free(v_delta_alpha);
  gsl_vector_free(v_lambda);
}
示例#2
0
int
main(int argc, char *argv[])
{
  gen_workspace *gen_workspace_p;
  lapack_workspace *lapack_workspace_p;
  size_t N;
  int c;
  int lower;
  int upper;
  int incremental;
  size_t nmat;
  gsl_matrix *A, *B;
  gsl_rng *r;
  int s;
  int compute_schur;
  size_t i;

  gsl_ieee_env_setup();
  gsl_rng_env_setup();

  N = 30;
  lower = -10;
  upper = 10;
  incremental = 0;
  nmat = 0;
  compute_schur = 0;

  while ((c = getopt(argc, argv, "ic:n:l:u:z")) != (-1))
    {
      switch (c)
        {
          case 'i':
            incremental = 1;
            break;

          case 'n':
            N = strtol(optarg, NULL, 0);
            break;

          case 'l':
            lower = strtol(optarg, NULL, 0);
            break;

          case 'u':
            upper = strtol(optarg, NULL, 0);
            break;

          case 'c':
            nmat = strtoul(optarg, NULL, 0);
            break;

          case 'z':
            compute_schur = 1;
            break;

          case '?':
          default:
            printf("usage: %s [-i] [-z] [-n size] [-l lower-bound] [-u upper-bound] [-c num]\n", argv[0]);
            exit(1);
            break;
        } /* switch (c) */
    }

  A = gsl_matrix_alloc(N, N);
  B = gsl_matrix_alloc(N, N);
  gen_workspace_p = gen_alloc(N, compute_schur);
  lapack_workspace_p = lapack_alloc(N);

  r = gsl_rng_alloc(gsl_rng_default);

  if (incremental)
    {
      make_start_matrix(A, lower);

      /* we need B to be non-singular */
      make_random_integer_matrix(B, r, lower, upper);
    }

  fprintf(stderr, "testing N = %d", N);
  if (incremental)
    fprintf(stderr, " incrementally");
  else
    fprintf(stderr, " randomly");

  fprintf(stderr, " on element range [%d, %d]", lower, upper);

  if (compute_schur)
    fprintf(stderr, ", with Schur vectors");

  fprintf(stderr, "\n");

  while (1)
    {
      if (nmat && (count >= nmat))
        break;

      ++count;

      if (!incremental)
        {
          make_random_matrix(A, r, lower, upper);
          make_random_matrix(B, r, lower, upper);
        }
      else
        {
          s = inc_matrix(A, lower, upper);
          if (s)
            break; /* all done */

          make_random_integer_matrix(B, r, lower, upper);
        }

      /*if (count != 89120)
        continue;*/

      /* make copies of matrices */
      gsl_matrix_memcpy(gen_workspace_p->A, A);
      gsl_matrix_memcpy(gen_workspace_p->B, B);
      gsl_matrix_transpose_memcpy(lapack_workspace_p->A, A);
      gsl_matrix_transpose_memcpy(lapack_workspace_p->B, B);

      /* compute eigenvalues with LAPACK */
      s = lapack_proc(lapack_workspace_p);

      if (s != GSL_SUCCESS)
        {
          printf("LAPACK failed, case %lu\n", count);
          exit(1);
        }

#if 0
      print_matrix(A, "A");
      print_matrix(B, "B");
      gsl_matrix_transpose(lapack_workspace_p->A);
      gsl_matrix_transpose(lapack_workspace_p->B);
      print_matrix(lapack_workspace_p->A, "S_lapack");
      print_matrix(lapack_workspace_p->B, "T_lapack");
#endif

      /* compute eigenvalues with GSL */
      s = gen_proc(gen_workspace_p);

      if (s != GSL_SUCCESS)
        {
          printf("=========== CASE %lu ============\n", count);
          printf("Failed to converge: found %u eigenvalues\n",
                 gen_workspace_p->n_evals);
          print_matrix(A, "A");
          print_matrix(B, "B");
          print_matrix(gen_workspace_p->A, "Af");
          print_matrix(gen_workspace_p->B, "Bf");
          print_matrix(lapack_workspace_p->A, "Ae");
          print_matrix(lapack_workspace_p->B, "Be");
          exit(1);
        }

#if 0
      print_matrix(gen_workspace_p->A, "S_gsl");
      print_matrix(gen_workspace_p->B, "T_gsl");
#endif

      /* compute alpha / beta vectors */
      for (i = 0; i < N; ++i)
        {
          double beta;
          gsl_complex alpha, z;

          beta = gsl_vector_get(gen_workspace_p->beta, i);
          if (beta == 0.0)
            GSL_SET_COMPLEX(&z, GSL_POSINF, GSL_POSINF);
          else
            {
              alpha = gsl_vector_complex_get(gen_workspace_p->alpha, i);
              z = gsl_complex_div_real(alpha, beta);
            }

          gsl_vector_complex_set(gen_workspace_p->evals, i, z);

          beta = gsl_vector_get(lapack_workspace_p->beta, i);
          GSL_SET_COMPLEX(&alpha,
                          lapack_workspace_p->alphar[i],
                          lapack_workspace_p->alphai[i]);

          if (beta == 0.0)
            GSL_SET_COMPLEX(&z, GSL_POSINF, GSL_POSINF);
          else
            z = gsl_complex_div_real(alpha, beta);

          gsl_vector_complex_set(lapack_workspace_p->evals, i, z);
          gsl_vector_complex_set(lapack_workspace_p->alpha, i, alpha);
        }

#if 0
      gsl_sort_vector(gen_workspace_p->beta);
      gsl_sort_vector(lapack_workspace_p->beta);
      sort_complex_vector(gen_workspace_p->alpha);
      sort_complex_vector(lapack_workspace_p->alpha);

      s = test_alpha(gen_workspace_p->alpha,
                     lapack_workspace_p->alpha,
                     A,
                     B,
                     "gen",
                     "lapack");
      s = test_beta(gen_workspace_p->beta,
                    lapack_workspace_p->beta,
                    A,
                    B,
                    "gen",
                    "lapack");
#endif
#if 1
      sort_complex_vector(gen_workspace_p->evals);
      sort_complex_vector(lapack_workspace_p->evals);

      s = test_evals(gen_workspace_p->evals,
                     lapack_workspace_p->evals,
                     A,
                     B,
                     "gen",
                     "lapack");
#endif

      if (compute_schur)
        {
          test_schur(A,
                     gen_workspace_p->A,
                     gen_workspace_p->Q,
                     gen_workspace_p->Z);
          test_schur(B,
                     gen_workspace_p->B,
                     gen_workspace_p->Q,
                     gen_workspace_p->Z);
        }
    }

  gsl_matrix_free(A);
  gsl_matrix_free(B);
  gen_free(gen_workspace_p);
  lapack_free(lapack_workspace_p);

  if (r)
    gsl_rng_free(r);

  return 0;
} /* main() */
示例#3
0
static int
test_COD_lssolve_eps(const gsl_matrix * m, const double * actual, const double eps, const char *desc)
{
  int s = 0;
  size_t i, M = m->size1, N = m->size2;

  gsl_vector * lhs = gsl_vector_alloc(M);
  gsl_vector * rhs = gsl_vector_alloc(M);
  gsl_matrix * QRZT  = gsl_matrix_alloc(M, N);
  gsl_vector * tau_Q = gsl_vector_alloc(GSL_MIN(M, N));
  gsl_vector * tau_Z = gsl_vector_alloc(GSL_MIN(M, N));
  gsl_vector * work = gsl_vector_alloc(N);
  gsl_vector * x = gsl_vector_alloc(N);
  gsl_vector * r = gsl_vector_alloc(M);
  gsl_vector * res = gsl_vector_alloc(M);
  gsl_permutation * perm = gsl_permutation_alloc(N);
  size_t rank;

  gsl_matrix_memcpy(QRZT, m);

  for (i = 0; i < M; i++)
    gsl_vector_set(rhs, i, i + 1.0);

  s += gsl_linalg_COD_decomp(QRZT, tau_Q, tau_Z, perm, &rank, work);
  s += gsl_linalg_COD_lssolve(QRZT, tau_Q, tau_Z, perm, rank, rhs, x, res);

  for (i = 0; i < N; i++)
    {
      double xi = gsl_vector_get(x, i);
      gsl_test_rel(xi, actual[i], eps,
                   "%s (%3lu,%3lu)[%lu]: %22.18g   %22.18g\n",
                   desc, M, N, i, xi, actual[i]);
    }

  /* compute residual r = b - m x */
  if (M == N)
    {
      gsl_vector_set_zero(r);
    }
  else
    {
      gsl_vector_memcpy(r, rhs);
      gsl_blas_dgemv(CblasNoTrans, -1.0, m, x, 1.0, r);
    }

  for (i = 0; i < N; i++)
    {
      double r1 = gsl_vector_get(res, i);
      double r2 = gsl_vector_get(r, i);

      if (fabs(r2) < 1.0e3 * GSL_DBL_EPSILON)
        {
          gsl_test_abs(r1, r2, 10.0 * eps,
                       "%s res (%3lu,%3lu)[%lu]: %22.18g   %22.18g\n",
                       desc, M, N, i, r1, r2);
        }
      else
        {
          gsl_test_rel(r1, r2, eps,
                       "%s res (%3lu,%3lu)[%lu]: %22.18g   %22.18g\n",
                       desc, M, N, i, r1, r2);
        }
    }

  gsl_vector_free(r);
  gsl_vector_free(res);
  gsl_vector_free(x);
  gsl_vector_free(tau_Q);
  gsl_vector_free(tau_Z);
  gsl_matrix_free(QRZT);
  gsl_vector_free(rhs);
  gsl_vector_free(lhs);
  gsl_vector_free(work);
  gsl_permutation_free(perm);

  return s;
}
示例#4
0
文件: pcholesky.c 项目: ampl/gsl
int
gsl_linalg_pcholesky_invert(const gsl_matrix * LDLT, const gsl_permutation * p,
                            gsl_matrix * Ainv)
{
  const size_t M = LDLT->size1;
  const size_t N = LDLT->size2;

  if (M != N)
    {
      GSL_ERROR ("LDLT matrix must be square", GSL_ENOTSQR);
    }
  else if (LDLT->size1 != p->size)
    {
      GSL_ERROR ("matrix size must match permutation size", GSL_EBADLEN);
    }
  else if (Ainv->size1 != Ainv->size2)
    {
      GSL_ERROR ("Ainv matrix must be square", GSL_ENOTSQR);
    }
  else if (Ainv->size1 != M)
    {
      GSL_ERROR ("Ainv matrix has wrong dimensions", GSL_EBADLEN);
    }
  else
    {
      size_t i, j;
      gsl_vector_view v1, v2;

      /* invert the lower triangle of LDLT */
      gsl_matrix_memcpy(Ainv, LDLT);
      gsl_linalg_tri_lower_unit_invert(Ainv);

      /* compute sqrt(D^{-1}) L^{-1} in the lower triangle of Ainv */
      for (i = 0; i < N; ++i)
        {
          double di = gsl_matrix_get(LDLT, i, i);
          double sqrt_di = sqrt(di);

          for (j = 0; j < i; ++j)
            {
              double *Lij = gsl_matrix_ptr(Ainv, i, j);
              *Lij /= sqrt_di;
            }

          gsl_matrix_set(Ainv, i, i, 1.0 / sqrt_di);
        }

      /*
       * The lower triangle of Ainv now contains D^{-1/2} L^{-1}. Now compute
       * A^{-1} = L^{-T} D^{-1} L^{-1}
       */

      for (i = 0; i < N; ++i)
        {
          double aii = gsl_matrix_get(Ainv, i, i);

          if (i < N - 1)
            {
              double tmp;

              v1 = gsl_matrix_subcolumn(Ainv, i, i, N - i);
              gsl_blas_ddot(&v1.vector, &v1.vector, &tmp);
              gsl_matrix_set(Ainv, i, i, tmp);

              if (i > 0)
                {
                  gsl_matrix_view m = gsl_matrix_submatrix(Ainv, i + 1, 0, N - i - 1, i);

                  v1 = gsl_matrix_subcolumn(Ainv, i, i + 1, N - i - 1);
                  v2 = gsl_matrix_subrow(Ainv, i, 0, i);

                  gsl_blas_dgemv(CblasTrans, 1.0, &m.matrix, &v1.vector, aii, &v2.vector);
                }
            }
          else
            {
              v1 = gsl_matrix_row(Ainv, N - 1);
              gsl_blas_dscal(aii, &v1.vector);
            }
        }

      /* copy lower triangle to upper */
      gsl_matrix_transpose_tricpy('L', 0, Ainv, Ainv);

      /* now apply permutation p to the matrix */

      /* compute L^{-T} D^{-1} L^{-1} P^T */
      for (i = 0; i < N; ++i)
        {
          v1 = gsl_matrix_row(Ainv, i);
          gsl_permute_vector_inverse(p, &v1.vector);
        }

      /* compute P L^{-T} D^{-1} L^{-1} P^T */
      for (i = 0; i < N; ++i)
        {
          v1 = gsl_matrix_column(Ainv, i);
          gsl_permute_vector_inverse(p, &v1.vector);
        }

      return GSL_SUCCESS;
    }
}
示例#5
0
int
gsl_multifit_linear (const gsl_matrix * X,
                     const gsl_vector * y,
                     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 (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;

      gsl_matrix *A = work->A;
      gsl_matrix *Q = work->Q;
      gsl_matrix *QSI = work->QSI;
      gsl_vector *S = work->S;
      gsl_vector *xt = work->xt;
      gsl_vector *D = work->D;

      /* Copy X to workspace,  A <= X */

      gsl_matrix_memcpy (A, X);

      /* Balance the columns of the matrix A */

      gsl_linalg_balance_columns (A, D);

      /* Decompose A into U S Q^T */

      gsl_linalg_SV_decomp_mod (A, QSI, Q, S, xt);

      /* Solve y = A c for c */

      gsl_blas_dgemv (CblasTrans, 1.0, A, y, 0.0, xt);

      /* Scale the matrix Q,  Q' = Q S^-1 */

      gsl_matrix_memcpy (QSI, Q);

      for (j = 0; j < p; j++)
        {
          gsl_vector_view column = gsl_matrix_column (QSI, j);
          double alpha = gsl_vector_get (S, j);
          if (alpha != 0)
            alpha = 1.0 / alpha;
          gsl_vector_scale (&column.vector, alpha);
        }

      gsl_vector_set_zero (c);

      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 s2 = 0, r2 = 0;

        for (i = 0; i < n; i++)
          {
            double yi = gsl_vector_get (y, 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 += ri * ri;
          }

        s2 = r2 / (n - p);

        *chisq = r2;

        /* Form variance-covariance matrix cov = s2 * (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 * s2 / (d_i * d_j));
                gsl_matrix_set (cov, j, i, s * s2 / (d_i * d_j));
              }
          }
      }

      return GSL_SUCCESS;
    }
}
示例#6
0
AnovaTest::AnovaTest(mv_Method *mm, gsl_matrix *Y, gsl_matrix *X, gsl_matrix *isXvarIn):mmRef(mm), Yref(Y), Xref(X), inRef(isXvarIn)
{
    unsigned int hid, aid;
    unsigned int i, j, count;
    nModels=inRef->size1, nParam=Xref->size2;
    nRows=Yref->size1, nVars=Yref->size2; 

//  printf("initialize public variables: stats\n");
    multstat=(double *)malloc((nModels-1)*sizeof(double));
    Pmultstat = (double *)malloc((nModels-1)*sizeof(double));
    for (j=0; j<nModels-1; j++) *(Pmultstat+j)=0.0; 
    dfDiff = (unsigned int *)malloc((nModels-1)*sizeof(unsigned int));

    statj = gsl_matrix_alloc(nModels-1, nVars);
    Pstatj = gsl_matrix_alloc(nModels-1, nVars);
    gsl_matrix_set_zero(Pstatj);

    bStatj = gsl_vector_alloc(nVars);
    Hats = (mv_mat *)malloc(nModels*sizeof(mv_mat)); 
    sortid = (gsl_permutation **)malloc((nModels-1)*sizeof(gsl_permutation *));
    
    for (i=0; i<nModels; i++ ) {
        // Hats[i]
        Hats[i].mat=gsl_matrix_alloc(nRows, nRows);
        Hats[i].SS=gsl_matrix_alloc(nVars, nVars);
        Hats[i].R=gsl_matrix_alloc(nVars, nVars);
        Hats[i].Res=gsl_matrix_alloc(nRows, nVars);
        Hats[i].Y = gsl_matrix_alloc(nRows, nVars);
        Hats[i].sd = gsl_vector_alloc(nVars);
	count = 0;
	for (j=0; j<nParam; j++){
	    count+=(unsigned int)gsl_matrix_get(inRef, i, j);
	}
//	printf("count=%d \n", count);
	Hats[i].X = gsl_matrix_alloc(nRows, count);
	Hats[i].Coef=gsl_matrix_alloc(count, nVars);
        gsl_vector_view refi=gsl_matrix_row(inRef, i);
	subX(Xref, &refi.vector, Hats[i].X);
        calcSS(Yref, &(Hats[i]), mmRef);
//	displaymatrix(Hats[i].SS, "SS");
    }

    for (i=1; i<nModels; i++) {
        hid = i; aid = i-1;
        if ( mmRef->resamp != CASEBOOT ) {
            // fit = Y- resi 
            gsl_matrix_memcpy (Hats[i].Y, Yref);
            gsl_matrix_sub (Hats[i].Y, Hats[i].Res);
        } 
        gsl_vector_view statij = gsl_matrix_row(statj, aid);
        testStatCalc(&(Hats[hid]), &(Hats[aid]), mmRef, TRUE, (multstat+aid), &statij.vector); 
	dfDiff[aid] = Hats[aid].X->size2-Hats[hid].X->size2;
        // sortid
        sortid[aid] = gsl_permutation_alloc(nVars);
        gsl_sort_vector_index (sortid[aid], &statij.vector); 
        // rearrange sortid in descending order
        gsl_permutation_reverse (sortid[aid]);
    }  

    // initialize resampling indices 
//    getBootID(); done in R
    bootID = NULL;


    // Initialize GSL rnd environment variables
    const gsl_rng_type *T;
    gsl_rng_env_setup();
    T = gsl_rng_default;
    // an mt19937 generator with a seed of 0
    rnd = gsl_rng_alloc(T);
    if (mmRef->reprand!=TRUE){
       struct timeval tv;  // seed generation based on time
       gettimeofday(&tv, 0);
       unsigned long mySeed=tv.tv_sec + tv.tv_usec;
       gsl_rng_set(rnd, mySeed);  // reset seed
    }

//    printf("Anova test initialized.\n");

}
示例#7
0
static int
md_eigen(lua_State *L)                                         /* (-1,+2,e) */
{
    mMatReal *m = qlua_checkMatReal(L, 1);
    gsl_matrix_view mx;
    gsl_eigen_symmv_workspace *w;
    gsl_vector *ev;
    mVecReal *lambda;
    mMatReal *trans;
    mMatReal *tmp;
    int n;
    int i;
    int lo, hi;

    switch (lua_gettop(L)) {
    case 1:
        if (m->l_size != m->r_size)
            return luaL_error(L, "matrix:eigen() expects square matrix");
        lo = 0;
        hi = m->l_size;
        break;
    case 2:
        lo = 0;
        hi = luaL_checkint(L, 2);
        if ((hi > m->l_size) || (hi > m->r_size))
            return slice_out(L);
        break;
    case 3:
        lo = luaL_checkint(L, 2);
        hi = luaL_checkint(L, 3);
        if ((lo >= hi) ||
            (lo > m->l_size) || (lo > m->r_size) ||
            (hi > m->l_size) || (hi > m->r_size))
            return slice_out(L);
        break;
    default:
        return luaL_error(L, "matrix:eigen(): illegal arguments");
    }

    n = hi - lo;
    mx = gsl_matrix_submatrix(m->m, lo, lo, n, n);
    tmp = qlua_newMatReal(L, n, n);
    gsl_matrix_memcpy(tmp->m, &mx.matrix);
    lambda = qlua_newVecReal(L, n);
    trans = qlua_newMatReal(L, n, n);

    ev = new_gsl_vector(L, n);
    w = gsl_eigen_symmv_alloc(n);
    if (w == 0) {
        lua_gc(L, LUA_GCCOLLECT, 0);
        w = gsl_eigen_symmv_alloc(n);
        if (w == 0)
            luaL_error(L, "not enough memory");
    }
    
    if (gsl_eigen_symmv(tmp->m, ev, trans->m, w))
        luaL_error(L, "matrix:eigen() failed");

    if (gsl_eigen_symmv_sort(ev, trans->m, GSL_EIGEN_SORT_VAL_ASC))
        luaL_error(L, "matrix:eigen() eigenvalue ordering failed");

    for (i = 0; i < n; i++)
        lambda->val[i] = gsl_vector_get(ev, i);

    gsl_vector_free(ev);
    gsl_eigen_symmv_free(w);

    return 2;
}
int Holling2(double t, const double y[], double ydot[], void *params){

	double alpha	= 0.3;						// respiration
	double lambda	= 0.65;						// ecologic efficiency
	double hand	= 0.35;						// handling time
	double beta	= 0.5;						// intraspecific competition
	double aij	= 6.0;						// attack rate
	
	int i, j,l	= 0;						// Hilfsvariablen
	double rowsum	= 0;	
	double colsum	= 0;		  

//-- Struktur zerlegen-------------------------------------------------------------------------------------------------------------------------------

  	struct foodweb *nicheweb = (struct foodweb *)params;			// pointer cast from (void*) to (struct foodweb*)
	//printf("t in Holling 2=%f\n", t);
	gsl_vector *network = (nicheweb->network);						// Inhalt: A+linksA+Y+linksY+Massen+Trophische_Level = (Rnum+S)²+1+Y²+1+(Rnum+S)+S

	int S 	 	= nicheweb->S;
	int Y 	 	= nicheweb->Y;
	int Rnum	= nicheweb->Rnum;
	double d  	= nicheweb->d;
	int Z 		= nicheweb->Z;
	double dij 	= pow(10, d);

	double nu,mu, tau;
	
	int SpeciesNumber;
	
	tau =  gsl_vector_get(nicheweb->migrPara,0);
	
	mu = gsl_vector_get(nicheweb->migrPara,1);
	if((int)nu!=0)
	{
	  //printf("nu ist nicht null sondern %f\n",nu);
	}
	
	nu = gsl_vector_get(nicheweb->migrPara,2);
	
	SpeciesNumber = gsl_vector_get(nicheweb->migrPara,3);
	double tlast = gsl_vector_get(nicheweb->migrPara,4);
	
 	if(SpeciesNumber!= 0)
	{
	  //printf("SpeciesNumber %i\n", SpeciesNumber);
	}
	  //printf("t oben %f\n",t);
		//int len	 = (Rnum+S)*(Rnum+S)+2+Y*Y+(Rnum+S)+S;
	
	gsl_vector_view A_view = gsl_vector_subvector(network, 0, (Rnum+S)*(Rnum+S));						// Fressmatrix A als Vektor
	gsl_matrix_view EA_mat = gsl_matrix_view_vector(&A_view.vector, (Rnum+S), (Rnum+S));				// A als Matrix_view
	gsl_matrix *EAmat	   = &EA_mat.matrix;															// A als Matrix

	gsl_vector_view D_view = gsl_vector_subvector(network, (Rnum+S)*(Rnum+S)+1, Y*Y);					// Migrationsmatrix D als Vektor
	gsl_matrix_view ED_mat = gsl_matrix_view_vector(&D_view.vector, Y, Y);								// D als Matrixview
	gsl_matrix *EDmat	   = &ED_mat.matrix;		// D als Matrix
	
	
	gsl_vector_view M_vec  = gsl_vector_subvector(network, ((Rnum+S)*(Rnum+S))+1+(Y*Y)+1, (Rnum+S));	// Massenvektor
	gsl_vector *Mvec	   = &M_vec.vector;
	
	
 //-- verändere zu dem gewünschten Zeitpunkt Migrationsmatrix	
	
	if( (t > tau) && (tlast < tau))
	{	
	    //printf("mu ist %f\n", gsl_vector_get(nicheweb->migrPara,1));
	    //printf("nu ist %f\n", nu);
	    gsl_vector_set(nicheweb->migrPara,4,t);

	    //printf("Setze Link für gewünschte Migration\n");
	    //printf("t oben %f\n",t);
	    gsl_matrix_set(EDmat, nu, mu, 1.);
	    int m;
// 	    for(l = 0; l< Y;l++)
// 	    {
// 		for(m=0;m<Y;m++)
// 		{
// 		  printf("%f\t",gsl_matrix_get(EDmat,l,m));
// 		}
// 	     printf("\n");
// 	    }
	}
	else
	{
	  gsl_matrix_set_zero(EDmat);
	}
	

	


			
// 			printf("\ncheckpoint Holling2 I\n");
// 			printf("\nS = %i\n", S);
// 			printf("\nS + Rnum = %i\n", S+Rnum);
// 
// 			printf("\nSize A_view = %i\n", (int)A_view.vector.size);
// 			printf("\nSize D_view = %i\n", (int)D_view.vector.size);
// 			printf("\nSize M_vec  = %i\n", (int)M_vec.vector.size);


// 			for(i=0; i<(Rnum+S)*Y; i++){
// 				printf("\ny = %f\n", y[i]);
// 				}

// 			for(i=0; i<(Rnum+S)*Y; i++){
// 			printf("\nydot = %f\n", ydot[i]);
// 			}
		

//--zusätzliche Variablen anlegen-------------------------------------------------------------------------------------------------------------

  double ytemp[(Rnum+S)*Y];		 
	for(i=0; i<(Rnum+S)*Y; i++) ytemp[i] = y[i];							// temp array mit Kopie der Startwerte
 	
  for(i=0; i<(Rnum+S)*Y; i++) ydot[i] = 0;									// Ergebnis, in das evolve_apply schreibt
 						
  gsl_vector_view yfddot_vec	= gsl_vector_view_array(ydot, (Rnum+S)*Y);		//Notiz: vector_view_array etc. arbeiten auf den original Daten der ihnen zugeordneten Arrays/Vektoren!
  gsl_vector *yfddotvec		= &yfddot_vec.vector;							// zum einfacheren Rechnen ydot über vector_view_array ansprechen
  
  gsl_vector_view yfd_vec	= gsl_vector_view_array(ytemp, (Rnum+S)*Y);
  gsl_vector *yfdvec		= &yfd_vec.vector;								// Startwerte der Populationen

//-- neue Objekte zum Rechnen anlegen--------------------------------------------------------------------------------------------------------

  gsl_matrix *AFgsl	= gsl_matrix_calloc(Rnum+S, Rnum+S);	// matrix of foraging efforts
  gsl_matrix *ADgsl	= gsl_matrix_calloc(Y,Y); 				// matrix of migration efforts
  
  gsl_matrix *Emat	= gsl_matrix_calloc(Rnum+S, Rnum+S);	// gsl objects for calculations of populations 
  gsl_vector *tvec	= gsl_vector_calloc(Rnum+S);
  gsl_vector *rvec	= gsl_vector_calloc(Rnum+S);
  gsl_vector *svec	= gsl_vector_calloc(Rnum+S);
  
  gsl_matrix *Dmat	= gsl_matrix_calloc(Y,Y);				// gsl objects for calculations of migration
  gsl_vector *d1vec	= gsl_vector_calloc(Y);
  gsl_vector *d2vec	= gsl_vector_calloc(Y);
  gsl_vector *d3vec	= gsl_vector_calloc(Y);
  
//	printf("\ncheckpoint Holling2 III\n");

//-- Einzelne Patches lösen------------------------------------------------------------------------------------------------------------    
  for(l=0; l<Y; l++)								// start of patch solving
  {
    gsl_matrix_set_zero(AFgsl);						// Objekte zum Rechnen vor jedem Patch nullen 
    gsl_matrix_set_zero(Emat);
    gsl_vector_set_zero(tvec);
    gsl_vector_set_zero(rvec);
    gsl_vector_set_zero(svec);
    
    gsl_vector_view ydot_vec = gsl_vector_subvector(yfddotvec, (Rnum+S)*l, (Rnum+S));	// enthält ydot von Patch l
    gsl_vector *ydotvec 	 = &ydot_vec.vector;

    gsl_vector_view y_vec	 = gsl_vector_subvector(yfdvec, (Rnum+S)*l, (Rnum+S));		// enthält Startwerte der Population in l
    gsl_vector *yvec 		 = &y_vec.vector;
    
    gsl_matrix_memcpy(AFgsl, EAmat);
    
    for(i=0; i<Rnum+S; i++)
    {
      gsl_vector_view rowA   = gsl_matrix_row(AFgsl,i);
      				  rowsum = gsl_blas_dasum(&rowA.vector);
      if(rowsum !=0 )
      {
		for(j=0; j<Rnum+S; j++)
	    gsl_matrix_set(AFgsl, i, j, (gsl_matrix_get(AFgsl,i,j)/rowsum));				// normiere Beute Afgsl = A(Beutelinks auf 1 normiert) = f(i,j)
      }
    }
    
    gsl_matrix_memcpy(Emat, EAmat);									//  Emat = A
    gsl_matrix_scale(Emat, aij);									//  Emat(i,j) = a(i,j)
    gsl_matrix_mul_elements(Emat, AFgsl);							//  Emat(i,j) = a(i,j)*f(i,j)

    gsl_vector_memcpy(svec, yvec);									// s(i) = y(i)
    gsl_vector_scale(svec, hand);									// s(i) = y(i)*h
    gsl_blas_dgemv(CblasNoTrans, 1, Emat, svec, 0, rvec);			// r(i) = Sum_k h*a(i,k)*f(i,k)*y(k)
    gsl_vector_add_constant(rvec, 1);								// r(i) = 1+Sum_k h*a(i,k)*f(i,k)*y(k)
    	
    gsl_vector_memcpy(tvec, Mvec);									// t(i) = masse(i)^(-0.25)
    gsl_vector_div(tvec, rvec);										// t(i) = masse(i)^(-0.25)/(1+Sum_k h*a(i,k)*f(i,k)*y(k))
    gsl_vector_mul(tvec, yvec);										// t(i) = masse(i)^(-0.25)*y(i)/(1+Sum_k h*a(i,k)*f(i,k)*y(k))

    gsl_blas_dgemv(CblasTrans, 1, Emat, tvec, 0, rvec);				// r(i) = Sum_j a(j,i)*f(j,i)*t(j)
    gsl_vector_mul(rvec, yvec);										// r(i) = Sum_j a(j,i)*f(j,i)*t(j)*y(i) [rvec: Praedation]

    gsl_blas_dgemv(CblasNoTrans, lambda, Emat, yvec, 0, ydotvec);	// ydot(i) = Sum_j lambda*a(i,j)*f(i,j)*y(j)
    gsl_vector_mul(ydotvec, tvec);									// ydot(i) = Sum_j lambda*a(i,j)*f(i,j)*y(j)*t(i)
    
    gsl_vector_memcpy(svec, Mvec);
    gsl_vector_scale(svec, alpha);								// s(i) = alpha*masse^(-0.25) [svec=Respiration bzw. Mortalitaet]

    gsl_vector_memcpy(tvec, Mvec);
    gsl_vector_scale(tvec, beta);								// t(i) = beta*masse^(-0.25)
    gsl_vector_mul(tvec, yvec);									// t(i) = beta*y(i)
    gsl_vector_add(svec, tvec);									// s(i) = alpha*masse^(-0.25)+beta*y(i)
    	
    gsl_vector_mul(svec, yvec);									// s(i) = alpha*masse^(-0.25)*y(i)+beta*y(i)*y(i)
    gsl_vector_add(svec, rvec);									// [svec: Respiration, competition und Praedation]
    
    gsl_vector_sub(ydotvec, svec);								// ydot(i) = Fressen-Respiration-Competition-Praedation
    
    for(i=0; i<Rnum; i++)
      gsl_vector_set(ydotvec, i, 0.0);							// konstante Ressourcen
      
  }// Ende Einzelpatch, Ergebnis steht in ydotvec 

//	printf("\ncheckpoint Holling2 IV\n");
  
//-- Migration lösen---------------------------------------------------------------------------------------------------------    
  gsl_vector *ydottest	= gsl_vector_calloc(Y);
  double ydotmigr = gsl_vector_get(nicheweb->migrPara, 5);

  int count=0,m;
  for(l = 0; l< Y;l++)
  {
	for(m=0;m<Y;m++)
	{
	  count += gsl_matrix_get(EDmat,l,m);
	} 
  }
//   if(count!=0)
//   {
//     //printf("count %i\n",count);
//     //printf("t unten %f\n",t);
//     //printf("tau %f\n",tau);
//     for(l = 0; l< Y;l++)
//     {
// 	for(m=0;m<Y;m++)
// 	{
// 	  printf("%f\t",gsl_matrix_get(EDmat,l,m));
// 	}
//      printf("\n");
//      }
//   }
  double max = gsl_matrix_max(EDmat); 
  for(l = Rnum; l< Rnum+S; l++)								// start of migration solving
  {
    if(l == SpeciesNumber+Rnum && max !=0)
    {
      //printf("max ist %f\n",max);
      //printf("l ist %i\n",l);
      gsl_matrix_set_zero(ADgsl);								// reset gsl objects for every patch
      gsl_matrix_set_zero(Dmat);    
      gsl_vector_set_zero(d1vec);
      gsl_vector_set_zero(d2vec);
      gsl_vector_set_zero(d3vec);
      gsl_vector_set_zero(ydottest);

	// Untervektor von yfddot (enthält ydot[]) mit offset l (Rnum...Rnum+S) und Abstand zwischen den Elementen (stride) von Rnum+S.
	// Dies ergibt gerade die Größe einer Spezies in jedem Patch in einem Vektor
      gsl_vector_view dydot_vec = gsl_vector_subvector_with_stride(yfddotvec, l, (Rnum+S), Y);	// ydot[]		
      gsl_vector *dydotvec	  = &dydot_vec.vector;

      gsl_vector_view dy_vec	  = gsl_vector_subvector_with_stride(yfdvec, l, (Rnum+S), Y);			// Startgrößen der Spezies pro Patch
      gsl_vector *dyvec		  = &dy_vec.vector;
          
      gsl_matrix_memcpy(ADgsl, EDmat);		// ADgsl = D
    
      if(nicheweb->M == 1)				// umschalten w: patchwise (Abwanderung aus jedem Patch gleich), sonst linkwise (Abwanderung pro link gleich) 
	   {
		  for(i=0; i<Y; i++)
		   {
				gsl_vector_view colD = gsl_matrix_column(ADgsl, i);					// Spalte i aus Migrationsmatrix
							  colsum = gsl_blas_dasum(&colD.vector);
				if(colsum!=0)
					{
					  for(j=0;j<Y;j++)
					  gsl_matrix_set(ADgsl,j,i,(gsl_matrix_get(ADgsl,j,i)/colsum));		// ADgsl: D mit normierten Links
					}
		    }
	   }

      gsl_matrix_memcpy(Dmat, EDmat);					// Dmat = D
      gsl_matrix_scale(Dmat, dij);					// Dmat(i,j) = d(i,j) (Migrationsstärke)
      gsl_matrix_mul_elements(Dmat, ADgsl);				// Dmat(i,j) = d(i,j)*xi(i,j)   (skalierte und normierte Migrationsmatrix)
     
      gsl_vector_set_all(d1vec, 1/gsl_vector_get(Mvec, l));		// d1(i)= m(l)^0.25
      gsl_vector_mul(d1vec, dyvec);					// d1(i)= m(l)^0.25*y(i)
      gsl_blas_dgemv(CblasNoTrans, 1, Dmat, d1vec, 0, d2vec);		// d2(i)= Sum_j d(i,j)*xi(i,j)*m(l)^0.25*y(j)
    
      gsl_vector_set_all(d1vec, 1);					// d1(i)= 1
      gsl_blas_dgemv(CblasTrans, 1, Dmat, d1vec, 0, d3vec);		// d3(i)= Sum_j d(i,j)*xi(i,j)
      gsl_vector_scale(d3vec, 1/gsl_vector_get(Mvec,l));			// d3(i)= Sum_j d(i,j)*xi(i,j)*m(l)^0.25
      gsl_vector_mul(d3vec, dyvec);					// d3(i)= Sum_j d(i,j)*xi(i,j)*m(l)^0.25*y(i)
    
      gsl_vector_add(ydottest,d2vec);
      gsl_vector_sub(ydottest,d3vec);
      //printf("d2vec ist %f\n",gsl_vector_get(d2vec,0));
      //printf("d3vec ist %f\n",gsl_vector_get(d3vec,0));
      //if(gsl_vector_get(ydottest,mu)!=0)
      //{
      ydotmigr += gsl_vector_get(ydottest,nu);
      
      
      gsl_vector_set(nicheweb->migrPara,5,ydotmigr);
      //printf("ydotmigr ist %f\n",gsl_vector_get(nicheweb->migrPara,5));
//     if(ydotmigr !=0)
//     {
//       printf("ydottest aufaddiert ist %f\n",ydotmigr);
//       printf("ydottest aufaddiert ist %f\n",gsl_vector_get(nicheweb->migrPara,5));
//     }
    
    
    
      gsl_vector_add(dydotvec, d2vec);				// 
      gsl_vector_sub(dydotvec, d3vec);				// Ergebnis in dydotvec (also ydot[]) = Sum_j d(i,j)*xi(i,j)*m(l)^0.25*y(j) - Sum_j d(i,j)*xi(i,j)*m(l)^0.25*y(i) 
      }
  }// Patch i gewinnt das was aus allen j Patches zuwandert und verliert was von i auswandert
  //printf("ydot ist %f\n",gsl_vector_get(ydottest,0));

	//printf("\ncheckpoint Holling2 V\n");

	/*
	for(i=0; i<(Rnum+S)*Y; i++){
		printf("\ny = %f\tydot=%f\n", y[i], ydot[i]);
		}
    */
//--check for fixed point attractor-----------------------------------------------------------------------------------
	
	if(t>7800){

		gsl_vector_set(nicheweb->fixpunkte, 0, 0);	
		gsl_vector_set(nicheweb->fixpunkte, 1, 0);
		gsl_vector_set(nicheweb->fixpunkte, 2, 0);		 

		int fix0 = (int)gsl_vector_get(nicheweb->fixpunkte, 0);
		int fix1 = (int)gsl_vector_get(nicheweb->fixpunkte, 1);
		int fix2 = (int)gsl_vector_get(nicheweb->fixpunkte, 2);


	//printf("t unten = %f\n", t);
	
		for(i=0; i<(Rnum+S)*Y; i++)
		  {
			  if(y[i] <= 0)
			  {
				fix0++;
				fix1++;
				fix2++;
			  }
			  else 
			  {
				if((ydot[i]/y[i]<0.0001) || (ydot[i]<0.0001)) fix0++;
				if(ydot[i]/y[i]<0.0001) fix1++;
				if(ydot[i]<0.0001) fix2++;
			  }
		  }

    if(fix0==(Rnum+S)*Y) gsl_vector_set(nicheweb->fixpunkte, 3, 1);
    if(fix1==(Rnum+S)*Y) gsl_vector_set(nicheweb->fixpunkte, 4, 1);
    if(fix2==(Rnum+S)*Y) gsl_vector_set(nicheweb->fixpunkte, 5, 1);
  }

//--Speicher leeren----------------------------------------------------------------------------------------------------- 

  gsl_matrix_free(Emat);  
  gsl_matrix_free(Dmat);  
  gsl_matrix_free(AFgsl);  
  gsl_matrix_free(ADgsl);
  
  gsl_vector_free(tvec);
  gsl_vector_free(rvec);
  gsl_vector_free(svec);
  gsl_vector_free(d1vec);
  gsl_vector_free(d2vec);
  gsl_vector_free(d3vec);
  gsl_vector_free(ydottest);
  
//	printf("\nCheckpoint Holling2 VI\n");

  return GSL_SUCCESS;

}
示例#9
0
static int
set (void *vstate, gsl_multifit_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_matrix * J, gsl_vector * dx, int scale)
{
  lmder_state_t *state = (lmder_state_t *) vstate;

  gsl_matrix *r = state->r;
  gsl_vector *tau = state->tau;
  gsl_vector *diag = state->diag;
  gsl_vector *work1 = state->work1;
  gsl_permutation *perm = state->perm;

  int signum;

  /* Evaluate function at x */
  /* return immediately if evaluation raised error */
  {
    int status = GSL_MULTIFIT_FN_EVAL_F_DF (fdf, x, f, J);
    if (status)
      return status;
  }

  state->par = 0;
  state->iter = 1;
  state->fnorm = enorm (f);

  gsl_vector_set_all (dx, 0.0);

  /* store column norms in diag */

  if (scale)
    {
      compute_diag (J, diag);
    }
  else
    {
      gsl_vector_set_all (diag, 1.0);
    }

  /* set delta to 100 |D x| or to 100 if |D x| is zero */

  state->xnorm = scaled_enorm (diag, x);
  state->delta = compute_delta (diag, x);

  /* Factorize J into QR decomposition */

  gsl_matrix_memcpy (r, J);
  gsl_linalg_QRPT_decomp (r, tau, perm, &signum, work1);

  gsl_vector_set_zero (state->rptdx);
  gsl_vector_set_zero (state->w);

  /* Zero the trial vector, as in the alloc function */

  gsl_vector_set_zero (state->f_trial);

#ifdef DEBUG
  printf("r = "); gsl_matrix_fprintf(stdout, r, "%g");
  printf("perm = "); gsl_permutation_fprintf(stdout, perm, "%d");
  printf("tau = "); gsl_vector_fprintf(stdout, tau, "%g");
#endif

  return GSL_SUCCESS;
}
示例#10
0
int main(int argc, char **argv) {
	const int MAX_ITER  = 50;
	const double RELTOL = 1e-2;
	const double ABSTOL = 1e-4;

	/*
	 * Some bookkeeping variables for MPI. The 'rank' of a process is its numeric id
	 * in the process pool. For example, if we run a program via `mpirun -np 4 foo', then
	 * the process ranks are 0 through 3. Here, N and size are the total number of processes 
	 * running (in this example, 4).
	 */

	int rank;
	int size;

	MPI_Init(&argc, &argv);               // Initialize the MPI execution environment
	MPI_Comm_rank(MPI_COMM_WORLD, &rank); // Determine current running process
	MPI_Comm_size(MPI_COMM_WORLD, &size); // Total number of processes
	double N = (double) size;             // Number of subsystems/slaves for ADMM

	/* Read in local data */

	int skinny;           // A flag indicating whether the matrix A is fat or skinny
	FILE *f;
	int m, n;
	int row, col;
	double entry;

	/*
	 * Subsystem n will look for files called An.dat and bn.dat 
	 * in the current directory; these are its local data and do not need to be
	 * visible to any other processes. Note that
	 * m and n here refer to the dimensions of the *local* coefficient matrix.
	 */

	/* Read A */
	char s[20];
	sprintf(s, "data/A%d.dat", rank + 1);
	printf("[%d] reading %s\n", rank, s);

	f = fopen(s, "r");
	if (f == NULL) {
		printf("[%d] ERROR: %s does not exist, exiting.\n", rank, s);
		exit(EXIT_FAILURE);
	}
	mm_read_mtx_array_size(f, &m, &n);	
	gsl_matrix *A = gsl_matrix_calloc(m, n);
	for (int i = 0; i < m*n; i++) {
		row = i % m;
		col = floor(i/m);
		fscanf(f, "%lf", &entry);
		gsl_matrix_set(A, row, col, entry);
	}
	fclose(f);

	/* Read b */
	sprintf(s, "data/b%d.dat", rank + 1);
	printf("[%d] reading %s\n", rank, s);

	f = fopen(s, "r");
	if (f == NULL) {
		printf("[%d] ERROR: %s does not exist, exiting.\n", rank, s);
		exit(EXIT_FAILURE);
	}
	mm_read_mtx_array_size(f, &m, &n);
	gsl_vector *b = gsl_vector_calloc(m);
	for (int i = 0; i < m; i++) {
		fscanf(f, "%lf", &entry);
		gsl_vector_set(b, i, entry);
	}
	fclose(f);

	m = A->size1;
	n = A->size2;
	skinny = (m >= n);

	/*
	 * These are all variables related to ADMM itself. There are many
	 * more variables than in the Matlab implementation because we also
	 * require vectors and matrices to store various intermediate results.
	 * The naming scheme follows the Matlab version of this solver.
	 */ 

	double rho = 1.0;

	gsl_vector *x      = gsl_vector_calloc(n);
	gsl_vector *u      = gsl_vector_calloc(n);
	gsl_vector *z      = gsl_vector_calloc(n);
	gsl_vector *y      = gsl_vector_calloc(n);
	gsl_vector *r      = gsl_vector_calloc(n);
	gsl_vector *zprev  = gsl_vector_calloc(n);
	gsl_vector *zdiff  = gsl_vector_calloc(n);

	gsl_vector *q      = gsl_vector_calloc(n);
	gsl_vector *w      = gsl_vector_calloc(n);
	gsl_vector *Aq     = gsl_vector_calloc(m);
	gsl_vector *p      = gsl_vector_calloc(m);

	gsl_vector *Atb    = gsl_vector_calloc(n);

	double send[3]; // an array used to aggregate 3 scalars at once
	double recv[3]; // used to receive the results of these aggregations

	double nxstack  = 0;
	double nystack  = 0;
	double prires   = 0;
	double dualres  = 0;
	double eps_pri  = 0;
	double eps_dual = 0;

	/* Precompute and cache factorizations */

	gsl_blas_dgemv(CblasTrans, 1, A, b, 0, Atb); // Atb = A^T b

	/*
	 * The lasso regularization parameter here is just hardcoded
	 * to 0.5 for simplicity. Using the lambda_max heuristic would require 
	 * network communication, since it requires looking at the *global* A^T b.
	 */

	double lambda = 0.5;
	if (rank == 0) {
		printf("using lambda: %.4f\n", lambda);
	}

	gsl_matrix *L;

	/* Use the matrix inversion lemma for efficiency; see section 4.2 of the paper */
	if (skinny) {
		/* L = chol(AtA + rho*I) */
		L = gsl_matrix_calloc(n,n);

		gsl_matrix *AtA = gsl_matrix_calloc(n,n);
		gsl_blas_dsyrk(CblasLower, CblasTrans, 1, A, 0, AtA);

		gsl_matrix *rhoI = gsl_matrix_calloc(n,n);
		gsl_matrix_set_identity(rhoI);
		gsl_matrix_scale(rhoI, rho);

		gsl_matrix_memcpy(L, AtA);
		gsl_matrix_add(L, rhoI);
		gsl_linalg_cholesky_decomp(L);

		gsl_matrix_free(AtA);
		gsl_matrix_free(rhoI);
	} else {
		/* L = chol(I + 1/rho*AAt) */
		L = gsl_matrix_calloc(m,m);

		gsl_matrix *AAt = gsl_matrix_calloc(m,m);
		gsl_blas_dsyrk(CblasLower, CblasNoTrans, 1, A, 0, AAt);
		gsl_matrix_scale(AAt, 1/rho);

		gsl_matrix *eye = gsl_matrix_calloc(m,m);
		gsl_matrix_set_identity(eye);

		gsl_matrix_memcpy(L, AAt);
		gsl_matrix_add(L, eye);
		gsl_linalg_cholesky_decomp(L);

		gsl_matrix_free(AAt);
		gsl_matrix_free(eye);
	}

	/* Main ADMM solver loop */

	int iter = 0;
	if (rank == 0) {
		printf("%3s %10s %10s %10s %10s %10s\n", "#", "r norm", "eps_pri", "s norm", "eps_dual", "objective");		
	}
    double startAllTime, endAllTime;
	startAllTime = MPI_Wtime();
	while (iter < MAX_ITER) {

        /* u-update: u = u + x - z */
		gsl_vector_sub(x, z);
		gsl_vector_add(u, x);

		/* x-update: x = (A^T A + rho I) \ (A^T b + rho z - y) */
		gsl_vector_memcpy(q, z);
		gsl_vector_sub(q, u);
		gsl_vector_scale(q, rho);
		gsl_vector_add(q, Atb);   // q = A^T b + rho*(z - u)


        double tmp, tmpq;
		gsl_blas_ddot(x, x, &tmp);
		gsl_blas_ddot(q, q, &tmpq);

		if (skinny) {
			/* x = U \ (L \ q) */
			gsl_linalg_cholesky_solve(L, q, x);
		} else {
			/* x = q/rho - 1/rho^2 * A^T * (U \ (L \ (A*q))) */
			gsl_blas_dgemv(CblasNoTrans, 1, A, q, 0, Aq);
			gsl_linalg_cholesky_solve(L, Aq, p);
			gsl_blas_dgemv(CblasTrans, 1, A, p, 0, x); /* now x = A^T * (U \ (L \ (A*q)) */
			gsl_vector_scale(x, -1/(rho*rho));
			gsl_vector_scale(q, 1/rho);
			gsl_vector_add(x, q);
		}

		/*
		 * Message-passing: compute the global sum over all processors of the
		 * contents of w and t. Also, update z.
		 */

		gsl_vector_memcpy(w, x);
		gsl_vector_add(w, u);   // w = x + u

		gsl_blas_ddot(r, r, &send[0]); 
		gsl_blas_ddot(x, x, &send[1]);
		gsl_blas_ddot(u, u, &send[2]);
		send[2] /= pow(rho, 2);

		gsl_vector_memcpy(zprev, z);

		// could be reduced to a single Allreduce call by concatenating send to w
		MPI_Allreduce(w->data, z->data,  n, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
		MPI_Allreduce(send,    recv,     3, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);

		prires  = sqrt(recv[0]);  /* sqrt(sum ||r_i||_2^2) */
		nxstack = sqrt(recv[1]);  /* sqrt(sum ||x_i||_2^2) */
		nystack = sqrt(recv[2]);  /* sqrt(sum ||y_i||_2^2) */

		gsl_vector_scale(z, 1/N);
		soft_threshold(z, lambda/(N*rho));

		/* Termination checks */

		/* dual residual */
		gsl_vector_memcpy(zdiff, z);
		gsl_vector_sub(zdiff, zprev);
		dualres = sqrt(N) * rho * gsl_blas_dnrm2(zdiff); /* ||s^k||_2^2 = N rho^2 ||z - zprev||_2^2 */

		/* compute primal and dual feasibility tolerances */
		eps_pri  = sqrt(n*N)*ABSTOL + RELTOL * fmax(nxstack, sqrt(N)*gsl_blas_dnrm2(z));
		eps_dual = sqrt(n*N)*ABSTOL + RELTOL * nystack;

		if (rank == 0) {
			printf("%3d %10.4f %10.4f %10.4f %10.4f %10.4f\n", iter, 
					prires, eps_pri, dualres, eps_dual, objective(A, b, lambda, z));
		}

		if (prires <= eps_pri && dualres <= eps_dual) {
			break;
		}

		/* Compute residual: r = x - z */
		gsl_vector_memcpy(r, x);
		gsl_vector_sub(r, z);

		iter++;
	}

	/* Have the master write out the results to disk */
	if (rank == 0) { 
        endAllTime = MPI_Wtime();
        printf("Elapsed time is: %lf \n", endAllTime - startAllTime);

		f = fopen("data/solution.dat", "w");
		gsl_vector_fprintf(f, z, "%lf");
		fclose(f);
	}

	MPI_Finalize(); /* Shut down the MPI execution environment */

	/* Clear memory */
	gsl_matrix_free(A);
	gsl_matrix_free(L);
	gsl_vector_free(b);
	gsl_vector_free(x);
	gsl_vector_free(u);
	gsl_vector_free(z);
	gsl_vector_free(y);
	gsl_vector_free(r);
	gsl_vector_free(w);
	gsl_vector_free(zprev);
	gsl_vector_free(zdiff);
	gsl_vector_free(q);
	gsl_vector_free(Aq);
	gsl_vector_free(Atb);
	gsl_vector_free(p);

	return EXIT_SUCCESS;
}
// measurement update (correction)
bool DiscreteExtendedKalmanFilter::updateMeasurement(const size_t step, const gsl_vector *actualMeasurement, const gsl_vector *input)
{
	if (!x_hat_ || /*!y_hat_ ||*/ !P_ || !K_) return false;

	const gsl_vector *h_eval = system_.evaluateMeasurementEquation(step, x_hat_, input, NULL);  // h = h(k, x(k), u(k), 0)

	const gsl_matrix *Cd = system_.getOutputMatrix(step, x_hat_);  // Cd(k) = dh(k, x-(k), u(k), 0)/dx
#if 0
	const gsl_matrix *V = system_.getMeasurementNoiseCouplingMatrix(step);  // V(k) = dh(k, x-(k), u(k), 0)/dv
	const gsl_matrix *R = system_.getMeasurementNoiseCovarianceMatrix(step);  // R(k)
#else
	const gsl_matrix *Rd = system_.getMeasurementNoiseCovarianceMatrix(step);  // Rd(k) = V(k) * R(k) * V(k)^T
#endif
	if (!Cd || !Rd || !h_eval || !actualMeasurement) return false;

	// 1. calculate Kalman gain: K(k) = P-(k) * Cd(k)^T * (Cd(k) * P-(k) * Cd(k)^T + Rd(k))^-1 where Cd(k) = dh(k, x-(k), u(k), 0)/dx, Rd(k) = V(k) * R(k) * V(k)^T, V(k) = dh(k, x-(k), u(k), 0)/dv
	// inverse of matrix using LU decomposition
	gsl_matrix_memcpy(RR_, Rd);
	if (GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, P_, Cd, 0.0, PCt_) ||
		GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Cd, PCt_, 1.0, RR_))
		return false;

	int signum;
	if (GSL_SUCCESS != gsl_linalg_LU_decomp(RR_, permutation_, &signum) ||
		GSL_SUCCESS != gsl_linalg_LU_invert(RR_, permutation_, invRR_))
		return false;

	if (GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, PCt_, invRR_, 0.0, K_))  // calculate Kalman gain
		return false;

	// 2. update measurement: x(k) = x-(k) + K(k) * (y_tilde(k) - y_hat(k)) where y_hat(k) = h(k, x-(k), u(k), 0)
#if 0
	// save an estimated measurement, y_hat
	gsl_vector_memcpy(y_hat_, h_eval);
	gsl_vector_memcpy(residual_, y_hat_);
	if (GSL_SUCCESS != gsl_vector_sub(residual_, actualMeasurement) ||  // calculate residual = y_tilde(k) - y_hat(k)
		GSL_SUCCESS != gsl_blas_dgemv(CblasNoTrans, -1.0, K_, residual_, 1.0, x_hat_))  // calculate x_hat(k)
		return false;
#else
	gsl_vector_memcpy(residual_, h_eval);
	if (GSL_SUCCESS != gsl_vector_sub(residual_, actualMeasurement) ||  // calculate residual = y_tilde(k) - y_hat(k)
		GSL_SUCCESS != gsl_blas_dgemv(CblasNoTrans, -1.0, K_, residual_, 1.0, x_hat_))  // calculate x_hat(k)
		return false;
#endif

	// 3. update covariance: P(k) = (I - K(k) * Cd(k)) * P-(k)
#if 0
	// not working
	gsl_matrix_set_identity(M_);
	if (GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, K_, Cd, 1.0, M_) ||
		GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, M_, P_, 0.0, P_))
		return false;
#else
	gsl_matrix_set_identity(M_);
	if (GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, K_, Cd, 1.0, M_) ||
		GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, M_, P_, 0.0, M2_))
		return false;
	gsl_matrix_memcpy(P_, M2_);
#endif

	// preserve symmetry of P
	gsl_matrix_transpose_memcpy(M_, P_);
	gsl_matrix_add(P_, M_);
	gsl_matrix_scale(P_, 0.5);

	return true;
}
示例#12
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);

      /* Form covariance matrix cov = (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));
            }
        }

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

      return GSL_SUCCESS;
    }
}
示例#13
0
static int
msbdf_update (void *vstate, const size_t dim, gsl_matrix * dfdy, double *dfdt,
              const double t, const double *y, const gsl_odeiv2_system * sys,
              gsl_matrix * M, gsl_permutation * p,
              const size_t iter, size_t * nJ, size_t * nM,
              const double tprev, const double failt,
              const double gamma, const double gammaprev, const double hratio)
{
  /* Evaluates Jacobian dfdy and updates iteration matrix M
     if criteria for update is met.
   */

  /* Jacobian is evaluated
     - at first step
     - if MSBDF_JAC_WAIT steps have been made without re-evaluation
     - in case of a convergence failure if
     --- change in gamma is small, or 
     --- convergence failure resulted in step size decrease
   */

  const double c = 0.2;
  const double gammarel = fabs (gamma / gammaprev - 1.0);

  if (*nJ == 0 || *nJ > MSBDF_JAC_WAIT ||
      (t == failt && (gammarel < c || hratio < 1.0)))
    {
#ifdef DEBUG
      printf ("-- evaluate jacobian\n");
#endif
      int s = GSL_ODEIV_JA_EVAL (sys, t, y, dfdy->data, dfdt);

      if (s == GSL_EBADFUNC)
        {
          return s;
        }

      if (s != GSL_SUCCESS)
        {
          msbdf_failurehandler (vstate, dim, t);
#ifdef DEBUG
          printf ("-- FAIL at jacobian function evaluation\n");
#endif
          return s;
        }

      /* Reset counter */

      *nJ = 0;
    }

  /* Iteration matrix M (and it's LU decomposition) is generated
     - at first step
     - if MSBDF_M_WAIT steps have been made without an update
     - if change in gamma is significant (e.g. change in step size)
     - if previous step was rejected
   */

  if (*nM == 0 || *nM > MSBDF_M_WAIT || gammarel >= c ||
      t == tprev || t == failt)
    {
#ifdef DEBUG
      printf ("-- update M, gamma=%.5e\n", gamma);
#endif
      size_t i;
      gsl_matrix_memcpy (M, dfdy);
      gsl_matrix_scale (M, -gamma);

      for (i = 0; i < dim; i++)
        {
          gsl_matrix_set (M, i, i, gsl_matrix_get (M, i, i) + 1.0);
        }

      {
        int signum;
        int s = gsl_linalg_LU_decomp (M, p, &signum);
        
        if (s != GSL_SUCCESS)
          {
            return GSL_FAILURE;
          }
      }

      /* Reset counter */

      *nM = 0;
    }

  return GSL_SUCCESS;
}
示例#14
0
/* solve: min ||b - A x||^2 + lambda^2 ||x||^2 */
static int
test_COD_lssolve2_eps(const double lambda, const gsl_matrix * A, const gsl_vector * b, const double eps, const char *desc)
{
  int s = 0;
  size_t i, M = A->size1, N = A->size2;

  gsl_vector * lhs = gsl_vector_alloc(M);
  gsl_matrix * QRZT  = gsl_matrix_alloc(M, N);
  gsl_vector * tau_Q = gsl_vector_alloc(GSL_MIN(M, N));
  gsl_vector * tau_Z = gsl_vector_alloc(GSL_MIN(M, N));
  gsl_vector * work = gsl_vector_alloc(N);
  gsl_vector * x = gsl_vector_alloc(N);
  gsl_vector * x_aug = gsl_vector_alloc(N);
  gsl_vector * r = gsl_vector_alloc(M);
  gsl_vector * res = gsl_vector_alloc(M);
  gsl_permutation * perm = gsl_permutation_alloc(N);
  size_t rank;

  /* form full rank augmented system B = [ A ; lambda*I_N ], f = [ rhs ; 0 ] and solve with QRPT */
  {
    gsl_vector_view v;
    gsl_matrix_view m;
    gsl_permutation *p = gsl_permutation_alloc(N);
    gsl_matrix * B = gsl_matrix_calloc(M + N, N);
    gsl_vector * f = gsl_vector_calloc(M + N);
    gsl_vector * tau = gsl_vector_alloc(N);
    gsl_vector * residual = gsl_vector_alloc(M + N);
    int signum;
    
    m = gsl_matrix_submatrix(B, 0, 0, M, N);
    gsl_matrix_memcpy(&m.matrix, A);

    m = gsl_matrix_submatrix(B, M, 0, N, N);
    v = gsl_matrix_diagonal(&m.matrix);
    gsl_vector_set_all(&v.vector, lambda);

    v = gsl_vector_subvector(f, 0, M);
    gsl_vector_memcpy(&v.vector, b);

    /* solve: [ A ; lambda*I ] x_aug = [ b ; 0 ] */
    gsl_linalg_QRPT_decomp(B, tau, p, &signum, work);
    gsl_linalg_QRPT_lssolve(B, tau, p, f, x_aug, residual);

    gsl_permutation_free(p);
    gsl_matrix_free(B);
    gsl_vector_free(f);
    gsl_vector_free(tau);
    gsl_vector_free(residual);
  }

  gsl_matrix_memcpy(QRZT, A);

  s += gsl_linalg_COD_decomp(QRZT, tau_Q, tau_Z, perm, &rank, work);

  {
    gsl_matrix *S = gsl_matrix_alloc(rank, rank);
    gsl_vector *workr = gsl_vector_alloc(rank);

    s += gsl_linalg_COD_lssolve2(lambda, QRZT, tau_Q, tau_Z, perm, rank, b, x, res, S, workr);

    gsl_matrix_free(S);
    gsl_vector_free(workr);
  }

  for (i = 0; i < N; i++)
    {
      double xi = gsl_vector_get(x, i);
      double yi = gsl_vector_get(x_aug, i);
      gsl_test_rel(xi, yi, eps,
                   "%s (%3lu,%3lu)[%lu]: %22.18g   %22.18g\n",
                   desc, M, N, i, xi, yi);
    }

  /* compute residual r = b - A x */
  if (M == N)
    {
      gsl_vector_set_zero(r);
    }
  else
    {
      gsl_vector_memcpy(r, b);
      gsl_blas_dgemv(CblasNoTrans, -1.0, A, x, 1.0, r);
    }

  for (i = 0; i < N; i++)
    {
      double xi = gsl_vector_get(res, i);
      double yi = gsl_vector_get(r, i);

      gsl_test_rel(xi, yi, sqrt(eps),
                   "%s res (%3lu,%3lu)[%lu]: %22.18g   %22.18g\n",
                   desc, M, N, i, xi, yi);
    }

  gsl_vector_free(r);
  gsl_vector_free(res);
  gsl_vector_free(x);
  gsl_vector_free(x_aug);
  gsl_vector_free(tau_Q);
  gsl_vector_free(tau_Z);
  gsl_matrix_free(QRZT);
  gsl_vector_free(lhs);
  gsl_vector_free(work);
  gsl_permutation_free(perm);

  return s;
}
示例#15
0
 matrix<double>::matrix(const gsl_matrix* m)
 {
   _matrix = gsl_matrix_alloc(m->size1,m->size2);
   gsl_matrix_memcpy(_matrix, m);
 }
示例#16
0
 matrix<double>::matrix(const gsl_matrix& m)
 {
   _matrix = gsl_matrix_alloc(m.size1,m.size2);
   gsl_matrix_memcpy(_matrix, &m);
 }
示例#17
0
 /** Copy constructor  */
 matrix<double>::matrix(const matrix<double>& m)
 {
   _matrix = gsl_matrix_alloc(m.size_i(), m.size_j());
   gsl_matrix_memcpy(_matrix, m.as_gsl_type_ptr());
 }
示例#18
0
/** *******************************************************************************************************************************************/
int generate_gaus_rv_inits(gsl_vector *myBeta,struct fnparams *gparams){

    /** this is the SAME CODE as in the Gaussian case  */
    
    /** beta_hat= (X^T X)^{-1} X^T y **/
    const datamatrix *designdata = ((struct fnparams *) gparams)->designdata;/** all design data inc Y and priors **/
    
       const gsl_vector *Y = designdata->Y;/** response vector **/
       const gsl_matrix *X = designdata->datamatrix_noRV ;/** design matrix - with one too few cols! **/
       gsl_vector *vectmp1= gparams->vectmp1;/** numparams long*/
       gsl_vector *vectmp2 = gparams->vectmp2;/** numparams long*/
       gsl_matrix *mattmp2 = gparams->mattmp2;/** same dim as X*/
       gsl_matrix *mattmp3 = gparams->mattmp3;/** p x p **/
       gsl_matrix *mattmp4 = gparams->mattmp4;/** p x p **/
       gsl_vector *vectmp1long = gparams->vectmp1long;/** scratch space **/
       gsl_vector *vectmp2long = gparams->vectmp2long;/** scratch space **/
       gsl_permutation *perm = gparams->perm;
     unsigned int i;
     int ss;
     int haveError;
     double variance=0.0;
     double n=Y->size;/** no. observations **/
     double m=X->size2;/** number of coefficients excluding tau-precision */
     
    /*Rprintf("X: %d %d %d %d %d %d\n",X->size1,X->size2,mattmp2->size1,mattmp2->size2,mattmp3->size1,mattmp3->size2); */
    gsl_matrix_memcpy(mattmp2,X);
    gsl_blas_dgemm (CblasTrans, CblasNoTrans,    /** mattmp3 is p x p matrix X^T X **/
                       1.0, X, mattmp2,
                       0.0, mattmp3);
    
    gsl_permutation_init(perm);/** reset - might not be needed */                   
    gsl_linalg_LU_decomp(mattmp3,perm,&ss);
    gsl_set_error_handler_off();/**Turning off GSL Error handler as this may fail as mattmp3 may be singular */
    haveError=gsl_linalg_LU_invert (mattmp3, perm, mattmp4);/** mattmp4 is now inv (X^T X) */ 
    
    if(!haveError){/** no error */
      /** copy Y into vectmp1long and +1 and take logs since poisson has log link - this is a fudge */
      /*for(i=0;i<vectmp1long->size;i++){gsl_vector_set(vectmp1long,i,log(gsl_vector_get(Y,i)+DBL_MIN)/(log(1-gsl_vector_get(Y,i)+DBL_MIN)));}  */               
    /*for(i=0;i<vectmp1long->size;i++){gsl_vector_set(vectmp1long,i,log(gsl_vector_get(Y,i)+1)/(log(1-gsl_vector_get(Y,i)+1)));} */
    
    gsl_blas_dgemv (CblasTrans, 1.0, X, Y, 0.0, vectmp1);  /** X^T Y */
    gsl_blas_dgemv (CblasNoTrans, 1.0, mattmp4, vectmp1, 0.0, vectmp2);
    for(i=0;i<myBeta->size-2;i++){gsl_vector_set(myBeta,i,gsl_vector_get(vectmp2,i));} /** size myBeta->size-2 as last two entries are precisions **/
    } else {/** singular to set initial values all to zero **/ 
           Rprintf("caught gsl error - singular matrix in initial guess estimates\n"); 
           for(i=0;i<myBeta->size;i++){gsl_vector_set(myBeta,i,0.01);}}
    gsl_set_error_handler (NULL);/** restore the error handler*/
   /*Rprintf("inits\n");for(i=0;i<myBeta->size;i++){Rprintf("%10.15e ",gsl_vector_get(myBeta,i));} Rprintf("\n");*//** set to Least squares estimate */
     /** now for variance estimate */
    /** first get y_hat estimate */
  
    gsl_blas_dgemv (CblasNoTrans, 1.0, X, vectmp2, 0.0, vectmp1long); /** vectmp1 is y_hat */ 
    /*for(i=0;i<vectmp1long->size;i++){Rprintf("y_hat=%f\n",gsl_vector_get(vectmp1long,i));}*/
    /*error("");*/
    gsl_vector_scale(vectmp1long,-1.0);/** - y_hat */
    gsl_vector_add(vectmp1long,Y);/** now have Y-y_hat (or -y_hat + Y) */
    /*for(i=0;i<vectmp1long->size;i++){gsl_vector_set(vectmp1long,i,fabs(gsl_vector_get(vectmp1long,i)));}
    for(i=0;i<vectmp1long->size;i++){Rprintf("y_hat=%f\n",gsl_vector_get(vectmp1long,i));}
    for(i=0;i<vectmp1long->size;i++){gsl_vector_set(vectmp1long,i,log(gsl_vector_get(vectmp1long,i))/log(1-gsl_vector_get(vectmp1long,i)));}*/ /** errors on logit scale **/
    /*gsl_vector_set_all(vectmp2long,1);*/
    gsl_vector_memcpy(vectmp2long,vectmp1long);
    gsl_blas_ddot (vectmp1long, vectmp2long, &variance);/** got sum((Y-Y_hat)^2) */
    variance=variance/(n-m);/** unbiased estimator using denominator n-#term in regression equation **/
   /* Rprintf("variance estimator=%f precision=%f\n",variance,1/variance);*/
  /* variance=0.086;*/
    /*variance=exp(gsl_vector_get(myBeta,0))/(1+exp(gsl_vector_get(myBeta,0)));*/
    /** variance here is for plain glm - just split this 50/50 between residual error and group level variance **/
    
    gsl_vector_set(myBeta,myBeta->size-2,1.0/(0.5*variance));/** - estimate for rv precision **/
    gsl_vector_set(myBeta,myBeta->size-1,1.0/(0.5*variance));/** - estimate for residual precision **/
    /*gsl_vector_set(myBeta,myBeta->size-1,1.0/variance); */
    /*gsl_vector_set(myBeta,0,0.9 );gsl_vector_set(myBeta,1,0.9);gsl_vector_set(myBeta,2,1.5);*/
   
    #ifdef junk
    Rprintf("------------ TEMP: Using Fixed initial values from LME4----------------\n");
    gsl_vector_set(myBeta,0,0.062044233);/** intercept */
    gsl_vector_set(myBeta,1,-0.1229382094322);/** slope g2 */
    gsl_vector_set(myBeta,2,1.0/0.1570366587829);/** group level precision */ 
    gsl_vector_set(myBeta,3,1.0/0.8628565204966);/** residual precision */ 
    #endif
    /*Rprintf("inits\n");for(i=0;i<myBeta->size;i++){Rprintf("%10.15e ",gsl_vector_get(myBeta,i));} Rprintf("\n");*//** set to Least squares estimate */  
    
    
    
    
    return GSL_SUCCESS;
}   
示例#19
-1
void fnIMIS(const size_t InitSamples, const size_t StepSamples, const size_t FinalResamples, const size_t MaxIter, const size_t NumParam, unsigned long int rng_seed, const char * runName)
{

  // Declare and configure GSL RNG
  gsl_rng * rng;
  const gsl_rng_type * T;

  gsl_rng_env_setup();
  T = gsl_rng_default;
  rng = gsl_rng_alloc (T);
  gsl_rng_set(rng, rng_seed);

  char strDiagnosticsFile[strlen(runName) + 15 +1];
  char strResampleFile[strlen(runName) + 12 +1];
  strcpy(strDiagnosticsFile, runName); strcat(strDiagnosticsFile, "Diagnostics.txt");
  strcpy(strResampleFile, runName); strcat(strResampleFile, "Resample.txt");
  FILE * diagnostics_file = fopen(strDiagnosticsFile, "w");
  fprintf(diagnostics_file, "Seeded RNG: %zu\n", rng_seed);
  fprintf(diagnostics_file, "Running IMIS. InitSamples: %zu, StepSamples: %zu, FinalResamples %zu, MaxIter %zu\n", InitSamples, StepSamples, FinalResamples, MaxIter);

  // Setup IMIS arrays
  gsl_matrix * Xmat = gsl_matrix_alloc(InitSamples + StepSamples*MaxIter, NumParam);
  double * prior_all = (double*) malloc(sizeof(double) * (InitSamples + StepSamples*MaxIter));
  double * likelihood_all = (double*) malloc(sizeof(double) * (InitSamples + StepSamples*MaxIter));
  double * imp_weight_denom = (double*) malloc(sizeof(double) * (InitSamples + StepSamples*MaxIter));  // proportional to q(k) in stage 2c of Raftery & Bao
  double * gaussian_sum = (double*) calloc(InitSamples + StepSamples*MaxIter, sizeof(double));      // sum of mixture distribution for mode
  struct dst * distance = (struct dst *) malloc(sizeof(struct dst) * (InitSamples + StepSamples*MaxIter)); // Mahalanobis distance to most recent mode
  double * imp_weights = (double*) malloc(sizeof(double) * (InitSamples + StepSamples*MaxIter));
  double * tmp_MVNpdf = (double*) malloc(sizeof(double) * (InitSamples + StepSamples*MaxIter));

  gsl_matrix * nearestX = gsl_matrix_alloc(StepSamples, NumParam);
  double center_all[MaxIter][NumParam];
  gsl_matrix * sigmaChol_all[MaxIter];
  gsl_matrix * sigmaInv_all[MaxIter];

  // Initial prior samples
  sample_prior(rng, InitSamples, Xmat);

  // Calculate prior covariance
  double prior_invCov_diag[NumParam];
  /*
    The paper describing the algorithm uses the full prior covariance matrix.
    This follows the code in the IMIS R package and diagonalizes the prior 
    covariance matrix to ensure invertibility.
  */
  for(size_t i = 0; i < NumParam; i++){
    gsl_vector_view tmpCol = gsl_matrix_subcolumn(Xmat, i, 0, InitSamples);
    prior_invCov_diag[i] = gsl_stats_variance(tmpCol.vector.data, tmpCol.vector.stride, InitSamples);
    prior_invCov_diag[i] = 1.0/prior_invCov_diag[i];
  }

  // IMIS steps
  fprintf(diagnostics_file, "Step Var(w_i)  MargLik    Unique Max(w_i)     ESS     Time\n");
  printf("Step Var(w_i)  MargLik    Unique Max(w_i)     ESS     Time\n");
  time_t time1, time2;
  time(&time1);
  size_t imisStep = 0, numImisSamples;
  for(imisStep = 0; imisStep < MaxIter; imisStep++){
    numImisSamples = (InitSamples + imisStep*StepSamples);
    
    // Evaluate prior and likelihood
    if(imisStep == 0){ // initial stage
      #pragma omp parallel for
      for(size_t i = 0; i < numImisSamples; i++){
        gsl_vector_const_view theta = gsl_matrix_const_row(Xmat, i);
        prior_all[i] = prior(&theta.vector);
        likelihood_all[i] = likelihood(&theta.vector);
      }
    } else {  // imisStep > 0
      #pragma omp parallel for
      for(size_t i = InitSamples + (imisStep-1)*StepSamples; i < numImisSamples; i++){
        gsl_vector_const_view theta = gsl_matrix_const_row(Xmat, i);
        prior_all[i] = prior(&theta.vector);
        likelihood_all[i] = likelihood(&theta.vector);
      }
    }

    // Determine importance weights, find current maximum, calculate monitoring criteria

    #pragma omp parallel for
    for(size_t i = 0; i < numImisSamples; i++){
      imp_weight_denom[i] = (InitSamples*prior_all[i] + StepSamples*gaussian_sum[i])/(InitSamples + StepSamples * imisStep);
      imp_weights[i] = (prior_all[i] > 0)?likelihood_all[i]*prior_all[i]/imp_weight_denom[i]:0;
    }

    double sumWeights = 0.0;
    for(size_t i = 0; i < numImisSamples; i++){
      sumWeights += imp_weights[i];
    }

    double maxWeight = 0.0, varImpW = 0.0, entropy = 0.0, expectedUnique = 0.0, effSampSize = 0.0, margLik;
    size_t maxW_idx;
    #pragma omp parallel for reduction(+: varImpW, entropy, expectedUnique, effSampSize)
    for(size_t i = 0; i < numImisSamples; i++){
      imp_weights[i] /= sumWeights;
      varImpW += pow(numImisSamples * imp_weights[i] - 1.0, 2.0);
      entropy += imp_weights[i] * log(imp_weights[i]);
      expectedUnique += (1.0 - pow((1.0 - imp_weights[i]), FinalResamples));
      effSampSize += pow(imp_weights[i], 2.0);
    }

    for(size_t i = 0; i < numImisSamples; i++){
      if(imp_weights[i] > maxWeight){
        maxW_idx = i;
        maxWeight = imp_weights[i];
      }
    }
    for(size_t i = 0; i < NumParam; i++)
      center_all[imisStep][i] = gsl_matrix_get(Xmat, maxW_idx, i);

    varImpW /= numImisSamples;
    entropy = -entropy / log(numImisSamples);
    effSampSize = 1.0/effSampSize;
    margLik = log(sumWeights/numImisSamples);

    fprintf(diagnostics_file, "%4zu %8.2f %8.2f %8.2f %8.2f %8.2f %8.2f\n", imisStep, varImpW, margLik, expectedUnique, maxWeight, effSampSize, difftime(time(&time2), time1));
    printf("%4zu %8.2f %8.2f %8.2f %8.2f %8.2f %8.2f\n", imisStep, varImpW, margLik, expectedUnique, maxWeight, effSampSize, difftime(time(&time2), time1));
    time1 = time2;

    // Check for convergence
    if(expectedUnique > FinalResamples*(1.0 - exp(-1.0))){
      break;
    }

    // Calculate Mahalanobis distance to current mode
    GetMahalanobis_diag(Xmat, center_all[imisStep],  prior_invCov_diag, numImisSamples, NumParam, distance);

    // Find StepSamples nearest points
    // (Note: this was a major bottleneck when InitSamples and StepResamples are large. qsort substantially outperformed GSL sort options.)
    qsort(distance, numImisSamples, sizeof(struct dst), cmp_dst);

    #pragma omp parallel for
    for(size_t i = 0; i < StepSamples; i++){
      gsl_vector_const_view tmpX = gsl_matrix_const_row(Xmat, distance[i].idx);
      gsl_matrix_set_row(nearestX, i, &tmpX.vector);
    }

    // Calculate weighted covariance of nearestX

    // (a) Calculate weights for nearest points 1...StepSamples
    double weightsCov[StepSamples];
    #pragma omp parallel for
    for(size_t i = 0; i < StepSamples; i++){
      weightsCov[i] = 0.5*(imp_weights[distance[i].idx] + 1.0/numImisSamples); // cov_wt function will normalize the weights
    }

    // (b) Calculate weighted covariance
    sigmaChol_all[imisStep] = gsl_matrix_alloc(NumParam, NumParam);
    covariance_weighted(nearestX, weightsCov, StepSamples, center_all[imisStep], NumParam, sigmaChol_all[imisStep]);

    // (c) Do Cholesky decomposition and inverse of covariance matrix
    gsl_linalg_cholesky_decomp(sigmaChol_all[imisStep]);
    for(size_t j = 0; j < NumParam; j++) // Note: GSL outputs a symmetric matrix rather than lower tri, so have to set upper tri to zero
      for(size_t k = j+1; k < NumParam; k++)
        gsl_matrix_set(sigmaChol_all[imisStep], j, k, 0.0);

    sigmaInv_all[imisStep] = gsl_matrix_alloc(NumParam, NumParam);
    gsl_matrix_memcpy(sigmaInv_all[imisStep], sigmaChol_all[imisStep]);

    gsl_linalg_cholesky_invert(sigmaInv_all[imisStep]);

    // Sample new inputs
    gsl_matrix_view newSamples = gsl_matrix_submatrix(Xmat, numImisSamples, 0, StepSamples, NumParam);
    GenerateRandMVnorm(rng, StepSamples, center_all[imisStep], sigmaChol_all[imisStep], NumParam, &newSamples.matrix);

    // Evaluate sampling probability from mixture distribution
    // (a) For newly sampled points, sum over all previous centers
    for(size_t pastStep = 0; pastStep < imisStep; pastStep++){
      GetMVNpdf(&newSamples.matrix, center_all[pastStep], sigmaInv_all[pastStep], sigmaChol_all[pastStep], StepSamples, NumParam, tmp_MVNpdf);
      #pragma omp parallel for
      for(size_t i = 0; i < StepSamples; i++)
        gaussian_sum[numImisSamples + i] += tmp_MVNpdf[i];
    }
    // (b) For all points, add weight for most recent center
    gsl_matrix_const_view Xmat_curr = gsl_matrix_const_submatrix(Xmat, 0, 0, numImisSamples + StepSamples, NumParam);
    GetMVNpdf(&Xmat_curr.matrix, center_all[imisStep], sigmaInv_all[imisStep], sigmaChol_all[imisStep], numImisSamples + StepSamples, NumParam, tmp_MVNpdf);
    #pragma omp parallel for
    for(size_t i = 0; i < numImisSamples + StepSamples; i++)
      gaussian_sum[i] += tmp_MVNpdf[i];
  } // loop over imisStep

  //// FINISHED IMIS ROUTINE
  fclose(diagnostics_file);
  
  // Resample posterior outputs
  int resampleIdx[FinalResamples];
  walker_ProbSampleReplace(rng, numImisSamples, imp_weights, FinalResamples, resampleIdx); // Note: Random sampling routine used in R sample() function.
  
  // Print results
  FILE * resample_file = fopen(strResampleFile, "w");
  for(size_t i = 0; i < FinalResamples; i++){
    for(size_t j = 0; j < NumParam; j++)
      fprintf(resample_file, "%.15e\t", gsl_matrix_get(Xmat, resampleIdx[i], j));
    gsl_vector_const_view theta = gsl_matrix_const_row(Xmat, resampleIdx[i]);
    fprintf(resample_file, "\n");
  }
  fclose(resample_file);
  
  /*  
  // This outputs Xmat (parameter matrix), centers, and covariance matrices to files for debugging
  FILE * Xmat_file = fopen("Xmat.txt", "w");
  for(size_t i = 0; i < numImisSamples; i++){
    for(size_t j = 0; j < NumParam; j++)
      fprintf(Xmat_file, "%.15e\t", gsl_matrix_get(Xmat, i, j));
    fprintf(Xmat_file, "%e\t%e\t%e\t%e\t%e\t\n", prior_all[i], likelihood_all[i], imp_weights[i], gaussian_sum[i], distance[i]);
  }
  fclose(Xmat_file);
  
  FILE * centers_file = fopen("centers.txt", "w");
  for(size_t i = 0; i < imisStep; i++){
  for(size_t j = 0; j < NumParam; j++)
  fprintf(centers_file, "%f\t", center_all[i][j]);
  fprintf(centers_file, "\n");
  }
  fclose(centers_file);

  FILE * sigmaInv_file = fopen("sigmaInv.txt", "w");
  for(size_t i = 0; i < imisStep; i++){
  for(size_t j = 0; j < NumParam; j++)
  for(size_t k = 0; k < NumParam; k++)
  fprintf(sigmaInv_file, "%f\t", gsl_matrix_get(sigmaInv_all[i], j, k));
  fprintf(sigmaInv_file, "\n");
  }
  fclose(sigmaInv_file);
  */

  // free memory allocated by IMIS
  for(size_t i = 0; i < imisStep; i++){
    gsl_matrix_free(sigmaChol_all[i]);
    gsl_matrix_free(sigmaInv_all[i]);
  }

  // release RNG
  gsl_rng_free(rng);
  gsl_matrix_free(Xmat);
  gsl_matrix_free(nearestX);

  free(prior_all);
  free(likelihood_all);
  free(imp_weight_denom);
  free(gaussian_sum);
  free(distance);
  free(imp_weights);
  free(tmp_MVNpdf);

  return;
}