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