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