miscmaths.cc

    apk = A * pk;  // the *big* calculation in this algorithm

    ColumnVector pap = pk.t() * apk;
    if (pap.AsScalar()<0) { 
      cerr << "ERROR:: Conj Grad - negative eigenvector found (matrix must be symmetric and positive-definite)\nAborting ... " << endl; 
      return 2;
    } else if (pap.AsScalar()<1e-10) { 
      cerr << "WARNING:: Conj Grad - nearly null eigenvector found (terminating early)" << endl; 
      return 1;
    } else {
      alphak = rk1rk1 / pap.AsScalar();
    }
    x = x + alphak * pk;  // note LHS is x(k) and RHS is x(k-1) in algorithm
    rk2 = rk1;  // update prior to the next step
    rk1 = rk1 - alphak * apk;  // note LHS is r(k) in algorithm
  }
  return 0;
}

int conjgrad(ColumnVector& x, const Matrix& A, const ColumnVector& b, int maxit)
{ 
  return conjgrad(x,A,b,maxit,1e-10);
}

float csevl(const float x, const ColumnVector& cs, const int n)
  {
 
    float b0 = 0;
    float b1 = 0;
    float b2 = 0;
    const float twox=2*x;
    
    for(int i=1; i<=n; i++)
      {
	b2=b1;
	b1=b0;
	b0=twox*b1-b2+cs(n+1-i);
      }
    
    return 0.5*(b0-b2);
  }

  float digamma(const float x)
  { 
    int ntapsi(16);
    int ntpsi(23);
    ColumnVector psics(ntpsi);
    ColumnVector apsics(ntapsi);

    psics << -.038057080835217922E0<<
      .49141539302938713E0<<
      -.056815747821244730E0<<
      .008357821225914313E0<<
      -.001333232857994342E0<<
      .000220313287069308E0<<
      -.000037040238178456E0<<
      .000006283793654854E0<<
      -.000001071263908506E0<<
      .000000183128394654E0<<
      -.000000031353509361E0<<
      .000000005372808776E0<<
      -.000000000921168141E0<<
      .000000000157981265E0<<
      -.000000000027098646E0<<
      .000000000004648722E0<<
      -.000000000000797527E0<<
      .000000000000136827E0<<
      -.000000000000023475E0<<
      .000000000000004027E0<<
      -.000000000000000691E0<<
      .000000000000000118E0<<
      -.000000000000000020E0;
	  
    apsics <<-.0204749044678185E0<<
      -.0101801271534859E0<<
      .0000559718725387E0<<
      -.0000012917176570E0<<
      .0000000572858606E0<<
      -.0000000038213539E0<<
      .0000000003397434E0<<
      -.0000000000374838E0<<
      .0000000000048990E0<<
      -.0000000000007344E0<<
      .0000000000001233E0<<
      -.0000000000000228E0<<
      .0000000000000045E0<<
      -.0000000000000009E0<<
      .0000000000000002E0<<
      -.0000000000000000E0;

    float y = fabs(x);
    float psi;

    if(y<2.0)
      {
	// do we need to deal with the following case?
	// c psi(x) for -2. .lt. x .lt. 2.

	int n = int(floor(x));
	y = x - n;
	n = n - 1;
	psi = csevl(2*y-1, psics, ntpsi);
	if(n==-1)
	  {
	    psi = psi - 1.0/x;
	  }
      }
    else
      {
	const float aux = csevl(8/(Sqr(y))-1, apsics, ntapsi);
	psi = std::log(x) - 0.5/x + aux;
      }
    
    return psi;
  }

  void glm_vb(const Matrix& X, const ColumnVector& Y, ColumnVector& B, SymmetricMatrix& ilambda_B, int niters)
  {
    // Does Variational Bayes inference on GLM Y=XB+e with ARD priors on B
    // design matrix X should be num_tpts*num_evs

    /////////////////////
    // setup
    OUT("Setup");

    int ntpts=Y.Nrows();
    int nevs=X.Ncols();

    if(ntpts!=X.Nrows())
      throw Exception("COCK");

    OUT(nevs);
    OUT(ntpts);

    ColumnVector gam_m(nevs);
    gam_m=1e10;
    float gam_y;

    ColumnVector lambdaB(nevs);
    if(nevs<ntpts-10)
      {
	// initialise with OLS
	B=pinv(X)*Y;
	ColumnVector res=Y-X*B;
	gam_y=(ntpts-nevs)/(res.t()*res).AsScalar();

	ilambda_B << (X.t()*X*gam_y).i();
	lambdaB=0;
	for(int l=1; l <= nevs; l++)
	  {
	    lambdaB(l)=ilambda_B(l,l);
	  }
      }
    else
      {
	OUT("no ols");
	B.ReSize(nevs);
	B=0;
	lambdaB=1;

// 	ColumnVector res=Y-X*B;
// 	gam_y=ntpts/(res.t()*res).AsScalar();

	gam_y=10;
      }

//     OUT(B(1));
//     OUT(lambdaB(1));

    float trace_ilambdaZZ=1;

    SymmetricMatrix ZZ;
    ZZ << X.t()*X;

    Matrix ZY = X.t()*Y;

    float YY=0;
    for(int t=1; t <= ntpts; t++)
      YY += Sqr(Y(t));

    /////////////////////
    // iterate
    OUT("Iterate");

    int i = 1;;
    for(; i<=niters; i++)
      {
	cout<<i<<",";
	////////////////////
	// update phim
	for(int li=1; li <= nevs; li++)
	  {
	    float b_m0 = 1e10;
	    float c_m0 = 2;

	    float c_m = 1.0/2.0 + c_m0;	    
	    float b_m = 1.0/(0.5*(Sqr(B(li))+lambdaB(li))+1.0/b_m0);
	    gam_m(li) = b_m*c_m;	    
	  }

// 	OUT(gam_m(1));

	////////////////////
	// update B
	ColumnVector beta(nevs);
	beta = 0;
	SymmetricMatrix lambda_B(nevs);
	lambda_B = 0;

	for(int lj=1; lj <= nevs; lj++)
	  lambda_B(lj,lj)=gam_m(lj);

	SymmetricMatrix tmp = lambda_B + gam_y*ZZ;
	lambda_B << tmp;

	beta += gam_y*ZY;

	ilambda_B << lambda_B.i();
	B=ilambda_B*beta;

	lambdaB.ReSize(nevs);
	lambdaB=0;
	for(int l=1; l <= nevs; l++)
	  {
	    lambdaB(l)=ilambda_B(l,l);
	  }
	
	////////////////////
	// compute trace for noise precision phiy update
	
	SymmetricMatrix tmp3;
	tmp3 << ilambda_B;
	
	SymmetricMatrix tmp2;
	tmp2 << tmp3*ZZ;
	
	trace_ilambdaZZ=tmp2.Trace();	
// 	OUT(trace_ilambdaZZ);


	/////////////////////
	// update phiy
	float b_y0 = 1e10;
	float c_y0 = 1;

	float c_y = (ntpts-1)/2.0 + c_y0;

	float sum = YY + (B.t()*ZZ*B).AsScalar() - 2*(B.t()*ZY).AsScalar();
	
	float b_y = 1.0/(0.5*(sum + trace_ilambdaZZ)+1/b_y0);
	
	gam_y = b_y*c_y;

// 	OUT(gam_y);	     

      }

    cout << endl;
  }

vector<float> ColumnVector2vector(const ColumnVector& col)
{
  vector<float> vec(col.Nrows());
  
  for(int c = 0; c < col.Nrows(); c++)
    vec[c] = col(c+1);
  
  return vec;
}

/////////////////////////////////////////////////////////////////////////////////////////////////////

// Uninteresting byte swapping functions

typedef struct { unsigned char a,b ; } TWObytes ;

void Swap_2bytes( int n , void *ar )    /* 2 bytes at a time */
{
  register int ii ;
   register TWObytes *tb = (TWObytes *)ar ;
   register unsigned char tt ;

   for( ii=0 ; ii < n ; ii++ ){
     tt = tb[ii].a ; tb[ii].a = tb[ii].b ; tb[ii].b = tt ;
   }
   return ;
}

/*---------------------------------------------------------------------------*/

typedef struct { unsigned char a,b,c,d ; } FOURbytes ;

void Swap_4bytes( int n , void *ar )    /* 4 bytes at a time */
{
   register int ii ;
   register FOURbytes *tb = (FOURbytes *)ar ;
   register unsigned char tt ;

   for( ii=0 ; ii < n ; ii++ ){
     tt = tb[ii].a ; tb[ii].a = tb[ii].d ; tb[ii].d = tt ;
     tt = tb[ii].b ; tb[ii].b = tb[ii].c ; tb[ii].c = tt ;
   }
   return ;
}

/*---------------------------------------------------------------------------*/

typedef struct { unsigned char a,b,c,d , D,C,B,A ; } EIGHTbytes ;

void Swap_8bytes( int n , void *ar )    /* 8 bytes at a time */
{
   register int ii ;
   register EIGHTbytes *tb = (EIGHTbytes *)ar ;
   register unsigned char tt ;

   for( ii=0 ; ii < n ; ii++ ){
     tt = tb[ii].a ; tb[ii].a = tb[ii].A ; tb[ii].A = tt ;
     tt = tb[ii].b ; tb[ii].b = tb[ii].B ; tb[ii].B = tt ;
     tt = tb[ii].c ; tb[ii].c = tb[ii].C ; tb[ii].C = tt ;
     tt = tb[ii].d ; tb[ii].d = tb[ii].D ; tb[ii].D = tt ;
   }
   return ;
}

/*---------------------------------------------------------------------------*/

typedef struct { unsigned char a,b,c,d,e,f,g,h ,
                               H,G,F,E,D,C,B,A  ; } SIXTEENbytes ;

void Swap_16bytes( int n , void *ar )    /* 16 bytes at a time */
{
   register int ii ;
   register SIXTEENbytes *tb = (SIXTEENbytes *)ar ;
   register unsigned char tt ;

   for( ii=0 ; ii < n ; ii++ ){
     tt = tb[ii].a ; tb[ii].a = tb[ii].A ; tb[ii].A = tt ;
     tt = tb[ii].b ; tb[ii].b = tb[ii].B ; tb[ii].B = tt ;
     tt = tb[ii].c ; tb[ii].c = tb[ii].C ; tb[ii].C = tt ;
     tt = tb[ii].d ; tb[ii].d = tb[ii].D ; tb[ii].D = tt ;

     tt = tb[ii].e ; tb[ii].e = tb[ii].E ; tb[ii].E = tt ;
     tt = tb[ii].f ; tb[ii].f = tb[ii].F ; tb[ii].F = tt ;
     tt = tb[ii].g ; tb[ii].g = tb[ii].G ; tb[ii].G = tt ;
     tt = tb[ii].h ; tb[ii].h = tb[ii].H ; tb[ii].H = tt ;
   }
   return ;
}

/*---------------------------------------------------------------------------*/

void Swap_Nbytes( int n , int siz , void *ar )  /* subsuming case */
{
   switch( siz ){
     case 2:  Swap_2bytes ( n , ar ) ; break ;
     case 4:  Swap_4bytes ( n , ar ) ; break ;
     case 8:  Swap_8bytes ( n , ar ) ; break ;
     case 16: Swap_16bytes( n , ar ) ; break ;
   }
   return ;
}

// end namespace MISCMATHS
}