/* IO(1) */

/*
 * kalman_gain_update: update Ke, Kv, and G
 */
void kalman_gain_update(void)
{
  Ke = A*Ke*AT + Ku;		/* Ke(l|l-1) */

  Kv = Ke(1,1) + Kn;		/* assuming C=[1 0] */

  G = Ke(:,1) / Kv;		/* Ke * C' * inv(Kv) */

  GT = G';

  Ke = Ke - G * Ke(1,:);	/* Ke(l|l) = Ke(l|l-1) - G * C * Ke(l|l-1) */
}


/*
 * kalman_predict: compute innovations for next step
 */
void kalman_predict( int _cur)
{
  vector i1 = [ 1+(_cur-1)*MM:_cur*MM];
  vector i2 = i1 + MM;
  int el;
  vector el12, el34;

  for( el=0; el<L; ++el) {
    el12 = [1:2] + el*4;
    el34 = [3:4] + el*4;
    kxv(i2,el12) = kxv(i1,el12)*AT;	/* x's and xv's */
    kxv(i2,el34) = kxv(i1,el34)*AT;	/* y's and yv's */
  }
}


/*
 * kalman_init: initialize kalman stuff
 */
void kalman_init(void)
{
  if( K > 1) {
    print("*** kalman is not yet implemented for K>1 ***");
    return;
  }

  A = [ 1 dt; 0 1];	/* for one dimension, position and velocity */
  AT = A';

  C = [ 1 0];
  Ke = eye(2,2);
  S2a = 0.1;		/* S2a*dt is noise acceleration spectral height */

  Ku = [ dt*dt*dt*dt/3  dt*dt*dt/2;  dt*dt*dt/2  dt*dt] * S2a;

  kxv = zeros( MM*N, 4*L);	/* each row: [x xv y yv] L times */

  kxv(1:MM,1:4) = xv(1:MM,[1 3 2 4]);

  kalman_gain_update();		/* initial update */

  kalman_predict(1);
}

/*
 * kalman_update: update predicted values and gain
 *
 * _v comes from the v array in viterbi and specifies
 * the indices of the previous states representing the
 * lowest cost paths
 */
void kalman_update( int _j, matrix _v, matrix _vl)
{
  vector i;
  matrix khat;

  if( _j == 1)	/* kalman_init() already handled this case */
    return;

  i = [ 1+(_j-1)*MM:_j*MM];

  khat = kxv(i,:);

  khat = khat(_v,:);

  kxv(i,1:2) = khat(:,1:2) + (xv(i,1) - khat(:,1)) * GT;	/* update x position and velocity */

  kxv(i,3:4) = khat(:,3:4) + (xv(i,2) - khat(:,3)) * GT;	/* update y position and velocity */

  if( _j < N) {
    kalman_gain_update();
    kalman_predict(_j);
  }
}
