/* calculate percent error between A and B: 100*norm(A - B)/norm(A) */
double get_percent_error_between_two_mats(gsl_matrix *A, gsl_matrix *B){
    int m,n;
    double normA, normB, normA_minus_B;
    m = A->size1;
    n = A->size2;
    gsl_matrix *A_minus_B = gsl_matrix_alloc(m,n);
    gsl_matrix_memcpy(A_minus_B, A);
    gsl_matrix_sub(A_minus_B,B);
    normA = matrix_frobenius_norm(A);
    normB = matrix_frobenius_norm(B);
    normA_minus_B = matrix_frobenius_norm(A_minus_B);
    return 100.0*normA_minus_B/normA;
}
Ejemplo n.º 2
0
Matrix* 
Matrix::subtractMatrix(Matrix* b)
{
	if(!dimequal(b))
	{
		cout << "Dimensions do not match" << endl;
		return NULL;	
	}
	Matrix* res=new Matrix(row,col);
	gsl_matrix_memcpy (res->matrix, matrix);
	gsl_matrix_sub(res->matrix,b->matrix);
	return res;	
}
Ejemplo n.º 3
0
static int
rm_sub_rm(lua_State *L)
{
    mMatReal *a = qlua_checkMatReal(L, 1);
    mMatReal *b = qlua_checkMatReal(L, 2);
    int al = a->l_size;
    int ar = a->r_size;
    mMatReal *r = qlua_newMatReal(L, al, ar);

    if ((al != b->l_size) || (ar != b->r_size))
        return luaL_error(L, "matrix sizes mismatch in m - m");

    gsl_matrix_memcpy(r->m, a->m);
    gsl_matrix_sub(r->m, b->m);

    return 1;
}
Ejemplo n.º 4
0
float CalcEuclideanNorm(const gsl_matrix * const A,
			const gsl_matrix * const B,
			gsl_matrix * const R1,
			gsl_matrix * const R2,
			const gsl_matrix * const Assg,
			const gsl_matrix * const AssgT)
{
	unsigned int i, j;
	float score = 0;
	float variance;

	// Use AssgT as Assg for multiplication
	MulGSLMatrices(Assg, A, R1);
	// PrintGSLMatrix(A, "1 - A");
	// PrintGSLMatrix(R, "1 - R = Assg x A");

	// Compute ScoreMatrix
	MulGSLMatrices(R1, AssgT, R2);
	// PrintGSLMatrix(AssgT, "AssgT");
	// PrintGSLMatrix(R, "2 - R = Assg x A x AssgT");

	gsl_matrix_sub(R2, B);
	// PrintGSLMatrix(B, "3 - B");
	// PrintGSLMatrix(R, "3 - R = Assg x A x AssgT - B");
	
	// Compute variance (for normalization)
	variance = CalcVariance(R2);

	for(i = 0; i < R2->size1; i++)
	{
		for(j = 0; j < R2->size2; j++)
		{
			float val = gsl_matrix_get(R2, i, j);
			// fprintf(stderr, "[%d, %d] = %f\n", i, j, val);
			//if(variance == (float)0)
				score += val*val;
			//else
			//	score += ((val*val)/variance);
			//score += val*val;
		}
	}
	//score = sqrt(score);

	return score;
}
Ejemplo n.º 5
0
gsl_matrix* calc_Ws(double s, double tres, double* Qxx_m, double* Qyy_m, double* Qxy_m, double* Qyx_m, int kx, int ky){
   

   gsl_matrix *ws;
   ws=gsl_matrix_alloc(kx,kx); 
   gsl_matrix_set_identity(ws);
   gsl_matrix_scale(ws,s);
       
   //Calculate H(s)
   gsl_matrix *hs = calc_Hs(s,tres,Qxx_m,Qyy_m,Qxy_m,Qyx_m, kx, ky);
   gsl_matrix_sub(ws,hs);
   
   //cleanup
   gsl_matrix_free(hs); 
   return ws;
    
    
}
Ejemplo n.º 6
0
Archivo: stage3b.c Proyecto: pa345/lib
int
solve_PCA(const size_t P, const gsl_matrix * knm,
          const gsl_matrix * U, gsl_matrix * alpha,
          gsl_matrix * knmt)
{
  int status = 0;
  const size_t nt = knm->size2; /* number of time stamps */
  const size_t nnm = U->size1;
  gsl_matrix *R;                /* R = knm - U*alpha */
  struct timeval tv0, tv1;
  double residual;           /* || knm - U*alpha || */
  int rank;

  /* select largest P eigenvectors of SDM */
  gsl_matrix_const_view Uv = gsl_matrix_const_submatrix(U, 0, 0, nnm, P);

  /* solve: U*alpha = Q */
  fprintf(stderr, "solve_PCA: solving PCA problem for alpha...");
  gettimeofday(&tv0, NULL);
  status = lapack_lls(&Uv.matrix, knm, alpha, &rank);
  gettimeofday(&tv1, NULL);

  /* compute: knm~ = U*alpha */
  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &Uv.matrix, alpha, 0.0, knmt);

  /* compute: R = knm - knm~ */
  R = gsl_matrix_alloc(nnm, nt);
  gsl_matrix_memcpy(R, knm);
  gsl_matrix_sub(R, knmt);

  residual = norm_fro(R);

  fprintf(stderr, "done (%g seconds, status = %d, rank = %d, residual = %.12e)\n",
          time_diff(tv0, tv1), status, rank, residual);

  gsl_matrix_free(R);

  return status;
}
Ejemplo n.º 7
0
double epidemicGrowthRate(const double theta[numParam], const double r0time, double * eigenvec)
{

  gsl_matrix * Fmat = gsl_matrix_calloc(NG*(DS-1)*RG, NG*(DS-1)*RG);
  gsl_matrix * Vmat = gsl_matrix_calloc(NG*(DS-1)*RG, NG*(DS-1)*RG);

  createNGM(theta, r0time, Fmat, Vmat);
  gsl_matrix_sub(Fmat, Vmat);

  gsl_eigen_nonsymmv_workspace * w = gsl_eigen_nonsymmv_alloc(NG*(DS-1)*RG);
  gsl_vector_complex * eval = gsl_vector_complex_alloc(NG*(DS-1)*RG);
  gsl_matrix_complex * evec = gsl_matrix_complex_alloc(NG*(DS-1)*RG, NG*(DS-1)*RG);

  gsl_set_error_handler_off();
  gsl_eigen_nonsymmv(Fmat, eval, evec, w);

  size_t growth_rate_idx = 0;
  double growth_rate = -INFINITY;
  for(size_t i = 0; i < NG*(DS-1)*RG; i++){
    if(GSL_REAL(gsl_vector_complex_get(eval, i)) > growth_rate){
      growth_rate_idx = i;
      growth_rate = GSL_REAL(gsl_vector_complex_get(eval, i));
    }
  }

  if(eigenvec != NULL){
    for(size_t i = 0; i < NG*(DS-1)*RG; i++)
      eigenvec[i] = GSL_REAL(gsl_matrix_complex_get(evec, i, growth_rate_idx));
  }

  gsl_matrix_free(Fmat);
  gsl_matrix_free(Vmat);
  gsl_vector_complex_free(eval);
  gsl_matrix_complex_free(evec);
  gsl_eigen_nonsymmv_free(w);

  return growth_rate;
}
Ejemplo n.º 8
0
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);
}
Ejemplo n.º 9
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;

}
Ejemplo n.º 10
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);
}
Ejemplo n.º 11
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);
}
Ejemplo n.º 12
0
void infomax(gsl_matrix *x_white, gsl_matrix *weights, gsl_matrix *S, int  verbose){
  /*Computes ICA infomax in whitened data
    Decomposes x_white as x_white=AS
    *Input
    x_white: whitened data (Use PCAwhiten)
    *Output
    A : mixing matrix
    S : source matrix
  */
  // int verbose = 1; //true
  size_t NCOMP = x_white->size1;
  size_t NVOX = x_white->size2;

  //getting permutation vector
  const gsl_rng_type * T;
  gsl_rng * r;
  gsl_permutation * p = gsl_permutation_alloc (NVOX);
  // gsl_rng_env_setup();
  T = gsl_rng_default;
  r = gsl_rng_alloc (T);
  gsl_permutation_init (p);

  gsl_matrix *old_weights    = gsl_matrix_alloc(NCOMP,NCOMP);
  gsl_matrix *bias           = gsl_matrix_calloc(NCOMP, 1);
  gsl_matrix *d_weights      = gsl_matrix_calloc(NCOMP,NCOMP);
  gsl_matrix *temp_change    = gsl_matrix_alloc(NCOMP,NCOMP);
  gsl_matrix *old_d_weights  = gsl_matrix_calloc(NCOMP,NCOMP);
  gsl_matrix *shuffled_x_white  = gsl_matrix_calloc(NCOMP,x_white->size2);
  gsl_matrix_memcpy(shuffled_x_white, x_white);
  gsl_matrix_set_identity(weights);
  gsl_matrix_set_identity(old_weights);
  double lrate = 0.005/log((double)NCOMP);
  double change=1;
  double angle_delta =0;
  size_t step = 1;
  int error = 0;
  while( (step < MAX_STEP) && (change > W_STOP)){
    error = w_update(weights, x_white, bias,
      shuffled_x_white, p, r, lrate);
    if (error==1 || error==2){
      // It blowed up! RESTART!
      step = 1;
      // change = 1;
      error = 0;
      lrate *= ANNEAL;
      gsl_matrix_set_identity(weights);
      gsl_matrix_set_identity(old_weights);
      gsl_matrix_set_zero(d_weights);
      gsl_matrix_set_zero(old_d_weights);
      gsl_matrix_set_zero(bias);

      if (lrate > MIN_LRATE){
        printf("\nLowering learning rate to %g and starting again.\n",lrate);
      }
      else{
        printf("\nMatrix may not be invertible");
      }
    }
    else if (error==0){
      gsl_matrix_memcpy(d_weights, weights);
      gsl_matrix_sub(d_weights, old_weights);
      change = matrix_norm(d_weights);

      if (step > 2){
        // Compute angle delta
        gsl_matrix_memcpy(temp_change, d_weights);
        gsl_matrix_mul_elements(temp_change, old_d_weights);
        angle_delta = acos(matrix_sum(temp_change) / sqrt(matrix_norm(d_weights)*(matrix_norm(old_d_weights))));
        angle_delta *= (180.0 / M_PI);
      }

      gsl_matrix_memcpy(old_weights, weights);

      if (angle_delta > 60){
        lrate *= ANNEAL;
        gsl_matrix_memcpy(old_d_weights, d_weights);
      } else if (step==1) {
        gsl_matrix_memcpy(old_d_weights, d_weights);
      }

      if ((verbose && (step % 10)== 0) || change < W_STOP){
        printf("\nStep %zu: Lrate %.1e, Wchange %.1e, Angle %.2f",
          step, lrate, change, angle_delta);
      }

      step ++;
    }
  }

  matrix_mmul(weights, x_white, S);
  gsl_matrix_free(old_d_weights);
  gsl_matrix_free(old_weights);
  gsl_matrix_free(bias);
  gsl_matrix_free(d_weights);
  gsl_matrix_free(shuffled_x_white);
  gsl_rng_free (r);
  gsl_permutation_free (p);
}
Ejemplo n.º 13
0
 	matrix operator-(const matrix& m1, const matrix& m2) {
 		matrix out(m1);
 		gsl_matrix_sub(out.ptr_, m2.ptr_);
 		return out;
 	}
Ejemplo n.º 14
0
/* test if A = Q S Z^t */
void
test_schur(gsl_matrix *A, gsl_matrix *S, gsl_matrix *Q, gsl_matrix *Z)
{
  const size_t N = A->size1;
  gsl_matrix *T1, *T2;
  size_t i, j, k;
  double lhs, rhs;
  double abserr;

  T1 = gsl_matrix_alloc(N, N);
  T2 = gsl_matrix_alloc(N, N);

  /* compute T1 = S Z^t */
  gsl_blas_dgemm(CblasNoTrans,
                 CblasTrans,
                 1.0,
                 S,
                 Z,
                 0.0,
                 T1);

  /* compute T2 = Q T1 = Q S Z^t */
  gsl_blas_dgemm(CblasNoTrans,
                 CblasNoTrans,
                 1.0,
                 Q,
                 T1,
                 0.0,
                 T2);

  k = 0;
  for (i = 0; i < N; ++i)
    {
      for (j = 0; j < N; ++j)
        {
          lhs = gsl_matrix_get(A, i, j);
          rhs = gsl_matrix_get(T2, i, j);

          abserr = fabs(lhs - rhs);

          if (abserr > 1.0e-6)
            ++k;
        }
    }

  if (k)
    {
      printf("==== CASE %lu ===========================\n\n", count);

      print_matrix(A, "A");

      printf("=== Schur Form matrix ===\n");
      print_matrix(S, "S");

      printf("=== Left Schur matrix ===\n");
      print_matrix(Q, "Q");

      printf("=== Right Schur matrix ===\n");
      print_matrix(Z, "Z");

      printf("=== Q S Z^t ===\n");
      print_matrix(T1, "Q S Z^t");

      printf("=== A - Q S Z^t ===\n");
      gsl_matrix_sub(T2, A);
      print_matrix(T1, "A - Q S Z^t");

      printf("=========================================\n\n");
    }

  gsl_matrix_free(T1);
  gsl_matrix_free(T2);
} /* test_schur() */
Ejemplo n.º 15
0
/// subtract a matrix from this
/// @param M :: A matrix
GSLMatrix &GSLMatrix::operator-=(const GSLMatrix &M) {
  gsl_matrix_sub(&m_view.matrix, M.gsl());
  return *this;
}
Ejemplo n.º 16
0
/**
	this method subtracts the matrix that is passed in as a parameter from the current matrix. In addition, it scales, both matrices by the two
	numbers that are passed in as parameters:
	*this = a * *this - b * other.
*/
void Matrix::sub(const Matrix& other, double scaleA, double scaleB){
	if (scaleA == 1.0 && scaleB == 1)
		gsl_matrix_sub(this->matrix, other.matrix);
	else
		gsl_matrix_add_scaled(this->matrix, other.matrix, scaleA, -scaleB);	
}
Ejemplo n.º 17
0
int model::predict(const dataset &tds, gsl_matrix **pp)
{
	int ret = -1;

	gsl_matrix *mat = NULL;
	gsl_matrix *ptv = NULL;
	gsl_matrix *km1 = NULL;
	gsl_matrix *km2 = NULL;
	gsl_matrix *res = NULL;
	gsl_matrix *stm = NULL;

        gsl_vector_view avg_col;
        gsl_vector_view dv;
        
	if (tds.ins_num() <= 0 || tds.fea_num() != (int)_col_mean->size) {
		ULIB_FATAL("invalid test dimensions, (ins_num=%d,fea_num=%d)",
                           tds.ins_num(), tds.fea_num());
		goto done;
	}

	mat = gsl_matrix_alloc(tds.ins_num(), tds.fea_num());
	if (mat == NULL) {
		ULIB_FATAL("couldn't allocate test feature matrix");
		goto done;
	}
	ptv = gsl_matrix_alloc(tds.ins_num(), 2);
	if (ptv == NULL) {
		ULIB_FATAL("couldn't allocate prediction matrix");
		goto done;
	}
        if (tds.get_matrix(mat)) {
                ULIB_FATAL("couldn't get test matrix");
                goto done;
        }
	dbg_print_mat(mat, "Test Matrix:");

	zero_out_mat(mat);
	norm_mat(mat);

	dbg_print_mat(mat, "Normalized Test Matrix:");

	km1 = comp_kern_mat(mat, _fm, _kern);
	if (km1 == NULL) {
		ULIB_FATAL("couldn't compute test1 kernel matrix");
		goto done;
	}
	dbg_print_mat(km1, "Test Kernel Matrix:");

	km2 = comp_kern_mat(mat, mat, _kern);
	if (km2 == NULL) {
		ULIB_FATAL("couldn't compute test2 kernel matrix");
		goto done;
	}
	dbg_print_mat(km1, "Test Kernel Matrix:");
	dv = gsl_matrix_diagonal(km2);

	res = gsl_matrix_alloc(km1->size1, _ikm->size2);
	if (res == NULL) {
		ULIB_FATAL("couldn't allocate temporary matrix");
		goto done;
	}
	stm = gsl_matrix_alloc(km2->size1, km2->size2);
	if (stm == NULL) {
		ULIB_FATAL("couldn't allocate std matrix");
		goto done;
	}

	gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, km1, _ikm, 0.0, res);
	gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, res, km1, 0.0, stm);
        gsl_matrix_sub(km2, stm);
	dbg_print_mat(res, "Predictive Matrix:");
        avg_col = gsl_matrix_column(ptv, 0);
	gsl_blas_dgemv(CblasNoTrans, 1.0, res, _tv, 0.0, &avg_col.vector);
        gsl_vector_add_constant(&avg_col.vector, _t_avg);
        gsl_matrix_scale(km2, _t_std*_t_std);
	gsl_vector_add_constant(&dv.vector, _noise_var);
        for (size_t i = 0; i < km2->size1; ++i)
                gsl_matrix_set(ptv, i, 1, sqrt(gsl_vector_get(&dv.vector, i)));
	*pp = ptv;
	ptv = NULL;
	ret = 0;
done:
	gsl_matrix_free(mat);
	gsl_matrix_free(ptv);
	gsl_matrix_free(km1);
	gsl_matrix_free(km2);
	gsl_matrix_free(res);
	gsl_matrix_free(stm);
	return ret;
}
Ejemplo n.º 18
0
int main() {

  srand(time(NULL));

  int states = 6;
  int unfiltered_states = 3;

  gsl_matrix *state_mean = gsl_matrix_calloc(states,1);
  gsl_matrix_set(state_mean, 0,0, 3);
  gsl_matrix_set(state_mean, 1,0, 3);
  gsl_matrix *state_covariance = gsl_matrix_calloc(states,states);

  gsl_matrix *observation_mean = gsl_matrix_calloc(states,1);
  gsl_matrix *observation_covariance = gsl_matrix_calloc(states,states);
  //gsl_matrix_set(observation_covariance, 0, 0, hdop);
  //gsl_matrix_set(observation_covariance, 1, 1, hdop);
  //gsl_matrix_set(observation_covariance, 2, 2, vert_err);
  //gsl_matrix_set(observation_covariance, 3, 3, (2*hdop)/pow(timestep,2));
  //gsl_matrix_set(observation_covariance, 4, 4, (2*hdop)/pow(timestep,2));
  //gsl_matrix_set(observation_covariance, 5, 5, (2*vert_err)/pow(timestep,2));
  //gsl_matrix_set(observation_covariance, 0, 3, timestep*(2*hdop)/pow(timestep,2));
  //gsl_matrix_set(observation_covariance, 3, 0, timestep*(2*hdop)/pow(timestep,2));
  //gsl_matrix_set(observation_covariance, 1, 4, timestep*(2*hdop)/pow(timestep,2));
  //gsl_matrix_set(observation_covariance, 4, 1, timestep*(2*hdop)/pow(timestep,2));
  //gsl_matrix_set(observation_covariance, 2, 5, timestep*(2*vert_err)/pow(timestep,2));
  //gsl_matrix_set(observation_covariance, 5, 2, timestep*(2*vert_err)/pow(timestep,2));

  gsl_matrix *observation_transformation = gsl_matrix_calloc(states,states);
  
  gsl_matrix *estimate_mean = gsl_matrix_calloc(states,1);
  gsl_matrix *estimate_covariance = gsl_matrix_calloc(states,states);
  gsl_matrix *kalman_gain = gsl_matrix_calloc(states,states);

  gsl_matrix *temp21a = gsl_matrix_calloc(states,1);
  gsl_matrix *temp21b = gsl_matrix_calloc(states,1);
  gsl_matrix *temp22a = gsl_matrix_calloc(states,states);
  gsl_matrix *temp22b = gsl_matrix_calloc(states,states);

  gsl_matrix *predict = gsl_matrix_calloc(states,states);
  //gsl_matrix_set(predict, 0, 0, 1);
  //gsl_matrix_set(predict, 1, 1, 1);
  //gsl_matrix_set(predict, 2, 2, 1);
  gsl_matrix_set(predict, 3, 3, 1);
  gsl_matrix_set(predict, 4, 4, 1);
  gsl_matrix_set(predict, 5, 5, 1);
  //gsl_matrix_set(predict, 0, 3, timestep);
  //gsl_matrix_set(predict, 1, 4, timestep);
  //gsl_matrix_set(predict, 2, 5, timestep);

  gsl_matrix *control = gsl_matrix_calloc(states, unfiltered_states);
  //gsl_matrix_set(control, 0, 0, 0.5*pow(timestep,2));
  //gsl_matrix_set(control, 1, 1, 0.5*pow(timestep,2));
  //gsl_matrix_set(control, 2, 2, 0.5*pow(timestep,2));
  //gsl_matrix_set(control, 3, 0, timestep);
  //gsl_matrix_set(control, 4, 1, timestep);
  //gsl_matrix_set(control, 5, 2, timestep);

  gsl_vector *acceleration = gsl_vector_calloc(unfiltered_states);
  gsl_vector *velocity = gsl_vector_calloc(unfiltered_states);
  gsl_vector *location = gsl_vector_calloc(unfiltered_states);
  gsl_vector *delta_location = gsl_vector_calloc(unfiltered_states);
  gsl_vector *temp_location = gsl_vector_calloc(unfiltered_states);

  double timestep;
  int hdop;
  int vert_err;

  int s;
  double error;

  for (int t = 1; t < 1000000; t++) {

    timestep = 1;
    gsl_vector_set(acceleration, 0, (rand()%101)-52);
    gsl_vector_set(acceleration, 1, (rand()%101)-46);
    gsl_vector_set(acceleration, 2, (rand()%101)-54);
    gsl_vector_add(velocity, acceleration);
    gsl_vector_add(location, velocity);

    gsl_vector_memcpy(delta_location, location);
    gsl_vector_sub(delta_location, temp_location);
    gsl_vector_memcpy(temp_location, location);

    gsl_matrix_set(predict, 0, 3, timestep);
    gsl_matrix_set(predict, 1, 4, timestep);
    gsl_matrix_set(predict, 2, 5, timestep);
    gsl_matrix_set(control, 0, 0, 0.5*pow(timestep,2));
    gsl_matrix_set(control, 1, 1, 0.5*pow(timestep,2));
    gsl_matrix_set(control, 2, 2, 0.5*pow(timestep,2));
    gsl_matrix_set(control, 3, 0, timestep);
    gsl_matrix_set(control, 4, 1, timestep);
    gsl_matrix_set(control, 5, 2, timestep);
    hdop = 5;
    vert_err = 5;
    gsl_matrix_set(observation_covariance, 0, 0, hdop);
    gsl_matrix_set(observation_covariance, 1, 1, hdop);
    gsl_matrix_set(observation_covariance, 2, 2, vert_err);
    gsl_matrix_set(observation_covariance, 3, 3, (2*hdop)/pow(timestep,2));
    gsl_matrix_set(observation_covariance, 4, 4, (2*hdop)/pow(timestep,2));
    gsl_matrix_set(observation_covariance, 5, 5, (2*vert_err)/pow(timestep,2));
    gsl_matrix_set(observation_covariance, 0, 3, timestep*(2*hdop)/pow(timestep,2));
    gsl_matrix_set(observation_covariance, 3, 0, timestep*(2*hdop)/pow(timestep,2));
    gsl_matrix_set(observation_covariance, 1, 4, timestep*(2*hdop)/pow(timestep,2));
    gsl_matrix_set(observation_covariance, 4, 1, timestep*(2*hdop)/pow(timestep,2));
    gsl_matrix_set(observation_covariance, 2, 5, timestep*(2*vert_err)/pow(timestep,2));
    gsl_matrix_set(observation_covariance, 5, 2, timestep*(2*vert_err)/pow(timestep,2));

    //observation_transformation = observation_mean[k] * pseudoinverse(state_mean[k-1])
    gsl_matrix_set_zero(temp21a);
    gsl_matrix_set(temp21a, 0, 0, gsl_vector_get(delta_location,0));
    gsl_matrix_set(temp21a, 1, 0, gsl_vector_get(delta_location,1));
    gsl_matrix_set(temp21a, 2, 0, gsl_vector_get(delta_location,2));
    gsl_matrix_set(temp21a, 3, 0, gsl_vector_get(velocity,0));
    gsl_matrix_set(temp21a, 4, 0, gsl_vector_get(velocity,1));
    gsl_matrix_set(temp21a, 5, 0, gsl_vector_get(velocity,2));
    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, temp21a, pseudo_inverse(state_mean), 0, observation_transformation);

    //observation_mean[k] = observation_transformation * state_mean[k-1]
    gsl_matrix_memcpy(temp21a, state_mean);
    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, observation_transformation, temp21a, 0, observation_mean);

    //observation_covariance[k] = observation_transformation * state_covariance * transpose(observation_transformation)
/*    gsl_matrix_set_zero(temp22a);
    gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1, state_covariance, observation_transformation, 0, temp22a); //notice observation_transformation is transposed
    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, observation_transformation, temp22a, 0, observation_covariance);
*/
    gsl_matrix_set(observation_covariance, 0, 3, timestep*3);
    gsl_matrix_set(observation_covariance, 3, 0, timestep*3);
    gsl_matrix_set(observation_covariance, 1, 4, timestep*5);
    gsl_matrix_set(observation_covariance, 4, 1, timestep*5);
    gsl_matrix_set(observation_covariance, 2, 5, timestep*6);
    gsl_matrix_set(observation_covariance, 5, 2, timestep*6);

    //estimate_mean = predict * state_mean + control * acceleration;
    gsl_matrix_set_zero(estimate_mean);
    gsl_matrix_set_zero(temp21a);
    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, predict, state_mean, 0, estimate_mean);
    gsl_matrix_view test = gsl_matrix_view_vector(acceleration, unfiltered_states, 1);
    gsl_matrix * tmp = &test.matrix;
    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, control, tmp, 0, temp21a);
    gsl_matrix_add(estimate_mean, temp21a);

    //estimate_covariance = predict * state_covariance * transpose(predict) + noise;
    gsl_matrix_set_zero(estimate_covariance);
    gsl_matrix_set_zero(temp22a);
    gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1, state_covariance, predict, 0, temp22a); //notice predict is transposed
    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, predict, temp22a, 0, estimate_covariance);
    gsl_matrix_add_constant(estimate_covariance, 0.1); //adxl345 noise, this is completely wrong
    
    //kalman_gain = estimate_covariance * pseudoinverse(estimate_covariance + observation_covariance);
    gsl_matrix_set_zero(kalman_gain);
    gsl_matrix_set_zero(temp22a);
    gsl_matrix_memcpy(temp22a, observation_covariance);
    gsl_matrix_add(temp22a, estimate_covariance);
    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, estimate_covariance, pseudo_inverse(temp22a), 0, kalman_gain);
    
    //state_mean = estimate_mean +  kalman_gain * ( observation_mean - estimate_mean );
    gsl_matrix_set_zero(state_mean);
    gsl_matrix_set_zero(temp21a);
    gsl_matrix_memcpy(temp21a, observation_mean);
    gsl_matrix_sub(temp21a, estimate_mean);
    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, kalman_gain, temp21a, 0, state_mean);
    gsl_matrix_add(state_mean, estimate_mean);
    
    //state_covariance = estimate_covariance - kalman_gain * ( estimate_covariance );
    gsl_matrix_set_zero(state_covariance);
    gsl_matrix_set_zero(temp22a);
    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, kalman_gain, estimate_covariance, 0, temp22a);
    gsl_matrix_add(state_covariance, estimate_covariance);
    gsl_matrix_sub(state_covariance, temp22a);

    printf("state_mean:");
    gsl_matrix_fprintf(stdout, state_mean, "%f");
    //printf("state_covariance:");
    //gsl_matrix_fprintf(stdout, state_covariance, "%f");

    printf("observation_mean:");
    gsl_matrix_fprintf(stdout, observation_mean, "%f");
    //printf("observation_covariance:");
    //gsl_matrix_fprintf(stdout, observation_covariance, "%f");

    printf("estimate_mean:");
    gsl_matrix_fprintf(stdout, estimate_mean, "%f");
    //printf("estimate_covariance:");
    //gsl_matrix_fprintf(stdout, estimate_covariance, "%f");

    gsl_matrix_set_zero(temp21a);
    gsl_matrix_memcpy(temp21a, observation_mean);
    gsl_matrix_sub(temp21a, state_mean);
    gsl_matrix_div_elements(temp21a, observation_mean);
    gsl_matrix_mul_elements(temp21a, temp21a);

    for (int i = states-1; i >= 0; i--) {
      error += gsl_matrix_get(temp21a, i, 0); 
    }

    printf("error: %f\n", error);
    printf("error/time: %f\n", error/t);
    printf("\n");

    usleep(1000000);
  
  }
}
Ejemplo n.º 19
0
/* Performs LSE of consequent parameters for all branches in the network
 *
 * PARAMETERS:	A ---> predictor variables matrix
 *		y ---> expected results for this sample set  (P in total)
 *
 * PRE: A != NULL
 *	y != NULL
 *
 * POS: result != NULL   &&   best fit consequent parameters vector returned
 *	or
 *	result == NULL
 */
static gsl_vector *
anfis_fit_linear (const gsl_matrix *A, const gsl_vector *y, size_t P, size_t M)
{
	gsl_matrix  *S =    NULL,	/* Covariance matrix */
		    *Snew = NULL,
		    *Saux = NULL;
	gsl_vector  *X = NULL,		/* Future best fit parameters */
		    *Xnew = NULL;
	unsigned int i = 0;
	double den = 0.0, factor = 0.0;
	
	assert (A != NULL);
	assert (y != NULL);
	
	/* Generating necessary workspace */
	S = gsl_matrix_alloc (M,M);
	if (S == NULL)
		goto exit_point;
	Snew = gsl_matrix_calloc (M,M);
	if (Snew == NULL)
		goto exit_point;
	Saux = gsl_matrix_calloc (M,M);
	if (Saux == NULL)
		goto exit_point;
	Xnew = gsl_vector_alloc (M);
	if (Xnew == NULL)
		goto exit_point;
	X = gsl_vector_calloc (M);
	if (X == NULL)
		goto exit_point;
	
	/* S = γ*Id , where γ is a large number */
	gsl_matrix_set_identity  (S);
	gsl_matrix_scale (S, _gamma);
	
	/* Performing Least Square Estimation */
	for (i=0 ; i < P ; i++) {
		/* Matrix A i-th row (row At_i+1 in Jang's paper) */
		gsl_vector_const_view A_i = gsl_matrix_const_row (A, i);
		
		/* Snew = S(i) * A_i+1 * At_i+1 * S(i) */
		calc_num (S, &(A_i.vector), Snew, Saux, Xnew, M);
		/* scale = At_i+1 * S(i) * A_i+1 */
		den = calc_den (S, &(A_i.vector), Xnew);
		/* Snew = Snew / (1+scale) */
		gsl_matrix_scale (Snew, 1.0 / (1.0+den));
		/* S(i+1) = S(i) - Snew */
		gsl_matrix_sub (S, Snew);
		
		/* factor = At_i+1 * X(i) */
		gsl_blas_ddot (&(A_i.vector), X, &factor);
		/* factor = yt_i+1 - factor */
		factor = gsl_vector_get (y, i) - factor;
		/* Xnew = S(i+1) * A_i+1 */
		gsl_blas_dgemv (CblasNoTrans, 1.0, S, &(A_i.vector), 0.0, Xnew);
		/* Xnew = Xnew * factor */
		gsl_vector_scale (Xnew, factor);
		/* X(i+1) = X(i) + Xnew */
		gsl_vector_add (X, Xnew);
	}
	
	exit_point:
		if (S != NULL)    { gsl_matrix_free (S);    S    = NULL;}
		if (Snew != NULL) { gsl_matrix_free (Snew); Snew = NULL;}
		if (Saux != NULL) { gsl_matrix_free (Saux); Saux = NULL;}
		if (Xnew != NULL) { gsl_vector_free (Xnew); Xnew = NULL;}
		
		return X;
}
Ejemplo n.º 20
0
/** @a Main Kalman Algorithm - Final process Update position and update P(k+1|k+1)
 * 	/*  General correction algorithm equations, as per main report document
 *
 *
	Correction equations:

	1) Pose correction
		x(k + 1|k + 1) = x(k + 1|k) + w(k + 1).v(k + 1)   	(5.10)

			 where:
				 x(k + 1|k), predicted pose from Stage 1 of the EKF
				 W(k + 1), Kalman Gain
				 v(k + 1), innovation vector obtained from
						   feature matching stage

			definition of the terms of the equation 5.10:

				w(k + 1) = R(k + 1|k).GgT(k + 1).S^−1(k + 1)			 	(5.11)

				v(k + 1) = [ v1(k+1) ]    									(5.6)
						   [ v2(k+1) ]
							   ...
						   [ vn(k+1) ]
				where:
					R(k + 1|k), pose covariance matrix
					Gg(k + 1), aggregated pose gradient matrix obtained from
							   feature prediction stage
					S(k + 1), aggregated feature covariance

			definition of the terms of the equation 5.11:

					Gg(k + 1) = [ Gg1(k+1) ]    	(5.7)
								[ Gg2(k+1) ]
									...
								[ Ggn(k+1) ]

					S(k + 1) = Gg(k+1).R(k + 1|k).GgT(k+1) + Q(k + 1) (5.8)

						where:
							Q(k + 1), aggregated feature covariace matrix

								Q(k + 1) = [ D1  	0 	  ... 	0  ]
										   [ 0 		D2 	  ... 	0  ]
										   [ ...  	...   ...	...]
										   [ 0  	0     ...	Dn ]

								Dn, diagonal elements of the features covariace

	2) Covariance correction
		R(k + 1|k + 1) = R(k + 1|k) − w(k + 1).S(k + 1).wT(k + 1) 	(5.12)


 **/
void Nav_MotionEstimator::Nav_EKFStage3Correction( )
{
	// features matched during
	boost::ptr_list<MatchedFeatures_C>::iterator MatchedIterator;

	int Featincorrection = 0;

	cout << "FEATURE MATCHED: " << FeatMatched.size() << endl;
	/** Count the number of mature features that can be used in the correction */
	for (MatchedIterator = FeatMatched.begin(); MatchedIterator != FeatMatched.end(); MatchedIterator++)
	{
		if ( (*MatchedIterator).InMapFeat->isMature () ) {
			Featincorrection++;
		}
	}

	cout << " CORRECTION WITH : " << Featincorrection << endl;

	if ( Featincorrection == 0 ) return;

	/* Allocate matrixes */
	gsl_matrix * Vk = gsl_matrix_alloc ( 2*Featincorrection, 1);
	gsl_matrix * Gg = gsl_matrix_alloc ( 2 * Featincorrection, 3);
	gsl_matrix * GgT = gsl_matrix_alloc ( 3, 2 * Featincorrection );
	gsl_matrix * W	= gsl_matrix_alloc ( 3, 2 * Featincorrection );
	gsl_matrix * WT	= gsl_matrix_alloc ( 2 * Featincorrection, 3 );

	gsl_matrix * QK = gsl_matrix_alloc(Featincorrection * 2, Featincorrection * 2);
	gsl_matrix * SK = gsl_matrix_alloc(Featincorrection * 2, Featincorrection * 2);
	gsl_matrix * SKinv = gsl_matrix_alloc(Featincorrection * 2, Featincorrection * 2);

	gsl_matrix * mat2nx3 = gsl_matrix_alloc(Featincorrection * 2, 3);
	gsl_matrix * mat3xn2 = gsl_matrix_alloc(3, Featincorrection * 2);
	gsl_matrix * mat3x3 = gsl_matrix_alloc(3, 3);
	gsl_matrix * mat3x1 = gsl_matrix_alloc(3, 1);

	// fill the matrixes with the relevant data
	int InnovIndex = 0;
	int AggreFeatIndex = 0;
	int FeaturePoseGradIndex = 0;
	for (MatchedIterator = FeatMatched.begin(); MatchedIterator != FeatMatched.end(); MatchedIterator++)
	{
		if ( (*MatchedIterator).InMapFeat->isMature () ) {
			// get position grandient, from observed predicted feature
			(*MatchedIterator).Observed->getPoseGradient(Gg,&FeaturePoseGradIndex);
			// set innovation vector
			for (int i=0;i<(*MatchedIterator).InovVect.size();i++) {
				gsl_matrix_set (Vk, InnovIndex, 0, (*MatchedIterator).InovVect[i] );
				InnovIndex++;
			}
			// aggregated feature covariance matrix
			(*MatchedIterator).Observed->getCovarianceDiagonalElems (QK,&AggreFeatIndex);
		}
	}
	// transpose the matrix GT
	gsl_matrix_transpose_memcpy(GgT, Gg);

	/* Pose Correction of EKF
	 *
	 * S(k + 1) = Gg(k+1).R(k + 1|k).GgT(k+1) + Q(k + 1) (5.8)
	 * */
	gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Gg, CovPose, 0.0, mat2nx3);
	gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, mat2nx3, GgT, 0.0, SK);
	gsl_matrix_add(SK,QK);

	printgslMatrix ( Gg, 2*Featincorrection , 3, "Gg");
	printgslMatrix ( QK, 2*Featincorrection , 2*Featincorrection, "QK");
	printgslMatrix ( SK, 2*Featincorrection , 2*Featincorrection, "SK");

	/* matrix inversion Sk */
	CalcInvMatrix(SKinv, SK, Featincorrection * 2 );

	/* Kalman gain
	 * w(k + 1) = R(k + 1|k).GgT(k + 1).S^−1(k + 1)			 	(5.11)
	 * */
	gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, CovPose, GgT, 0.0, mat3xn2);
	gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, mat3xn2, SKinv, 0.0, W);

	// transpose the Gain
	gsl_matrix_transpose_memcpy(WT, W);

	/* Pose Correction eq.
	 * x(k + 1|k + 1) = x(k + 1|k) + w(k + 1).v(k + 1)   	(5.10)
	 * */
	gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, W, Vk, 0.0, mat3x1);
	gsl_matrix_add(MeanPose,mat3x1);

	/* Covariance Correction eq.
	 * R(k + 1|k + 1) = R(k + 1|k) − w(k + 1).S(k + 1).wT(k + 1) 	(5.12)
	 * */
	gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, W, SK, 0.0, mat3xn2);
	gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, mat3xn2, WT, 0.0, mat3x3);
	gsl_matrix_sub(CovPose,mat3x3);

	// free matrix elements
	gsl_matrix_free(Vk);
	gsl_matrix_free(Gg);
	gsl_matrix_free(GgT);
	gsl_matrix_free(W);
	gsl_matrix_free(WT);
	gsl_matrix_free(QK);
	gsl_matrix_free(SK);
	gsl_matrix_free(SKinv);
	gsl_matrix_free(mat2nx3);
	gsl_matrix_free(mat3xn2);
	gsl_matrix_free(mat3x3);
	gsl_matrix_free(mat3x1);

} //end update position
Ejemplo n.º 21
0
gsl_matrix* calc_Hs(double s, double tres, double* Qxx_m, double* Qyy_m, double* Qxy_m, double* Qyx_m, int kx, int ky) {
    
    /*
     *
     *
     *     *H(s)
     *I=eye(ky); 
     *expMat=mat_exp(-(s*I-Qyy)*tres); 
     *Hxx_s=Qxx+(Qxy*(s*I-Qyy)^-1)*(I-expMat)*Qyx; 
     *
    */
    
       /* Initialise Qxx,Qxy,Qyx,Qyy  */
   
    gsl_matrix *Qxx=setup_matrix(Qxx_m,kx,kx);
    gsl_matrix *Qxy=setup_matrix(Qxy_m,kx,ky);
    gsl_matrix *Qyx=setup_matrix(Qyx_m,ky,kx);
    gsl_matrix *Qyy=setup_matrix(Qyy_m,ky,ky);
    
    
    gsl_matrix *ky_eye; //identitny matrix of dimension ky,ky
    gsl_matrix *e_eye; //to hold the matrix exponential expMat
    gsl_matrix *detected_tres; //matrix representing period in states y for tres followed by transition to x
    gsl_matrix *seye2;
    gsl_matrix *inv_sI_Qyy;
    gsl_matrix *res;
    gsl_matrix *res2;
    gsl_matrix *res3;
    gsl_matrix *eye3;
    //printf("\tAllocate memory for the matrices\n");
    
    //allocate memory for all the matrices
    
    ky_eye=gsl_matrix_alloc(ky,ky);
    e_eye=gsl_matrix_alloc(ky,ky);
    seye2=gsl_matrix_alloc(ky,ky);
    detected_tres=gsl_matrix_alloc(ky,ky);
    inv_sI_Qyy=gsl_matrix_alloc(ky,ky);
    res=gsl_matrix_alloc(kx,ky);
    res2=gsl_matrix_alloc(ky,kx);
    res3 = gsl_matrix_alloc(kx,kx);
    eye3=gsl_matrix_alloc(ky,ky);
    
    //printf("\tAllocated memory for the matrices\n");
    
    gsl_matrix_set_identity(ky_eye); 
    //exp(-(s*I-Qyy)*tres)
    //build from the identity matrix
    gsl_matrix_memcpy (detected_tres,ky_eye);
    gsl_matrix_scale(detected_tres,s);
    gsl_matrix_sub(detected_tres,Qyy);
    gsl_matrix_scale(detected_tres,-1);
    gsl_matrix_scale(detected_tres,tres);
    gsl_linalg_exponential_ss(detected_tres, e_eye, .01);
    
    //printf("\tCalculated exp(-(s*I-Qyy)*tres)\n");
    
   
    //(s*I-Qyy)             
    gsl_matrix_memcpy (seye2,ky_eye);    
    gsl_matrix_scale(seye2,s);
    gsl_matrix_sub(seye2,Qyy);
    
    //printf("\tCalculated s*I-Qyy\n");
    
    //invert s*I-Qyy
    int sc;    
    gsl_permutation * p = gsl_permutation_alloc (ky);
    gsl_linalg_LU_decomp(seye2, p, &sc); 
    gsl_linalg_LU_invert(seye2, p, inv_sI_Qyy);
    gsl_permutation_free(p);
    
    //printf("\tInverted s*I-Qyy\n");
    
    /*
    for (int i=0; i<ky; i++){
        for (int j=0; j<ky; j++){
            mexPrintf("\t%f",gsl_matrix_get(inv_sI_Qyy,i,j));
        }
        mexPrintf("\n");
    }
    */
    
    //multiply Qxy * (s*I-Qyy)^-1
    gsl_matrix_set_zero(res);
    // res = (Qxy*(s*I-Qyy)^-1)
    gsl_blas_dgemm (CblasNoTrans, CblasNoTrans,
                  1.0, Qxy, inv_sI_Qyy,
                  0.0, res);
    

    
    //printf("\tCalculated Qxy * (s*I-Qyy)^-1\n");
    
    //res2 =(I-expMat)*Qyx;
    gsl_matrix_set_zero(res2);   
    gsl_matrix_memcpy (eye3,ky_eye);
    //printf("\tMemcpy (I-expMat)\n");
    gsl_matrix_sub(eye3,e_eye);
    
    //printf("\tSubtract (I-expMat)\n");
    
    gsl_blas_dgemm (CblasNoTrans, CblasNoTrans,
                  1.0, eye3, Qyx,
                  0.0, res2);
    
    //res3 = (Qxy*(s*I-Qyy)^-1)*(I-expMat)*Qyx;
    //res3 = res *res2
    //res3 is the result we want to return
    //printf("\t (I-expMat)*Qyx\n");
    
    gsl_matrix_set_zero(res3);
    gsl_blas_dgemm (CblasNoTrans, CblasNoTrans,
                  1.0, res, res2,
                  0.0, res3);
    
    
    gsl_matrix_add(res3,Qxx);
    
    /*
    for (int i=0; i<kx; i++){
        for (int j=0; j<kx; j++){
            mexPrintf("\t%f",gsl_matrix_get(res3,i,j));
        }
        mexPrintf("\n");
    }
     */
    
    //printf("\t Calced H(s)\n");
    
    //cleanup
    gsl_matrix_free(Qxx);
    gsl_matrix_free(Qxy);
    gsl_matrix_free(Qyx);
    gsl_matrix_free(Qyy);
    gsl_matrix_free(ky_eye);
    gsl_matrix_free(e_eye);
    gsl_matrix_free(detected_tres);
    gsl_matrix_free(seye2);
    gsl_matrix_free(inv_sI_Qyy);
    gsl_matrix_free(res);
    gsl_matrix_free(res2);
    gsl_matrix_free(eye3);
    
    //printf("\t Cleaned up H(s)\n");
    
    return res3;
}
Ejemplo n.º 22
0
void KF_steady (int *dim, double *sy, double *sZ, double *sT, double *sH, 
  double *sR, double *sV, double *sQ, double *sa0, double *sP0, 
  double *mll, 
  std::vector<double> *v, std::vector<double> *f, 
  std::vector<double> *invf, std::vector<double> *vof,
  gsl_matrix *K, std::vector<gsl_matrix*> *L,
  double *tol, int *maxiter)
{
  //int s, p = dim[1], r = dim[3];
  int i, n = dim[0], m = dim[2], conv = 0, counter = 0;
  //double Kisum, Kim1sum = 0.0;
  
  mll[0] = 0.0;

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

  gsl_vector * a_pred = gsl_vector_alloc(m);
  //gsl_matrix * P_pred = gsl_matrix_alloc(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_matrix * Mmm = gsl_matrix_alloc(m, m);
  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, K_im1row, Kri;
  gsl_matrix_view maux1, maux2;

  // filtering recursions

  for (i = 0; i < n; i++)
  {
    gsl_blas_dgemv(CblasNoTrans, 1.0, &T.matrix, a_upd, 0.0, a_pred);

    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_upd);
      gsl_matrix_add(P_upd, &Q.matrix);
    }

    gsl_blas_ddot(&Z.vector, a_pred, &v->at(i));
    v->at(i) = sy[i] - v->at(i);

    if (conv == 0) {    
      gsl_blas_dgemv(CblasNoTrans, 1.0, P_upd, &Z.vector, 0.0, Vm);
      gsl_blas_ddot(&Z.vector, Vm, &f->at(i));
      f->at(i) += *sH;
      invf->at(i) = 1.0 / f->at(i);
    } else {
      f->at(i) = f->at(i-1);
      invf->at(i) = invf->at(i-1);
    }

    gsl_vector_memcpy(Vm_cp, Vm);
    gsl_vector_memcpy(Vm_cp2, Vm);

    vof->at(i) = v->at(i) * invf->at(i);

    mll[0] += 0.5 * (log(M_2PI * f->at(i)) + (v->at(i) * vof->at(i)));
    
    if (conv == 0) {    
      maux1 = gsl_matrix_view_array(gsl_vector_ptr(Vm_cp, 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_sub(P_upd, Mmm);
    }

    gsl_vector_memcpy(a_upd, a_pred);
    gsl_vector_scale(Vm_cp2, vof->at(i));
    gsl_vector_add(a_upd, Vm_cp2);

    K_irow = gsl_matrix_row(K, i);
    if (conv == 0) {    
      gsl_vector_scale(Vm_cp, invf->at(i));
      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);
      maux2 = gsl_matrix_view_array(gsl_vector_ptr(&Z.vector, 0), 1, m);
      gsl_matrix_memcpy(L[0].at(i), &T.matrix);
      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, &maux1.matrix, 
        &maux2.matrix, 1.0, L[0].at(i));
    } else {
      gsl_matrix_memcpy(L[0].at(i), L[0].at(i-1));
    }

    // check if convergence to the steady state has been reached

    if ((i > 0) & (conv == 0))
    {
      //if (i == 1)
      //{
      //  Kim1sum = 0.0;
      //}

      Kri = gsl_matrix_row(K, i);
      std::vector<double> vm((&Kri.vector)->data, (&Kri.vector)->data + m);
      //Kisum = std::accumulate(vm.begin(), vm.end(), 0.0);

      if (fabs(f->at(i) - f->at(i-1)) < *tol)
      {
        counter += 1;
      }
      //Kim1sum = Kisum;
      
      if (counter == *maxiter) {
        conv = 1;
        dim[5] = i;
      }
    }      
  }
  
  // deallocate memory
  
  gsl_vector_free(a_pred);
  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_matrix_free(Mmm);
}
Ejemplo n.º 23
0
int transient_simulation(LIST* list, gsl_matrix *matrix , gsl_matrix *c_matrix , gsl_vector *vector , gsl_vector *x, gsl_permutation* permutation)
{
    LIST* v_i_list;
    double fin_time = list->fin_time;
    double curr_time = 0;
    double time_step = list->time_step;
    time_step = 0.1;
    int flag;
    gsl_vector *prev_vector;
    gsl_vector *temp_vector;
    gsl_vector *e_t;
    gsl_vector *e_t_minus_one;

    gsl_vector *prev_x;

    gsl_matrix *right_matrix;
    gsl_matrix *tmp_matrix;
    gsl_matrix *curr_matrix;
    int i;

//  sparse simulation
    int vector_size;
    char method;
    int sign;

    sparse_matrix* sp_matrix;
    sparse_vector* sp_vector;
    sparse_vector* sp_x;
    gsl_vector* x_sparse;
    gsl_vector* vector_sparse;
    gsl_vector* vector_gsl;


    if(!list->sparse )
    {
        flag = create_mna(list, &matrix, &vector, 1 ,&c_matrix);
        //print_matrix_gsl(matrix);

        if(!flag ) {
            printf("Error creating mna system\n");
            return -1;
        }

        v_i_list = create_source_list(list);


        tmp_matrix = gsl_matrix_calloc(matrix->size1 , matrix->size1);
        curr_matrix = gsl_matrix_calloc(matrix->size1 , matrix->size1);
        right_matrix = gsl_matrix_calloc(matrix->size1,matrix->size2);

        if( !tmp_matrix || !right_matrix)
            return 0;

        gsl_matrix_memcpy(right_matrix,matrix);
        gsl_matrix_memcpy(tmp_matrix,matrix);
        gsl_matrix_memcpy(curr_matrix,matrix);


        x = gsl_vector_calloc(matrix->size1);
        if( !x ) {
            printf("X vector : no memory\n");
            exit(1);
        }

        if(list->transient_sim == METHOD_TR)
        {
            gsl_matrix_scale(c_matrix, (2/time_step));
            gsl_matrix_add(tmp_matrix,c_matrix);

            gsl_matrix_sub(right_matrix,c_matrix);

        } else
        {
            gsl_matrix_scale (c_matrix, 1/time_step);
            gsl_matrix_add(tmp_matrix,c_matrix);

            gsl_matrix_memcpy(right_matrix , c_matrix);
        }


        x = gsl_vector_calloc(matrix->size1);
        if( !x ) {
            printf("X vector : no memory\n");
            exit(1);
        }
    } else
    {
        method = list->solving_method;

        sp_matrix = (sparse_matrix*)create_mna_sparse( list , &sp_vector , &vector_size);
        if( !sp_matrix ) {
            fprintf(stderr, "Error creating MNA matrix \n");
            exit(1);
        }

        /* conversion of a double into a gsl */
        x_sparse = gsl_vector_calloc(sp_matrix->n);
        vector_sparse = gsl_vector_calloc(sp_matrix->n);
        double_vector_to_gsl_vector(vector_sparse,sp_vector,vector_size);

        sp_x = (sparse_vector*) malloc( vector_size * sizeof(sparse_vector));

    }
    /*
    printf("matrix:\n");
    print_matrix_gsl(matrix);
    printf("Tmp Matrix:\n");
    print_matrix_gsl(tmp_matrix);
    printf("right_matrix:\n");
    print_matrix_gsl(right_matrix);
    printf("c_matrix:\n");
    print_matrix_gsl(c_matrix);
    */

    prev_vector = gsl_vector_calloc(vector->size);
    temp_vector = gsl_vector_calloc(vector->size);

    if( !prev_vector || !temp_vector) {
        printf("Couldn't allocate memory for prev_vector & temp_vector");
        return 0;
    }

    prev_x = gsl_vector_calloc(x->size);

    if( !prev_x ) {
        printf("Couldn't allocate memory for prev_x");
        return 0;
    }

    e_t_minus_one = gsl_vector_calloc(vector->size);
    e_t = gsl_vector_calloc(vector->size);

    if( !e_t || !e_t_minus_one) {
        printf("Couldn't allocate memory for e(t) & e(t-1)");
        return 0;
    }



    printf("TRANSIENT: Solving Method = %d\n",list->solving_method);
    printf("fin_time: %lf\n",fin_time);

    for(curr_time = (-1)*time_step; curr_time <= 3; curr_time += time_step)
    {

        if(curr_time == 0)
        {
            gsl_vector_memcpy(e_t_minus_one,vector);		//dc initialize
            gsl_matrix_memcpy(matrix,tmp_matrix);
        }
        if(curr_time >= 0)
        {
            // x(t-1)
            gsl_vector_memcpy(prev_x,x);

            //print_vector_gsl(temp_vector);

            // right_matrix * x(t-1)
            lh_matrix_vector_mul( prev_x, right_matrix , temp_vector , NON_TRANSP);

            //print_vector_gsl(temp_vector);

            // calculate e(t)
            e_t = calc_right_hand_vect(v_i_list , e_t_minus_one ,curr_time);

            /*gsl_vector_memcpy(prev_vector,e_t);
            gsl_vector_add(prev_vector,e_t_minus_one);
            printf("e(t) + e(t-1) = \n");
            print_vector_gsl(prev_vector); */

            // get ready for next math operations
            gsl_vector_memcpy(prev_vector,e_t_minus_one);
            gsl_vector_memcpy(vector,e_t);

            if(list->transient_sim == METHOD_TR)
            {
                // e(t-1) - right_matrix*x(t-1)
                gsl_vector_sub(prev_vector, temp_vector);


                // e(t) + e(t-1) - right_matrix*x(t-1)
                gsl_vector_add(vector,prev_vector);

                printf("b = \n");
                print_vector_gsl(vector);

                // e_t_minus_one = e_t for the next iteration
                gsl_vector_memcpy(e_t_minus_one,e_t);

            }

            LIST_NODE* curr;

        }

        for(i = 0; i < x->size; i++)
            gsl_vector_set(x,i,0);


        if ( !list->sparse ) {

            /* Cholesky or LU */
            if( list->solving_method == METHOD_LU || list->solving_method == METHOD_CHOLESKY ) {

                gsl_matrix_memcpy(curr_matrix , matrix);

                decomposition(matrix,&permutation,&sign,list->solving_method);


                if(list->dc_sweep.node != NULL)
                {
                    dc_sweep(*list,matrix,vector,x,permutation,list->solving_method);
                }
                else
                {
                    int array_size = 1;

                    solve(matrix,vector,x,permutation,list->solving_method);
                    print_vector_gsl(x);

                    if(list->plot == PLOT_ON)
                    {
                        gsl_vector ** plot_array;

                        plot_array = plot_create_vector( array_size , x->size);
                        if(plot_array == NULL)
                        {
                            perror("Error while allocating the ploting array\n");
                            exit(0);
                        }

                        plot_set_vector_index(plot_array ,x ,0);
                        if ( list->solving_method == METHOD_LU )
                            plot_to_file(list->hashtable,plot_array,array_size,"results_plot_file_lu.txt");
                        else
                            plot_to_file(list->hashtable,plot_array,array_size,"results_plot_file_chol.txt");
                    }
                }
                gsl_matrix_memcpy(matrix , curr_matrix);

            }
            else if ( list->solving_method == METHOD_CG ) {

                if( list->dc_sweep.node != NULL ) {
                    dc_sweep(*list,matrix,vector,x,permutation,list->solving_method);
                }
                else {
                    iter_solve_cg( matrix , vector , x);


                    gsl_vector ** plot_array;

                    plot_array = plot_create_vector( 1 , x->size);
                    if(plot_array == NULL)
                    {
                        perror("Error while allocating the ploting array\n");
                        exit(0);
                    }

                    plot_set_vector_index(plot_array ,x ,0);

                    plot_to_file(list->hashtable,plot_array,1  ,"results_plot_file_cg.txt");
                }
            }
            else if( list->solving_method == METHOD_BICG) {

                if( list->dc_sweep.node != NULL ) {
                    dc_sweep(*list,matrix,vector,x,permutation,list->solving_method);
                }
                else {
                    iter_solve_bicg( matrix , vector , x);


                    gsl_vector ** plot_array;

                    plot_array = plot_create_vector( 1 , x->size);
                    if(plot_array == NULL)
                    {
                        perror("Error while allocating the ploting array\n");
                        exit(0);
                    }

                    plot_set_vector_index(plot_array ,x ,0);

                    plot_to_file(list->hashtable,plot_array,1  ,"results_plot_file_bicg.txt");
                }

            }
        }
        else {

            if( method == METHOD_LU_SPARSE ) {
                if( !sparse_solve_LU( sp_matrix,sp_vector,sp_x,vector_size) ) {
                    fprintf(stderr, "Solving Method Sparse LU failed\n" );
                }

                gsl_vector ** plot_array;

                plot_array = plot_create_vector( 1 , vector_size);
                if(plot_array == NULL)
                {
                    perror("Error while allocating the ploting array\n");
                    exit(0);
                }
                vector_gsl = gsl_vector_calloc(vector_size);
                double_vector_to_gsl_vector(vector_gsl,sp_x,vector_size);

                plot_set_vector_index(plot_array ,vector_gsl ,0);

                plot_to_file(list->hashtable,plot_array,1  ,"results_plot_file_lu_sparse.txt");


            }
            else if( method == METHOD_CHOLESKY_SPARSE ) {
                if( !sparse_solve_cholesky( sp_matrix,sp_vector,sp_x,vector_size) ) {
                    fprintf(stderr, "Solving Method Sparse Cholesky failed\n" );
                }

            }
            else if ( method == METHOD_CG_SPARSE ) {
                if( !sparse_solve_cg( sp_matrix,vector_sparse,x_sparse) ) {
                    fprintf(stderr, "Solving Method Sparse CG failed\n" );
                }
                gsl_vector ** plot_array;

                plot_array = plot_create_vector( 1 , x_sparse->size);
                if(plot_array == NULL)
                {
                    perror("Error while allocating the ploting array\n");
                    exit(0);
                }

                plot_set_vector_index(plot_array ,x_sparse ,0);

                plot_to_file(list->hashtable,plot_array,1  ,"results_plot_file_sparse_cg.txt");
            }
            else if( method == METHOD_BICG_SPARSE ) {
                if( !sparse_solve_bicg( sp_matrix, vector_sparse, x_sparse) ) {
                    fprintf(stderr, "Solving Method Sparse BiCG failed\n" );
                }
                gsl_vector ** plot_array;

                plot_array = plot_create_vector( 1 , x_sparse->size);
                if(plot_array == NULL)
                {
                    perror("Error while allocating the ploting array\n");
                    exit(0);
                }

                plot_set_vector_index(plot_array ,x_sparse ,0);

                plot_to_file(list->hashtable,plot_array,1  ,"results_plot_file_sparse_bicg.txt");


            }
            else {
                fprintf(stderr, "Solving method not specified\n");
            }
        }
    }
    /* clean up before exit */
    if(list->sparse)
        cs_spfree(sp_matrix);
    free(vector);
    free(x);
    return 1;

}
Ejemplo n.º 24
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");

}