double raninvwis(double *wis, int t, int d, double *s)
// inverse Wishart: t d.o.f. d dimension S data matrix
// Ref Liu: Monte Carlo Strategies pp 40-41
{
double *b, *n, *v, *cf, *ww, y ;
int i, j ;
if (t < d) {
fatalx("(raninvwis) d.o.f. too small %d %d\n", t, d) ;
}
ZALLOC(b, d*d, double) ;
ZALLOC(n, d*d, double) ;
ZALLOC(v, d, double) ;
ZALLOC(cf, d*d, double) ;
ZALLOC(ww, d*d, double) ;
for (i=0; i<d; i++) {
v[i] = ranchi(t-i) ;
for (j=0; j<i; j++) {
n[i*d+j] = n[j*d+i] = gauss() ;
}
}
y = b[0] = v[0] ;
for (j=1; j<d; j++) {
b[j*d+0] = b[j] = sqrt(y)*n[j] ;
}
for (j=1; j<d; j++) {
b[j*d+j] = v[j] ;
for (i=0; i<j; i++) {
y = n[i*d+j] ;
b[j*d+j] += y*y ;
b[j*d+i] = b[i*d+j] = y*sqrt(v[i]) + vdot(n+i*d, n+j*d, i-1) ;
}
}
cholesky(cf, s, d) ;
mulmat(ww, cf, b, d, d, d) ;
transpose(cf, cf, d, d) ;
mulmat(wis, ww, cf, d, d, d) ;
free(b) ;
free(n) ;
free(v) ;
free(cf) ;
free(ww) ;
}
int ekf_step(void * v, double * z)
{
/* unpack incoming structure */
int * ptr = (int *)v;
int n = *ptr;
ptr++;
int m = *ptr;
ptr++;
int s = *ptr;
ekf_t ekf;
unpack(v, &ekf, n, m, s);
// NON-ADDITIVE
/* P_k = F_{k-1} P_{k-1} F^T_{k-1} + L_{k-1} Y_{k-1} L^T_{k-1} + Q_{k-1} */
mulmat(ekf.F, ekf.P, ekf.tmp0, n, n, n);
transpose(ekf.F, ekf.Ft, n, n);
mulmat(ekf.tmp0, ekf.Ft, ekf.Pp, n, n, n);
mulmat(ekf.L, ekf.Y, ekf.tmp6, n, s, s);
transpose(ekf.L, ekf.Lt, n, s);
mulmat(ekf.tmp6, ekf.Lt, ekf.tmp0, n, s, n);
accum(ekf.Pp, ekf.tmp0, n, n);
accum(ekf.Pp, ekf.Q, n, n);
/* G_k = P_k H^T_k (H_k P_k H^T_k + R)^{-1} */
transpose(ekf.H, ekf.Ht, m, n);
mulmat(ekf.Pp, ekf.Ht, ekf.tmp1, n, n, m);
mulmat(ekf.H, ekf.Pp, ekf.tmp2, m, n, n);
mulmat(ekf.tmp2, ekf.Ht, ekf.tmp3, m, n, m);
accum(ekf.tmp3, ekf.R, m, m);
if (cholsl(ekf.tmp3, ekf.tmp4, ekf.tmp5, m)) return 1;
mulmat(ekf.tmp1, ekf.tmp4, ekf.G, n, m, m);
/* \hat{x}_k = \hat{x_k} + G_k(z_k - h(\hat{x}_k)) */
sub(z, ekf.hx, ekf.tmp5, m);
mulvec(ekf.G, ekf.tmp5, ekf.tmp2, n, m);
add(ekf.fx, ekf.tmp2, ekf.x, n);
/* P_k = (I - G_k H_k) P_k */
mulmat(ekf.G, ekf.H, ekf.tmp0, n, m, n);
negate(ekf.tmp0, n, n);
mat_addeye(ekf.tmp0, n);
mulmat(ekf.tmp0, ekf.Pp, ekf.P, n, n, n);
/* success */
return 0;
}
void
genmultgauss(double *rvec, int num, int n, double *covar)
// rvec contains num mvg variates
{
double *cf ;
ZALLOC(cf, n*n, double) ;
cholesky(cf, covar, n) ;
transpose(cf, cf, n, n) ;
gaussa(rvec, num*n) ;
mulmat(rvec, rvec, cf, num, n, n) ;
free(cf) ;
}
