/* npdf: multivariate normal probability density function. * @x: the x vector to calculate for. * @mean: the mean vector to calculate for. * @cov: the covariance matrix to calculate for. */ double npdf (struct vector *x, struct vector *mean, struct matrix *cov) { /* declare required variables. */ struct matrix *L; double k, det, pdf; /* ensure the inputs are valid. */ if (x->n != mean->n || x->n != cov->m || x->n != cov->n) { /* output an error message and return nothing. */ ferror ("invalid arguments to normal pdf"); return NAN; } /* allocate the cholesky matrix. */ L = matrix_new (cov->m, cov->n); /* calculate the cholesky decomposition. */ if (!L || !matrix_chol (cov, L)) { /* hmm... covariance matrices should always be PSD. */ ferror ("invalid covariance matrix"); return NAN; } /* calculate the dimensionality. */ k = (double) x->n; /* calculate the determinant. */ det = matrix_chol_det (L); /* free the cholesky decomposition. */ matrix_free (L); /* calculate the pdf value. */ pdf = exp (-0.5 * diff_matrix_diff_mul (x, mean, cov)); pdf *= pow (2.0 * M_PI, -0.5 * k) * pow (det, -0.5); /* return the pdf value. */ return pdf; }
void FUNC( smc_ref, TYPE)( T* x_init, T* x, T* w, T* y, int N, int Dx, int Dy, int total_time, T* h_args_l, T* scale_step, T* cov_step, T* ll) { T* cumw = (T*) malloc(N * sizeof(T)); T* steps = (T*) malloc(N * Dx * sizeof(T)); T* randn = (T*) malloc(N * Dx * sizeof(T)); T* randu = (T*) malloc(N * sizeof(T)); int* indices = (int*) malloc(N * sizeof(int)); T* xt_copy = (T*) malloc(N * Dx * total_time * sizeof(T)); int i, t; for (i = 0; i < N; i++) { w[i] = 1.0; } int* history = (int*) malloc(N * total_time * sizeof(int)); int* history_id = (int*) malloc(N * sizeof(int)); history_identity_ref(history_id, N); T* L_step = (T*) malloc(Dx * Dx * sizeof(T)); matrix_chol(cov_step, L_step, Dx); double old_sumw = (double) N; double lld = 0; for (t = 0; t < total_time; t++) { // printf("%d\n", t); populate_randn(randn, N * Dx); populate_rand(randu, N); for (i = 0; i < N; i++) { matrix_times(L_step, vector_get(randn, Dx, i), vector_get(steps, Dx, i), Dx, Dx, Dx, 1); } FUNC(smc_step, TYPE)(x_init, w, N, Dx, Dy, y, scale_step, steps, x + t * Dx * N, t, h_args_l); double sumw = 0; double sumw2 = 0; for (i = 0; i < N; i++) { sumw += w[i]; sumw2 += w[i] * w[i]; } lld += log(sumw / old_sumw); old_sumw = sumw; double ESS = sumw * sumw / sumw2; if (ESS < N / 2) { scan_ref(N, w, cumw); resample_get_indices_ref(cumw, N, randu, indices, (T) sumw); for (i = 0; i < N; i++) { history[t*N + i] = indices[i]; } resample_ref(x_init, N, Dx, indices, x + N * Dx * t); for (i = 0; i < N; i++) { w[i] = 1.0; } old_sumw = (double) N; } else { if (sumw < 1) { for (i = 0; i < N; i++) { w[i] = (T) (w[i] * N / sumw); } old_sumw = (double) N; } x_init = x + t * Dx * N; for (i = 0; i < N; i++) { history[t*N + i] = history_id[i]; } } } *ll = (T) lld; for (i = 0; i < N * Dx * total_time; i++) { xt_copy[i] = x[i]; } historify_ref(x, N, Dx, total_time, history, xt_copy); free(randn); free(cumw); free(steps); free(randu); free(indices); free(xt_copy); free(L_step); free(history); free(history_id); }