% cost_init() - initialize kalman cost stuff
%
function cost_init()

global combos;

global K;
global M;
global N;
global L;
global MM;
global XY;

global dt;
global A;
global AT;
global C;
global Ke;
global S2a;
global Ku;

global kxv;

global MM_max;
global missed_cost;
global Winc;
global Wtot;

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

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

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

% each column of kxv has [x xv y yv]' repeated L times

if( K > 1)
  kxv = zeros( 4*L, M*K*N);
  txy = zeros( 2, max(M,MM(1)));	% max(M,MM(1)) handles K==MM(1) case
  txy(:,1:MM(1)) = XY(:,1:MM(1));
  for i = 1:M
    i1 = 1 + (i-1)*K;
    i1 = [i1:i1+K-1];
    kxv(1,i1) = txy(1,combos(:,i));
    kxv(3,i1) = txy(2,combos(:,i));
  end
else
  kxv = zeros( 4*L, M*N);
  kxv(1,1:MM(1)) = XY(1,1:MM(1));
  kxv(3,1:MM(1)) = XY(2,1:MM(1));
end

for i=1:10	% try to get close to convergence 
  kalman_gain_update;		% initial update
end

if( K > 1)
  Winc = zeros( L*K*M, MM_max+1);
  Winc(:, MM_max+1) = missed_cost;
  Wtot = zeros( K*M, L);
end

if( 1 < N)
  kalman_predict(1);
end
