annotate hmm/hmm.c @ 321:f1e6be2de9a5

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