mylinprog.m

一个目标跟踪系统的MATLAB 源程序包

% function associate_prob = myLinProg(cost)
%
% use Linear programming to solve the relaxed S-D assignment problem
%
% cost --- the cost matrix in high-dim matrix form, dimensionality = S
%          cost(i,:,:,...) is the cost of having the i-th track
%          cost(:,j,:,...) is the cost of having the j-th measurement of the oldest time depth (scan index)
%          cost(:,1,:,...) is the dummy measurement (denoting the missed detection) of the oldest time depth
% associate_prob --- the association probabilities of a specific track-to-meas. path, same form as cost
%                    can be used for JPDA filter update

% by Huimin Chen, 09/29/01
function associate_prob = myLinProg(cost)

dim = size(cost);
S = length(dim);
% number of variables
n = prod(dim);

% form the equality constraints
b1 = ones(dim(1), 1);
n1 = length(b1);
A1 = zeros(n1, n);
for i=1:n1
    index = 1:dim(1):n-1;
    A1(i, index+(i-1)) = ones(size(index));
end

% form the inequality constraints
len = sum(dim(2:S));
b = ones(len, 1);
n2 = length(b);
A = zeros(n2, n);
for i=2:S
    col_index = [];
    len3 = prod(dim(1:i-1));
    for k=1:n/len3/dim(i)
        col_index = [col_index, [1:len3]+(k-1)*prod(dim(1:i))];
    end    
    for j=1:dim(i)
        row_index = sum(dim(2:i-1)) + j;
        if j==1
            b(row_index) = dim(1);
        end
        A(row_index, col_index+(j-1)*len3) = ones(size(col_index));
    end
end

% upper and lower bounds
LB = zeros(1,n);
UB = ones(1,n);

[X, FVAL, EXITFLAG] = linprog(reshape(cost, [n,1]), A, b, A1, b1, LB, UB);
if EXITFLAG > 0
    associate_prob = reshape(X, size(cost));
else
    disp('Linear Programming failed to find a solution!');
    associate_prob = [];
end