% This was an initial test for defining an array containing
% incremental costs per track for state transitions for K>1.
%
% This has been vectorized, eliminating the three inner loops,
% and incorporated into kalman_predict.m
%
% R. Perry, 16 Feb. 1999

extra = log2pi + log(Kv);

Winc = zeros( L*K*M, MM_max);

i1 = (col-1)*M*K;		% offset for kxv

if( MM(col) > 0)

  for j = 1:MM(col)		% for each measurement

    ii2 = XY_offset(col) + j;

    for el = 1:L		% for each L-best

      for m = 1:M		% for each state

        for k = 1:K		% for each track

	  kel = k + ((el-1)*M + (m-1))*K;

          if( XY(1,ii2) == HUGE_VAL) % for testing with false detections never choosen
            Winc(kel,j) = HUGE_VAL;
          else
            ii1 = i1 + k + (m-1)*K;
            rx = XY(1,ii2) - kxv(1+(el-1)*4,ii1);
            ry = XY(2,ii2) - kxv(3+(el-1)*4,ii1);
            Winc(kel,j) = (rx*rx + ry*ry)/(2*Kv) + extra;
	  end

        end % k

      end % m

    end % el

  end % j

end % if
