

static char *rcsid = "kalman.c";



#include <kalman.h>


KalmanFilter kalmNew(matrix phi, matrix Q, matrix H)
{
  KalmanFilter kf;

  assert(matHeight(phi) == matWidth(phi));
  assert(matHeight(Q)   == matWidth(Q));
  assert(matHeight(phi) == matWidth(Q));
  assert(matHeight(phi) == matWidth(H));

  
  kf = (KalmanFilter) malloc(sizeof(*kf));
  if (kf == NULL) {
    return NULL;
  }

  kf->phi = kf->Q = kf->H = kf->K = kf->P = kf->xhat = kf->zhat = matNULL;
  kf->k = 0;
  kf->state = KalmNew;
  
  kf->phi  = phi;
  kf->Q    = Q;
  kf->H    = H;
  kf->K    = matNew(matWidth(phi), matHeight(H));
  kf->P    = matNew(matHeight(Q), matWidth(Q));
  kf->xhat = matNew(matWidth(phi), 1);
  kf->zhat = matNew(matHeight(H), 1);

  return kf;
}

void kalmFree(KalmanFilter kf)
{                                            /* phi, Q, H owned by user */
  matFree(kf->K);
  matFree(kf->P);
  matFree(kf->xhat);
  matFree(kf->zhat);
}

KalmanFilter kalmClone(KalmanFilter kf)
{
  return NULL;
}

void kalmInitialize(KalmanFilter kf, matrix b, matrix M)
{

  assert(kf->state == KalmNew);
  kf->state = KalmInit;
  kf->k = 0;

  matCopy(kf->xhat, b);
  matCopy(kf->P, M);
}

void kalmExtrapolate(KalmanFilter kf)
{
  matrix a, b;
  
  assert(kf->state == KalmUpdate  ||  kf->state == KalmInit);

  if (kf->state != KalmInit) {               /* don't extrapolate the init. vals. */
    matMulR(kf->xhat, kf->phi, kf->xhat);

    a = matTrans(kf->phi);
    b = matMul(kf->phi, kf->P);
    matMulR(kf->P, b, a);
    matAddR(kf->P, kf->P, kf->Q);
    matFree(a);
    matFree(b);
  }

  matMulR(kf->zhat, kf->H, kf->xhat);
  kf->state = KalmExtrapolate;
  kf->k++;
}

void kalmUpdate(KalmanFilter kf, matrix z, matrix R)
{
  matrix a, b, c;
  
  if (kf->state != KalmExtrapolate) {
    kalmExtrapolate(kf);
  }

  c = matTrans(kf->H);
  a = matMul(kf->P, c);
  b = matMul(kf->H, a);
  matAddR(b, b, R);
  matDivR(kf->K, a, b);
  matFree(a);
  matFree(b);
  matFree(c);

  a = matMul(kf->K, kf->H);
  b = matMul(a, kf->P);
  matSubR(kf->P, kf->P, b);
  matFree(a);
  matFree(b);

  a = matSub(z, kf->zhat);
  b = matMul(kf->K, a);
  matAddR(kf->xhat, kf->xhat, b);
  matFree(a);
  matFree(b);

  matMulR(kf->zhat, kf->H, kf->xhat);

  kf->state = KalmUpdate;
}


double kalmWeighCand(KalmanFilter kf, matrix z, matrix R)
{
  return 0.0;
}
