function P = computeTrackProb(V, A, betaFA)
% COMPUTETRACKPROB Probability of associating measurements and tracks
%
% P = computeTrackProb(V, A, betaFA)
% Computes the probabilities P(T_j <-> y_i|Y, {T_k}) as needed to implement
% JPDA. The resulting probabilities are given in the matrix P
% (# measurements+1 x # tracks), where the element P_ij contains P(T_j <->
% y_i|Y, {T_k}) and the last row is used to indicate no associated
% measurement. V is the validation matrix for the measurements and tracks,
% and A the association matrix (without EX columns). betaFA the false
% alarm rate.
% Copyright (2019) Gustaf Hendeby
A = A - log(betaFA);
P = zeros(size(A, 1)+1, size(A, 2));
[P, ptot] = recurseComputeTrackProb(V, A, 1, P, 0, zeros(0, 2));
P = P/ptot;
end
% No claim of being efficient, but gets the job done in relatively few
% lines of code. Many tricka would apply to speed up the implementation.
function [P, ptot] = recurseComputeTrackProb(V, A, t, P, ptot, ind)
if t > size(A, 2)
p = 0;
for i=1:size(ind, 1)
if ind(i, 1) > size(A, 1); continue; end % Mo association is free in this formulation
p = p + A(ind(i, 1), ind(i, 2));
end
for i=1:size(ind, 1)
P(ind(i, 1), ind(i, 2)) = P(ind(i, 1), ind(i, 2)) + exp(p);
end
ptot = ptot + exp(p);
return
end
for m=1:size(V, 1)
if ~V(m, t); continue; end % Skip if not compatible
v = V(m, :);
V(m, :) = 0;
[P, ptot] = recurseComputeTrackProb(V, A, t+1, P, ptot, [ind; m, t]);
V(m, :) = v;
end
[P, ptot] = recurseComputeTrackProb(V, A, t+1, P, ptot, [ind; size(A, 1)+1, t]);
end