% cost_update( col, v, vl) - update kalman predicted values and gain for K=1
%
% v comes from the v array in viterbi and specifies
% the indices of the previous states representing the
% lowest cost paths.
% 
% vl comes from the vl array in viterbi and specifies
% the "l" values associated with the previous states.
% 
function cost_update( col, v, vl)

global DEBUG;
global N;
global L;
global M;
global MM;
global MM_max;
global G;
global XY;
global XY_offset;
global kxv;

if( col == 1)	% cost_init() already handled this case
  return;
end

jj_m = (col-1)*M + [1:M];		% indices for kxv

if( MM(col) > 0)
  jj_mm = XY_offset(col) + [1:MM(col)];	% indices for XY
end

k1 = kxv(:,jj_m);
k2 = k1;

% if( DEBUG) col, v, vl, jj_mm, end;

for j = 1:M
  for i = 1:L

    jv = v(i,j);

%    if( DEBUG) j, i, jv, end;

    if( jv ~= 0)
    
      el12 = [1:2] + (vl(i,j)-1)*4;
      el34 = el12 + 2;
    
      i12 = [1:2] + (i-1)*4;
      i34 = i12 + 2;

      if( j > MM_max)			% for missed target event, no update needed
        k2(i12,j) = k1(el12,jv);
        k2(i34,j) = k1(el34,jv);
      else
        jjj = jj_mm(j);
        k2(i12,j) = k1(el12,jv) + G * (XY(1,jjj) - k1(el12(1),jv));	% update x position and velocity
        k2(i34,j) = k1(el34,jv) + G * (XY(2,jjj) - k1(el34(1),jv));	% update y position and velocity
      end

    end

  end
end

kxv(:,jj_m) = k2;	% update kxv

if( col < N)
  kalman_gain_update;
  kalman_predict(col);
end
