/* 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);
}
