%%%% Code to compute the kernel between two Gaussian emission HMMs


function [ba] = bhatt(mu1,mu2,cov1,cov2)
%   function [ba] = bhatt(mu1,mu2,cov1,cov2)
%       Calculates the bhattacharya affinity for two distributions
%       mu1, mu2, cov1 and cov2 are the means and covariances
%           mu1 and mu2 are column vectors, cov1 and cov2 are square
%           matrices
%
%   Yingbo Song (yingbo@cs.columbia.edu)

  [n m] = size(cov1);

% add a small regularizer to the diagonal, ensure covm is full rank
cov1 = cov1 + 1e-5*trace(cov1)*eye(n);
cov2 = cov2 + 1e-5*trace(cov2)*eye(n);

iC1 = inv(cov1);
iC2 = inv(cov2);
Cd = inv(iC1 + iC2);
Md = iC1*mu1 + iC2*mu2;

% calculate the affinity
rho = 0.5;     % exponential, should be 0.5 for Bhattacharyya affinity

normalizer = (2*pi)^((1-2*rho)*(n/2)) * rho^(-n/2);
normalizer = normalizer * det(cov1)^(-rho/2) * det(cov2)^(-rho/2) * sqrt(det(Cd));
ba = normalizer * exp((-rho/2)*(mu1'*(iC1)*mu1 + mu2'*(iC2)*mu2 - Md'*(Cd)*Md));

% end of function


function [K] = elkernel(hmm1,hmm2,T)
%   function [K] = elkernel(hmm1,hmm2,T)
%       Tony's iterative PPK
%   NOT in logspace
if ( sum(hmm1.prior) < 0 )
  error('elkernel: The hmm parameters cannot be in log-space!'); 
end

prior1 = hmm1.prior;
prior2 = hmm2.prior;
transmat1 = hmm1.trans;
transmat2 = hmm2.trans;
M = length(prior1);
N = length(prior2);

pad = 0.45;
pot = zeros(M,N);
for i=1:M
  for j=1:N
    pot(i,j) = bhatt(hmm1.model(i).mu,hmm2.model(j).mu,...
    hmm1.model(i).covv + pad,hmm2.model(j).covv + pad);
  end
end

K = 0;
if (T==1)
  for i=1:M
    for j=1:N
      K=K+prior1(i)*prior2(j)*pot(i,j);
    end
  end
else
  sep1 = zeros(M,N);
  for i=1:M
    for j=1:N
      sep1 = sep1 + prior1(i)*prior2(j)*pot(i,j)*transmat1(i,:)'*transmat2(j,:);
    end
  end
for t=2:T
  sep2 = zeros(M,N);
  for i=1:M
    for j=1:N
      sep2 = sep2 + sep1(i,j)*pot(i,j)*transmat1(i,:)'*transmat2(j,:);
    end
  end
  sep1 = sep2;
  end
  K = sum(sum(sep2.*pot));
end
