c@241: /*
c@241:  *  hmm.c
c@241:  *  soundbite
c@241:  *
c@241:  *  Created by Mark Levy on 12/02/2006.
c@241:  *  Copyright 2006 Centre for Digital Music, Queen Mary, University of London. All rights reserved.
c@241:  *
c@241:  */
c@241: 
c@241: #include <stdio.h>
c@241: #include <math.h>
c@241: #include <stdlib.h>
c@241: #include <float.h>
c@241: #include <time.h>				/* to seed random number generator */
c@241: #include "clapack.h"		/* LAPACK for matrix inversion */
c@241: #ifdef _MAC_OS_X
c@241: #include <vecLib/cblas.h>
c@241: #else
c@241: #include <cblas.h>		/* BLAS for matrix multiplication */
c@241: #endif
c@241: 
c@241: #include "hmm.h"
c@241: 
c@241: model_t* hmm_init(double** x, int T, int L, int N)
c@241: {
c@241: 	int i, j, d, e, t;
c@241: 	double s, ss;
c@241: 	
c@241: 	model_t* model;
c@241: 	model = (model_t*) malloc(sizeof(model_t));
c@241: 	model->N = N;
c@241: 	model->L = L;	
c@241: 	model->p0 = (double*) malloc(N*sizeof(double));
c@241: 	model->a = (double**) malloc(N*sizeof(double*));
c@241: 	model->mu = (double**) malloc(N*sizeof(double*));
c@241: 	for (i = 0; i < N; i++)
c@241: 	{
c@241: 		model->a[i] = (double*) malloc(N*sizeof(double));
c@241: 		model->mu[i] = (double*) malloc(L*sizeof(double));
c@241: 	}
c@241: 	model->cov = (double**) malloc(L*sizeof(double*));
c@241: 	for (i = 0; i < L; i++)
c@241: 		model->cov[i] = (double*) malloc(L*sizeof(double));
c@241: 	
c@241: 	srand(time(0));
c@241: 	double* global_mean = (double*) malloc(L*sizeof(double));
c@241: 	
c@241: 	/* find global mean */
c@241: 	for (d = 0; d < L; d++)
c@241: 	{
c@241: 		global_mean[d] = 0;
c@241: 		for (t = 0; t < T; t++)
c@241: 			global_mean[d] += x[t][d];
c@241: 		global_mean[d] /= T;
c@241: 	}
c@241: 	
c@241: 	/* calculate global diagonal covariance */
c@241: 	for (d = 0; d < L; d++)
c@241: 	{
c@241: 		for (e = 0; e < L; e++)
c@241: 			model->cov[d][e] = 0;
c@241: 		for (t = 0; t < T; t++)
c@241: 			model->cov[d][d] += (x[t][d] - global_mean[d]) * (x[t][d] - global_mean[d]);
c@241: 		model->cov[d][d] /= T-1;
c@241: 	}
c@241: 	
c@241: 	/* set all means close to global mean */
c@241: 	for (i = 0; i < N; i++)
c@241: 	{
c@241: 		for (d = 0; d < L; d++)
c@241: 		{
c@241: 			/* add some random noise related to covariance */
c@241: 			/* ideally the random number would be Gaussian(0,1), as a hack we make it uniform on [-0.25,0.25] */
c@241: 			model->mu[i][d] = global_mean[d] + (0.5 * rand() / (double) RAND_MAX - 0.25) * sqrt(model->cov[d][d]);
c@241: 		}
c@241: 	}	
c@241: 	
c@241: 	/* random intial and transition probs */
c@241: 	s = 0;
c@241: 	for (i = 0; i < N; i++)
c@241: 	{
c@241: 		model->p0[i] = 1 + rand() / (double) RAND_MAX;
c@241: 		s += model->p0[i];
c@241: 		ss = 0;
c@241: 		for (j = 0; j < N; j++)
c@241: 		{
c@241: 			model->a[i][j] = 1 + rand() / (double) RAND_MAX;
c@241: 			ss += model->a[i][j];
c@241: 		}
c@241: 		for (j = 0; j < N; j++)
c@241: 		{
c@241: 			model->a[i][j] /= ss;
c@241: 		}
c@241: 	}
c@241: 	for (i = 0; i < N; i++)
c@241: 		model->p0[i] /= s;
c@241: 	
c@241: 	free(global_mean);
c@241: 	
c@241: 	return model;
c@241: }
c@241: 
c@241: void hmm_close(model_t* model)
c@241: {
c@241: 	int i;
c@241: 	
c@241: 	for (i = 0; i < model->N; i++)
c@241: 	{
c@241: 		free(model->a[i]);
c@241: 		free(model->mu[i]);
c@241: 	}
c@241: 	free(model->a);
c@241: 	free(model->mu);
c@241: 	for (i = 0; i < model->L; i++)
c@241: 		free(model->cov[i]);
c@241: 	free(model->cov);	  
c@241: 	free(model);	
c@241: }
c@241: 
c@241: void hmm_train(double** x, int T, model_t* model)
c@241: {
c@241: 	int i, t;
c@241: 	double loglik;	/* overall log-likelihood at each iteration */
c@241: 	
c@241: 	int N = model->N;
c@241: 	int L = model->L;
c@241: 	double* p0 = model->p0;
c@241: 	double** a = model->a;
c@241: 	double** mu = model->mu;
c@241: 	double** cov = model->cov;
c@241: 	
c@241: 	/* allocate memory */	
c@241: 	double** gamma = (double**) malloc(T*sizeof(double*));
c@241: 	double*** xi = (double***) malloc(T*sizeof(double**));
c@241: 	for (t = 0; t < T; t++)
c@241: 	{
c@241: 		gamma[t] = (double*) malloc(N*sizeof(double));
c@241: 		xi[t] = (double**) malloc(N*sizeof(double*));
c@241: 		for (i = 0; i < N; i++)
c@241: 			xi[t][i] = (double*) malloc(N*sizeof(double));
c@241: 	}
c@241: 	
c@241: 	/* temporary memory */
c@241: 	double* gauss_y = (double*) malloc(L*sizeof(double));
c@241: 	double* gauss_z = (double*) malloc(L*sizeof(double));	
c@241: 			
c@241: 	/* obs probs P(j|{x}) */
c@241: 	double** b = (double**) malloc(T*sizeof(double*));
c@241: 	for (t = 0; t < T; t++)
c@241: 		b[t] = (double*) malloc(N*sizeof(double));
c@241: 	
c@241: 	/* inverse covariance and its determinant */
c@241: 	double** icov = (double**) malloc(L*sizeof(double*));
c@241: 	for (i = 0; i < L; i++)
c@241: 		icov[i] = (double*) malloc(L*sizeof(double));
c@241: 	double detcov;
c@241: 	
c@241: 	double thresh = 0.0001;
c@241: 	int niter = 50;	
c@241: 	int iter = 0;
c@241: 	double loglik1, loglik2;
c@241: 	while(iter < niter && !(iter > 1 && (loglik - loglik1) < thresh * (loglik1 - loglik2)))	
c@241: 	{
c@241: 		++iter;
c@241: 		
c@241: 		fprintf(stderr, "calculating obsprobs...\n");
c@241: 		fflush(stderr);
c@241: 		
c@241: 		/* precalculate obs probs */
c@241: 		invert(cov, L, icov, &detcov);
c@241: 		
c@241: 		for (t = 0; t < T; t++)
c@241: 		{
c@241: 			//int allzero = 1;
c@241: 			for (i = 0; i < N; i++)
c@241: 			{
c@241: 				b[t][i] = exp(loggauss(x[t], L, mu[i], icov, detcov, gauss_y, gauss_z));
c@241: 		
c@241: 				//if (b[t][i] != 0)
c@241: 				//	allzero = 0;
c@241: 			}
c@241: 			/*
c@241: 			if (allzero)
c@241: 			{
c@241: 				printf("all the b[t][i] were zero for t = %d, correcting...\n", t);
c@241: 				for (i = 0; i < N; i++)
c@241: 				{
c@241: 					b[t][i] = 0.00001;
c@241: 				}
c@241: 			}
c@241: 			*/
c@241: 		}
c@241: 		
c@241: 		fprintf(stderr, "forwards-backwards...\n");
c@241: 		fflush(stderr);
c@241: 		
c@241: 		forward_backwards(xi, gamma, &loglik, &loglik1, &loglik2, iter, N, T, p0, a, b);
c@241: 		
c@241: 		fprintf(stderr, "iteration %d: loglik = %f\n", iter, loglik);		
c@241: 		fprintf(stderr, "re-estimation...\n");
c@241: 		fflush(stderr);
c@241: 		
c@241: 		baum_welch(p0, a, mu, cov, N, T, L, x, xi, gamma);
c@241: 			
c@241: 		/*
c@241: 		printf("a:\n");
c@241: 		for (i = 0; i < model->N; i++)
c@241: 		{
c@241: 			for (j = 0; j < model->N; j++)
c@241: 				printf("%f ", model->a[i][j]);
c@241: 			printf("\n");
c@241: 		}
c@241: 		printf("\n\n");
c@241: 		 */
c@241: 		//hmm_print(model);
c@241: 	}
c@241: 	
c@241: 	/* deallocate memory */
c@241: 	for (t = 0; t < T; t++)
c@241: 	{
c@241: 		free(gamma[t]);
c@241: 		free(b[t]);
c@241: 		for (i = 0; i < N; i++)
c@241: 			free(xi[t][i]);
c@241: 		free(xi[t]);
c@241: 	}
c@241: 	free(gamma);
c@241: 	free(xi);
c@241: 	free(b);	
c@241: 	
c@241: 	for (i = 0; i < L; i++)
c@241: 		free(icov[i]);
c@241: 	free(icov);
c@241: 	
c@241: 	free(gauss_y);
c@241: 	free(gauss_z);
c@241: }
c@241: 
c@241: void mlss_reestimate(double* p0, double** a, double** mu, double** cov, int N, int T, int L, int* q, double** x)
c@241: {
c@241: 	/* fit a single Gaussian to observations in each state */
c@241: 	
c@241: 	/* calculate the mean observation in each state */
c@241: 	
c@241: 	/* calculate the overall covariance */
c@241: 	
c@241: 	/* count transitions */
c@241: 	
c@241: 	/* estimate initial probs from transitions (???) */
c@241: }
c@241: 
c@241: void baum_welch(double* p0, double** a, double** mu, double** cov, int N, int T, int L, double** x, double*** xi, double** gamma)
c@241: {
c@241: 	int i, j, t;
c@241: 	
c@241: 	double* sum_gamma = (double*) malloc(N*sizeof(double));
c@241: 	
c@241: 	/* temporary memory */
c@241: 	double* u = (double*) malloc(L*L*sizeof(double));
c@241: 	double* yy = (double*) malloc(T*L*sizeof(double));
c@241: 	double* yy2 = (double*) malloc(T*L*sizeof(double));	
c@241: 	
c@241: 	/* re-estimate transition probs */
c@241: 	for (i = 0; i < N; i++)
c@241: 	{
c@241: 		sum_gamma[i] = 0;
c@241: 		for (t = 0; t < T-1; t++)
c@241: 			sum_gamma[i] += gamma[t][i];
c@241: 	}
c@241: 	
c@241: 	for (i = 0; i < N; i++)
c@241: 	{
c@241: 		if (sum_gamma[i] == 0)
c@241: 		{
c@241: 			fprintf(stderr, "sum_gamma[%d] was zero...\n", i);
c@241: 		}
c@241: 		//double s = 0;
c@241: 		for (j = 0; j < N; j++)
c@241: 		{
c@241: 			a[i][j] = 0;
c@241: 			for (t = 0; t < T-1; t++)
c@241: 				a[i][j] += xi[t][i][j];
c@241: 			//s += a[i][j];
c@241: 			a[i][j] /= sum_gamma[i];	
c@241: 		}
c@241: 		/*
c@241: 		 for (j = 0; j < N; j++)
c@241: 		 {
c@241: 			 a[i][j] /= s;
c@241: 		 }
c@241: 		 */
c@241: 	}
c@241: 	
c@241: 	/* NB: now we need to sum gamma over all t */
c@241: 	for (i = 0; i < N; i++)
c@241: 		sum_gamma[i] += gamma[T-1][i];
c@241: 	
c@241: 	/* re-estimate initial probs */
c@241: 	for (i = 0; i < N; i++)
c@241: 		p0[i] = gamma[0][i];
c@241: 	
c@241: 	/* re-estimate covariance */
c@241: 	int d, e;
c@241: 	double sum_sum_gamma = 0;
c@241: 	for (i = 0; i < N; i++)
c@241: 		sum_sum_gamma += sum_gamma[i];		
c@241: 	
c@241: 	/*
c@241: 	 for (d = 0; d < L; d++)
c@241: 	 {
c@241: 		 for (e = d; e < L; e++)
c@241: 		 {
c@241: 			 cov[d][e] = 0;
c@241: 			 for (t = 0; t < T; t++)
c@241: 				 for (j = 0; j < N; j++)
c@241: 					 cov[d][e] += gamma[t][j] * (x[t][d] - mu[j][d]) * (x[t][e] - mu[j][e]);
c@241: 			 
c@241: 			 cov[d][e] /= sum_sum_gamma;
c@241: 			 
c@241: 			 if (isnan(cov[d][e]))
c@241: 			 {
c@241: 				 printf("cov[%d][%d] was nan\n", d, e);
c@241: 				 for (j = 0; j < N; j++)
c@241: 					 for (i = 0; i < L; i++)
c@241: 						 if (isnan(mu[j][i]))
c@241: 							 printf("mu[%d][%d] was nan\n", j, i);
c@241: 				 for (t = 0; t < T; t++)
c@241: 					 for (j = 0; j < N; j++)
c@241: 						 if (isnan(gamma[t][j]))
c@241: 							 printf("gamma[%d][%d] was nan\n", t, j);
c@241: 				 exit(-1);
c@241: 			 }
c@241: 		 }
c@241: 	 }
c@241: 	 for (d = 0; d < L; d++)
c@241: 	 for (e = 0; e < d; e++)
c@241: 	 cov[d][e] = cov[e][d];
c@241: 	 */
c@241: 	
c@241: 	/* using BLAS */
c@241: 	for (d = 0; d < L; d++)
c@241: 		for (e = 0; e < L; e++)
c@241: 			cov[d][e] = 0;
c@241: 	
c@241: 	for (j = 0; j < N; j++)
c@241: 	{
c@241: 		for (d = 0; d < L; d++)
c@241: 			for (t = 0; t < T; t++)
c@241: 			{
c@241: 				yy[d*T+t] = x[t][d] - mu[j][d];
c@241: 				yy2[d*T+t] = gamma[t][j] * (x[t][d] - mu[j][d]);
c@241: 			}
c@241: 				
c@241: 				cblas_dgemm(CblasColMajor, CblasTrans, CblasNoTrans, L, L, T, 1.0, yy, T, yy2, T, 0, u, L);
c@241: 		
c@241: 		for (e = 0; e < L; e++)
c@241: 			for (d = 0; d < L; d++)
c@241: 				cov[d][e] += u[e*L+d];
c@241: 	}
c@241: 	
c@241: 	for (d = 0; d < L; d++)
c@241: 		for (e = 0; e < L; e++)
c@241: 			cov[d][e] /= T; /* sum_sum_gamma; */			
c@241: 	
c@241: 	//printf("sum_sum_gamma = %f\n", sum_sum_gamma); /* fine, = T IS THIS ALWAYS TRUE with pooled cov?? */
c@241: 	
c@241: 	/* re-estimate means */
c@241: 	for (j = 0; j < N; j++)
c@241: 	{
c@241: 		for (d = 0; d < L; d++)
c@241: 		{
c@241: 			mu[j][d] = 0;
c@241: 			for (t = 0; t < T; t++)
c@241: 				mu[j][d] += gamma[t][j] * x[t][d];
c@241: 			mu[j][d] /= sum_gamma[j];
c@241: 		}
c@241: 	}
c@241: 	
c@241: 	/* deallocate memory */
c@241: 	free(sum_gamma);
c@241: 	free(yy);
c@241: 	free(yy2);
c@241: 	free(u);
c@241: }
c@241: 
c@241: void forward_backwards(double*** xi, double** gamma, double* loglik, double* loglik1, double* loglik2, int iter, int N, int T, double* p0, double** a, double** b)
c@241: {
c@241: 	/* forwards-backwards with scaling */
c@241: 	int i, j, t;
c@241: 	
c@241: 	double** alpha = (double**) malloc(T*sizeof(double*));
c@241: 	double** beta = (double**) malloc(T*sizeof(double*));
c@241: 	for (t = 0; t < T; t++)
c@241: 	{
c@241: 		alpha[t] = (double*) malloc(N*sizeof(double));
c@241: 		beta[t] = (double*) malloc(N*sizeof(double));
c@241: 	}
c@241: 	
c@241: 	/* scaling coefficients */
c@241: 	double* c = (double*) malloc(T*sizeof(double));
c@241: 	
c@241: 	/* calculate forward probs and scale coefficients */
c@241: 	c[0] = 0;
c@241: 	for (i = 0; i < N; i++)
c@241: 	{
c@241: 		alpha[0][i] = p0[i] * b[0][i];
c@241: 		c[0] += alpha[0][i];
c@241: 		
c@241: 		//printf("p0[%d] = %f, b[0][%d] = %f\n", i, p0[i], i, b[0][i]);
c@241: 	}
c@241: 	c[0] = 1 / c[0];
c@241: 	for (i = 0; i < N; i++)
c@241: 	{
c@241: 		alpha[0][i] *= c[0];		
c@241: 		
c@241: 		//printf("alpha[0][%d] = %f\n", i, alpha[0][i]);	/* OK agrees with Matlab */
c@241: 	}
c@241: 	
c@241: 	*loglik1 = *loglik;
c@241: 	*loglik = -log(c[0]);
c@241: 	if (iter == 2)
c@241: 		*loglik2 = *loglik;
c@241: 	
c@241: 	for (t = 1; t < T; t++)
c@241: 	{			
c@241: 		c[t] = 0;
c@241: 		for (j = 0; j < N; j++)
c@241: 		{
c@241: 			alpha[t][j] = 0;
c@241: 			for (i = 0; i < N; i++)
c@241: 				alpha[t][j] += alpha[t-1][i] * a[i][j];
c@241: 			alpha[t][j] *= b[t][j];
c@241: 			
c@241: 			c[t] += alpha[t][j];
c@241: 		}
c@241: 		
c@241: 		/*
c@241: 		 if (c[t] == 0)
c@241: 		 {
c@241: 			 printf("c[%d] = 0, going to blow up so exiting\n", t);
c@241: 			 for (i = 0; i < N; i++)
c@241: 				 if (b[t][i] == 0)
c@241: 					 fprintf(stderr, "b[%d][%d] was zero\n", t, i);
c@241: 			 fprintf(stderr, "x[t] was \n");
c@241: 			 for (i = 0; i < L; i++)
c@241: 				 fprintf(stderr, "%f ", x[t][i]);
c@241: 			 fprintf(stderr, "\n\n");
c@241: 			 exit(-1);
c@241: 		 }
c@241: 		 */
c@241: 		
c@241: 		c[t] = 1 / c[t];
c@241: 		for (j = 0; j < N; j++)
c@241: 			alpha[t][j] *= c[t];
c@241: 		
c@241: 		//printf("c[%d] = %e\n", t, c[t]);
c@241: 		
c@241: 		*loglik -= log(c[t]);
c@241: 	}
c@241: 	
c@241: 	/* calculate backwards probs using same coefficients */
c@241: 	for (i = 0; i < N; i++)
c@241: 		beta[T-1][i] = 1;
c@241: 	t = T - 1;
c@241: 	while (1)
c@241: 	{
c@241: 		for (i = 0; i < N; i++)
c@241: 			beta[t][i] *= c[t];
c@241: 		
c@241: 		if (t == 0)
c@241: 			break;
c@241: 		
c@241: 		for (i = 0; i < N; i++)
c@241: 		{
c@241: 			beta[t-1][i] = 0;
c@241: 			for (j = 0; j < N; j++)
c@241: 				beta[t-1][i] += a[i][j] * b[t][j] * beta[t][j];
c@241: 		}
c@241: 		
c@241: 		t--;
c@241: 	}
c@241: 	
c@241: 	/*
c@241: 	 printf("alpha:\n");
c@241: 	 for (t = 0; t < T; t++)
c@241: 	 {
c@241: 		 for (i = 0; i < N; i++)
c@241: 			 printf("%4.4e\t\t", alpha[t][i]);
c@241: 		 printf("\n");
c@241: 	 }
c@241: 	 printf("\n\n");printf("beta:\n");
c@241: 	 for (t = 0; t < T; t++)
c@241: 	 {
c@241: 		 for (i = 0; i < N; i++)
c@241: 			 printf("%4.4e\t\t", beta[t][i]);
c@241: 		 printf("\n");
c@241: 	 }
c@241: 	 printf("\n\n");
c@241: 	 */
c@241: 	
c@241: 	/* calculate posterior probs */
c@241: 	double tot;
c@241: 	for (t = 0; t < T; t++)
c@241: 	{
c@241: 		tot = 0;
c@241: 		for (i = 0; i < N; i++)
c@241: 		{
c@241: 			gamma[t][i] = alpha[t][i] * beta[t][i];
c@241: 			tot += gamma[t][i];
c@241: 		}
c@241: 		for (i = 0; i < N; i++)
c@241: 		{
c@241: 			gamma[t][i] /= tot;				
c@241: 			
c@241: 			//printf("gamma[%d][%d] = %f\n", t, i, gamma[t][i]);				
c@241: 		}
c@241: 	}		
c@241: 	
c@241: 	for (t = 0; t < T-1; t++)
c@241: 	{
c@241: 		tot = 0;
c@241: 		for (i = 0; i < N; i++)
c@241: 		{
c@241: 			for (j = 0; j < N; j++)
c@241: 			{
c@241: 				xi[t][i][j] = alpha[t][i] * a[i][j] * b[t+1][j] * beta[t+1][j];
c@241: 				tot += xi[t][i][j];
c@241: 			}
c@241: 		}
c@241: 		for (i = 0; i < N; i++)
c@241: 			for (j = 0; j < N; j++)
c@241: 				xi[t][i][j] /= tot;
c@241: 	}
c@241: 	
c@241: 	/*
c@241: 	 // CHECK - fine
c@241: 	 // gamma[t][i] = \sum_j{xi[t][i][j]}
c@241: 	 tot = 0;
c@241: 	 for (j = 0; j < N; j++)
c@241: 	 tot += xi[3][1][j];
c@241: 	 printf("gamma[3][1] = %f, sum_j(xi[3][1][j]) = %f\n", gamma[3][1], tot);
c@241: 	 */	
c@241: 	
c@241: 	for (t = 0; t < T; t++)
c@241: 	{
c@241: 		free(alpha[t]);
c@241: 		free(beta[t]);
c@241: 	}
c@241: 	free(alpha);
c@241: 	free(beta);
c@241: 	free(c);
c@241: }
c@241: 
c@241: void viterbi_decode(double** x, int T, model_t* model, int* q)
c@241: {
c@241: 	int i, j, t;
c@241: 	double max;
c@241: 	
c@241: 	int N = model->N;
c@241: 	int L = model->L;
c@241: 	double* p0 = model->p0;
c@241: 	double** a = model->a;
c@241: 	double** mu = model->mu;
c@241: 	double** cov = model->cov;
c@241: 	
c@241: 	/* inverse covariance and its determinant */
c@241: 	double** icov = (double**) malloc(L*sizeof(double*));
c@241: 	for (i = 0; i < L; i++)
c@241: 		icov[i] = (double*) malloc(L*sizeof(double));
c@241: 	double detcov;
c@241: 	
c@241: 	double** logb = (double**) malloc(T*sizeof(double*));
c@241: 	double** phi = (double**) malloc(T*sizeof(double*));
c@241: 	int** psi = (int**) malloc(T*sizeof(int*));
c@241: 	for (t = 0; t < T; t++)
c@241: 	{
c@241: 		logb[t] = (double*) malloc(N*sizeof(double));
c@241: 		phi[t] = (double*) malloc(N*sizeof(double));
c@241: 		psi[t] = (int*) malloc(N*sizeof(int));
c@241: 	}
c@241: 	
c@241: 	/* temporary memory */
c@241: 	double* gauss_y = (double*) malloc(L*sizeof(double));
c@241: 	double* gauss_z = (double*) malloc(L*sizeof(double));	
c@241: 	
c@241: 	/* calculate observation logprobs */
c@241: 	invert(cov, L, icov, &detcov);
c@241: 	for (t = 0; t < T; t++)
c@241: 		for (i = 0; i < N; i++)
c@241: 			logb[t][i] = loggauss(x[t], L, mu[i], icov, detcov, gauss_y, gauss_z);
c@241: 	
c@241: 	/* initialise */
c@241: 	for (i = 0; i < N; i++)
c@241: 	{
c@241: 		phi[0][i] = log(p0[i]) + logb[0][i];
c@241: 		psi[0][i] = 0;
c@241: 	}
c@241: 	
c@241: 	for (t = 1; t < T; t++)
c@241: 	{
c@241: 		for (j = 0; j < N; j++)
c@241: 		{
c@241: 			max = -1000000;		// TODO: what should this be??  = smallest possible sumlogprob
c@241: 			psi[t][j] = 0;
c@241: 			for (i = 0; i < N; i++)
c@241: 			{
c@241: 				if (phi[t-1][i] + log(a[i][j]) > max)
c@241: 				{
c@241: 					max = phi[t-1][i] + log(a[i][j]);
c@241: 					phi[t][j] = max + logb[t][j];
c@241: 					psi[t][j] = i;
c@241: 				}
c@241: 			}
c@241: 		}
c@241: 	}
c@241: 	
c@241: 	/* find maximising state at time T-1 */
c@241: 	max = phi[T-1][0];
c@241: 	q[T-1] = 0;
c@241: 	for (i = 1; i < N; i++)
c@241: 	{
c@241: 		if (phi[T-1][i] > max)
c@241: 		{
c@241: 			max = phi[T-1][i];
c@241: 			q[T-1] = i;
c@241: 		}
c@241: 	}
c@241: 
c@241: 	
c@241: 	/* track back */
c@241: 	t = T - 2;
c@241: 	while (t >= 0)
c@241: 	{
c@241: 		q[t] = psi[t+1][q[t+1]];
c@241: 		t--;
c@241: 	}
c@241: 	
c@241: 	/* de-allocate memory */
c@241: 	for (i = 0; i < L; i++)
c@241: 		free(icov[i]);
c@241: 	free(icov);
c@241: 	for (t = 0; t < T; t++)
c@241: 	{
c@241: 		free(logb[t]);
c@241: 		free(phi[t]);
c@241: 		free(psi[t]);
c@241: 	}
c@241: 	free(logb);
c@241: 	free(phi);
c@241: 	free(psi);
c@241: 	
c@241: 	free(gauss_y);
c@241: 	free(gauss_z);
c@241: }
c@241: 
c@241: /* invert matrix and calculate determinant using LAPACK */
c@241: void invert(double** cov, int L, double** icov, double* detcov)
c@241: {
c@241: 	/* copy square matrix into a vector in column-major order */
c@241: 	double* a = (double*) malloc(L*L*sizeof(double));
c@241: 	int i, j;
c@241: 	for(j=0; j < L; j++)
c@241: 		for (i=0; i < L; i++) 
c@241: 			a[j*L+i] = cov[i][j];
c@241: 	
c@241: 	long M = (long) L;	
c@241: 	long* ipiv = (long*) malloc(L*L*sizeof(int));
c@241: 	long ret;
c@241: 	
c@241: 	/* LU decomposition */
c@241: 	ret = dgetrf_(&M, &M, a, &M, ipiv, &ret);	/* ret should be zero, negative if cov is singular */
c@241: 	if (ret < 0)
c@241: 	{
c@241: 		fprintf(stderr, "Covariance matrix was singular, couldn't invert\n");
c@241: 		exit(-1);
c@241: 	}
c@241: 	
c@241: 	/* find determinant */
c@241: 	double det = 1;
c@241: 	for(i = 0; i < L; i++)
c@241: 		det *= a[i*L+i];
c@241: 	// TODO: get this to work!!! If detcov < 0 then cov is bad anyway...
c@241: 	/*
c@241: 	int sign = 1;
c@241: 	for (i = 0; i < L; i++)
c@241: 		if (ipiv[i] != i)
c@241: 			sign = -sign;
c@241: 	det *= sign;
c@241: 	 */
c@241: 	if (det < 0)
c@241: 		det = -det;
c@241: 	*detcov = det;
c@241: 	
c@241: 	/* allocate required working storage */
c@241: 	long lwork = -1;
c@241: 	double lwbest;
c@241: 	dgetri_(&M, a, &M, ipiv, &lwbest, &lwork, &ret);
c@241: 	lwork = (long) lwbest;	
c@241: 	double* work  = (double*) malloc(lwork*sizeof(double));
c@241: 	
c@241: 	/* find inverse */
c@241: 	dgetri_(&M, a, &M, ipiv, work, &lwork, &ret);
c@241: 	
c@241: 	for(j=0; j < L; j++)
c@241: 		for (i=0; i < L; i++) 
c@241: 			icov[i][j] = a[j*L+i];	
c@241: 	
c@241: 	free(work);
c@241: 	free(a);	
c@241: }
c@241: 
c@241: /* probability of multivariate Gaussian given mean, inverse and determinant of covariance */
c@241: double gauss(double* x, int L, double* mu, double** icov, double detcov, double* y, double* z)
c@241: {
c@241: 	int i, j;
c@241: 	double s = 0;
c@241: 	for (i = 0; i < L; i++)
c@241: 		y[i] = x[i] - mu[i];
c@241: 	for (i = 0; i < L; i++)
c@241: 	{
c@241: 		//z[i] = 0;
c@241: 		//for (j = 0; j < L; j++)
c@241: 		//	z[i] += icov[i][j] *  y[j];
c@241: 		z[i] = cblas_ddot(L, &icov[i][0], 1, y, 1);
c@241: 	}
c@241: 	s = cblas_ddot(L, z, 1, y, 1);
c@241: 	//for (i = 0; i < L; i++)
c@241: 	//	s += z[i] * y[i];	
c@241: 	
c@241: 	return exp(-s/2.0) / (pow(2*PI, L/2.0) * sqrt(detcov));
c@241: }
c@241: 
c@241: /* log probability of multivariate Gaussian given mean, inverse and determinant of covariance */
c@241: double loggauss(double* x, int L, double* mu, double** icov, double detcov, double* y, double* z)
c@241: {
c@241: 	int i, j;
c@241: 	double s = 0;
c@241: 	double ret;
c@241: 	for (i = 0; i < L; i++)
c@241: 		y[i] = x[i] - mu[i];
c@241: 	for (i = 0; i < L; i++)
c@241: 	{
c@241: 		//z[i] = 0;
c@241: 		//for (j = 0; j < L; j++)
c@241: 		//	z[i] += icov[i][j] *  y[j];
c@241: 		z[i] = cblas_ddot(L, &icov[i][0], 1, y, 1);
c@241: 	}
c@241: 	s = cblas_ddot(L, z, 1, y, 1);
c@241: 	//for (i = 0; i < L; i++)
c@241: 	//	s += z[i] * y[i];	
c@241: 	
c@241: 	ret = -0.5 * (s + L * log(2*PI) + log(detcov));
c@241: 	
c@241: 	/*
c@241: 	// TEST
c@241: 	if (isinf(ret) > 0)
c@241: 		printf("loggauss returning infinity\n");
c@241: 	if (isinf(ret) < 0)
c@241: 		printf("loggauss returning -infinity\n");
c@241: 	if (isnan(ret))
c@241: 		printf("loggauss returning nan\n");	
c@241: 	*/
c@241: 	
c@241: 	return ret;
c@241: }
c@241: 
c@241: void hmm_print(model_t* model)
c@241: {
c@241: 	int i, j;
c@241: 	printf("p0:\n");
c@241: 	for (i = 0; i < model->N; i++)
c@241: 		printf("%f ", model->p0[i]);
c@241: 	printf("\n\n");
c@241: 	printf("a:\n");
c@241: 	for (i = 0; i < model->N; i++)
c@241: 	{
c@241: 		for (j = 0; j < model->N; j++)
c@241: 			printf("%f ", model->a[i][j]);
c@241: 		printf("\n");
c@241: 	}
c@241: 	printf("\n\n");
c@241: 	printf("mu:\n");
c@241: 	for (i = 0; i < model->N; i++)
c@241: 	{
c@241: 		for (j = 0; j < model->L; j++)
c@241: 			printf("%f ", model->mu[i][j]);
c@241: 		printf("\n");
c@241: 	}
c@241: 	printf("\n\n");
c@241: 	printf("cov:\n");
c@241: 	for (i = 0; i < model->L; i++)
c@241: 	{
c@241: 		for (j = 0; j < model->L; j++)
c@241: 			printf("%f ", model->cov[i][j]);
c@241: 		printf("\n");
c@241: 	}
c@241: 	printf("\n\n");
c@241: }
c@241: 
c@241: