% get_cost( zz, cur, next, col, el) - Kalman cost function for K=1
%
function cost = 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 Cmiss;
global Cfalse;

cost = HUGE_VAL;

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

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

% next is non-existant measurement
%
if( next > MM(col))
  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) + (MM(col)-1)*missed_cost + Cmiss(2,K) + Cfalse(MM(col));
