mog2.c

he AutoMix package is a C program for Unix-like systems, implementing the automatic reversible jum

#include<stdio.h>
#include<math.h>
#include<stdlib.h>

#include "mog2.h"

#define hpi 1.5707963267948966192313216916398
#define pi 3.141592653589793238
#define tpi 6.283185307179586477

#define MOD(a,b) ( a-b*floor(a/b) )

double my_infinity(void) {
  double zero = 0;
  return 1.0/zero;
}
double my_nan(void) {
  double zero = 0;
  return zero/zero;
}
#define INFINITY my_infinity()
#define NAN my_nan()

#define KMAX 4

double hyper_mean[2] = {0.0, 0.0};
double hyper_precScale = 1.0;
double hyper_a = 2.0;
double hyper_b = 1.0;
double hyper_delta = 1.0;
double hyper_w[KMAX];

double hyper_nu = 2; /* wishart DOF */
double hyper_W[2][2] = { {1.0,0.0}, {0.0,1.0} };

double normrnd(double mean, double prec);
double gammarnd(double a, double b);
void dirichletrnd(int dim, double *param, double *sample);

double normprob(double mean, double var, double x);

double lognormprob(double mean, double var, double x);
double loggammaprob( double a, double b, double x );
double logdirichletprob(int dim, double *param, double *data);
double lognormprob2(double mean[2], double prec[2][2], double scale, double x[2]);
double logwishartprob2(double nu, double W[2][2], double x[2][2]);

double boxm();

extern double loggamma(double s);
extern double sdrand();
extern double rgamma(double s);

/* Function to return number of models */
void getkmax(int *kmax){
	*kmax = KMAX;
	return;
}

/* Function to return the dimension of each model */ 
/* each model has nc componenets, each having a mean and covariance */
/* and nc-1 mixing coefficients (sum-to-1 constraint) */
/* covariance is parameterized by eigenvalues + axis direction */
void getnk(int kmax,int *nk){

	int k;
	int nc; // # mixture componenents
	for(k=0;k<kmax;k++){
		nc = k+1;
		nk[k] = 5*nc + nc-1;
	}
	return;

}

/* Function to return initial conditions for RWM runs */
/* sample them from the prior */

int haveDataStats = 0; // only do the mean/std computation once
double dataMean[2] = {0,0}, dataVar[2] = {0,0};

void getic(int k, int nkk, double *theta){
	int ci, nc, di;
	double pmean[2], e1, e2, th, pprec[2][2];
	double pweight[KMAX];
	double c, s;

	if( !haveDataStats ) {
		for( di=0; di<nData; di++ ) {
			dataMean[0] += data[di][0];
			dataMean[1] += data[di][1];
		}
		dataMean[0] /= (double)nData;
		dataMean[1] /= (double)nData;
		for( di=0; di<nData; di++ ) {
			dataVar[0] += (data[di][0]-dataMean[0])*(data[di][0]-dataMean[0]);
			dataVar[1] += (data[di][1]-dataMean[1])*(data[di][1]-dataMean[1]);
		}
		dataVar[0] /= (double)(nData-1);
		dataVar[1] /= (double)(nData-1);

		hyper_mean[0] = dataMean[0];
		hyper_mean[1] = dataMean[1];

		hyper_W[0][0] = 1;//1.0/dataVar[0];
		hyper_W[1][1] = 1;//1.0/dataVar[1];
	}

	nc = k+1;

	for( ci=0; ci<nc; ci++ ) 
		hyper_w[ci] = hyper_delta;

	dirichletrnd(nc, hyper_w, pweight);

	for( ci=0; ci<nc; ci++ ) {
		e1 = gammarnd(2, 2.0/dataVar[0]);
		e2 = gammarnd(2, 2.0/dataVar[1]);
		th = sdrand()*tpi;
		
		c = cos(th); s = sin(th);
		pprec[0][0] = e1*c*c + e2*s*s;
		pprec[0][1] = - e1*s*c + e2*s*c;
		pprec[1][1] = e1*s*s + e2*c*c;

		pmean[0] = normrnd(hyper_mean[0], 1.0/(hyper_precScale*pprec[0][0]) );
		pmean[1] = normrnd(hyper_mean[1]+pprec[0][1]/pprec[0][0]*(pmean[0]-hyper_mean[0]), 
			pprec[1][1] - pprec[0][1]/pprec[0][0]*pprec[0][1] );

		theta[ci*6+0] = pmean[0];
		theta[ci*6+1] = pmean[1];
		theta[ci*6+2] = e1;
		theta[ci*6+3] = e2;
		theta[ci*6+4] = th;

		if( ci<nc-1 )
			theta[ci*6+5] = pweight[ci];
	}
}

/* Function to return log target distribution up to additive const at (k,theta)
   value also returned in llh */

double weight[KMAX], mean[KMAX][2], prec[KMAX][2][2], covDet[KMAX];
int ccount=0;
double lpost(int k,int nkk,double *theta, double *llh){ 	 

	int di, nd;
	int ci, nc;

	double sum;
	double llikelihood, lprior;
	
	double e1, e2, th;
	double c, s;

	nc = k+1;

	// log prior contribution
	lprior = 0;

	sum = 0;
	// load the current parameters
	for( ci=0; ci<nc; ci++ ) {
		hyper_w[ci] = hyper_delta;

		mean[ci][0] = theta[ci*6+0];
		mean[ci][1] = theta[ci*6+1];
		e1 = theta[ci*6+2];
		e2 = theta[ci*6+3];
		th = theta[ci*6+4];
		
//		if( th<0 || th>tpi ) {
//			return -INFINITY;
//		}
		if( th<-hpi || th>hpi ) {
			th += hpi;
			th = MOD(th,pi) - hpi;
		}
		theta[ci*6+4] = th;

//		if( e1<=0 || e2<=0 ) {
//			return -INFINITY; // RWM proposed impossible precision
//		}
		if( e1==0 || e2==0 ) return -INFINITY;
		if( e1<0 ) e1 = -e1;
		if( e2<0 ) e2 = -e2;

		theta[ci*6+2] = e1;
		theta[ci*6+3] = e2;
	
		c = cos(th); s = sin(th);
		prec[ci][0][0] = e1*c*c + e2*s*s;
		prec[ci][0][1] = -e1*s*c + e2*s*c;
		prec[ci][1][0] = prec[ci][0][1];
		prec[ci][1][1] = e1*s*s + e2*c*c;

		covDet[ci] = 1.0/( prec[ci][0][0]*prec[ci][1][1] - prec[ci][1][0]*prec[ci][0][1] );

		if( ci<nc-1 ) {
			weight[ci] = theta[ci*6+5];
			sum += weight[ci];
		} else {
			weight[ci] = 1.0-sum;
		}

		if( nc>1 && ((weight[ci]<=0.0)||(weight[ci]>=1.0) ) ){
			return -INFINITY; // RWM proposed impossible weights
		}

	}

		
	// prior on mixing coefficients
	lprior += logdirichletprob(nc, hyper_w, weight );
	for( ci=0; ci<nc; ci++ ) {
		// prior on mean
		lprior += lognormprob2( hyper_mean, prec[ci], hyper_precScale, mean[ci] ) ;

		// prior on precision
		lprior += logwishartprob2( hyper_nu, hyper_W, prec[ci] );
	}
  

	// log likelihood contribution
	llikelihood = 0;

	nd = nData;
	for( di=0; di<nd; di++ ) {
		double ldata = 0;
		double coeff[KMAX];
		double exponent[KMAX];
		double maxExponent = -INFINITY;
		double d[2];

		double mahalad;

		for( ci=0; ci<nc; ci++ ) {
			d[0] = data[di][0] - mean[ci][0];
			d[1] = data[di][1] - mean[ci][1];

			mahalad = ( d[0]*( prec[ci][0][0]*d[0] + prec[ci][0][1]*d[1]) +
				d[1]*( prec[ci][1][0]*d[0] + prec[ci][1][1]*d[1] ) );

			exponent[ci] = -mahalad/2.0;
			coeff[ci] = weight[ci]/(tpi*sqrt(covDet[ci]));

			if(exponent[ci]>maxExponent) maxExponent = exponent[ci];
		}
		for( ci=0; ci<nc; ci++ ) {
			ldata += coeff[ci]*exp( exponent[ci] - maxExponent );
		}
		ldata = log(ldata) + maxExponent;

		llikelihood += ldata;
	}

	*llh = llikelihood;

	return llikelihood + lprior;
} 

double normrnd(double mean, double var) {
	return mean + boxm()*sqrt(var);
}

double gammarnd(double a, double b) {
	return rgamma(a)*b;
}

void dirichletrnd(int dim, double *param, double *sample) {

	int di;
	double sum = 0.0;
	for(di=0; di<dim; di++) {
		sample[di] = rgamma(param[di]);
		sum += sample[di];
	}
	for(di=0; di<dim; di++) {
		sample[di] /= sum;
	}
}

double normprob(double mean, double var, double x) {
	double arg = x-mean;
	return exp(-arg*arg/(2.0*var)) / sqrt(tpi*var);
}

double lognormprob(double mean, double var, double x) {
	double arg = x-mean;
	return -arg*arg/(2.0*var) - log(sqrt(tpi*var));
}

double lognormprob2(double mean[2], double prec[2][2], double scale, double xx[2]) {
	double x[2];
	double mahalad;
	double det;

	x[0] = xx[0] - mean[0];
	x[1] = xx[1] - mean[1];

	mahalad = scale*(x[0]*( prec[0][0]*x[0] + prec[0][1]*x[1]) +
		x[1]*( prec[1][0]*x[0] + prec[1][1]*x[1] ));
	det = 1.0 / (scale*(prec[0][0]*prec[1][1] - prec[0][1]*prec[1][0]) );

	return -mahalad/2.0 - log(tpi*sqrt(det));
}

// using bishop's defn, but realize that what he calls the wishart distro 
// is actually the inverse (it's confusing precision with covariance)
double logwishartprob2(double nu, double W[2][2], double x[2][2]) {
	double xdet, Wdet, xWdet;
	double xW[2][2];
	double d = 2.0;
	double d2 = (d+1.0)/2.0;

	Wdet = W[0][0]*W[1][1] - W[0][1]*W[1][0];
	xdet = x[0][0]*x[1][1] - x[0][1]*x[1][0];

	xW[0][0] = x[0][0]*W[0][0] + x[0][1]*W[1][0];
	xW[1][0] = x[1][0]*W[0][0] + x[1][1]*W[1][0];
	xW[0][1] = x[0][0]*W[0][1] + x[0][1]*W[1][1];
	xW[1][1] = x[1][0]*W[0][1] + x[1][1]*W[1][1];

	xWdet = xW[0][0]*xW[1][1] - xW[0][1]*xW[1][0];

	return (nu-d2)*log(xWdet) - (xW[0][0]+xW[1][1]) + d2*log(Wdet) - (loggamma(nu/2.0) + loggamma((nu-1)/2.0));

	/*
		double xdet, Wdet;
	double xW[2][2];
	double B;

	Wdet = W[0][0]*W[1][1] - W[0][1]*W[1][0];
	xdet = x[0][0]*x[1][1] - x[0][1]*x[1][0];
	xW[0][0] = x[0][0]*W[0][0] + x[0][1]*W[1][0];
//	xW[1][0] = x[1][0]*W[0][0] + x[1][1]*W[1][0];
//	xW[0][1] = x[0][0]*W[0][1] + x[0][1]*W[1][1];
	xW[1][1] = x[1][0]*W[0][1] + x[1][1]*W[1][1];

	B = - ( nu*log(2.0) + log(pi)/2.0 + loggamma(nu/2.0) + loggamma((nu-1)/2.0) ) + nu/2.0*log(Wdet);

	return B + (nu-3.0)/2.0*log(xdet) -(xW[0][0]+xW[1][1])/2.0;
	*/
}

double loggammaprob( double a, double b, double x ) {
	 return -( a*log(b) + loggamma(a) ) + (a-1.0)*log(x) -x/b;
}

double logdirichletprob(int dim, double *param, double *data) {
	
	int di;
	double pr = 0;
	double paramSum = 0;
	double gammaSum = 0;
	for( di=0; di<dim; di++ ) {
		pr += (param[di]-1.0)*log( data[di] );

		paramSum += param[di];
		gammaSum += loggamma(param[di]);
	}
	pr += loggamma(paramSum) - gammaSum;
	
	return pr;
}

double boxm(){

  /*Function for returning single N(0,1) variable */

  double u1,u2,out;

  u1=sdrand();
  u2=sdrand();
  u1=tpi*u1;
  u2=sqrt(-2.0*log(u2));
  out=u2*cos(u1);

  return out;

}