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 |