% ML cost function for Gauss-Markov h
%
% originally written by Hai Chen, May 2000
%
function cost = get_GM_ML_cost( b)

global z;
global N;
global var_noise;
global inv_Rh;
global inv_Rh_d;
global alpha;
global alpha2;
global FIR_save;

A = get_A_C( b);
  
% original:
% t = A(1,:)';
% q = t*(z(1)/var_noise) + inv_Rh_d;   
% G = inv( t*(t'/var_noise) + inv_Rh );
%
% modified to use known h_1, RP 11/09/2000, not quite right
%
t = A(1,:)';
q = t*(z(1)/var_noise) + inv_Rh*FIR_save(:,1);
G = inv( t*(t'/var_noise) + (1 + alpha2)*inv_Rh );
  
cost = - log(det(G)) - q'*G*q;
  
for k = 2:N-1

  t = A(k,:)';
  q = t*z(k)/var_noise + (1-alpha)*inv_Rh_d + alpha*inv_Rh*G*q;
  G = inv( t*(t'/var_noise) + inv_Rh + alpha2*(inv_Rh-inv_Rh*G*inv_Rh) );
  
  cost = cost - log(det(G)) - q'*G*q;
end      

t = A(N,:)';
q = t*z(N)/var_noise + inv_Rh_d + alpha*inv_Rh*G*q;
G = inv( t*(t'/var_noise) + inv_Rh - alpha2*inv_Rh*G*inv_Rh );
  
cost = cost - log(det(G)) - q'*G*q;
