% OLD_get_cost( zz, cur, next, col, el) - Kalman cost function for K=1
%
function cost = OLD_get_cost( zz, cur, next, col, el)

global HUGE_VAL;
global MM_max;
global MM;
global M;
global K;
global missed_cost;
global XY_offset;
global XY;
global kxv;
global Kv;
global log2pi;
global UNUSED_PENALTY;

APRIORI = 1;
UNUSED_PENALTY = 1;

global Cmiss;
global Cfalse;

cost = HUGE_VAL;

if( col == 1)			% handle dummy initial state
  if( cur == next & next <= MM(1))
    if( APRIORI)
      cost = Cmiss(K+1,K) + Cfalse(MM(1)-K+1);
    else
      cost = 0;
    end
  end
  return;
end

if( next > MM_max)		% next is missed target state
  cost = MM(col)*missed_cost;
  if( APRIORI)
    cost = cost + Cmiss(1,K) + Cfalse(MM(col)+1);
  end
  return;
end

if( next > MM(col))		% next is non-existant measurement
  return;
end

i2 = next + XY_offset(col);	% index for XY

if( XY(1,i2) == HUGE_VAL)	% for testing with false detections never choosen
  return;
end

i1 = cur  + (col - 1) * M;	% index for kxv

rx = XY(1,i2) - kxv(1+(el-1)*4,i1);
ry = XY(2,i2) - kxv(3+(el-1)*4,i1);

cost = (rx*rx + ry*ry)/(2*Kv) + log2pi + log(Kv);	% added log2pi+log(Kv)  28 Jan. 1999

if( UNUSED_PENALTY)
  cost = cost + (MM(col)-1)*missed_cost;
end

if( APRIORI)
  cost = cost + Cmiss(2,K) + Cfalse(MM(col));
end
