cannam@19: /*
cannam@19:  *  hmm.c
cannam@19:  *
cannam@19:  *  Created by Mark Levy on 12/02/2006.
Chris@84:  *  Copyright 2006 Centre for Digital Music, Queen Mary, University of London.
Chris@84: 
Chris@84:     This program is free software; you can redistribute it and/or
Chris@84:     modify it under the terms of the GNU General Public License as
Chris@84:     published by the Free Software Foundation; either version 2 of the
Chris@84:     License, or (at your option) any later version.  See the file
Chris@84:     COPYING included with this distribution for more information.
cannam@19:  *
cannam@19:  */
cannam@19: 
cannam@19: #include <stdio.h>
cannam@19: #include <math.h>
cannam@19: #include <stdlib.h>
cannam@19: #include <float.h>
cannam@19: #include <time.h>				/* to seed random number generator */
cannam@44: 
cannam@19: #include <clapack.h>		/* LAPACK for matrix inversion */
cannam@44: 
cannam@79: #include "maths/nan-inf.h"
cannam@79: 
cannam@44: #ifdef ATLAS_ORDER
cannam@44: #define HAVE_ATLAS 1
cannam@44: #endif
cannam@44: 
cannam@44: #ifdef HAVE_ATLAS
cannam@44: // Using ATLAS C interface to LAPACK
cannam@44: #define dgetrf_(m, n, a, lda, ipiv, info) \
cannam@44:         clapack_dgetrf(CblasColMajor, *m, *n, a, *lda, ipiv)
cannam@44: #define dgetri_(n, a, lda, ipiv, work, lwork, info) \
cannam@44:         clapack_dgetri(CblasColMajor, *n, a, *lda, ipiv)
cannam@44: #endif
cannam@44: 
cannam@19: #ifdef _MAC_OS_X
cannam@19: #include <vecLib/cblas.h>
cannam@19: #else
cannam@19: #include <cblas.h>		/* BLAS for matrix multiplication */
cannam@19: #endif
cannam@19: 
cannam@19: #include "hmm.h"
cannam@19: 
cannam@19: model_t* hmm_init(double** x, int T, int L, int N)
cannam@19: {
cannam@19: 	int i, j, d, e, t;
cannam@19: 	double s, ss;
cannam@19: 	
cannam@19: 	model_t* model;
cannam@19: 	model = (model_t*) malloc(sizeof(model_t));
cannam@19: 	model->N = N;
cannam@19: 	model->L = L;	
cannam@19: 	model->p0 = (double*) malloc(N*sizeof(double));
cannam@19: 	model->a = (double**) malloc(N*sizeof(double*));
cannam@19: 	model->mu = (double**) malloc(N*sizeof(double*));
cannam@19: 	for (i = 0; i < N; i++)
cannam@19: 	{
cannam@19: 		model->a[i] = (double*) malloc(N*sizeof(double));
cannam@19: 		model->mu[i] = (double*) malloc(L*sizeof(double));
cannam@19: 	}
cannam@19: 	model->cov = (double**) malloc(L*sizeof(double*));
cannam@19: 	for (i = 0; i < L; i++)
cannam@19: 		model->cov[i] = (double*) malloc(L*sizeof(double));
cannam@19: 	
cannam@19: 	srand(time(0));
cannam@19: 	double* global_mean = (double*) malloc(L*sizeof(double));
cannam@19: 	
cannam@19: 	/* find global mean */
cannam@19: 	for (d = 0; d < L; d++)
cannam@19: 	{
cannam@19: 		global_mean[d] = 0;
cannam@19: 		for (t = 0; t < T; t++)
cannam@19: 			global_mean[d] += x[t][d];
cannam@19: 		global_mean[d] /= T;
cannam@19: 	}
cannam@19: 	
cannam@19: 	/* calculate global diagonal covariance */
cannam@19: 	for (d = 0; d < L; d++)
cannam@19: 	{
cannam@19: 		for (e = 0; e < L; e++)
cannam@19: 			model->cov[d][e] = 0;
cannam@19: 		for (t = 0; t < T; t++)
cannam@19: 			model->cov[d][d] += (x[t][d] - global_mean[d]) * (x[t][d] - global_mean[d]);
cannam@19: 		model->cov[d][d] /= T-1;
cannam@19: 	}
cannam@19: 	
cannam@19: 	/* set all means close to global mean */
cannam@19: 	for (i = 0; i < N; i++)
cannam@19: 	{
cannam@19: 		for (d = 0; d < L; d++)
cannam@19: 		{
cannam@19: 			/* add some random noise related to covariance */
cannam@19: 			/* ideally the random number would be Gaussian(0,1), as a hack we make it uniform on [-0.25,0.25] */
cannam@19: 			model->mu[i][d] = global_mean[d] + (0.5 * rand() / (double) RAND_MAX - 0.25) * sqrt(model->cov[d][d]);
cannam@19: 		}
cannam@19: 	}	
cannam@19: 	
cannam@19: 	/* random intial and transition probs */
cannam@19: 	s = 0;
cannam@19: 	for (i = 0; i < N; i++)
cannam@19: 	{
cannam@19: 		model->p0[i] = 1 + rand() / (double) RAND_MAX;
cannam@19: 		s += model->p0[i];
cannam@19: 		ss = 0;
cannam@19: 		for (j = 0; j < N; j++)
cannam@19: 		{
cannam@19: 			model->a[i][j] = 1 + rand() / (double) RAND_MAX;
cannam@19: 			ss += model->a[i][j];
cannam@19: 		}
cannam@19: 		for (j = 0; j < N; j++)
cannam@19: 		{
cannam@19: 			model->a[i][j] /= ss;
cannam@19: 		}
cannam@19: 	}
cannam@19: 	for (i = 0; i < N; i++)
cannam@19: 		model->p0[i] /= s;
cannam@19: 	
cannam@19: 	free(global_mean);
cannam@19: 	
cannam@19: 	return model;
cannam@19: }
cannam@19: 
cannam@19: void hmm_close(model_t* model)
cannam@19: {
cannam@19: 	int i;
cannam@19: 	
cannam@19: 	for (i = 0; i < model->N; i++)
cannam@19: 	{
cannam@19: 		free(model->a[i]);
cannam@19: 		free(model->mu[i]);
cannam@19: 	}
cannam@19: 	free(model->a);
cannam@19: 	free(model->mu);
cannam@19: 	for (i = 0; i < model->L; i++)
cannam@19: 		free(model->cov[i]);
cannam@19: 	free(model->cov);	  
cannam@19: 	free(model);	
cannam@19: }
cannam@19: 
cannam@19: void hmm_train(double** x, int T, model_t* model)
cannam@19: {
cannam@19: 	int i, t;
cannam@19: 	double loglik;	/* overall log-likelihood at each iteration */
cannam@19: 	
cannam@19: 	int N = model->N;
cannam@19: 	int L = model->L;
cannam@19: 	double* p0 = model->p0;
cannam@19: 	double** a = model->a;
cannam@19: 	double** mu = model->mu;
cannam@19: 	double** cov = model->cov;
cannam@19: 	
cannam@19: 	/* allocate memory */	
cannam@19: 	double** gamma = (double**) malloc(T*sizeof(double*));
cannam@19: 	double*** xi = (double***) malloc(T*sizeof(double**));
cannam@19: 	for (t = 0; t < T; t++)
cannam@19: 	{
cannam@19: 		gamma[t] = (double*) malloc(N*sizeof(double));
cannam@19: 		xi[t] = (double**) malloc(N*sizeof(double*));
cannam@19: 		for (i = 0; i < N; i++)
cannam@19: 			xi[t][i] = (double*) malloc(N*sizeof(double));
cannam@19: 	}
cannam@19: 	
cannam@19: 	/* temporary memory */
cannam@19: 	double* gauss_y = (double*) malloc(L*sizeof(double));
cannam@19: 	double* gauss_z = (double*) malloc(L*sizeof(double));	
cannam@19: 			
cannam@19: 	/* obs probs P(j|{x}) */
cannam@19: 	double** b = (double**) malloc(T*sizeof(double*));
cannam@19: 	for (t = 0; t < T; t++)
cannam@19: 		b[t] = (double*) malloc(N*sizeof(double));
cannam@19: 	
cannam@19: 	/* inverse covariance and its determinant */
cannam@19: 	double** icov = (double**) malloc(L*sizeof(double*));
cannam@19: 	for (i = 0; i < L; i++)
cannam@19: 		icov[i] = (double*) malloc(L*sizeof(double));
cannam@19: 	double detcov;
cannam@19: 	
cannam@19: 	double thresh = 0.0001;
cannam@19: 	int niter = 50;	
cannam@19: 	int iter = 0;
cannam@19: 	double loglik1, loglik2;
cannam@30: 	int foundnan = 0;
cannam@30: 
cannam@30: 	while (iter < niter && !foundnan && !(iter > 1 && (loglik - loglik1) < thresh * (loglik1 - loglik2)))	
cannam@19: 	{
cannam@19: 		++iter;
cannam@58: /*		
cannam@19: 		fprintf(stderr, "calculating obsprobs...\n");
cannam@19: 		fflush(stderr);
cannam@58: */		
cannam@19: 		/* precalculate obs probs */
cannam@19: 		invert(cov, L, icov, &detcov);
cannam@19: 		
cannam@19: 		for (t = 0; t < T; t++)
cannam@19: 		{
cannam@19: 			//int allzero = 1;
cannam@19: 			for (i = 0; i < N; i++)
cannam@19: 			{
cannam@19: 				b[t][i] = exp(loggauss(x[t], L, mu[i], icov, detcov, gauss_y, gauss_z));
cannam@19: 		
cannam@19: 				//if (b[t][i] != 0)
cannam@19: 				//	allzero = 0;
cannam@19: 			}
cannam@19: 			/*
cannam@19: 			if (allzero)
cannam@19: 			{
cannam@19: 				printf("all the b[t][i] were zero for t = %d, correcting...\n", t);
cannam@19: 				for (i = 0; i < N; i++)
cannam@19: 				{
cannam@19: 					b[t][i] = 0.00001;
cannam@19: 				}
cannam@19: 			}
cannam@19: 			*/
cannam@19: 		}
cannam@58: /*		
cannam@19: 		fprintf(stderr, "forwards-backwards...\n");
cannam@19: 		fflush(stderr);
cannam@58: */		
cannam@19: 		forward_backwards(xi, gamma, &loglik, &loglik1, &loglik2, iter, N, T, p0, a, b);
cannam@58: /*		
cannam@19: 		fprintf(stderr, "iteration %d: loglik = %f\n", iter, loglik);		
cannam@19: 		fprintf(stderr, "re-estimation...\n");
cannam@19: 		fflush(stderr);
cannam@58: */
cannam@79: 		if (ISNAN(loglik)) {
cannam@30: 		    foundnan = 1;
cannam@30: 		    continue;
cannam@30: 		}
cannam@19: 		
cannam@19: 		baum_welch(p0, a, mu, cov, N, T, L, x, xi, gamma);
cannam@19: 			
cannam@19: 		/*
cannam@19: 		printf("a:\n");
cannam@19: 		for (i = 0; i < model->N; i++)
cannam@19: 		{
cannam@19: 			for (j = 0; j < model->N; j++)
cannam@19: 				printf("%f ", model->a[i][j]);
cannam@19: 			printf("\n");
cannam@19: 		}
cannam@19: 		printf("\n\n");
cannam@19: 		 */
cannam@19: 		//hmm_print(model);
cannam@19: 	}
cannam@19: 	
cannam@19: 	/* deallocate memory */
cannam@19: 	for (t = 0; t < T; t++)
cannam@19: 	{
cannam@19: 		free(gamma[t]);
cannam@19: 		free(b[t]);
cannam@19: 		for (i = 0; i < N; i++)
cannam@19: 			free(xi[t][i]);
cannam@19: 		free(xi[t]);
cannam@19: 	}
cannam@19: 	free(gamma);
cannam@19: 	free(xi);
cannam@19: 	free(b);	
cannam@19: 	
cannam@19: 	for (i = 0; i < L; i++)
cannam@19: 		free(icov[i]);
cannam@19: 	free(icov);
cannam@19: 	
cannam@19: 	free(gauss_y);
cannam@19: 	free(gauss_z);
cannam@19: }
cannam@19: 
cannam@19: void baum_welch(double* p0, double** a, double** mu, double** cov, int N, int T, int L, double** x, double*** xi, double** gamma)
cannam@19: {
cannam@19: 	int i, j, t;
cannam@19: 	
cannam@19: 	double* sum_gamma = (double*) malloc(N*sizeof(double));
cannam@19: 	
cannam@19: 	/* temporary memory */
cannam@19: 	double* u = (double*) malloc(L*L*sizeof(double));
cannam@19: 	double* yy = (double*) malloc(T*L*sizeof(double));
cannam@19: 	double* yy2 = (double*) malloc(T*L*sizeof(double));	
cannam@19: 	
cannam@19: 	/* re-estimate transition probs */
cannam@19: 	for (i = 0; i < N; i++)
cannam@19: 	{
cannam@19: 		sum_gamma[i] = 0;
cannam@19: 		for (t = 0; t < T-1; t++)
cannam@19: 			sum_gamma[i] += gamma[t][i];
cannam@19: 	}
cannam@19: 	
cannam@19: 	for (i = 0; i < N; i++)
cannam@19: 	{
cannam@19: 		if (sum_gamma[i] == 0)
cannam@19: 		{
cannam@58: /*			fprintf(stderr, "sum_gamma[%d] was zero...\n", i); */
cannam@19: 		}
cannam@19: 		//double s = 0;
cannam@19: 		for (j = 0; j < N; j++)
cannam@19: 		{
cannam@19: 			a[i][j] = 0;
cannam@30: 			if (sum_gamma[i] == 0.) continue;
cannam@19: 			for (t = 0; t < T-1; t++)
cannam@19: 				a[i][j] += xi[t][i][j];
cannam@19: 			//s += a[i][j];
cannam@19: 			a[i][j] /= sum_gamma[i];	
cannam@19: 		}
cannam@19: 		/*
cannam@19: 		 for (j = 0; j < N; j++)
cannam@19: 		 {
cannam@19: 			 a[i][j] /= s;
cannam@19: 		 }
cannam@19: 		 */
cannam@19: 	}
cannam@19: 	
cannam@19: 	/* NB: now we need to sum gamma over all t */
cannam@19: 	for (i = 0; i < N; i++)
cannam@19: 		sum_gamma[i] += gamma[T-1][i];
cannam@19: 	
cannam@19: 	/* re-estimate initial probs */
cannam@19: 	for (i = 0; i < N; i++)
cannam@19: 		p0[i] = gamma[0][i];
cannam@19: 	
cannam@19: 	/* re-estimate covariance */
cannam@19: 	int d, e;
cannam@19: 	double sum_sum_gamma = 0;
cannam@19: 	for (i = 0; i < N; i++)
cannam@19: 		sum_sum_gamma += sum_gamma[i];		
cannam@19: 	
cannam@19: 	/*
cannam@19: 	 for (d = 0; d < L; d++)
cannam@19: 	 {
cannam@19: 		 for (e = d; e < L; e++)
cannam@19: 		 {
cannam@19: 			 cov[d][e] = 0;
cannam@19: 			 for (t = 0; t < T; t++)
cannam@19: 				 for (j = 0; j < N; j++)
cannam@19: 					 cov[d][e] += gamma[t][j] * (x[t][d] - mu[j][d]) * (x[t][e] - mu[j][e]);
cannam@19: 			 
cannam@19: 			 cov[d][e] /= sum_sum_gamma;
cannam@19: 			 
cannam@79: 			 if (ISNAN(cov[d][e]))
cannam@19: 			 {
cannam@19: 				 printf("cov[%d][%d] was nan\n", d, e);
cannam@19: 				 for (j = 0; j < N; j++)
cannam@19: 					 for (i = 0; i < L; i++)
cannam@79: 						 if (ISNAN(mu[j][i]))
cannam@19: 							 printf("mu[%d][%d] was nan\n", j, i);
cannam@19: 				 for (t = 0; t < T; t++)
cannam@19: 					 for (j = 0; j < N; j++)
cannam@79: 						 if (ISNAN(gamma[t][j]))
cannam@19: 							 printf("gamma[%d][%d] was nan\n", t, j);
cannam@19: 				 exit(-1);
cannam@19: 			 }
cannam@19: 		 }
cannam@19: 	 }
cannam@19: 	 for (d = 0; d < L; d++)
cannam@19: 	 for (e = 0; e < d; e++)
cannam@19: 	 cov[d][e] = cov[e][d];
cannam@19: 	 */
cannam@19: 	
cannam@19: 	/* using BLAS */
cannam@19: 	for (d = 0; d < L; d++)
cannam@19: 		for (e = 0; e < L; e++)
cannam@19: 			cov[d][e] = 0;
cannam@19: 	
cannam@19: 	for (j = 0; j < N; j++)
cannam@19: 	{
cannam@19: 		for (d = 0; d < L; d++)
cannam@19: 			for (t = 0; t < T; t++)
cannam@19: 			{
cannam@19: 				yy[d*T+t] = x[t][d] - mu[j][d];
cannam@19: 				yy2[d*T+t] = gamma[t][j] * (x[t][d] - mu[j][d]);
cannam@19: 			}
cannam@19: 				
cannam@19: 				cblas_dgemm(CblasColMajor, CblasTrans, CblasNoTrans, L, L, T, 1.0, yy, T, yy2, T, 0, u, L);
cannam@19: 		
cannam@19: 		for (e = 0; e < L; e++)
cannam@19: 			for (d = 0; d < L; d++)
cannam@19: 				cov[d][e] += u[e*L+d];
cannam@19: 	}
cannam@19: 	
cannam@19: 	for (d = 0; d < L; d++)
cannam@19: 		for (e = 0; e < L; e++)
cannam@19: 			cov[d][e] /= T; /* sum_sum_gamma; */			
cannam@19: 	
cannam@19: 	//printf("sum_sum_gamma = %f\n", sum_sum_gamma); /* fine, = T IS THIS ALWAYS TRUE with pooled cov?? */
cannam@19: 	
cannam@19: 	/* re-estimate means */
cannam@19: 	for (j = 0; j < N; j++)
cannam@19: 	{
cannam@19: 		for (d = 0; d < L; d++)
cannam@19: 		{
cannam@19: 			mu[j][d] = 0;
cannam@19: 			for (t = 0; t < T; t++)
cannam@19: 				mu[j][d] += gamma[t][j] * x[t][d];
cannam@19: 			mu[j][d] /= sum_gamma[j];
cannam@19: 		}
cannam@19: 	}
cannam@19: 	
cannam@19: 	/* deallocate memory */
cannam@19: 	free(sum_gamma);
cannam@19: 	free(yy);
cannam@19: 	free(yy2);
cannam@19: 	free(u);
cannam@19: }
cannam@19: 
cannam@19: 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)
cannam@19: {
cannam@19: 	/* forwards-backwards with scaling */
cannam@19: 	int i, j, t;
cannam@19: 	
cannam@19: 	double** alpha = (double**) malloc(T*sizeof(double*));
cannam@19: 	double** beta = (double**) malloc(T*sizeof(double*));
cannam@19: 	for (t = 0; t < T; t++)
cannam@19: 	{
cannam@19: 		alpha[t] = (double*) malloc(N*sizeof(double));
cannam@19: 		beta[t] = (double*) malloc(N*sizeof(double));
cannam@19: 	}
cannam@19: 	
cannam@19: 	/* scaling coefficients */
cannam@19: 	double* c = (double*) malloc(T*sizeof(double));
cannam@19: 	
cannam@19: 	/* calculate forward probs and scale coefficients */
cannam@19: 	c[0] = 0;
cannam@19: 	for (i = 0; i < N; i++)
cannam@19: 	{
cannam@19: 		alpha[0][i] = p0[i] * b[0][i];
cannam@19: 		c[0] += alpha[0][i];
cannam@19: 		
cannam@19: 		//printf("p0[%d] = %f, b[0][%d] = %f\n", i, p0[i], i, b[0][i]);
cannam@19: 	}
cannam@19: 	c[0] = 1 / c[0];
cannam@19: 	for (i = 0; i < N; i++)
cannam@19: 	{
cannam@19: 		alpha[0][i] *= c[0];		
cannam@19: 		
cannam@19: 		//printf("alpha[0][%d] = %f\n", i, alpha[0][i]);	/* OK agrees with Matlab */
cannam@19: 	}
cannam@19: 	
cannam@19: 	*loglik1 = *loglik;
cannam@19: 	*loglik = -log(c[0]);
cannam@19: 	if (iter == 2)
cannam@19: 		*loglik2 = *loglik;
cannam@19: 	
cannam@19: 	for (t = 1; t < T; t++)
cannam@19: 	{			
cannam@19: 		c[t] = 0;
cannam@19: 		for (j = 0; j < N; j++)
cannam@19: 		{
cannam@19: 			alpha[t][j] = 0;
cannam@19: 			for (i = 0; i < N; i++)
cannam@19: 				alpha[t][j] += alpha[t-1][i] * a[i][j];
cannam@19: 			alpha[t][j] *= b[t][j];
cannam@19: 			
cannam@19: 			c[t] += alpha[t][j];
cannam@19: 		}
cannam@19: 		
cannam@19: 		/*
cannam@19: 		 if (c[t] == 0)
cannam@19: 		 {
cannam@19: 			 printf("c[%d] = 0, going to blow up so exiting\n", t);
cannam@19: 			 for (i = 0; i < N; i++)
cannam@19: 				 if (b[t][i] == 0)
cannam@19: 					 fprintf(stderr, "b[%d][%d] was zero\n", t, i);
cannam@19: 			 fprintf(stderr, "x[t] was \n");
cannam@19: 			 for (i = 0; i < L; i++)
cannam@19: 				 fprintf(stderr, "%f ", x[t][i]);
cannam@19: 			 fprintf(stderr, "\n\n");
cannam@19: 			 exit(-1);
cannam@19: 		 }
cannam@19: 		 */
cannam@19: 		
cannam@19: 		c[t] = 1 / c[t];
cannam@19: 		for (j = 0; j < N; j++)
cannam@19: 			alpha[t][j] *= c[t];
cannam@19: 		
cannam@19: 		//printf("c[%d] = %e\n", t, c[t]);
cannam@19: 		
cannam@19: 		*loglik -= log(c[t]);
cannam@19: 	}
cannam@19: 	
cannam@19: 	/* calculate backwards probs using same coefficients */
cannam@19: 	for (i = 0; i < N; i++)
cannam@19: 		beta[T-1][i] = 1;
cannam@19: 	t = T - 1;
cannam@19: 	while (1)
cannam@19: 	{
cannam@19: 		for (i = 0; i < N; i++)
cannam@19: 			beta[t][i] *= c[t];
cannam@19: 		
cannam@19: 		if (t == 0)
cannam@19: 			break;
cannam@19: 		
cannam@19: 		for (i = 0; i < N; i++)
cannam@19: 		{
cannam@19: 			beta[t-1][i] = 0;
cannam@19: 			for (j = 0; j < N; j++)
cannam@19: 				beta[t-1][i] += a[i][j] * b[t][j] * beta[t][j];
cannam@19: 		}
cannam@19: 		
cannam@19: 		t--;
cannam@19: 	}
cannam@19: 	
cannam@19: 	/*
cannam@19: 	 printf("alpha:\n");
cannam@19: 	 for (t = 0; t < T; t++)
cannam@19: 	 {
cannam@19: 		 for (i = 0; i < N; i++)
cannam@19: 			 printf("%4.4e\t\t", alpha[t][i]);
cannam@19: 		 printf("\n");
cannam@19: 	 }
cannam@19: 	 printf("\n\n");printf("beta:\n");
cannam@19: 	 for (t = 0; t < T; t++)
cannam@19: 	 {
cannam@19: 		 for (i = 0; i < N; i++)
cannam@19: 			 printf("%4.4e\t\t", beta[t][i]);
cannam@19: 		 printf("\n");
cannam@19: 	 }
cannam@19: 	 printf("\n\n");
cannam@19: 	 */
cannam@19: 	
cannam@19: 	/* calculate posterior probs */
cannam@19: 	double tot;
cannam@19: 	for (t = 0; t < T; t++)
cannam@19: 	{
cannam@19: 		tot = 0;
cannam@19: 		for (i = 0; i < N; i++)
cannam@19: 		{
cannam@19: 			gamma[t][i] = alpha[t][i] * beta[t][i];
cannam@19: 			tot += gamma[t][i];
cannam@19: 		}
cannam@19: 		for (i = 0; i < N; i++)
cannam@19: 		{
cannam@19: 			gamma[t][i] /= tot;				
cannam@19: 			
cannam@19: 			//printf("gamma[%d][%d] = %f\n", t, i, gamma[t][i]);				
cannam@19: 		}
cannam@19: 	}		
cannam@19: 	
cannam@19: 	for (t = 0; t < T-1; t++)
cannam@19: 	{
cannam@19: 		tot = 0;
cannam@19: 		for (i = 0; i < N; i++)
cannam@19: 		{
cannam@19: 			for (j = 0; j < N; j++)
cannam@19: 			{
cannam@19: 				xi[t][i][j] = alpha[t][i] * a[i][j] * b[t+1][j] * beta[t+1][j];
cannam@19: 				tot += xi[t][i][j];
cannam@19: 			}
cannam@19: 		}
cannam@19: 		for (i = 0; i < N; i++)
cannam@19: 			for (j = 0; j < N; j++)
cannam@19: 				xi[t][i][j] /= tot;
cannam@19: 	}
cannam@19: 	
cannam@19: 	/*
cannam@19: 	 // CHECK - fine
cannam@19: 	 // gamma[t][i] = \sum_j{xi[t][i][j]}
cannam@19: 	 tot = 0;
cannam@19: 	 for (j = 0; j < N; j++)
cannam@19: 	 tot += xi[3][1][j];
cannam@19: 	 printf("gamma[3][1] = %f, sum_j(xi[3][1][j]) = %f\n", gamma[3][1], tot);
cannam@19: 	 */	
cannam@19: 	
cannam@19: 	for (t = 0; t < T; t++)
cannam@19: 	{
cannam@19: 		free(alpha[t]);
cannam@19: 		free(beta[t]);
cannam@19: 	}
cannam@19: 	free(alpha);
cannam@19: 	free(beta);
cannam@19: 	free(c);
cannam@19: }
cannam@19: 
cannam@19: void viterbi_decode(double** x, int T, model_t* model, int* q)
cannam@19: {
cannam@19: 	int i, j, t;
cannam@19: 	double max;
cannam@48: 	int havemax;
cannam@19: 	
cannam@19: 	int N = model->N;
cannam@19: 	int L = model->L;
cannam@19: 	double* p0 = model->p0;
cannam@19: 	double** a = model->a;
cannam@19: 	double** mu = model->mu;
cannam@19: 	double** cov = model->cov;
cannam@19: 	
cannam@19: 	/* inverse covariance and its determinant */
cannam@19: 	double** icov = (double**) malloc(L*sizeof(double*));
cannam@19: 	for (i = 0; i < L; i++)
cannam@19: 		icov[i] = (double*) malloc(L*sizeof(double));
cannam@19: 	double detcov;
cannam@19: 	
cannam@19: 	double** logb = (double**) malloc(T*sizeof(double*));
cannam@19: 	double** phi = (double**) malloc(T*sizeof(double*));
cannam@19: 	int** psi = (int**) malloc(T*sizeof(int*));
cannam@19: 	for (t = 0; t < T; t++)
cannam@19: 	{
cannam@19: 		logb[t] = (double*) malloc(N*sizeof(double));
cannam@19: 		phi[t] = (double*) malloc(N*sizeof(double));
cannam@19: 		psi[t] = (int*) malloc(N*sizeof(int));
cannam@19: 	}
cannam@19: 	
cannam@19: 	/* temporary memory */
cannam@19: 	double* gauss_y = (double*) malloc(L*sizeof(double));
cannam@19: 	double* gauss_z = (double*) malloc(L*sizeof(double));	
cannam@19: 	
cannam@19: 	/* calculate observation logprobs */
cannam@19: 	invert(cov, L, icov, &detcov);
cannam@19: 	for (t = 0; t < T; t++)
cannam@19: 		for (i = 0; i < N; i++)
cannam@19: 			logb[t][i] = loggauss(x[t], L, mu[i], icov, detcov, gauss_y, gauss_z);
cannam@19: 	
cannam@19: 	/* initialise */
cannam@19: 	for (i = 0; i < N; i++)
cannam@19: 	{
cannam@19: 		phi[0][i] = log(p0[i]) + logb[0][i];
cannam@19: 		psi[0][i] = 0;
cannam@19: 	}
cannam@19: 	
cannam@19: 	for (t = 1; t < T; t++)
cannam@19: 	{
cannam@19: 		for (j = 0; j < N; j++)
cannam@19: 		{
cannam@48: 			max = -1000000;
cannam@48: 			havemax = 0;
cannam@48: 
cannam@19: 			psi[t][j] = 0;
cannam@19: 			for (i = 0; i < N; i++)
cannam@19: 			{
cannam@48: 				if (phi[t-1][i] + log(a[i][j]) > max || !havemax)
cannam@19: 				{
cannam@19: 					max = phi[t-1][i] + log(a[i][j]);
cannam@19: 					phi[t][j] = max + logb[t][j];
cannam@19: 					psi[t][j] = i;
cannam@48: 					havemax = 1;
cannam@19: 				}
cannam@19: 			}
cannam@19: 		}
cannam@19: 	}
cannam@19: 	
cannam@19: 	/* find maximising state at time T-1 */
cannam@19: 	max = phi[T-1][0];
cannam@19: 	q[T-1] = 0;
cannam@19: 	for (i = 1; i < N; i++)
cannam@19: 	{
cannam@19: 		if (phi[T-1][i] > max)
cannam@19: 		{
cannam@19: 			max = phi[T-1][i];
cannam@19: 			q[T-1] = i;
cannam@19: 		}
cannam@19: 	}
cannam@19: 
cannam@19: 	
cannam@19: 	/* track back */
cannam@19: 	t = T - 2;
cannam@19: 	while (t >= 0)
cannam@19: 	{
cannam@19: 		q[t] = psi[t+1][q[t+1]];
cannam@19: 		t--;
cannam@19: 	}
cannam@19: 	
cannam@19: 	/* de-allocate memory */
cannam@19: 	for (i = 0; i < L; i++)
cannam@19: 		free(icov[i]);
cannam@19: 	free(icov);
cannam@19: 	for (t = 0; t < T; t++)
cannam@19: 	{
cannam@19: 		free(logb[t]);
cannam@19: 		free(phi[t]);
cannam@19: 		free(psi[t]);
cannam@19: 	}
cannam@19: 	free(logb);
cannam@19: 	free(phi);
cannam@19: 	free(psi);
cannam@19: 	
cannam@19: 	free(gauss_y);
cannam@19: 	free(gauss_z);
cannam@19: }
cannam@19: 
cannam@19: /* invert matrix and calculate determinant using LAPACK */
cannam@19: void invert(double** cov, int L, double** icov, double* detcov)
cannam@19: {
cannam@19: 	/* copy square matrix into a vector in column-major order */
cannam@19: 	double* a = (double*) malloc(L*L*sizeof(double));
cannam@19: 	int i, j;
cannam@19: 	for(j=0; j < L; j++)
cannam@19: 		for (i=0; i < L; i++) 
cannam@19: 			a[j*L+i] = cov[i][j];
cannam@19: 	
cannam@44: 	int M = (int) L;	
cannam@44: 	int* ipiv = (int *) malloc(L*L*sizeof(int));
cannam@44: 	int ret;
cannam@19: 	
cannam@19: 	/* LU decomposition */
cannam@19: 	ret = dgetrf_(&M, &M, a, &M, ipiv, &ret);	/* ret should be zero, negative if cov is singular */
cannam@19: 	if (ret < 0)
cannam@19: 	{
cannam@19: 		fprintf(stderr, "Covariance matrix was singular, couldn't invert\n");
cannam@19: 		exit(-1);
cannam@19: 	}
cannam@19: 	
cannam@19: 	/* find determinant */
cannam@19: 	double det = 1;
cannam@19: 	for(i = 0; i < L; i++)
cannam@19: 		det *= a[i*L+i];
cannam@19: 	// TODO: get this to work!!! If detcov < 0 then cov is bad anyway...
cannam@19: 	/*
cannam@19: 	int sign = 1;
cannam@19: 	for (i = 0; i < L; i++)
cannam@19: 		if (ipiv[i] != i)
cannam@19: 			sign = -sign;
cannam@19: 	det *= sign;
cannam@19: 	 */
cannam@19: 	if (det < 0)
cannam@19: 		det = -det;
cannam@19: 	*detcov = det;
cannam@19: 	
cannam@19: 	/* allocate required working storage */
cannam@44: #ifndef HAVE_ATLAS
cannam@44: 	int lwork = -1;
cannam@44: 	double lwbest = 0.0;
cannam@19: 	dgetri_(&M, a, &M, ipiv, &lwbest, &lwork, &ret);
cannam@44: 	lwork = (int) lwbest;	
cannam@19: 	double* work  = (double*) malloc(lwork*sizeof(double));
cannam@44: #endif
cannam@19: 	
cannam@19: 	/* find inverse */
cannam@19: 	dgetri_(&M, a, &M, ipiv, work, &lwork, &ret);
cannam@44: 
cannam@19: 	for(j=0; j < L; j++)
cannam@19: 		for (i=0; i < L; i++) 
cannam@19: 			icov[i][j] = a[j*L+i];	
cannam@19: 	
cannam@44: #ifndef HAVE_ATLAS	
cannam@19: 	free(work);
cannam@44: #endif
cannam@19: 	free(a);	
cannam@19: }
cannam@19: 
cannam@19: /* probability of multivariate Gaussian given mean, inverse and determinant of covariance */
cannam@19: double gauss(double* x, int L, double* mu, double** icov, double detcov, double* y, double* z)
cannam@19: {
Chris@189: 	int i;
cannam@19: 	double s = 0;
cannam@19: 	for (i = 0; i < L; i++)
cannam@19: 		y[i] = x[i] - mu[i];
cannam@19: 	for (i = 0; i < L; i++)
cannam@19: 	{
cannam@19: 		//z[i] = 0;
cannam@19: 		//for (j = 0; j < L; j++)
cannam@19: 		//	z[i] += icov[i][j] *  y[j];
cannam@19: 		z[i] = cblas_ddot(L, &icov[i][0], 1, y, 1);
cannam@19: 	}
cannam@19: 	s = cblas_ddot(L, z, 1, y, 1);
cannam@19: 	//for (i = 0; i < L; i++)
cannam@19: 	//	s += z[i] * y[i];	
cannam@19: 	
cannam@19: 	return exp(-s/2.0) / (pow(2*PI, L/2.0) * sqrt(detcov));
cannam@19: }
cannam@19: 
cannam@19: /* log probability of multivariate Gaussian given mean, inverse and determinant of covariance */
cannam@19: double loggauss(double* x, int L, double* mu, double** icov, double detcov, double* y, double* z)
cannam@19: {
Chris@189: 	int i;
cannam@19: 	double s = 0;
cannam@19: 	double ret;
cannam@19: 	for (i = 0; i < L; i++)
cannam@19: 		y[i] = x[i] - mu[i];
cannam@19: 	for (i = 0; i < L; i++)
cannam@19: 	{
cannam@19: 		//z[i] = 0;
cannam@19: 		//for (j = 0; j < L; j++)
cannam@19: 		//	z[i] += icov[i][j] *  y[j];
cannam@19: 		z[i] = cblas_ddot(L, &icov[i][0], 1, y, 1);
cannam@19: 	}
cannam@19: 	s = cblas_ddot(L, z, 1, y, 1);
cannam@19: 	//for (i = 0; i < L; i++)
cannam@19: 	//	s += z[i] * y[i];	
cannam@19: 	
cannam@19: 	ret = -0.5 * (s + L * log(2*PI) + log(detcov));
cannam@19: 	
cannam@19: 	/*
cannam@19: 	// TEST
cannam@79: 	if (ISINF(ret) > 0)
cannam@19: 		printf("loggauss returning infinity\n");
cannam@79: 	if (ISINF(ret) < 0)
cannam@19: 		printf("loggauss returning -infinity\n");
cannam@79: 	if (ISNAN(ret))
cannam@19: 		printf("loggauss returning nan\n");	
cannam@19: 	*/
cannam@19: 	
cannam@19: 	return ret;
cannam@19: }
cannam@19: 
cannam@19: void hmm_print(model_t* model)
cannam@19: {
cannam@19: 	int i, j;
cannam@19: 	printf("p0:\n");
cannam@19: 	for (i = 0; i < model->N; i++)
cannam@19: 		printf("%f ", model->p0[i]);
cannam@19: 	printf("\n\n");
cannam@19: 	printf("a:\n");
cannam@19: 	for (i = 0; i < model->N; i++)
cannam@19: 	{
cannam@19: 		for (j = 0; j < model->N; j++)
cannam@19: 			printf("%f ", model->a[i][j]);
cannam@19: 		printf("\n");
cannam@19: 	}
cannam@19: 	printf("\n\n");
cannam@19: 	printf("mu:\n");
cannam@19: 	for (i = 0; i < model->N; i++)
cannam@19: 	{
cannam@19: 		for (j = 0; j < model->L; j++)
cannam@19: 			printf("%f ", model->mu[i][j]);
cannam@19: 		printf("\n");
cannam@19: 	}
cannam@19: 	printf("\n\n");
cannam@19: 	printf("cov:\n");
cannam@19: 	for (i = 0; i < model->L; i++)
cannam@19: 	{
cannam@19: 		for (j = 0; j < model->L; j++)
cannam@19: 			printf("%f ", model->cov[i][j]);
cannam@19: 		printf("\n");
cannam@19: 	}
cannam@19: 	printf("\n\n");
cannam@19: }
cannam@19: 
cannam@19: