libs/qm-dsp/hmm/hmm.c

   1 /*
   2  *  hmm.c
   3  *
   4  *  Created by Mark Levy on 12/02/2006.
   5  *  Copyright 2006 Centre for Digital Music, Queen Mary, University of London.
   6
   7     This program is free software; you can redistribute it and/or
   8     modify it under the terms of the GNU General Public License as
   9     published by the Free Software Foundation; either version 2 of the
  10     License, or (at your option) any later version.  See the file
  11     COPYING included with this distribution for more information.
  12  *
  13  */
  14
  15 #include <stdio.h>
  16 #include <math.h>
  17 #include <stdlib.h>
  18 #include <float.h>
  19 #include <time.h>                               /* to seed random number generator */
  20
  21 #include <clapack.h>            /* LAPACK for matrix inversion */
  22
  23 #include "maths/nan-inf.h"
  24
  25 #ifdef ATLAS_ORDER
  26 #define HAVE_ATLAS 1
  27 #endif
  28
  29 #ifdef HAVE_ATLAS
  30 // Using ATLAS C interface to LAPACK
  31 #define dgetrf_(m, n, a, lda, ipiv, info) \
  32         clapack_dgetrf(CblasColMajor, *m, *n, a, *lda, ipiv)
  33 #define dgetri_(n, a, lda, ipiv, work, lwork, info) \
  34         clapack_dgetri(CblasColMajor, *n, a, *lda, ipiv)
  35 #endif
  36
  37 #ifdef _MAC_OS_X
  38 #include <vecLib/cblas.h>
  39 #else
  40 #include <cblas.h>              /* BLAS for matrix multiplication */
  41 #endif
  42
  43 #include "hmm.h"
  44
  45 model_t* hmm_init(double** x, int T, int L, int N)
  46 {
  47         int i, j, d, e, t;
  48         double s, ss;
  49
  50         model_t* model;
  51         model = (model_t*) malloc(sizeof(model_t));
  52         model->N = N;
  53         model->L = L;
  54         model->p0 = (double*) malloc(N*sizeof(double));
  55         model->a = (double**) malloc(N*sizeof(double*));
  56         model->mu = (double**) malloc(N*sizeof(double*));
  57         for (i = 0; i < N; i++)
  58         {
  59                 model->a[i] = (double*) malloc(N*sizeof(double));
  60                 model->mu[i] = (double*) malloc(L*sizeof(double));
  61         }
  62         model->cov = (double**) malloc(L*sizeof(double*));
  63         for (i = 0; i < L; i++)
  64                 model->cov[i] = (double*) malloc(L*sizeof(double));
  65
  66         srand(time(0));
  67         double* global_mean = (double*) malloc(L*sizeof(double));
  68
  69         /* find global mean */
  70         for (d = 0; d < L; d++)
  71         {
  72                 global_mean[d] = 0;
  73                 for (t = 0; t < T; t++)
  74                         global_mean[d] += x[t][d];
  75                 global_mean[d] /= T;
  76         }
  77
  78         /* calculate global diagonal covariance */
  79         for (d = 0; d < L; d++)
  80         {
  81                 for (e = 0; e < L; e++)
  82                         model->cov[d][e] = 0;
  83                 for (t = 0; t < T; t++)
  84                         model->cov[d][d] += (x[t][d] - global_mean[d]) * (x[t][d] - global_mean[d]);
  85                 model->cov[d][d] /= T-1;
  86         }
  87
  88         /* set all means close to global mean */
  89         for (i = 0; i < N; i++)
  90         {
  91                 for (d = 0; d < L; d++)
  92                 {
  93                         /* add some random noise related to covariance */
  94                         /* ideally the random number would be Gaussian(0,1), as a hack we make it uniform on [-0.25,0.25] */
  95                         model->mu[i][d] = global_mean[d] + (0.5 * rand() / (double) RAND_MAX - 0.25) * sqrt(model->cov[d][d]);
  96                 }
  97         }
  98
  99         /* random intial and transition probs */
 100         s = 0;
 101         for (i = 0; i < N; i++)
 102         {
 103                 model->p0[i] = 1 + rand() / (double) RAND_MAX;
 104                 s += model->p0[i];
 105                 ss = 0;
 106                 for (j = 0; j < N; j++)
 107                 {
 108                         model->a[i][j] = 1 + rand() / (double) RAND_MAX;
 109                         ss += model->a[i][j];
 110                 }
 111                 for (j = 0; j < N; j++)
 112                 {
 113                         model->a[i][j] /= ss;
 114                 }
 115         }
 116         for (i = 0; i < N; i++)
 117                 model->p0[i] /= s;
 118
 119         free(global_mean);
 120
 121         return model;
 122 }
 123
 124 void hmm_close(model_t* model)
 125 {
 126         int i;
 127
 128         for (i = 0; i < model->N; i++)
 129         {
 130                 free(model->a[i]);
 131                 free(model->mu[i]);
 132         }
 133         free(model->a);
 134         free(model->mu);
 135         for (i = 0; i < model->L; i++)
 136                 free(model->cov[i]);
 137         free(model->cov);
 138         free(model);
 139 }
 140
 141 void hmm_train(double** x, int T, model_t* model)
 142 {
 143         int i, t;
 144         double loglik;  /* overall log-likelihood at each iteration */
 145
 146         int N = model->N;
 147         int L = model->L;
 148         double* p0 = model->p0;
 149         double** a = model->a;
 150         double** mu = model->mu;
 151         double** cov = model->cov;
 152
 153         /* allocate memory */
 154         double** gamma = (double**) malloc(T*sizeof(double*));
 155         double*** xi = (double***) malloc(T*sizeof(double**));
 156         for (t = 0; t < T; t++)
 157         {
 158                 gamma[t] = (double*) malloc(N*sizeof(double));
 159                 xi[t] = (double**) malloc(N*sizeof(double*));
 160                 for (i = 0; i < N; i++)
 161                         xi[t][i] = (double*) malloc(N*sizeof(double));
 162         }
 163
 164         /* temporary memory */
 165         double* gauss_y = (double*) malloc(L*sizeof(double));
 166         double* gauss_z = (double*) malloc(L*sizeof(double));
 167
 168         /* obs probs P(j|{x}) */
 169         double** b = (double**) malloc(T*sizeof(double*));
 170         for (t = 0; t < T; t++)
 171                 b[t] = (double*) malloc(N*sizeof(double));
 172
 173         /* inverse covariance and its determinant */
 174         double** icov = (double**) malloc(L*sizeof(double*));
 175         for (i = 0; i < L; i++)
 176                 icov[i] = (double*) malloc(L*sizeof(double));
 177         double detcov;
 178
 179         double thresh = 0.0001;
 180         int niter = 50;
 181         int iter = 0;
 182         double loglik1, loglik2;
 183         int foundnan = 0;
 184
 185         while (iter < niter && !foundnan && !(iter > 1 && (loglik - loglik1) < thresh * (loglik1 - loglik2)))
 186         {
 187                 ++iter;
 188 /*
 189                 fprintf(stderr, "calculating obsprobs...\n");
 190                 fflush(stderr);
 191 */
 192                 /* precalculate obs probs */
 193                 invert(cov, L, icov, &detcov);
 194
 195                 for (t = 0; t < T; t++)
 196                 {
 197                         //int allzero = 1;
 198                         for (i = 0; i < N; i++)
 199                         {
 200                                 b[t][i] = exp(loggauss(x[t], L, mu[i], icov, detcov, gauss_y, gauss_z));
 201
 202                                 //if (b[t][i] != 0)
 203                                 //      allzero = 0;
 204                         }
 205                         /*
 206                         if (allzero)
 207                         {
 208                                 printf("all the b[t][i] were zero for t = %d, correcting...\n", t);
 209                                 for (i = 0; i < N; i++)
 210                                 {
 211                                         b[t][i] = 0.00001;
 212                                 }
 213                         }
 214                         */
 215                 }
 216 /*
 217                 fprintf(stderr, "forwards-backwards...\n");
 218                 fflush(stderr);
 219 */
 220                 forward_backwards(xi, gamma, &loglik, &loglik1, &loglik2, iter, N, T, p0, a, b);
 221 /*
 222                 fprintf(stderr, "iteration %d: loglik = %f\n", iter, loglik);
 223                 fprintf(stderr, "re-estimation...\n");
 224                 fflush(stderr);
 225 */
 226                 if (ISNAN(loglik)) {
 227                     foundnan = 1;
 228                     continue;
 229                 }
 230
 231                 baum_welch(p0, a, mu, cov, N, T, L, x, xi, gamma);
 232
 233                 /*
 234                 printf("a:\n");
 235                 for (i = 0; i < model->N; i++)
 236                 {
 237                         for (j = 0; j < model->N; j++)
 238                                 printf("%f ", model->a[i][j]);
 239                         printf("\n");
 240                 }
 241                 printf("\n\n");
 242                  */
 243                 //hmm_print(model);
 244         }
 245
 246         /* deallocate memory */
 247         for (t = 0; t < T; t++)
 248         {
 249                 free(gamma[t]);
 250                 free(b[t]);
 251                 for (i = 0; i < N; i++)
 252                         free(xi[t][i]);
 253                 free(xi[t]);
 254         }
 255         free(gamma);
 256         free(xi);
 257         free(b);
 258
 259         for (i = 0; i < L; i++)
 260                 free(icov[i]);
 261         free(icov);
 262
 263         free(gauss_y);
 264         free(gauss_z);
 265 }
 266
 267 void mlss_reestimate(double* p0, double** a, double** mu, double** cov, int N, int T, int L, int* q, double** x)
 268 {
 269         /* fit a single Gaussian to observations in each state */
 270
 271         /* calculate the mean observation in each state */
 272
 273         /* calculate the overall covariance */
 274
 275         /* count transitions */
 276
 277         /* estimate initial probs from transitions (???) */
 278 }
 279
 280 void baum_welch(double* p0, double** a, double** mu, double** cov, int N, int T, int L, double** x, double*** xi, double** gamma)
 281 {
 282         int i, j, t;
 283
 284         double* sum_gamma = (double*) malloc(N*sizeof(double));
 285
 286         /* temporary memory */
 287         double* u = (double*) malloc(L*L*sizeof(double));
 288         double* yy = (double*) malloc(T*L*sizeof(double));
 289         double* yy2 = (double*) malloc(T*L*sizeof(double));
 290
 291         /* re-estimate transition probs */
 292         for (i = 0; i < N; i++)
 293         {
 294                 sum_gamma[i] = 0;
 295                 for (t = 0; t < T-1; t++)
 296                         sum_gamma[i] += gamma[t][i];
 297         }
 298
 299         for (i = 0; i < N; i++)
 300         {
 301                 if (sum_gamma[i] == 0)
 302                 {
 303 /*                      fprintf(stderr, "sum_gamma[%d] was zero...\n", i); */
 304                 }
 305                 //double s = 0;
 306                 for (j = 0; j < N; j++)
 307                 {
 308                         a[i][j] = 0;
 309                         if (sum_gamma[i] == 0.) continue;
 310                         for (t = 0; t < T-1; t++)
 311                                 a[i][j] += xi[t][i][j];
 312                         //s += a[i][j];
 313                         a[i][j] /= sum_gamma[i];
 314                 }
 315                 /*
 316                  for (j = 0; j < N; j++)
 317                  {
 318                          a[i][j] /= s;
 319                  }
 320                  */
 321         }
 322
 323         /* NB: now we need to sum gamma over all t */
 324         for (i = 0; i < N; i++)
 325                 sum_gamma[i] += gamma[T-1][i];
 326
 327         /* re-estimate initial probs */
 328         for (i = 0; i < N; i++)
 329                 p0[i] = gamma[0][i];
 330
 331         /* re-estimate covariance */
 332         int d, e;
 333         double sum_sum_gamma = 0;
 334         for (i = 0; i < N; i++)
 335                 sum_sum_gamma += sum_gamma[i];
 336
 337         /*
 338          for (d = 0; d < L; d++)
 339          {
 340                  for (e = d; e < L; e++)
 341                  {
 342                          cov[d][e] = 0;
 343                          for (t = 0; t < T; t++)
 344                                  for (j = 0; j < N; j++)
 345                                          cov[d][e] += gamma[t][j] * (x[t][d] - mu[j][d]) * (x[t][e] - mu[j][e]);
 346
 347                          cov[d][e] /= sum_sum_gamma;
 348
 349                          if (ISNAN(cov[d][e]))
 350                          {
 351                                  printf("cov[%d][%d] was nan\n", d, e);
 352                                  for (j = 0; j < N; j++)
 353                                          for (i = 0; i < L; i++)
 354                                                  if (ISNAN(mu[j][i]))
 355                                                          printf("mu[%d][%d] was nan\n", j, i);
 356                                  for (t = 0; t < T; t++)
 357                                          for (j = 0; j < N; j++)
 358                                                  if (ISNAN(gamma[t][j]))
 359                                                          printf("gamma[%d][%d] was nan\n", t, j);
 360                                  exit(-1);
 361                          }
 362                  }
 363          }
 364          for (d = 0; d < L; d++)
 365          for (e = 0; e < d; e++)
 366          cov[d][e] = cov[e][d];
 367          */
 368
 369         /* using BLAS */
 370         for (d = 0; d < L; d++)
 371                 for (e = 0; e < L; e++)
 372                         cov[d][e] = 0;
 373
 374         for (j = 0; j < N; j++)
 375         {
 376                 for (d = 0; d < L; d++)
 377                         for (t = 0; t < T; t++)
 378                         {
 379                                 yy[d*T+t] = x[t][d] - mu[j][d];
 380                                 yy2[d*T+t] = gamma[t][j] * (x[t][d] - mu[j][d]);
 381                         }
 382
 383                                 cblas_dgemm(CblasColMajor, CblasTrans, CblasNoTrans, L, L, T, 1.0, yy, T, yy2, T, 0, u, L);
 384
 385                 for (e = 0; e < L; e++)
 386                         for (d = 0; d < L; d++)
 387                                 cov[d][e] += u[e*L+d];
 388         }
 389
 390         for (d = 0; d < L; d++)
 391                 for (e = 0; e < L; e++)
 392                         cov[d][e] /= T; /* sum_sum_gamma; */
 393
 394         //printf("sum_sum_gamma = %f\n", sum_sum_gamma); /* fine, = T IS THIS ALWAYS TRUE with pooled cov?? */
 395
 396         /* re-estimate means */
 397         for (j = 0; j < N; j++)
 398         {
 399                 for (d = 0; d < L; d++)
 400                 {
 401                         mu[j][d] = 0;
 402                         for (t = 0; t < T; t++)
 403                                 mu[j][d] += gamma[t][j] * x[t][d];
 404                         mu[j][d] /= sum_gamma[j];
 405                 }
 406         }
 407
 408         /* deallocate memory */
 409         free(sum_gamma);
 410         free(yy);
 411         free(yy2);
 412         free(u);
 413 }
 414
 415 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)
 416 {
 417         /* forwards-backwards with scaling */
 418         int i, j, t;
 419
 420         double** alpha = (double**) malloc(T*sizeof(double*));
 421         double** beta = (double**) malloc(T*sizeof(double*));
 422         for (t = 0; t < T; t++)
 423         {
 424                 alpha[t] = (double*) malloc(N*sizeof(double));
 425                 beta[t] = (double*) malloc(N*sizeof(double));
 426         }
 427
 428         /* scaling coefficients */
 429         double* c = (double*) malloc(T*sizeof(double));
 430
 431         /* calculate forward probs and scale coefficients */
 432         c[0] = 0;
 433         for (i = 0; i < N; i++)
 434         {
 435                 alpha[0][i] = p0[i] * b[0][i];
 436                 c[0] += alpha[0][i];
 437
 438                 //printf("p0[%d] = %f, b[0][%d] = %f\n", i, p0[i], i, b[0][i]);
 439         }
 440         c[0] = 1 / c[0];
 441         for (i = 0; i < N; i++)
 442         {
 443                 alpha[0][i] *= c[0];
 444
 445                 //printf("alpha[0][%d] = %f\n", i, alpha[0][i]);        /* OK agrees with Matlab */
 446         }
 447
 448         *loglik1 = *loglik;
 449         *loglik = -log(c[0]);
 450         if (iter == 2)
 451                 *loglik2 = *loglik;
 452
 453         for (t = 1; t < T; t++)
 454         {
 455                 c[t] = 0;
 456                 for (j = 0; j < N; j++)
 457                 {
 458                         alpha[t][j] = 0;
 459                         for (i = 0; i < N; i++)
 460                                 alpha[t][j] += alpha[t-1][i] * a[i][j];
 461                         alpha[t][j] *= b[t][j];
 462
 463                         c[t] += alpha[t][j];
 464                 }
 465
 466                 /*
 467                  if (c[t] == 0)
 468                  {
 469                          printf("c[%d] = 0, going to blow up so exiting\n", t);
 470                          for (i = 0; i < N; i++)
 471                                  if (b[t][i] == 0)
 472                                          fprintf(stderr, "b[%d][%d] was zero\n", t, i);
 473                          fprintf(stderr, "x[t] was \n");
 474                          for (i = 0; i < L; i++)
 475                                  fprintf(stderr, "%f ", x[t][i]);
 476                          fprintf(stderr, "\n\n");
 477                          exit(-1);
 478                  }
 479                  */
 480
 481                 c[t] = 1 / c[t];
 482                 for (j = 0; j < N; j++)
 483                         alpha[t][j] *= c[t];
 484
 485                 //printf("c[%d] = %e\n", t, c[t]);
 486
 487                 *loglik -= log(c[t]);
 488         }
 489
 490         /* calculate backwards probs using same coefficients */
 491         for (i = 0; i < N; i++)
 492                 beta[T-1][i] = 1;
 493         t = T - 1;
 494         while (1)
 495         {
 496                 for (i = 0; i < N; i++)
 497                         beta[t][i] *= c[t];
 498
 499                 if (t == 0)
 500                         break;
 501
 502                 for (i = 0; i < N; i++)
 503                 {
 504                         beta[t-1][i] = 0;
 505                         for (j = 0; j < N; j++)
 506                                 beta[t-1][i] += a[i][j] * b[t][j] * beta[t][j];
 507                 }
 508
 509                 t--;
 510         }
 511
 512         /*
 513          printf("alpha:\n");
 514          for (t = 0; t < T; t++)
 515          {
 516                  for (i = 0; i < N; i++)
 517                          printf("%4.4e\t\t", alpha[t][i]);
 518                  printf("\n");
 519          }
 520          printf("\n\n");printf("beta:\n");
 521          for (t = 0; t < T; t++)
 522          {
 523                  for (i = 0; i < N; i++)
 524                          printf("%4.4e\t\t", beta[t][i]);
 525                  printf("\n");
 526          }
 527          printf("\n\n");
 528          */
 529
 530         /* calculate posterior probs */
 531         double tot;
 532         for (t = 0; t < T; t++)
 533         {
 534                 tot = 0;
 535                 for (i = 0; i < N; i++)
 536                 {
 537                         gamma[t][i] = alpha[t][i] * beta[t][i];
 538                         tot += gamma[t][i];
 539                 }
 540                 for (i = 0; i < N; i++)
 541                 {
 542                         gamma[t][i] /= tot;
 543
 544                         //printf("gamma[%d][%d] = %f\n", t, i, gamma[t][i]);
 545                 }
 546         }
 547
 548         for (t = 0; t < T-1; t++)
 549         {
 550                 tot = 0;
 551                 for (i = 0; i < N; i++)
 552                 {
 553                         for (j = 0; j < N; j++)
 554                         {
 555                                 xi[t][i][j] = alpha[t][i] * a[i][j] * b[t+1][j] * beta[t+1][j];
 556                                 tot += xi[t][i][j];
 557                         }
 558                 }
 559                 for (i = 0; i < N; i++)
 560                         for (j = 0; j < N; j++)
 561                                 xi[t][i][j] /= tot;
 562         }
 563
 564         /*
 565          // CHECK - fine
 566          // gamma[t][i] = \sum_j{xi[t][i][j]}
 567          tot = 0;
 568          for (j = 0; j < N; j++)
 569          tot += xi[3][1][j];
 570          printf("gamma[3][1] = %f, sum_j(xi[3][1][j]) = %f\n", gamma[3][1], tot);
 571          */
 572
 573         for (t = 0; t < T; t++)
 574         {
 575                 free(alpha[t]);
 576                 free(beta[t]);
 577         }
 578         free(alpha);
 579         free(beta);
 580         free(c);
 581 }
 582
 583 void viterbi_decode(double** x, int T, model_t* model, int* q)
 584 {
 585         int i, j, t;
 586         double max;
 587         int havemax;
 588
 589         int N = model->N;
 590         int L = model->L;
 591         double* p0 = model->p0;
 592         double** a = model->a;
 593         double** mu = model->mu;
 594         double** cov = model->cov;
 595
 596         /* inverse covariance and its determinant */
 597         double** icov = (double**) malloc(L*sizeof(double*));
 598         for (i = 0; i < L; i++)
 599                 icov[i] = (double*) malloc(L*sizeof(double));
 600         double detcov;
 601
 602         double** logb = (double**) malloc(T*sizeof(double*));
 603         double** phi = (double**) malloc(T*sizeof(double*));
 604         int** psi = (int**) malloc(T*sizeof(int*));
 605         for (t = 0; t < T; t++)
 606         {
 607                 logb[t] = (double*) malloc(N*sizeof(double));
 608                 phi[t] = (double*) malloc(N*sizeof(double));
 609                 psi[t] = (int*) malloc(N*sizeof(int));
 610         }
 611
 612         /* temporary memory */
 613         double* gauss_y = (double*) malloc(L*sizeof(double));
 614         double* gauss_z = (double*) malloc(L*sizeof(double));
 615
 616         /* calculate observation logprobs */
 617         invert(cov, L, icov, &detcov);
 618         for (t = 0; t < T; t++)
 619                 for (i = 0; i < N; i++)
 620                         logb[t][i] = loggauss(x[t], L, mu[i], icov, detcov, gauss_y, gauss_z);
 621
 622         /* initialise */
 623         for (i = 0; i < N; i++)
 624         {
 625                 phi[0][i] = log(p0[i]) + logb[0][i];
 626                 psi[0][i] = 0;
 627         }
 628
 629         for (t = 1; t < T; t++)
 630         {
 631                 for (j = 0; j < N; j++)
 632                 {
 633                         max = -1000000;
 634                         havemax = 0;
 635
 636                         psi[t][j] = 0;
 637                         for (i = 0; i < N; i++)
 638                         {
 639                                 if (phi[t-1][i] + log(a[i][j]) > max || !havemax)
 640                                 {
 641                                         max = phi[t-1][i] + log(a[i][j]);
 642                                         phi[t][j] = max + logb[t][j];
 643                                         psi[t][j] = i;
 644                                         havemax = 1;
 645                                 }
 646                         }
 647                 }
 648         }
 649
 650         /* find maximising state at time T-1 */
 651         max = phi[T-1][0];
 652         q[T-1] = 0;
 653         for (i = 1; i < N; i++)
 654         {
 655                 if (phi[T-1][i] > max)
 656                 {
 657                         max = phi[T-1][i];
 658                         q[T-1] = i;
 659                 }
 660         }
 661
 662
 663         /* track back */
 664         t = T - 2;
 665         while (t >= 0)
 666         {
 667                 q[t] = psi[t+1][q[t+1]];
 668                 t--;
 669         }
 670
 671         /* de-allocate memory */
 672         for (i = 0; i < L; i++)
 673                 free(icov[i]);
 674         free(icov);
 675         for (t = 0; t < T; t++)
 676         {
 677                 free(logb[t]);
 678                 free(phi[t]);
 679                 free(psi[t]);
 680         }
 681         free(logb);
 682         free(phi);
 683         free(psi);
 684
 685         free(gauss_y);
 686         free(gauss_z);
 687 }
 688
 689 /* invert matrix and calculate determinant using LAPACK */
 690 void invert(double** cov, int L, double** icov, double* detcov)
 691 {
 692         /* copy square matrix into a vector in column-major order */
 693         double* a = (double*) malloc(L*L*sizeof(double));
 694         int i, j;
 695         for(j=0; j < L; j++)
 696                 for (i=0; i < L; i++)
 697                         a[j*L+i] = cov[i][j];
 698
 699         int M = (int) L;
 700         int* ipiv = (int *) malloc(L*L*sizeof(int));
 701         int ret;
 702
 703         /* LU decomposition */
 704         ret = dgetrf_(&M, &M, a, &M, ipiv, &ret);       /* ret should be zero, negative if cov is singular */
 705         if (ret < 0)
 706         {
 707                 fprintf(stderr, "Covariance matrix was singular, couldn't invert\n");
 708                 exit(-1);
 709         }
 710
 711         /* find determinant */
 712         double det = 1;
 713         for(i = 0; i < L; i++)
 714                 det *= a[i*L+i];
 715         // TODO: get this to work!!! If detcov < 0 then cov is bad anyway...
 716         /*
 717         int sign = 1;
 718         for (i = 0; i < L; i++)
 719                 if (ipiv[i] != i)
 720                         sign = -sign;
 721         det *= sign;
 722          */
 723         if (det < 0)
 724                 det = -det;
 725         *detcov = det;
 726
 727         /* allocate required working storage */
 728 #ifndef HAVE_ATLAS
 729         int lwork = -1;
 730         double lwbest = 0.0;
 731         dgetri_(&M, a, &M, ipiv, &lwbest, &lwork, &ret);
 732         lwork = (int) lwbest;
 733         double* work  = (double*) malloc(lwork*sizeof(double));
 734 #endif
 735
 736         /* find inverse */
 737         dgetri_(&M, a, &M, ipiv, work, &lwork, &ret);
 738
 739         for(j=0; j < L; j++)
 740                 for (i=0; i < L; i++)
 741                         icov[i][j] = a[j*L+i];
 742
 743 #ifndef HAVE_ATLAS
 744         free(work);
 745 #endif
 746         free(a);
 747 }
 748
 749 /* probability of multivariate Gaussian given mean, inverse and determinant of covariance */
 750 double gauss(double* x, int L, double* mu, double** icov, double detcov, double* y, double* z)
 751 {
 752         int i, j;
 753         double s = 0;
 754         for (i = 0; i < L; i++)
 755                 y[i] = x[i] - mu[i];
 756         for (i = 0; i < L; i++)
 757         {
 758                 //z[i] = 0;
 759                 //for (j = 0; j < L; j++)
 760                 //      z[i] += icov[i][j] *  y[j];
 761                 z[i] = cblas_ddot(L, &icov[i][0], 1, y, 1);
 762         }
 763         s = cblas_ddot(L, z, 1, y, 1);
 764         //for (i = 0; i < L; i++)
 765         //      s += z[i] * y[i];
 766
 767         return exp(-s/2.0) / (pow(2*PI, L/2.0) * sqrt(detcov));
 768 }
 769
 770 /* log probability of multivariate Gaussian given mean, inverse and determinant of covariance */
 771 double loggauss(double* x, int L, double* mu, double** icov, double detcov, double* y, double* z)
 772 {
 773         int i, j;
 774         double s = 0;
 775         double ret;
 776         for (i = 0; i < L; i++)
 777                 y[i] = x[i] - mu[i];
 778         for (i = 0; i < L; i++)
 779         {
 780                 //z[i] = 0;
 781                 //for (j = 0; j < L; j++)
 782                 //      z[i] += icov[i][j] *  y[j];
 783                 z[i] = cblas_ddot(L, &icov[i][0], 1, y, 1);
 784         }
 785         s = cblas_ddot(L, z, 1, y, 1);
 786         //for (i = 0; i < L; i++)
 787         //      s += z[i] * y[i];
 788
 789         ret = -0.5 * (s + L * log(2*PI) + log(detcov));
 790
 791         /*
 792         // TEST
 793         if (ISINF(ret) > 0)
 794                 printf("loggauss returning infinity\n");
 795         if (ISINF(ret) < 0)
 796                 printf("loggauss returning -infinity\n");
 797         if (ISNAN(ret))
 798                 printf("loggauss returning nan\n");
 799         */
 800
 801         return ret;
 802 }
 803
 804 void hmm_print(model_t* model)
 805 {
 806         int i, j;
 807         printf("p0:\n");
 808         for (i = 0; i < model->N; i++)
 809                 printf("%f ", model->p0[i]);
 810         printf("\n\n");
 811         printf("a:\n");
 812         for (i = 0; i < model->N; i++)
 813         {
 814                 for (j = 0; j < model->N; j++)
 815                         printf("%f ", model->a[i][j]);
 816                 printf("\n");
 817         }
 818         printf("\n\n");
 819         printf("mu:\n");
 820         for (i = 0; i < model->N; i++)
 821         {
 822                 for (j = 0; j < model->L; j++)
 823                         printf("%f ", model->mu[i][j]);
 824                 printf("\n");
 825         }
 826         printf("\n\n");
 827         printf("cov:\n");
 828         for (i = 0; i < model->L; i++)
 829         {
 830                 for (j = 0; j < model->L; j++)
 831                         printf("%f ", model->cov[i][j]);
 832                 printf("\n");
 833         }
 834         printf("\n\n");
 835 }
 836
 837