├── PMBM_Trajectory_Code ├── common │ ├── get_comps.m │ ├── assignmentoptimal.mexw64 │ └── assignmentoptimal.mexmaci64 ├── gospaMetric │ └── GOSPA code │ │ ├── README.md │ │ ├── ComputeGOSPAerror.m │ │ ├── exampleScript_angel.m │ │ ├── exampleScript.m │ │ ├── auctionAlgortihm.m │ │ ├── GOSPA.m │ │ └── assign2D.m ├── mbm01_onlineSmoothing_allTrajectory │ ├── approxSmoothedTrajectory.m │ ├── predictStep.m │ ├── stateExtract.m │ ├── backforwardFiltering.m │ ├── gentruth.m │ ├── main.m │ ├── updateStep.m │ └── dataAssoc.m ├── mbm_onlineSmoothing_allTrajectory │ ├── approxSmoothedTrajectory.m │ ├── predictStep.m │ ├── stateExtract.m │ ├── backforwardFiltering.m │ ├── gentruth.m │ ├── main.m │ ├── updateStep.m │ └── dataAssoc.m ├── pmbm_onlineSmoothing_allTrajectory │ ├── approxSmoothedTrajectory.m │ ├── stateExtract.m │ ├── predictStep.m │ ├── backforwardFiltering.m │ ├── gentruth.m │ ├── main.m │ ├── updateStep.m │ └── dataAssoc.m └── trajectoryMetric │ └── LPTrajMetric_sparse.m ├── LICENSE └── README.md /PMBM_Trajectory_Code/common/get_comps.m: -------------------------------------------------------------------------------- 1 | function Xc= get_comps(X,c) 2 | if isempty(X) 3 | Xc= zeros(2,0); 4 | else 5 | Xc= X(c,:); 6 | end 7 | 8 | -------------------------------------------------------------------------------- /PMBM_Trajectory_Code/common/assignmentoptimal.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yuhsuansia/Multi-scan-trajectory-PMBM-filter/HEAD/PMBM_Trajectory_Code/common/assignmentoptimal.mexw64 -------------------------------------------------------------------------------- /PMBM_Trajectory_Code/common/assignmentoptimal.mexmaci64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yuhsuansia/Multi-scan-trajectory-PMBM-filter/HEAD/PMBM_Trajectory_Code/common/assignmentoptimal.mexmaci64 -------------------------------------------------------------------------------- /PMBM_Trajectory_Code/gospaMetric/GOSPA code/README.md: -------------------------------------------------------------------------------- 1 | # GOSPA 2 | 3 | ## Synopsis 4 | 5 | This repository includes a MATLAB implementation of the generalized optimal sub-pattern assignment metric (GOSPA) to measure the distance between two finite sets of real vectors. Note that the two sets can have different cardinality. For details about GOSPA, check . Also visit for a 15-min presentation about the metric. 6 | 7 | ## Usage 8 | The provided implementation of GOSPA (in GOSPA.m) is based on the auction algorithm, the code (in auctionAlgortihm.m) for which is also available in the repository. An example 'exampleScript.m' is added to illustrate the usage of the GOSPA implementation. 9 | -------------------------------------------------------------------------------- /PMBM_Trajectory_Code/mbm01_onlineSmoothing_allTrajectory/approxSmoothedTrajectory.m: -------------------------------------------------------------------------------- 1 | function trajectoryMBM = approxSmoothedTrajectory(trajectoryMBM,trajectorySmoothEst,model) 2 | % Use part of smoothed trajectories to replace unsmoothed ones 3 | 4 | aMBM =[trajectoryMBM.a]; 5 | if ~isempty(trajectorySmoothEst) 6 | aSmoothEst = [trajectorySmoothEst.a]; 7 | else 8 | aSmoothEst = []; 9 | end 10 | 11 | numBernoulli = length(trajectoryMBM); 12 | [lia,locb] = ismember(aMBM,aSmoothEst); 13 | 14 | for i = 1:numBernoulli 15 | if lia(i)==true && trajectoryMBM(i).r~=0 16 | trajectoryLength = size(trajectoryMBM(i).x,2); 17 | smoothingLength = min(trajectoryLength,model.slideWindow+model.L); 18 | if smoothingLength < trajectoryLength 19 | trajectoryMBM(i).x(:,end-smoothingLength+1) = trajectorySmoothEst(locb(i)).x(:,end-smoothingLength+1); 20 | end 21 | end 22 | end 23 | 24 | end -------------------------------------------------------------------------------- /PMBM_Trajectory_Code/mbm_onlineSmoothing_allTrajectory/approxSmoothedTrajectory.m: -------------------------------------------------------------------------------- 1 | function trajectoryMBM = approxSmoothedTrajectory(trajectoryMBM,trajectorySmoothEst,model) 2 | % Use part of smoothed trajectories to replace unsmoothed ones 3 | 4 | aMBM =[trajectoryMBM.a]; 5 | if ~isempty(trajectorySmoothEst) 6 | aSmoothEst = [trajectorySmoothEst.a]; 7 | else 8 | aSmoothEst = []; 9 | end 10 | 11 | numBernoulli = length(trajectoryMBM); 12 | [lia,locb] = ismember(aMBM,aSmoothEst); 13 | 14 | for i = 1:numBernoulli 15 | if lia(i)==true && trajectoryMBM(i).r~=0 16 | trajectoryLength = size(trajectoryMBM(i).x,2); 17 | smoothingLength = min(trajectoryLength,model.slideWindow+model.L); 18 | if smoothingLength < trajectoryLength 19 | trajectoryMBM(i).x(:,end-smoothingLength+1) = trajectorySmoothEst(locb(i)).x(:,end-smoothingLength+1); 20 | end 21 | end 22 | end 23 | 24 | end -------------------------------------------------------------------------------- /PMBM_Trajectory_Code/pmbm_onlineSmoothing_allTrajectory/approxSmoothedTrajectory.m: -------------------------------------------------------------------------------- 1 | function trajectoryMBM = approxSmoothedTrajectory(trajectoryMBM,trajectorySmoothEst,model) 2 | % Use part of smoothed trajectories to replace unsmoothed ones 3 | 4 | aMBM =[trajectoryMBM.a]; 5 | if ~isempty(trajectorySmoothEst) 6 | aSmoothEst = [trajectorySmoothEst.a]; 7 | else 8 | aSmoothEst = []; 9 | end 10 | 11 | numBernoulli = length(trajectoryMBM); 12 | [lia,locb] = ismember(aMBM,aSmoothEst); 13 | 14 | for i = 1:numBernoulli 15 | if lia(i)==true && trajectoryMBM(i).r~=0 16 | trajectoryLength = size(trajectoryMBM(i).x,2); 17 | smoothingLength = min(trajectoryLength,model.slideWindow+model.L); 18 | if smoothingLength < trajectoryLength 19 | trajectoryMBM(i).x(:,end-smoothingLength+1) = trajectorySmoothEst(locb(i)).x(:,end-smoothingLength+1); 20 | end 21 | end 22 | end 23 | 24 | end -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 yuhsuansia 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /PMBM_Trajectory_Code/gospaMetric/GOSPA code/ComputeGOSPAerror.m: -------------------------------------------------------------------------------- 1 | function [squared_gospa,gospa_loc,gospa_mis,gospa_fal]=ComputeGOSPAerror(X_estimate,X_truth,t_birth,t_death,c_gospa,k) 2 | %Author: Angel F. Garcia-Fernandez 3 | %Computes the squared GOSPA error (alpha=2 and its decomposition, which is only possible for this choice of alpha) 4 | 5 | 6 | %We need to convert the ground truth and estimated set into a different 7 | %forma to apply GOSPA code. 8 | 9 | alive_targets_index=and(k>=t_birth,k model.threshold 20 | 21 | xpre = F*trajectoryMBM(i).x(:,end); 22 | Ppre = F*trajectoryMBM(i).P(:,:,end)*F'+Q; 23 | 24 | % trajectory case 25 | trajectoryMBM(i).x = [trajectoryMBM(i).x xpre]; 26 | trajectoryMBM(i).P = cat(3,trajectoryMBM(i).P,Ppre); 27 | 28 | % store prediction 29 | trajectoryMBM(i).xpre = [trajectoryMBM(i).xpre xpre]; 30 | trajectoryMBM(i).Ppre = cat(3,trajectoryMBM(i).Ppre,Ppre); 31 | 32 | % beta remains the same, only update epsilon 33 | trajectoryMBM(i).epsilon = [trajectoryMBM(i).epsilon trajectoryMBM(i).epsilon(end)+1]; 34 | 35 | % update weight vector of death time 36 | trajectoryMBM(i).w = [trajectoryMBM(i).w(1:end-1) trajectoryMBM(i).w(end)*(1-Ps) trajectoryMBM(i).w(end)*Ps]; 37 | end 38 | end 39 | 40 | % Incorporate birth intensity 41 | MB_birth = repmat(struct,nb,1); 42 | % Allocate memory 43 | for i = 1:nb 44 | MB_birth(i).r = model.rb(i); 45 | MB_birth(i).x = model.xb(:,i); 46 | MB_birth(i).P = model.Pb(:,:,i); 47 | MB_birth(i).beta = t; 48 | MB_birth(i).epsilon = t; 49 | end 50 | 51 | -------------------------------------------------------------------------------- /PMBM_Trajectory_Code/mbm_onlineSmoothing_allTrajectory/predictStep.m: -------------------------------------------------------------------------------- 1 | function [trajectoryMBM,MB_birth] = predictStep(trajectoryMBM,model,t) 2 | %PREDICT: PREDICT MULTI-BERNOULLI AND POISSON COMPONENTS 3 | 4 | % Get multi-Bernoulli prediction parameters from model 5 | F = model.F; 6 | Q = model.Q; 7 | Ps = model.Ps; 8 | 9 | % Interpret length of inputs 10 | nb = length(model.rb); % number of bernoulli components in MB birth 11 | n = length(trajectoryMBM); 12 | 13 | % Implement prediction algorithm 14 | 15 | % Predict existing tracks (single target hypotheses) 16 | for i = 1:n 17 | % if the probability that the trajectory still exists at current time 18 | % step is very small, then there is no need to predict it 19 | if trajectoryMBM(i).r~=0 && trajectoryMBM(i).w(end) > model.threshold 20 | 21 | xpre = F*trajectoryMBM(i).x(:,end); 22 | Ppre = F*trajectoryMBM(i).P(:,:,end)*F'+Q; 23 | 24 | % trajectory case 25 | trajectoryMBM(i).x = [trajectoryMBM(i).x xpre]; 26 | trajectoryMBM(i).P = cat(3,trajectoryMBM(i).P,Ppre); 27 | 28 | % store prediction 29 | trajectoryMBM(i).xpre = [trajectoryMBM(i).xpre xpre]; 30 | trajectoryMBM(i).Ppre = cat(3,trajectoryMBM(i).Ppre,Ppre); 31 | 32 | % beta remains the same, only update epsilon 33 | trajectoryMBM(i).epsilon = [trajectoryMBM(i).epsilon trajectoryMBM(i).epsilon(end)+1]; 34 | 35 | % update weight vector of death time 36 | trajectoryMBM(i).w = [trajectoryMBM(i).w(1:end-1) trajectoryMBM(i).w(end)*(1-Ps) trajectoryMBM(i).w(end)*Ps]; 37 | end 38 | end 39 | 40 | % Incorporate birth intensity 41 | MB_birth = repmat(struct,nb,1); 42 | % Allocate memory 43 | for i = 1:nb 44 | MB_birth(i).r = model.rb(i); 45 | MB_birth(i).x = model.xb(:,i); 46 | MB_birth(i).P = model.Pb(:,:,i); 47 | MB_birth(i).beta = t; 48 | MB_birth(i).epsilon = t; 49 | end 50 | 51 | -------------------------------------------------------------------------------- /PMBM_Trajectory_Code/mbm01_onlineSmoothing_allTrajectory/stateExtract.m: -------------------------------------------------------------------------------- 1 | function [est,multiTrajectories,Xest] = stateExtract(trajectoryEst) 2 | % Extract target states 3 | 4 | if isempty(trajectoryEst) 5 | est = zeros(4,0); 6 | multiTrajectories = struct('states',[],'beginTime',[],'endTime',[]); 7 | Xest.tVec = zeros(0,1); 8 | Xest.iVec = zeros(0,1); 9 | Xest.xState = zeros(2,0,0); 10 | return; 11 | end 12 | 13 | r = extractfield(trajectoryEst,'r'); 14 | ss = r == 1; 15 | trajectoryEst = trajectoryEst(ss); 16 | len = length(trajectoryEst); 17 | est = zeros(4,len); 18 | 19 | % extract the set of all trajectories 20 | 21 | multiTrajectories = struct('states',[],'beginTime',[],'endTime',[]); 22 | multiTrajectories = repmat(multiTrajectories,len,1); 23 | 24 | idx = true(len,1); 25 | for i = 1:len 26 | % select trajectory death time with the highest weight 27 | [~,tdeath] = max(trajectoryEst(i).w); 28 | % store estiamtes for set of current trajectories 29 | est(:,i) = trajectoryEst(i).x(:,end); 30 | if tdeath ~= length(trajectoryEst(i).w) 31 | idx(i) = false; 32 | end 33 | multiTrajectories(i).states = trajectoryEst(i).x(:,1:trajectoryEst(i).epsilon(tdeath)-trajectoryEst(i).beta+1); 34 | multiTrajectories(i).beginTime = trajectoryEst(i).beta; 35 | multiTrajectories(i).endTime = trajectoryEst(i).epsilon(tdeath); 36 | end 37 | % extract current set estimate 38 | est = est(:,idx); 39 | 40 | % re-construct output estimates for the use of trajectory metric 41 | if isempty(multiTrajectories) 42 | Xest.tVec = zeros(0,1); 43 | Xest.iVec = zeros(0,1); 44 | Xest.xState = zeros(2,0,0); 45 | else 46 | Xest.tVec = [multiTrajectories.beginTime]; 47 | Xest.iVec = [multiTrajectories.endTime]-[multiTrajectories.beginTime]+1; 48 | Xest.xState = zeros(2,max([multiTrajectories.endTime]),length(Xest.tVec)); 49 | for j = 1:length(Xest.tVec) 50 | Xest.xState(:,Xest.tVec(j):Xest.tVec(j)+Xest.iVec(j)-1,j) = multiTrajectories(j).states([1,3],:); 51 | end 52 | end 53 | 54 | end 55 | 56 | -------------------------------------------------------------------------------- /PMBM_Trajectory_Code/mbm_onlineSmoothing_allTrajectory/stateExtract.m: -------------------------------------------------------------------------------- 1 | function [est,multiTrajectories,Xest] = stateExtract(trajectoryEst) 2 | % Extract target states 3 | 4 | if isempty(trajectoryEst) 5 | est = zeros(4,0); 6 | multiTrajectories = struct('states',[],'beginTime',[],'endTime',[]); 7 | Xest.tVec = zeros(0,1); 8 | Xest.iVec = zeros(0,1); 9 | Xest.xState = zeros(2,0,0); 10 | return; 11 | end 12 | 13 | % MAP cardinality estimate 14 | r = [trajectoryEst.r]; 15 | % ensure that the case that r==1 counts 16 | r(r==1) = 1-eps; 17 | ss = false(size(r)); 18 | pcard = prod(1-r)*poly(-r./(1-r)); 19 | [~,n] = max(pcard); 20 | [~,o] = sort(-r); 21 | n = n - 1; 22 | ss(o(1:n)) = true; 23 | trajectoryEst = trajectoryEst(ss); 24 | len = length(trajectoryEst); 25 | est = zeros(4,len); 26 | 27 | % extract the set of all trajectories 28 | 29 | multiTrajectories = struct('states',[],'beginTime',[],'endTime',[]); 30 | multiTrajectories = repmat(multiTrajectories,len,1); 31 | 32 | idx = true(len,1); 33 | for i = 1:len 34 | % select trajectory death time with the highest weight 35 | [~,tdeath] = max(trajectoryEst(i).w); 36 | % store estiamtes for set of current trajectories 37 | est(:,i) = trajectoryEst(i).x(:,end); 38 | if tdeath ~= length(trajectoryEst(i).w) 39 | idx(i) = false; 40 | end 41 | multiTrajectories(i).states = trajectoryEst(i).x(:,1:trajectoryEst(i).epsilon(tdeath)-trajectoryEst(i).beta+1); 42 | multiTrajectories(i).beginTime = trajectoryEst(i).beta; 43 | multiTrajectories(i).endTime = trajectoryEst(i).epsilon(tdeath); 44 | end 45 | % extract current set estimate 46 | est = est(:,idx); 47 | 48 | % re-construct output estimates for the use of trajectory metric 49 | if isempty(multiTrajectories) 50 | Xest.tVec = zeros(0,1); 51 | Xest.iVec = zeros(0,1); 52 | Xest.xState = zeros(2,0,0); 53 | else 54 | Xest.tVec = [multiTrajectories.beginTime]; 55 | Xest.iVec = [multiTrajectories.endTime]-[multiTrajectories.beginTime]+1; 56 | Xest.xState = zeros(2,max([multiTrajectories.endTime]),length(Xest.tVec)); 57 | for j = 1:length(Xest.tVec) 58 | Xest.xState(:,Xest.tVec(j):Xest.tVec(j)+Xest.iVec(j)-1,j) = multiTrajectories(j).states([1,3],:); 59 | end 60 | end 61 | 62 | end 63 | 64 | -------------------------------------------------------------------------------- /PMBM_Trajectory_Code/pmbm_onlineSmoothing_allTrajectory/stateExtract.m: -------------------------------------------------------------------------------- 1 | function [est,multiTrajectories,Xest] = stateExtract(trajectoryEst) 2 | % Extract target states 3 | 4 | if isempty(trajectoryEst) 5 | est = zeros(4,0); 6 | multiTrajectories = struct('states',[],'beginTime',[],'endTime',[]); 7 | Xest.tVec = zeros(0,1); 8 | Xest.iVec = zeros(0,1); 9 | Xest.xState = zeros(2,0,0); 10 | return; 11 | end 12 | 13 | % MAP cardinality estimate 14 | r = extractfield(trajectoryEst,'r'); 15 | % ensure that the case that r==1 counts 16 | r(r==1) = 1-eps; 17 | ss = false(size(r)); 18 | pcard = prod(1-r)*poly(-r./(1-r)); 19 | [~,n] = max(pcard); 20 | [~,o] = sort(-r); 21 | n = n - 1; 22 | ss(o(1:n)) = true; 23 | 24 | trajectoryEst = trajectoryEst(ss); 25 | len = length(trajectoryEst); 26 | est = zeros(4,len); 27 | 28 | % extract the set of all trajectories 29 | 30 | multiTrajectories = struct('states',[],'beginTime',[],'endTime',[]); 31 | multiTrajectories = repmat(multiTrajectories,len,1); 32 | 33 | idx = true(len,1); 34 | for i = 1:len 35 | % select trajectory death time with the highest weight 36 | [~,tdeath] = max(trajectoryEst(i).w); 37 | % store estiamtes for set of current trajectories 38 | est(:,i) = trajectoryEst(i).x(:,end); 39 | if tdeath ~= length(trajectoryEst(i).w) 40 | idx(i) = false; 41 | end 42 | multiTrajectories(i).states = trajectoryEst(i).x(:,1:trajectoryEst(i).epsilon(tdeath)-trajectoryEst(i).beta+1); 43 | multiTrajectories(i).beginTime = trajectoryEst(i).beta; 44 | multiTrajectories(i).endTime = trajectoryEst(i).epsilon(tdeath); 45 | end 46 | % extract current set estimate 47 | est = est(:,idx); 48 | 49 | % re-construct output estimates for the use of trajectory metric 50 | if isempty(multiTrajectories) 51 | Xest.tVec = zeros(0,1); 52 | Xest.iVec = zeros(0,1); 53 | Xest.xState = zeros(2,0,0); 54 | else 55 | Xest.tVec = [multiTrajectories.beginTime]; 56 | Xest.iVec = [multiTrajectories.endTime]-[multiTrajectories.beginTime]+1; 57 | Xest.xState = zeros(2,max([multiTrajectories.endTime]),length(Xest.tVec)); 58 | for j = 1:length(Xest.tVec) 59 | Xest.xState(:,Xest.tVec(j):Xest.tVec(j)+Xest.iVec(j)-1,j) = multiTrajectories(j).states([1,3],:); 60 | end 61 | end 62 | 63 | end 64 | 65 | -------------------------------------------------------------------------------- /PMBM_Trajectory_Code/pmbm_onlineSmoothing_allTrajectory/predictStep.m: -------------------------------------------------------------------------------- 1 | function [trajectoryMBM,unknownPPP] = predictStep(trajectoryMBM,unknownPPP,model,t) 2 | %PREDICT: PREDICT MULTI-BERNOULLI AND POISSON COMPONENTS 3 | 4 | % Get multi-Bernoulli prediction parameters from model 5 | F = model.F; 6 | Q = model.Q; 7 | Ps = model.Ps; 8 | 9 | % Interpret length of inputs 10 | nb = length(model.lambdab); 11 | n = length(trajectoryMBM); 12 | 13 | % Implement prediction algorithm 14 | 15 | % Predict existing tracks (single target hypotheses) 16 | for i = 1:n 17 | % if the probability that the trajectory still exists at current time 18 | % step is very small, then there is no need to predict it 19 | if trajectoryMBM(i).r~=0 && trajectoryMBM(i).w(end) > model.threshold 20 | 21 | xpre = F*trajectoryMBM(i).x(:,end); 22 | Ppre = F*trajectoryMBM(i).P(:,:,end)*F'+Q; 23 | 24 | % trajectory case 25 | trajectoryMBM(i).x = [trajectoryMBM(i).x xpre]; 26 | trajectoryMBM(i).P = cat(3,trajectoryMBM(i).P,Ppre); 27 | 28 | % store prediction 29 | trajectoryMBM(i).xpre = [trajectoryMBM(i).xpre xpre]; 30 | trajectoryMBM(i).Ppre = cat(3,trajectoryMBM(i).Ppre,Ppre); 31 | 32 | % beta remains the same, only update epsilon 33 | trajectoryMBM(i).epsilon = [trajectoryMBM(i).epsilon trajectoryMBM(i).epsilon(end)+1]; 34 | 35 | % update weight vector of death time 36 | trajectoryMBM(i).w = [trajectoryMBM(i).w(1:end-1) trajectoryMBM(i).w(end)*(1-Ps) trajectoryMBM(i).w(end)*Ps]; 37 | end 38 | end 39 | 40 | nu = length(unknownPPP); 41 | % Predict existing PPP intensity 42 | for i = 1:nu 43 | unknownPPP(i).lambda = Ps*unknownPPP(i).lambda; 44 | unknownPPP(i).x = [unknownPPP(i).x F*unknownPPP(i).x(:,end)]; 45 | unknownPPP(i).P = cat(3,unknownPPP(i).P,F*unknownPPP(i).P(:,:,end)*F'+Q); 46 | % beta remains the same, only update epsilon 47 | unknownPPP(i).epsilon = t; 48 | end 49 | 50 | % Incorporate birth intensity into PPP 51 | for i = 1:nb 52 | unknownPPP(nu+i,1).lambda = model.lambdab(i); 53 | unknownPPP(nu+i,1).x = model.xb(:,i); 54 | unknownPPP(nu+i,1).P = model.Pb(:,:,i); 55 | unknownPPP(nu+i,1).beta = t; 56 | unknownPPP(nu+i,1).epsilon = t; 57 | end 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Multi-scan trajectory PMBM filter 2 | MATLAB implementations of the Multi-Scan Trajectory Poisson Multi-Bernoulli Mixture Filter, Multi-Bernoulli Mixture Filter and Multi-Bernoulli Mixture-01 Filter 3 | 4 | This repository contains the Matlab implementations of the Multi-Scan Trajectory Poisson Multi-Bernoulli Mixture Filter via dual decomposition proposed in 5 | 6 | Y. Xia, K. Granström, L. Svensson and Á. F. García-Fernández, "An Implementation of the Poisson Multi-Bernoulli Mixture Trajectory Filter via Dual Decomposition," 2018 21st International Conference on Information Fusion (FUSION), Cambridge, 2018 7 | 8 | Full text is available at https://arxiv.org/pdf/1811.12281.pdf 9 | 10 | and in 11 | 12 | Y. Xia, K. Granström, L. Svensson, Á. F. García-Fernández and Jason L. Williams, "Multi-Scan Implementation of the Trajectory Poisson Multi-Bernoulli Mixture Filter," Accepted for publication in Journal of Advances in Information Fusion (special issue on Multiple Hypothesis Tracker), 2020 13 | 14 | Full text is available at https://arxiv.org/abs/1912.01748 15 | 16 | More details on the point target PMBM tracker can be found in the paper 17 | 18 | Granström, Karl, Lennart Svensson, Yuxuan Xia, Jason Williams, and Ángel F. García-Femández. "Poisson multi-Bernoulli mixture trackers: continuity through random finite sets of trajectories." In 2018 21st International Conference on Information Fusion (FUSION). IEEE, 2018. 19 | 20 | Full text is available at https://arxiv.org/pdf/1812.05131.pdf 21 | 22 | More details on Dual Decomposition can be found in the paper 23 | 24 | N. Komodakis, N. Paragios, and G. Tziritas, "MRF energy minimization and beyond via dual decomposition," IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 33, no. 3, pp. 531–552, 2011. 25 | 26 | The filters are evaluated using 27 | 28 | 1. the generalised optimal subpattern-assignment (GOSPA) 29 | 30 | Abu Sajana Rahmathullah, Ángel F. García-Fernández, Lennart Svensson, Generalized optimal sub-pattern assignment metric, in 20th International 31 | Conference on Information Fusion, 2017. 32 | 33 | Video on GOSPA: https://www.youtube.com/watch?v=M79GTTytvCM 34 | 35 | 2. the trajectory metric 36 | 37 | Abu Sajana Rahmathullah, Ángel F. García-Fernández, Lennart Svensson, A metric on the space of finite sets of trajectories for evaluation of multi-target tracking algorithms 38 | 39 | -------------------------------------------------------------------------------- /PMBM_Trajectory_Code/gospaMetric/GOSPA code/exampleScript_angel.m: -------------------------------------------------------------------------------- 1 | % AUTHOR: Abu Sajana Rahmathullah 2 | % DATE OF CREATION: 7 August, 2017 3 | 4 | % set parameters 5 | p = 2; c = 8; alpha = 1; ndim = 2; 6 | 7 | % Generate random sets 8 | % columns of x_mat and y_mat correspond to different points in the sets 9 | nx = randi([0, 10]); 10 | ny = randi([0, 10]); 11 | x_mat = 20 * (rand(ndim, nx) - 0.5); 12 | y_mat = 20 * (rand(ndim, ny) - 0.5); 13 | 14 | % compute GOSPA 15 | [d_gospa, x_to_y_assignment] = GOSPA(x_mat, y_mat, p, c, alpha); 16 | 17 | % plot the input and the output if the vectors are two dimensional 18 | if ndim == 2 19 | % plot the input vectors 20 | x0 = 0; y0 = 0; width = 10; height = 4; 21 | f1 = figure('Units','inches','Position',[x0 y0 width height], ... 22 | 'PaperPositionMode','auto'); 23 | hold on; 24 | bluecol = [0.3 0.3 1]; 25 | redcol = [1 0.2 0.2]; 26 | text(x_mat(1,:), x_mat(2,:), num2str((1:nx)'), ... 27 | 'HorizontalAlignment', 'right'); 28 | text(y_mat(1,:), y_mat(2,:), num2str((1:ny)'), ... 29 | 'HorizontalAlignment', 'right', ... 30 | 'VerticalAlignment', 'bottom'); 31 | hx = plot(x_mat(1,:), x_mat(2,:), 'x', 'color', bluecol, 'MarkerSize', 10); 32 | hy = plot(y_mat(1,:), y_mat(2,:), 'o', 'color', redcol, 'MarkerSize', 10); 33 | 34 | % plot the output 35 | x_ind = 1:nx; 36 | x_ind = x_ind(x_to_y_assignment~=0); 37 | y_ind = x_to_y_assignment(x_to_y_assignment~=0); 38 | for ind = 1:sum(x_to_y_assignment~=0) 39 | db = min(sqrt(sum((x_mat(:,x_ind(ind)) - y_mat(:,y_ind(ind))).^2)),c); 40 | if db < c 41 | plot([x_mat(1, x_ind(ind)), y_mat(1, y_ind(ind))], ... 42 | [x_mat(2, x_ind(ind)), y_mat(2, y_ind(ind))], 'k-'); 43 | 44 | text((x_mat(1, x_ind(ind))+y_mat(1, y_ind(ind)))/2, ... 45 | (x_mat(2, x_ind(ind))+y_mat(2, y_ind(ind)))/2, ... 46 | num2str(db, 3), 'HorizontalAlignment', 'right', ... 47 | 'VerticalAlignment', 'bottom'); 48 | end 49 | end 50 | hlegend = legend([hx, hy], {'X', 'Y'}); 51 | set(hlegend, 'Location', 'Best'); 52 | set(gca, 'FontSize', 18, 'FontName', 'Times'); 53 | title(['c=' num2str(c), ', p=' num2str(p), ', \alpha=' ... 54 | num2str(alpha) ', GOSPA=' num2str(d_gospa, 3), ]); 55 | end -------------------------------------------------------------------------------- /PMBM_Trajectory_Code/gospaMetric/GOSPA code/exampleScript.m: -------------------------------------------------------------------------------- 1 | % AUTHOR: Abu Sajana Rahmathullah 2 | % DATE OF CREATION: 7 August, 2017 3 | 4 | % set parameters 5 | p = 2; c = 8; alpha = 2; ndim = 2; 6 | 7 | % Generate random sets 8 | % columns of x_mat and y_mat correspond to different points in the sets 9 | nx = randi([0, 10]); 10 | ny = randi([0, 10]); 11 | x_mat = 20 * (rand(ndim, nx) - 0.5); 12 | y_mat = 20 * (rand(ndim, ny) - 0.5); 13 | 14 | % compute GOSPA 15 | [d_gospa, x_to_y_assignment, decomp_cost] = GOSPA(x_mat, y_mat, p, c, alpha); 16 | 17 | % plot the input and the output if the vectors are two dimensional 18 | if ndim == 2 19 | % plot the input vectors 20 | x0 = 0; y0 = 0; width = 10; height = 4; 21 | f1 = figure('Units','inches','Position',[x0 y0 width height], ... 22 | 'PaperPositionMode','auto'); 23 | hold on; 24 | bluecol = [0.3 0.3 1]; 25 | redcol = [1 0.2 0.2]; 26 | text(x_mat(1,:), x_mat(2,:), num2str((1:nx)'), ... 27 | 'HorizontalAlignment', 'right'); 28 | text(y_mat(1,:), y_mat(2,:), num2str((1:ny)'), ... 29 | 'HorizontalAlignment', 'right', ... 30 | 'VerticalAlignment', 'bottom'); 31 | hx = plot(x_mat(1,:), x_mat(2,:), 'x', 'color', bluecol, 'MarkerSize', 10); 32 | hy = plot(y_mat(1,:), y_mat(2,:), 'o', 'color', redcol, 'MarkerSize', 10); 33 | 34 | % plot the output 35 | x_ind = 1:nx; 36 | x_ind = x_ind(x_to_y_assignment~=0); 37 | y_ind = x_to_y_assignment(x_to_y_assignment~=0); 38 | for ind = 1:sum(x_to_y_assignment~=0) 39 | db = min(sqrt(sum((x_mat(:,x_ind(ind)) - y_mat(:,y_ind(ind))).^2)),c); 40 | if db < c 41 | plot([x_mat(1, x_ind(ind)), y_mat(1, y_ind(ind))], ... 42 | [x_mat(2, x_ind(ind)), y_mat(2, y_ind(ind))], 'k-'); 43 | 44 | text((x_mat(1, x_ind(ind))+y_mat(1, y_ind(ind)))/2, ... 45 | (x_mat(2, x_ind(ind))+y_mat(2, y_ind(ind)))/2, ... 46 | num2str(db, 3), 'HorizontalAlignment', 'right', ... 47 | 'VerticalAlignment', 'bottom'); 48 | end 49 | end 50 | hlegend = legend([hx, hy], {'X', 'Y'}); 51 | set(hlegend, 'Location', 'Best'); 52 | set(gca, 'FontSize', 18, 'FontName', 'Times'); 53 | title(['c=' num2str(c), ', p=' num2str(p), ', \alpha=' ... 54 | num2str(alpha) ', GOSPA=' num2str(d_gospa, 3), ]); 55 | end -------------------------------------------------------------------------------- /PMBM_Trajectory_Code/mbm01_onlineSmoothing_allTrajectory/backforwardFiltering.m: -------------------------------------------------------------------------------- 1 | function trajectorySmoothEst = backforwardFiltering(trajectoryEst,trajectorySmoothEstPre,model) 2 | % Backforward filtering for trajectories in the most likely global 3 | % hypothesis 4 | 5 | % Input also include smoothing result of last time scan, try to reuse the 6 | % calculated smoothing gain for the most likely global hypothesis 7 | 8 | % Select trajectory with existence probability larger than 0 9 | rest = [trajectoryEst.r]; 10 | trajectoryEst = trajectoryEst(rest>0); 11 | 12 | % Preparation 13 | trajectorySmoothEst = trajectoryEst; 14 | numTrajectory = length(trajectoryEst); 15 | 16 | if ~isempty(trajectorySmoothEstPre) 17 | apre = [trajectorySmoothEstPre.a]; 18 | a = [trajectorySmoothEst.a]; 19 | [lia,locb] = ismember(a,apre); 20 | for i = 1:numTrajectory 21 | trajectoryLength = size(trajectoryEst(i).x,2); 22 | smoothingLength = min(trajectoryLength,model.slideWindow+model.L); 23 | trajectorySmoothEst(i).G = zeros(4,4,smoothingLength-1); 24 | if lia(i)==true && ~isempty(trajectorySmoothEstPre(locb(i)).G) 25 | l = trajectorySmoothEst(i).l(:,2); 26 | lpre = [trajectorySmoothEstPre(locb(i)).l(:,2);-1]; 27 | idx = find((l-lpre)~=0,1); 28 | if idx > 1 29 | idxDiverge = min(smoothingLength,trajectoryLength-(idx-1))-1; 30 | trajectorySmoothEst(i).G(:,:,1:idxDiverge) = trajectorySmoothEstPre(locb(i)).G(:,:,1:idxDiverge); 31 | end 32 | end 33 | end 34 | end 35 | 36 | % Backward filtering 37 | for i = 1:numTrajectory 38 | trajectoryLength = size(trajectoryEst(i).x,2); 39 | smoothingLength = min(trajectoryLength,model.slideWindow+model.L); 40 | 41 | if isempty(trajectorySmoothEstPre) 42 | trajectorySmoothEst(i).G = zeros(4,4,smoothingLength-1); 43 | end 44 | for j = 2:smoothingLength 45 | if ~any(trajectorySmoothEst(i).G(:,:,smoothingLength-j+1)) 46 | trajectorySmoothEst(i).G(:,:,smoothingLength-j+1) = ... 47 | trajectoryEst(i).P(:,:,end-j+1)*model.F'/trajectoryEst(i).Ppre(:,:,end-j+2); 48 | end 49 | G = trajectorySmoothEst(i).G(:,:,smoothingLength-j+1); 50 | trajectorySmoothEst(i).x(:,end-j+1) = trajectoryEst(i).x(:,end-j+1) + ... 51 | G*(trajectorySmoothEst(i).x(:,end-j+2) - trajectoryEst(i).xpre(:,end-j+2)); 52 | trajectorySmoothEst(i).P(:,:,end-j+1) = trajectoryEst(i).P(:,:,end-j+1) - ... 53 | G*(trajectoryEst(i).Ppre(:,:,end-j+2) - trajectorySmoothEst(i).P(:,:,end-j+2))*G'; 54 | end 55 | end 56 | 57 | trajectorySmoothEst = rmfield(trajectorySmoothEst,{'xpre','Ppre'}); 58 | 59 | end -------------------------------------------------------------------------------- /PMBM_Trajectory_Code/mbm_onlineSmoothing_allTrajectory/backforwardFiltering.m: -------------------------------------------------------------------------------- 1 | function trajectorySmoothEst = backforwardFiltering(trajectoryEst,trajectorySmoothEstPre,model) 2 | % Backforward filtering for trajectories in the most likely global 3 | % hypothesis 4 | 5 | % Input also include smoothing result of last time scan, try to reuse the 6 | % calculated smoothing gain for the most likely global hypothesis 7 | 8 | % Select trajectory with existence probability larger than 0 9 | rest = [trajectoryEst.r]; 10 | trajectoryEst = trajectoryEst(rest>0); 11 | 12 | % Preparation 13 | trajectorySmoothEst = trajectoryEst; 14 | numTrajectory = length(trajectoryEst); 15 | 16 | if ~isempty(trajectorySmoothEstPre) 17 | apre = [trajectorySmoothEstPre.a]; 18 | a = [trajectorySmoothEst.a]; 19 | [lia,locb] = ismember(a,apre); 20 | for i = 1:numTrajectory 21 | trajectoryLength = size(trajectoryEst(i).x,2); 22 | smoothingLength = min(trajectoryLength,model.slideWindow+model.L); 23 | trajectorySmoothEst(i).G = zeros(4,4,smoothingLength-1); 24 | if lia(i)==true && ~isempty(trajectorySmoothEstPre(locb(i)).G) 25 | l = trajectorySmoothEst(i).l(:,2); 26 | lpre = [trajectorySmoothEstPre(locb(i)).l(:,2);-1]; 27 | idx = find((l-lpre)~=0,1); 28 | if idx > 1 29 | idxDiverge = min(smoothingLength,trajectoryLength-(idx-1))-1; 30 | trajectorySmoothEst(i).G(:,:,1:idxDiverge) = trajectorySmoothEstPre(locb(i)).G(:,:,1:idxDiverge); 31 | end 32 | end 33 | end 34 | end 35 | 36 | % Backward filtering 37 | for i = 1:numTrajectory 38 | trajectoryLength = size(trajectoryEst(i).x,2); 39 | smoothingLength = min(trajectoryLength,model.slideWindow+model.L); 40 | 41 | if isempty(trajectorySmoothEstPre) 42 | trajectorySmoothEst(i).G = zeros(4,4,smoothingLength-1); 43 | end 44 | for j = 2:smoothingLength 45 | if ~any(trajectorySmoothEst(i).G(:,:,smoothingLength-j+1)) 46 | trajectorySmoothEst(i).G(:,:,smoothingLength-j+1) = ... 47 | trajectoryEst(i).P(:,:,end-j+1)*model.F'/trajectoryEst(i).Ppre(:,:,end-j+2); 48 | end 49 | G = trajectorySmoothEst(i).G(:,:,smoothingLength-j+1); 50 | trajectorySmoothEst(i).x(:,end-j+1) = trajectoryEst(i).x(:,end-j+1) + ... 51 | G*(trajectorySmoothEst(i).x(:,end-j+2) - trajectoryEst(i).xpre(:,end-j+2)); 52 | trajectorySmoothEst(i).P(:,:,end-j+1) = trajectoryEst(i).P(:,:,end-j+1) - ... 53 | G*(trajectoryEst(i).Ppre(:,:,end-j+2) - trajectorySmoothEst(i).P(:,:,end-j+2))*G'; 54 | end 55 | end 56 | 57 | trajectorySmoothEst = rmfield(trajectorySmoothEst,{'xpre','Ppre'}); 58 | 59 | end -------------------------------------------------------------------------------- /PMBM_Trajectory_Code/pmbm_onlineSmoothing_allTrajectory/backforwardFiltering.m: -------------------------------------------------------------------------------- 1 | function trajectorySmoothEst = backforwardFiltering(trajectoryEst,trajectorySmoothEstPre,model) 2 | % Backforward filtering for trajectories in the most likely global 3 | % hypothesis 4 | 5 | % Input also include smoothing result of last time scan, try to reuse the 6 | % calculated smoothing gain for the most likely global hypothesis 7 | 8 | % Select trajectory with existence probability larger than 0 9 | rest = [trajectoryEst.r]; 10 | trajectoryEst = trajectoryEst(rest>0); 11 | 12 | % Preparation 13 | trajectorySmoothEst = trajectoryEst; 14 | numTrajectory = length(trajectoryEst); 15 | 16 | if ~isempty(trajectorySmoothEstPre) 17 | apre = [trajectorySmoothEstPre.a]; 18 | a = [trajectorySmoothEst.a]; 19 | [lia,locb] = ismember(a,apre); 20 | for i = 1:numTrajectory 21 | trajectoryLength = size(trajectoryEst(i).x,2); 22 | smoothingLength = min(trajectoryLength,model.slideWindow+model.L); 23 | trajectorySmoothEst(i).G = zeros(4,4,smoothingLength-1); 24 | if lia(i)==true && ~isempty(trajectorySmoothEstPre(locb(i)).G) 25 | l = trajectorySmoothEst(i).l(:,2); 26 | lpre = [trajectorySmoothEstPre(locb(i)).l(:,2);-1]; 27 | idx = find((l-lpre)~=0,1); 28 | if idx > 1 29 | idxDiverge = min(smoothingLength,trajectoryLength-(idx-1))-1; 30 | trajectorySmoothEst(i).G(:,:,1:idxDiverge) = trajectorySmoothEstPre(locb(i)).G(:,:,1:idxDiverge); 31 | end 32 | end 33 | end 34 | end 35 | 36 | % Backward filtering 37 | for i = 1:numTrajectory 38 | trajectoryLength = size(trajectoryEst(i).x,2); 39 | smoothingLength = min(trajectoryLength,model.slideWindow+model.L); 40 | 41 | if isempty(trajectorySmoothEstPre) 42 | trajectorySmoothEst(i).G = zeros(4,4,smoothingLength-1); 43 | end 44 | for j = 2:smoothingLength 45 | if ~any(trajectorySmoothEst(i).G(:,:,smoothingLength-j+1)) 46 | trajectorySmoothEst(i).G(:,:,smoothingLength-j+1) = ... 47 | trajectoryEst(i).P(:,:,end-j+1)*model.F'/trajectoryEst(i).Ppre(:,:,end-j+2); 48 | end 49 | G = trajectorySmoothEst(i).G(:,:,smoothingLength-j+1); 50 | trajectorySmoothEst(i).x(:,end-j+1) = trajectoryEst(i).x(:,end-j+1) + ... 51 | G*(trajectorySmoothEst(i).x(:,end-j+2) - trajectoryEst(i).xpre(:,end-j+2)); 52 | trajectorySmoothEst(i).P(:,:,end-j+1) = trajectoryEst(i).P(:,:,end-j+1) - ... 53 | G*(trajectoryEst(i).Ppre(:,:,end-j+2) - trajectorySmoothEst(i).P(:,:,end-j+2))*G'; 54 | end 55 | end 56 | 57 | trajectorySmoothEst = rmfield(trajectorySmoothEst,{'xpre','Ppre'}); 58 | 59 | end -------------------------------------------------------------------------------- /PMBM_Trajectory_Code/pmbm_onlineSmoothing_allTrajectory/gentruth.m: -------------------------------------------------------------------------------- 1 | function [model,measlog,xlog,X] = gentruth(slideWindow,L,Pd,lfai,numtruth,Pmid,pmi) 2 | %GENTRUTH: GENERATE SIMULATION TRUTH 3 | 4 | if (pmi == 1) % in this case, targets are present to begin with 5 | birthtime = zeros(1,numtruth); 6 | deathtime = 81*ones(1,numtruth); 7 | else % in other cases, a new target appears every 10 time steps 8 | birthtime = 10*(0:numtruth-1); 9 | deathtime = 51+10*(0:numtruth-1); 10 | end 11 | 12 | % Initialise model 13 | T = 1; % sampling time 14 | % nearly constant velocity model 15 | model.F = kron(eye(2),[1 T; 0 1]); 16 | model.Q = 0.04*kron(eye(2),[T^3/3 T^2/2; T^2/2 T]); 17 | model.Qc = chol(model.Q); 18 | % linear measurement model 19 | model.H = kron(eye(2),[1 0]); 20 | model.R = 0.04*eye(2); 21 | model.Rc = chol(model.R); 22 | model.Pd = Pd; % detection probability 23 | model.Ps = 0.99; % survival probability 24 | model.existThresh = 0.4; % existence probability used to extract estimates 25 | model.threshold = 1e-4; % pruning threshold 26 | model.slideWindow = slideWindow; % N-scan pruning 27 | model.L = L; % L-scan trajectory density approximation 28 | 29 | % Gating parameters 30 | P_G= 0.999; %gate size in percentage 31 | model.gamma= chi2inv(P_G,2); %inv chi^2 dn gamma value 32 | 33 | % Initialise new target parameter structure 34 | model.lambdab = 0.05; % expect one new target to arrive every 20 scans on average 35 | model.xb = zeros(4,1); 36 | model.Pb = diag([100 1 100 1].^2); 37 | 38 | volume = 200*200; 39 | model.lfai = lfai; % expected number of false alarms (integral of lambda_fa) 40 | model.lambda_fa = lfai/volume; % intensity = expected number / state space volume 41 | 42 | simlen = 81; % must be odd 43 | midpoint = (simlen+1)/2; 44 | numfb = midpoint-1; 45 | xlog = cell(simlen,1); 46 | 47 | X.tVec = birthtime + 1; 48 | X.iVec = deathtime - birthtime; 49 | X.xState = zeros(2,simlen,numtruth); 50 | 51 | % Initialise at time midpoint and propagate forward and backwards 52 | 53 | % Run forward and backward simulation process 54 | for i = 1:numtruth 55 | x = chol(Pmid)'*randn(size(model.F,1),1); 56 | xf = x; xb = x; 57 | X.xState(:,midpoint,i) = x([1,3]); 58 | xlog{midpoint} = [xlog{midpoint} x]; 59 | for t = 1:numfb 60 | xf = model.F*xf + model.Qc'*randn(size(model.F,1),1); 61 | xb = model.F\(xb + model.Qc'*randn(size(model.F,1),1)); 62 | xlog{midpoint-t} = [xlog{midpoint-t} xb(:,midpoint-t>birthtime(i))]; 63 | xlog{midpoint+t} = [xlog{midpoint+t} xf(:,midpoint+t<=deathtime(i))]; 64 | if midpoint-t>birthtime(i) 65 | X.xState(:,midpoint-t,i) = xb([1,3],midpoint-t>birthtime(i)); 66 | end 67 | if midpoint+t<=deathtime(i) 68 | X.xState(:,midpoint+t,i) = xf([1,3],midpoint+t<=deathtime(i)); 69 | end 70 | end 71 | end 72 | 73 | measlog = cell(simlen,1); 74 | for j = 1:simlen 75 | measlog{j} = makemeas(xlog{j},model); 76 | end 77 | 78 | function z = makemeas(x,model) 79 | % Generate target measurements (for every target) 80 | z = model.H*x + model.Rc'*randn(size(model.H,1),size(x,2)); 81 | % Simulate missed detection process 82 | z = z(:,rand(size(z,2),1) < model.Pd); 83 | % Generate false alarms (spatially uniform on [-100,100]^2 84 | z = [z, 200*rand(size(model.H,1),poissrnd(model.lfai))-100]; 85 | % Shuffle order 86 | z = z(:,randperm(size(z,2))); 87 | end 88 | 89 | end 90 | 91 | 92 | -------------------------------------------------------------------------------- /PMBM_Trajectory_Code/mbm01_onlineSmoothing_allTrajectory/gentruth.m: -------------------------------------------------------------------------------- 1 | function [model,measlog,xlog,X] = gentruth(slideWindow,L,Pd,lfai,numtruth,Pmid,pmi) 2 | %GENTRUTH: GENERATE SIMULATION TRUTH 3 | 4 | if (pmi == 1) % in this case, targets are present to begin with 5 | birthtime = zeros(1,numtruth); 6 | deathtime = 81*ones(1,numtruth); 7 | else % in other cases, a new target appears every 10 time steps 8 | birthtime = 10*(0:numtruth-1); 9 | deathtime = 51+10*(0:numtruth-1); 10 | end 11 | 12 | % Initialise model 13 | T = 1; % sampling time 14 | % nearly constant velocity model 15 | model.F = kron(eye(2),[1 T; 0 1]); 16 | model.Q = 0.04*kron(eye(2),[T^3/3 T^2/2; T^2/2 T]); 17 | model.Qc = chol(model.Q); 18 | % linear measurement model 19 | model.H = kron(eye(2),[1 0]); 20 | model.R = 0.04*eye(2); 21 | model.Rc = chol(model.R); 22 | model.Pd = Pd; % detection probability 23 | model.Ps = 0.99; % survival probability 24 | model.threshold = 1e-4; % pruning threshold 25 | model.slideWindow = slideWindow; % N-scan pruning 26 | model.L = L; % L-scan trajectory density approximation 27 | 28 | % Gating parameters 29 | P_G= 0.999; %gate size in percentage 30 | model.gamma= chi2inv(P_G,2); %inv chi^2 dn gamma value 31 | 32 | % Initialise new target parameter structure 33 | % We consider an MB birth 34 | nb = 1; 35 | model.rb = zeros(nb,1); 36 | model.xb = zeros(4,nb); 37 | model.Pb = zeros(4,4,nb); 38 | for i = 1:nb 39 | model.rb(i) = 0.05; % expect one new target to arrive every 20 scans on average 40 | model.xb(:,i) = zeros(4,1); 41 | model.Pb(:,:,i) = diag([100 1 100 1].^2); 42 | end 43 | 44 | volume = 200*200; 45 | model.lfai = lfai; % expected number of false alarms (integral of lambda_fa) 46 | model.lambda_fa = lfai/volume; % intensity = expected number / state space volume 47 | 48 | simlen = 81; % must be odd 49 | midpoint = (simlen+1)/2; 50 | numfb = midpoint-1; 51 | xlog = cell(simlen,1); 52 | 53 | X.tVec = birthtime + 1; 54 | X.iVec = deathtime - birthtime; 55 | X.xState = zeros(2,simlen,numtruth); 56 | 57 | % Initialise at time midpoint and propagate forward and backwards 58 | 59 | % Run forward and backward simulation process 60 | for i = 1:numtruth 61 | x = chol(Pmid)'*randn(size(model.F,1),1); 62 | xf = x; xb = x; 63 | X.xState(:,midpoint,i) = x([1,3]); 64 | xlog{midpoint} = [xlog{midpoint} x]; 65 | for t = 1:numfb 66 | xf = model.F*xf + model.Qc'*randn(size(model.F,1),1); 67 | xb = model.F\(xb + model.Qc'*randn(size(model.F,1),1)); 68 | xlog{midpoint-t} = [xlog{midpoint-t} xb(:,midpoint-t>birthtime(i))]; 69 | xlog{midpoint+t} = [xlog{midpoint+t} xf(:,midpoint+t<=deathtime(i))]; 70 | if midpoint-t>birthtime(i) 71 | X.xState(:,midpoint-t,i) = xb([1,3],midpoint-t>birthtime(i)); 72 | end 73 | if midpoint+t<=deathtime(i) 74 | X.xState(:,midpoint+t,i) = xf([1,3],midpoint+t<=deathtime(i)); 75 | end 76 | end 77 | end 78 | 79 | measlog = cell(simlen,1); 80 | for j = 1:simlen 81 | measlog{j} = makemeas(xlog{j},model); 82 | end 83 | 84 | function z = makemeas(x,model) 85 | % Generate target measurements (for every target) 86 | z = model.H*x + model.Rc'*randn(size(model.H,1),size(x,2)); 87 | % Simulate missed detection process 88 | z = z(:,rand(size(z,2),1) < model.Pd); 89 | % Generate false alarms (spatially uniform on [-100,100]^2 90 | z = [z, 200*rand(size(model.H,1),poissrnd(model.lfai))-100]; 91 | % Shuffle order 92 | z = z(:,randperm(size(z,2))); 93 | end 94 | 95 | end 96 | 97 | -------------------------------------------------------------------------------- /PMBM_Trajectory_Code/mbm_onlineSmoothing_allTrajectory/gentruth.m: -------------------------------------------------------------------------------- 1 | function [model,measlog,xlog,X] = gentruth(slideWindow,L,Pd,lfai,numtruth,Pmid,pmi) 2 | %GENTRUTH: GENERATE SIMULATION TRUTH 3 | 4 | if (pmi == 1) % in this case, targets are present to begin with 5 | birthtime = zeros(1,numtruth); 6 | deathtime = 81*ones(1,numtruth); 7 | else % in other cases, a new target appears every 10 time steps 8 | birthtime = 10*(0:numtruth-1); 9 | deathtime = 51+10*(0:numtruth-1); 10 | end 11 | 12 | % Initialise model 13 | T = 1; % sampling time 14 | % nearly constant velocity model 15 | model.F = kron(eye(2),[1 T; 0 1]); 16 | model.Q = 0.04*kron(eye(2),[T^3/3 T^2/2; T^2/2 T]); 17 | model.Qc = chol(model.Q); 18 | % linear measurement model 19 | model.H = kron(eye(2),[1 0]); 20 | model.R = 0.04*eye(2); 21 | model.Rc = chol(model.R); 22 | model.Pd = Pd; % detection probability 23 | model.Ps = 0.99; % survival probability 24 | model.existThresh = 0.4; % existence probability used to extract estimates 25 | model.threshold = 1e-4; % pruning threshold 26 | model.slideWindow = slideWindow; % N-scan pruning 27 | model.L = L; % L-scan trajectory density approximation 28 | 29 | % Gating parameters 30 | P_G= 0.999; %gate size in percentage 31 | model.gamma= chi2inv(P_G,2); %inv chi^2 dn gamma value 32 | 33 | % Initialise new target parameter structure 34 | % We consider an MB birth 35 | nb = 1; 36 | model.rb = zeros(nb,1); 37 | model.xb = zeros(4,nb); 38 | model.Pb = zeros(4,4,nb); 39 | for i = 1:nb 40 | model.rb(i) = 0.05; % expect one new target to arrive every 20 scans on average 41 | model.xb(:,i) = zeros(4,1); 42 | model.Pb(:,:,i) = diag([100 1 100 1].^2); 43 | end 44 | 45 | volume = 200*200; 46 | model.lfai = lfai; % expected number of false alarms (integral of lambda_fa) 47 | model.lambda_fa = lfai/volume; % intensity = expected number / state space volume 48 | 49 | simlen = 81; % must be odd 50 | midpoint = (simlen+1)/2; 51 | numfb = midpoint-1; 52 | xlog = cell(simlen,1); 53 | 54 | X.tVec = birthtime + 1; 55 | X.iVec = deathtime - birthtime; 56 | X.xState = zeros(2,simlen,numtruth); 57 | 58 | % Initialise at time midpoint and propagate forward and backwards 59 | 60 | % Run forward and backward simulation process 61 | for i = 1:numtruth 62 | x = chol(Pmid)'*randn(size(model.F,1),1); 63 | xf = x; xb = x; 64 | X.xState(:,midpoint,i) = x([1,3]); 65 | xlog{midpoint} = [xlog{midpoint} x]; 66 | for t = 1:numfb 67 | xf = model.F*xf + model.Qc'*randn(size(model.F,1),1); 68 | xb = model.F\(xb + model.Qc'*randn(size(model.F,1),1)); 69 | xlog{midpoint-t} = [xlog{midpoint-t} xb(:,midpoint-t>birthtime(i))]; 70 | xlog{midpoint+t} = [xlog{midpoint+t} xf(:,midpoint+t<=deathtime(i))]; 71 | if midpoint-t>birthtime(i) 72 | X.xState(:,midpoint-t,i) = xb([1,3],midpoint-t>birthtime(i)); 73 | end 74 | if midpoint+t<=deathtime(i) 75 | X.xState(:,midpoint+t,i) = xf([1,3],midpoint+t<=deathtime(i)); 76 | end 77 | end 78 | end 79 | 80 | measlog = cell(simlen,1); 81 | for j = 1:simlen 82 | measlog{j} = makemeas(xlog{j},model); 83 | end 84 | 85 | function z = makemeas(x,model) 86 | % Generate target measurements (for every target) 87 | z = model.H*x + model.Rc'*randn(size(model.H,1),size(x,2)); 88 | % Simulate missed detection process 89 | z = z(:,rand(size(z,2),1) < model.Pd); 90 | % Generate false alarms (spatially uniform on [-100,100]^2 91 | z = [z, 200*rand(size(model.H,1),poissrnd(model.lfai))-100]; 92 | % Shuffle order 93 | z = z(:,randperm(size(z,2))); 94 | end 95 | 96 | end 97 | 98 | -------------------------------------------------------------------------------- /PMBM_Trajectory_Code/mbm_onlineSmoothing_allTrajectory/main.m: -------------------------------------------------------------------------------- 1 | clear;clc 2 | dbstop if error 3 | 4 | % Set simulation parameters 5 | numtruth = 4; % number of targets 6 | % pmi = 1; % simulation case 1 or 2 7 | pmi = 2; 8 | % covariance used for mid-point initialisation 9 | Pmid = 1e-6*eye(4); 10 | % detection probability 11 | Pd = 0.9; 12 | % clutter rate 13 | lfai = 10; 14 | 15 | % Algorithm setup 16 | slideWindow = 3; 17 | L = 1; 18 | 19 | % Generate Ground Truth 20 | [model,measlog,xlog,X] = gentruth(slideWindow,L,Pd,lfai,numtruth,Pmid,pmi); 21 | 22 | % time steps in total 23 | K = length(xlog); 24 | 25 | % GOSPA metric 26 | gospa_vals = zeros(K,4); 27 | gospa_c = 10; 28 | 29 | % Trajectory metric 30 | traMetric = zeros(K,5); 31 | 32 | % Simulation time 33 | runningTimeperMCandTimeStep = zeros(K,1); 34 | 35 | % Extract ground truth and re-construct for use of trajectory metric 36 | Xtrue = cell(K,1); 37 | for i = 1:K 38 | idx = i >= X.tVec; 39 | Xtrue{i}.tVec = X.tVec(idx); 40 | Xtrue{i}.iVec = min(X.iVec(idx),i-X.tVec(idx)+1); 41 | Xtrue{i}.xState = X.xState(:,1:max(Xtrue{i}.tVec+Xtrue{i}.iVec)-1,idx); 42 | end 43 | 44 | % Single trajectory hypothesis structure 45 | % r = existence probability; 46 | % xpre = predicted trajectory (target states across time); 47 | % x = updated trajectory (target states across time); 48 | % Ppre = predicted covariance; 49 | % P = updated covariance 50 | % l : trajectory (measurement indices across time) 51 | % c : the cost of each single target hypothesis 52 | % a : track index 53 | % beta: begin time of trajectory, absorbtion is used 54 | % epsilon: end time of trajectory, a vector 55 | % w = trajectory weight, a vector, w(i) specifies the weight of 56 | % trajectory with end time epsilon(i) 57 | 58 | % Initialisation 59 | trajectoryMBM = []; 60 | trajectorySmoothEst = []; 61 | xest = cell(K,1); 62 | trajectory = cell(K,1); 63 | 64 | % Loop through time 65 | fprintf('Time step: '); 66 | for t = 1:K 67 | 68 | fprintf('%g ', t); 69 | 70 | tstart = tic; 71 | 72 | % Predict all single target hypotheses of previous scan 73 | [trajectoryMBM,MB_birth] = predictStep(trajectoryMBM,model,t); 74 | 75 | % Update all predicted single target hypotheses of previous scan 76 | trajectoryUpdMBM = updateStep(MB_birth,trajectoryMBM,model,measlog{t},t); 77 | 78 | % Multi-scan data association 79 | [trajectoryEst,trajectoryMBM] = dataAssoc(trajectoryUpdMBM,model); 80 | 81 | if model.L ~= 0 82 | % Backforward Filtering for most likely global hypothesis 83 | trajectorySmoothEst = backforwardFiltering(trajectoryEst,trajectorySmoothEst,model); 84 | 85 | % Use part of smoothed trajectories to replace unsmoothed ones 86 | trajectoryMBM = approxSmoothedTrajectory(trajectoryMBM,trajectorySmoothEst,model); 87 | else 88 | trajectorySmoothEst = trajectoryEst; 89 | end 90 | 91 | % Target state extraction 92 | [xest{t},trajectory{t},Xest] = stateExtract(trajectorySmoothEst); 93 | 94 | % Record cycling time 95 | runningTimeperMCandTimeStep(t,1) = toc(tstart); 96 | 97 | % (Filtering) performance evaluation using GOSPA metric 98 | [d_gospa, ~, decomposed_cost] = GOSPA(get_comps(xlog{t},[1 3]), get_comps(xest{t},[1 3]), 2, gospa_c, 2); 99 | gospa_vals(t,:) = sqrt([d_gospa^2 decomposed_cost.localisation decomposed_cost.missed decomposed_cost.false]); 100 | 101 | % (Filtering or smoothing) performance evaluation using trajectory metric 102 | [dxy, ~, loc_cost, miss_cost, fa_cost, switch_cost] = LPTrajMetric_sparse(Xtrue{t}, Xest, gospa_c, 2, 2); 103 | traMetric(t,:) = sqrt([dxy^2/t sum(loc_cost)/t sum(miss_cost)/t sum(fa_cost)/t sum(switch_cost)/t]); 104 | 105 | end 106 | 107 | fprintf('\nAverage GOSPA: %g\nAverage Trajectory Metric: %g\n', mean(gospa_vals(:,1)), mean(traMetric(:,1))); 108 | -------------------------------------------------------------------------------- /PMBM_Trajectory_Code/mbm01_onlineSmoothing_allTrajectory/main.m: -------------------------------------------------------------------------------- 1 | clear;clc 2 | dbstop if error 3 | 4 | % Set simulation parameters 5 | numtruth = 4; % number of targets 6 | % pmi = 1; % simulation case 1 or 2 7 | pmi = 2; 8 | % covariance used for mid-point initialisation 9 | Pmid = 1e-6*eye(4); 10 | % detection probability 11 | Pd = 0.9; 12 | % clutter rate 13 | lfai = 10; 14 | 15 | % Algorithm setup 16 | slideWindow = 3; 17 | L = 1; 18 | 19 | % Generate Ground Truth 20 | [model,measlog,xlog,X] = gentruth(slideWindow,L,Pd,lfai,numtruth,Pmid,pmi); 21 | 22 | % time steps in total 23 | K = length(xlog); 24 | 25 | % GOSPA metric 26 | gospa_vals = zeros(K,4); 27 | gospa_c = 10; 28 | 29 | % Trajectory metric 30 | traMetric = zeros(K,5); 31 | 32 | % Simulation time 33 | runningTimeperMCandTimeStep = zeros(K,1); 34 | 35 | % Extract ground truth and re-construct for use of trajectory metric 36 | Xtrue = cell(K,1); 37 | for i = 1:K 38 | idx = i >= X.tVec; 39 | Xtrue{i}.tVec = X.tVec(idx); 40 | Xtrue{i}.iVec = min(X.iVec(idx),i-X.tVec(idx)+1); 41 | Xtrue{i}.xState = X.xState(:,1:max(Xtrue{i}.tVec+Xtrue{i}.iVec)-1,idx); 42 | end 43 | 44 | % Single trajectory hypothesis structure 45 | % r = existence probability; 46 | % xpre = predicted trajectory (target states across time); 47 | % x = updated trajectory (target states across time); 48 | % Ppre = predicted covariance; 49 | % P = updated covariance 50 | % l : trajectory (measurement indices across time) 51 | % c : the cost of each single target hypothesis 52 | % a : track index 53 | % beta: begin time of trajectory, absorbtion is used 54 | % epsilon: end time of trajectory, a vector 55 | % w = trajectory weight, a vector, w(i) specifies the weight of 56 | % trajectory with end time epsilon(i) 57 | 58 | % Initialisation 59 | trajectoryMBM = []; 60 | trajectorySmoothEst = []; 61 | xest = cell(K,1); 62 | trajectory = cell(K,1); 63 | 64 | % Loop through time 65 | fprintf('Time step: '); 66 | for t = 1:K 67 | 68 | fprintf('%g ', t); 69 | 70 | tstart = tic; 71 | 72 | % Predict all single target hypotheses of previous scan 73 | [trajectoryMBM,MB_birth] = predictStep(trajectoryMBM,model,t); 74 | 75 | % Update all predicted single target hypotheses of previous scan 76 | trajectoryUpdMBM = updateStep(MB_birth,trajectoryMBM,model,measlog{t},t); 77 | 78 | % Multi-scan data association 79 | [trajectoryEst,trajectoryMBM] = dataAssoc(trajectoryUpdMBM,model); 80 | 81 | if model.L ~= 0 82 | % Backforward Filtering for most likely global hypothesis 83 | trajectorySmoothEst = backforwardFiltering(trajectoryEst,trajectorySmoothEst,model); 84 | 85 | % Use part of smoothed trajectories to replace unsmoothed ones 86 | trajectoryMBM = approxSmoothedTrajectory(trajectoryMBM,trajectorySmoothEst,model); 87 | else 88 | trajectorySmoothEst = trajectoryEst; 89 | end 90 | 91 | % Target state extraction 92 | [xest{t},trajectory{t},Xest] = stateExtract(trajectorySmoothEst); 93 | 94 | % Record cycling time 95 | runningTimeperMCandTimeStep(t,1) = toc(tstart); 96 | 97 | % (Filtering) performance evaluation using GOSPA metric 98 | [d_gospa, ~, decomposed_cost] = GOSPA(get_comps(xlog{t},[1 3]), get_comps(xest{t},[1 3]), 2, gospa_c, 2); 99 | gospa_vals(t,:) = sqrt([d_gospa^2 decomposed_cost.localisation decomposed_cost.missed decomposed_cost.false]); 100 | 101 | % (Filtering or smoothing) performance evaluation using trajectory metric 102 | [dxy, ~, loc_cost, miss_cost, fa_cost, switch_cost] = LPTrajMetric_sparse(Xtrue{t}, Xest, gospa_c, 2, 2); 103 | traMetric(t,:) = sqrt([dxy^2/t sum(loc_cost)/t sum(miss_cost)/t sum(fa_cost)/t sum(switch_cost)/t]); 104 | 105 | end 106 | 107 | fprintf('\nAverage GOSPA: %g\nAverage Trajectory Metric: %g\n', mean(gospa_vals(:,1)), mean(traMetric(:,1))); 108 | -------------------------------------------------------------------------------- /PMBM_Trajectory_Code/pmbm_onlineSmoothing_allTrajectory/main.m: -------------------------------------------------------------------------------- 1 | clear;clc 2 | dbstop if error 3 | 4 | % Set simulation parameters 5 | numtruth = 4; % number of targets 6 | % pmi = 1; % simulation case 1 or 2 7 | pmi = 2; 8 | % covariance used for mid-point initialisation 9 | Pmid = 1e-6*eye(4); 10 | % detection probability 11 | Pd = 0.9; 12 | % clutter rate 13 | lfai = 10; 14 | 15 | % Algorithm setup 16 | slideWindow = 3; 17 | L = 1; 18 | 19 | % Generate Ground Truth 20 | [model,measlog,xlog,X] = gentruth(slideWindow,L,Pd,lfai,numtruth,Pmid,pmi); 21 | 22 | % time steps in total 23 | K = length(xlog); 24 | 25 | % GOSPA metric 26 | gospa_vals = zeros(K,4); 27 | gospa_c = 10; 28 | 29 | % Trajectory metric 30 | traMetric = zeros(K,5); 31 | 32 | % Simulation time 33 | runningTimeperMCandTimeStep = zeros(K,1); 34 | 35 | % Extract ground truth and re-construct for use of trajectory metric 36 | Xtrue = cell(K,1); 37 | for i = 1:K 38 | idx = i >= X.tVec; 39 | Xtrue{i}.tVec = X.tVec(idx); 40 | Xtrue{i}.iVec = min(X.iVec(idx),i-X.tVec(idx)+1); 41 | Xtrue{i}.xState = X.xState(:,1:max(Xtrue{i}.tVec+Xtrue{i}.iVec)-1,idx); 42 | end 43 | 44 | % Single trajectory hypothesis structure 45 | % r = existence probability; 46 | % xpre = predicted trajectory (target states across time); 47 | % x = updated trajectory (target states across time); 48 | % Ppre = predicted covariance; 49 | % P = updated covariance 50 | % l : trajectory (measurement indices across time) 51 | % c : the cost of each single target hypothesis 52 | % a : track index 53 | % beta: begin time of trajectory, absorbtion is used 54 | % epsilon: end time of trajectory, a vector 55 | % w = trajectory weight, a vector, w(i) specifies the weight of 56 | % trajectory with end time epsilon(i) 57 | 58 | % Initialisation 59 | trajectoryMBM = []; 60 | unknownPPP = []; 61 | trajectorySmoothEst = []; 62 | xest = cell(K,1); 63 | trajectory = cell(K,1); 64 | 65 | % Loop through time 66 | fprintf('Time step: '); 67 | for t = 1:K 68 | 69 | fprintf('%g ', t); 70 | 71 | tstart = tic; 72 | 73 | % Predict all single target hypotheses of previous scan 74 | [trajectoryMBM,unknownPPP] = predictStep(trajectoryMBM,unknownPPP,model,t); 75 | 76 | % Update all predicted single target hypotheses of previous scan 77 | [unknownPPP,trajectoryUpdMBM,trajectoryNewMBM] = ... 78 | updateStep(unknownPPP,trajectoryMBM,model,measlog{t},t); 79 | 80 | % Multi-scan data association 81 | [trajectoryEst,trajectoryMBM] = dataAssoc(trajectoryUpdMBM,trajectoryNewMBM,model); 82 | 83 | if model.L ~= 0 84 | % Backforward Filtering for most likely global hypothesis 85 | trajectorySmoothEst = backforwardFiltering(trajectoryEst,trajectorySmoothEst,model); 86 | 87 | % Use part of smoothed trajectories to replace unsmoothed ones 88 | trajectoryMBM = approxSmoothedTrajectory(trajectoryMBM,trajectorySmoothEst,model); 89 | else 90 | trajectorySmoothEst = trajectoryEst; 91 | end 92 | 93 | % Target state extraction 94 | [xest{t},trajectory{t},Xest] = stateExtract(trajectorySmoothEst); 95 | 96 | % Record cycling time 97 | runningTimeperMCandTimeStep(t,1) = toc(tstart); 98 | 99 | % (Filtering) performance evaluation using GOSPA metric 100 | [d_gospa, ~, decomposed_cost] = GOSPA(get_comps(xlog{t},[1 3]), get_comps(xest{t},[1 3]), 2, gospa_c, 2); 101 | gospa_vals(t,:) = sqrt([d_gospa^2 decomposed_cost.localisation decomposed_cost.missed decomposed_cost.false]); 102 | 103 | % (Filtering or smoothing) performance evaluation using trajectory 104 | % metric, normalised by time step 105 | [dxy, ~, loc_cost, miss_cost, fa_cost, switch_cost] = LPTrajMetric_sparse(Xtrue{t}, Xest, gospa_c, 2, 2); 106 | traMetric(t,:) = sqrt([dxy^2/t sum(loc_cost)/t sum(miss_cost)/t sum(fa_cost)/t sum(switch_cost)/t]); 107 | 108 | end 109 | 110 | fprintf('\nAverage GOSPA: %g\nAverage Trajectory Metric: %g\n', mean(gospa_vals(:,1)), mean(traMetric(:,1))); 111 | -------------------------------------------------------------------------------- /PMBM_Trajectory_Code/gospaMetric/GOSPA code/auctionAlgortihm.m: -------------------------------------------------------------------------------- 1 | function [person_to_obj, obj_to_person, opt_cost] ... 2 | = auctionAlgortihm(cost_mat, max_iter) 3 | % AUTHOR: Abu Sajana Rahmathullah 4 | % DATE OF CREATION: 7 August, 2017 5 | % 6 | % [person_to_obj, obj_to_person, opt_cost] ... 7 | % = auctionAlgortihm(cost_mat, epsil, max_iter) 8 | % returns the results of auction algorithm in assigning 'm' unique persons 9 | % to the 'n' objects, such that the overall cost is maximised. 10 | % 11 | % REFERENCE: Section 6.5.1 in 'Design and Analysis of Modern Tracking 12 | % Systems' by Blackman and Popoli, 1999 edition 13 | % 14 | % INPUT: 15 | % cost_mat: matrix of size mxn consising of cost of assigning one of m 16 | % persons to one of each n objects 17 | % max_iter: maximum number of iterations to terminate the alg 18 | % 19 | % OUTPUT: 20 | % person_to_obj: A vector of size 1xm which has indices of the assigned 21 | % objects or '0' if unassigned 22 | % obj_to_person: A vector of size 1xn, which has indices of the 23 | % assigned persons and a vector 24 | % opt_cost : A scalar, value of the (optimal) assignment made 25 | % 26 | 27 | m_person = size(cost_mat, 1); n_obj = size(cost_mat,2); 28 | 29 | % index for objects that will be left unassigned 30 | un_ass_ind = 0; 31 | 32 | % corner cases when there is only one person and/or one object 33 | if m_person == 1 34 | [opt_cost, person_to_obj] = max(cost_mat); 35 | obj_to_person = un_ass_ind * ones(n_obj, 1); 36 | obj_to_person(person_to_obj) = 1; 37 | return; 38 | end 39 | if n_obj == 1 40 | [opt_cost, obj_to_person] = max(cost_mat); 41 | person_to_obj = un_ass_ind * ones(m_person, 1); 42 | person_to_obj(obj_to_person) = 1; 43 | return; 44 | end 45 | 46 | swap_dim_flag = false; 47 | epsil = 1/ max(n_obj, m_person); 48 | 49 | if n_obj < m_person % the below implementation works for m_person<=n_obj. 50 | % If not satisifed, swap the cost matrix 51 | cost_mat = cost_mat'; 52 | m_person = size(cost_mat, 1); n_obj = size(cost_mat,2); 53 | swap_dim_flag = true; 54 | end 55 | 56 | 57 | % obj ind of assignment for person 1 to m 58 | person_to_obj = un_ass_ind * ones(m_person, 1); 59 | 60 | % person ind of assignemnt for obj 1 to n 61 | obj_to_person = un_ass_ind * ones(n_obj, 1); 62 | opt_cost = 0; 63 | p_obj = zeros(1, n_obj); % price for each object 64 | 65 | iter = 0; 66 | while(~all(person_to_obj ~= un_ass_ind)) 67 | if iter > max_iter 68 | warning(['Maximum number of iterations reached! Retry with ' ... 69 | 'more than ' num2str(max_iter) ' number of iterations.']); 70 | break; 71 | end 72 | for i = 1:m_person 73 | if(person_to_obj(i) == un_ass_ind) 74 | % pick the unassigned person i to bid for its best obj jStar 75 | 76 | % value for each object for the person i 77 | [val_i_j, j] = sort(cost_mat(i, :) - p_obj, 2, 'descend'); 78 | 79 | % j_star is the best object for person i 80 | j_star = j(1); 81 | 82 | % 1st and 2nd best value for person i 83 | v_ijStar = val_i_j(1); 84 | w_ijStar = val_i_j(2); 85 | 86 | 87 | % bid for obj jStar 88 | if w_ijStar ~= -Inf % if there is no 2nd best 89 | p_obj(j_star) = p_obj(j_star) + v_ijStar - w_ijStar + epsil; 90 | else 91 | p_obj(j_star) = p_obj(j_star) + v_ijStar + epsil; 92 | end 93 | 94 | 95 | if obj_to_person(j_star) ~= un_ass_ind % if j_star is uassigned 96 | % if j_star is assigned to obj_to_person(j_star) before, 97 | % remove the cost of old assignment 98 | % and unassign the person obj_to_person(j_star) 99 | opt_cost = opt_cost ... 100 | - cost_mat(obj_to_person(j_star), j_star); 101 | person_to_obj(obj_to_person(j_star)) = un_ass_ind; 102 | end 103 | obj_to_person(j_star) = i; % assign i to j_star 104 | person_to_obj(i) = j_star; % assign j_star to i 105 | 106 | % update the cost of new assignemnt 107 | opt_cost = opt_cost + cost_mat(i, j_star); 108 | end 109 | end 110 | iter = iter+1; 111 | end 112 | 113 | % swap the dimensions to compensate for the swapping of the cost matrix 114 | % done in the beginning 115 | if swap_dim_flag == true 116 | tmp = obj_to_person; 117 | obj_to_person = person_to_obj; 118 | person_to_obj = tmp; 119 | end 120 | -------------------------------------------------------------------------------- /PMBM_Trajectory_Code/gospaMetric/GOSPA code/GOSPA.m: -------------------------------------------------------------------------------- 1 | function [d_gospa, x_to_y_assignment, decomposed_cost] = ... 2 | GOSPA(x_mat, y_mat, p, c, alpha) 3 | % AUTHOR: Abu Sajana Rahmathullah 4 | % DATE OF CREATION: 7 August, 2017 5 | % 6 | % [d_gospa, x_to_y_assignment] = GOSPA(x_mat, y_mat, p, c, alpha) 7 | % computes the generalized optimal sub-pattern assignment metric (GOSPA) 8 | % metric between the two finite sets, x_mat and y_mat for the given 9 | % parameters c, p and alpha. Note that this implementation is based on 10 | % auction algortihm, implementation of which is also available in this 11 | % repository. For details about the metric, check 12 | % https://arxiv.org/abs/1601.05585. Also visit https://youtu.be/M79GTTytvCM 13 | % for a 15-min presentation about the metric. 14 | % 15 | % INPUT: 16 | % x_mat, y_mat: Input sets represented as real matrices, where the 17 | % columns represent the vectors in the set 18 | % p : 1<=p0, cutoff distance 20 | % alpha : 0 Penalty on missed & false targets 22 | % 23 | % OUTPUT: 24 | % d_gospa : Scalar, GOSPA distance between x_mat and y_mat 25 | % x_to_y_assignment: Integer vector, of length same as the number of 26 | % columns of x_mat. The i^th entry in the vector 27 | % denotes the column index of the vector in y_mat, 28 | % that is assigned to the vector in the i^th column of 29 | % x_mat. Note that these indices are based on the 30 | % permutation. Therefore, if #columns in x_mat <= 31 | % #columns in y_mat, the entries in this vector will 32 | % be between 1 and #columns in y_mat. Otherwise, the 33 | % entries will be between 0 and #columns in y_mat, 34 | % where 0 indicates that the corresponding columns in 35 | % x_mat are unassigned. 36 | % decomposed_cost : Struct that returns the decomposition of the GOSPA 37 | % metric for alpha=2 into 3 components: 38 | % 'localisation', 'missed', 'false'. 39 | % Note that 40 | % d_gospa = (decomposed_cost.localisation + 41 | % decomposed_cost.missed + 42 | % decomposed_cost.false)^(1/p) 43 | % 44 | % Note: Euclidean base distance between the vectors in x_mat and y_mat is 45 | % used in this function. One can change the function 'computeBaseDistance' 46 | % in this function for other choices. 47 | 48 | % check that the input parameters are within the valid range 49 | 50 | n_ouput_arg=nargout; 51 | 52 | checkInput(); 53 | 54 | nx = size(x_mat, 2); % no of points in x_mat 55 | ny = size(y_mat, 2); % no of points in y_mat 56 | 57 | % compute cost matrix 58 | cost_mat = zeros(nx, ny); 59 | for ix = 1:nx 60 | for iy = 1:ny 61 | cost_mat(ix, iy) ... 62 | = min(computeBaseDistance(x_mat(:, ix), y_mat(:, iy)), c); 63 | end 64 | end 65 | 66 | % intialise output values 67 | decomposed_cost = struct( ... 68 | 'localisation', 0, ... 69 | 'missed', 0, ... 70 | 'false', 0); 71 | 72 | 73 | x_to_y_assignment = []; 74 | opt_cost = 0; 75 | 76 | dummy_cost = (c^p) / alpha; % penalty for the cardinality mismatch 77 | 78 | % below, cost is negated to make it compatible with auction algorithm 79 | if nx == 0 % when x_mat is empty, all entries in y_mat are false 80 | opt_cost = -ny * dummy_cost; 81 | decomposed_cost.false = opt_cost; 82 | else 83 | if ny == 0 % when y_mat is empty, all entries in x_mat are missed 84 | opt_cost = -nx * dummy_cost; 85 | 86 | if(alpha==2) 87 | decomposed_cost.missed = opt_cost; 88 | end 89 | else % when both x_mat and y_mat are non-empty, use auction algorithm 90 | cost_mat = -(cost_mat.^p); 91 | [x_to_y_assignment, y_to_x_assignment, ~] ... 92 | = assign2D(cost_mat,true); 93 | % use the assignments to compute the cost 94 | for ind = 1:nx 95 | if x_to_y_assignment(ind) ~= 0 96 | opt_cost = opt_cost + cost_mat(ind,x_to_y_assignment(ind)); 97 | 98 | if(alpha==2) 99 | 100 | decomposed_cost.localisation = ... 101 | decomposed_cost.localisation ... 102 | + cost_mat(ind,x_to_y_assignment(ind)) ... 103 | .* double(cost_mat(ind,x_to_y_assignment(ind)) > -c^p); 104 | 105 | decomposed_cost.missed = decomposed_cost.missed ... 106 | - dummy_cost ... 107 | .* double(cost_mat(ind,x_to_y_assignment(ind)) == -c^p); 108 | 109 | decomposed_cost.false = ... 110 | decomposed_cost.false ... 111 | - dummy_cost ... 112 | .* double(cost_mat(ind,x_to_y_assignment(ind)) == -c^p); 113 | end 114 | else 115 | opt_cost = opt_cost - dummy_cost; 116 | if(alpha==2) 117 | decomposed_cost.missed = decomposed_cost.missed - dummy_cost; 118 | end 119 | end 120 | end 121 | opt_cost = opt_cost - sum(y_to_x_assignment == 0) * dummy_cost; 122 | if(alpha==2) 123 | decomposed_cost.false = decomposed_cost.false ... 124 | - sum(y_to_x_assignment == 0) * dummy_cost; 125 | end 126 | end 127 | end 128 | 129 | % final output 130 | d_gospa = (-opt_cost)^(1/p); 131 | decomposed_cost.localisation = (-decomposed_cost.localisation); 132 | decomposed_cost.missed = (-decomposed_cost.missed); 133 | decomposed_cost.false = (-decomposed_cost.false); 134 | 135 | function checkInput() 136 | if size(x_mat, 1) ~= size(y_mat, 1) 137 | error('The number of rows in x_mat & y_mat should be equal.'); 138 | end 139 | if ~((p >= 1) && (p < inf)) 140 | error('The value of exponent p should be within [1,inf).'); 141 | end 142 | if ~(c>0) 143 | error('The value of base distance c should be larger than 0.'); 144 | end 145 | 146 | if ~((alpha > 0) && (alpha <= 2)) 147 | error('The value of alpha should be within (0,2].'); 148 | end 149 | if alpha ~= 2 && n_ouput_arg==3 150 | warning(['decomposed_cost is not valid for alpha = ' ... 151 | num2str(alpha)]); 152 | end 153 | end 154 | end 155 | 156 | function db = computeBaseDistance(x_vec, y_vec) 157 | db = sum((x_vec - y_vec).^2)^(1/2); 158 | end 159 | -------------------------------------------------------------------------------- /PMBM_Trajectory_Code/pmbm_onlineSmoothing_allTrajectory/updateStep.m: -------------------------------------------------------------------------------- 1 | function [unknownPPP,trajectoryUpdMBM,trajectoryNewMBM] = ... 2 | updateStep(unknownPPP,trajectoryMBM,model,z,t) 3 | %UPDATE: CONSTRUCT COMPONENTS OF DISTRIBUTION UPDATED WITH MEASUREMENT 4 | 5 | % Extract parameters from model 6 | Pd = model.Pd; 7 | H = model.H; 8 | R = model.R; 9 | lambda_fa = model.lambda_fa; 10 | 11 | % Interpret sizes from inputs 12 | % If it is a hypothesis with zero existence probability, no update 13 | % e.g. n_invalid, n_valid, nupd = n_invalid + n_valid*(m+1); 14 | nu = length(unknownPPP); 15 | [~,m] = size(z); 16 | n = length(trajectoryMBM); % number of single target hypotheses to be updated 17 | % number of single target hypotheses after update 18 | if n > 0 19 | nupd = length(find([trajectoryMBM.r]~=0))*m + n; 20 | else 21 | nupd = 0; 22 | end 23 | 24 | % Allocate memory for existing tracks (single target hypotheses) 25 | trajectoryUpdMBM = repmat(struct,nupd,1); 26 | 27 | % Keep record ancestor information, i.e., the index of the single target 28 | % hypothesis being updated, need be called in N-scan pruning 29 | 30 | % Update existing tracks 31 | iupd = 0; % initiate updating index 32 | insideGating = true(nupd,1); 33 | usedMeas = false(m,1); 34 | for i = 1:n 35 | % first determine whether the single target hypothesis has valid 36 | % existence probability or not 37 | if trajectoryMBM(i).r == 0 % this case corresponds to non-existence single target hypothesis 38 | iupd = iupd+1; 39 | trajectoryUpdMBM(iupd).c = 0; 40 | trajectoryUpdMBM(iupd).r = 0; 41 | trajectoryUpdMBM(iupd).xpre = zeros(4,1); 42 | trajectoryUpdMBM(iupd).Ppre = zeros(4,4); 43 | trajectoryUpdMBM(iupd).x = zeros(4,1); 44 | trajectoryUpdMBM(iupd).P = zeros(4,4); 45 | trajectoryUpdMBM(iupd).l = [trajectoryMBM(i).l;[t,0]]; 46 | trajectoryUpdMBM(iupd).a = trajectoryMBM(i).a; 47 | trajectoryUpdMBM(iupd).beta = 0; 48 | trajectoryUpdMBM(iupd).epsilon = 0; 49 | else 50 | % Create missed detection hypothesis 51 | iupd = iupd+1; 52 | temp = 1-trajectoryMBM(i).r+trajectoryMBM(i).r*(1-Pd*trajectoryMBM(i).w(end)); 53 | trajectoryUpdMBM(iupd).c = trajectoryMBM(i).c-log(temp); 54 | trajectoryUpdMBM(iupd).w = [trajectoryMBM(i).w(1:end-1) trajectoryMBM(i).w(end)*(1-Pd)]/(1-trajectoryMBM(i).w(end)*Pd); 55 | trajectoryUpdMBM(iupd).r = trajectoryMBM(i).r*(1-Pd*trajectoryMBM(i).w(end))/temp; 56 | trajectoryUpdMBM(iupd).xpre = trajectoryMBM(i).xpre; 57 | trajectoryUpdMBM(iupd).Ppre = trajectoryMBM(i).Ppre; 58 | trajectoryUpdMBM(iupd).x = trajectoryMBM(i).x; 59 | trajectoryUpdMBM(iupd).P = trajectoryMBM(i).P; 60 | % If it is missed detection, add 0 to measurement history 61 | trajectoryUpdMBM(iupd).l = [trajectoryMBM(i).l;[t,0]]; 62 | trajectoryUpdMBM(iupd).a = trajectoryMBM(i).a; 63 | trajectoryUpdMBM(iupd).beta = trajectoryMBM(i).beta; 64 | trajectoryUpdMBM(iupd).epsilon = trajectoryMBM(i).epsilon; 65 | 66 | % Create hypotheses with measurement updates 67 | S = H*trajectoryMBM(i).P(:,:,end)*H' + R; 68 | sqrt_det2piS = sqrt(det(2*pi*S)); 69 | K = trajectoryMBM(i).P(:,:,end)*H'/S; 70 | Pplus = trajectoryMBM(i).P(:,:,end) - K*H*trajectoryMBM(i).P(:,:,end); 71 | 72 | for j = 1:m 73 | iupd = iupd+1; 74 | % do not update hypotheses with too small existence probability 75 | % at the current time step 76 | if trajectoryMBM(i).r*trajectoryMBM(i).w(end) < model.threshold 77 | insideGating(iupd) = false; 78 | else 79 | v = z(:,j) - H*trajectoryMBM(i).x(:,end); 80 | maha = v'/S*v; 81 | temp = exp(-0.5*maha)/sqrt_det2piS; 82 | % ellipsoidal gating 83 | if maha > model.gamma 84 | insideGating(iupd) = false; 85 | else 86 | usedMeas(j) = true; 87 | trajectoryUpdMBM(iupd).c = trajectoryMBM(i).c-log(trajectoryMBM(i).r*Pd*trajectoryMBM(i).w(end)*temp); 88 | trajectoryUpdMBM(iupd).w = 1; 89 | trajectoryUpdMBM(iupd).r = 1; 90 | trajectoryUpdMBM(iupd).xpre = trajectoryMBM(i).xpre; 91 | trajectoryUpdMBM(iupd).Ppre = trajectoryMBM(i).Ppre; 92 | trajectoryUpdMBM(iupd).x = trajectoryMBM(i).x; 93 | trajectoryUpdMBM(iupd).x(:,end) = trajectoryMBM(i).x(:,end) + K*v; 94 | trajectoryUpdMBM(iupd).P = trajectoryMBM(i).P; 95 | trajectoryUpdMBM(iupd).P(:,:,end) = Pplus; 96 | % Otherwise, add the index of the measurement at current scan 97 | trajectoryUpdMBM(iupd).l = [trajectoryMBM(i).l;[t,j]]; 98 | trajectoryUpdMBM(iupd).a = trajectoryMBM(i).a; 99 | trajectoryUpdMBM(iupd).beta = trajectoryMBM(i).beta; 100 | trajectoryUpdMBM(iupd).epsilon = trajectoryMBM(i).epsilon(end); 101 | end 102 | end 103 | end 104 | end 105 | end 106 | 107 | unusedMeas = ~usedMeas; 108 | 109 | % Only keep updated single target hypotheses 110 | idx_keep = insideGating; 111 | trajectoryUpdMBM = trajectoryUpdMBM(idx_keep); 112 | 113 | % Allocate memory for new tracks, each new track contains two single target 114 | % hypothese 115 | trajectoryNewMBM = struct('w',0,'r',0,'xpre',zeros(4,1),'x',zeros(4,1),... 116 | 'Ppre',zeros(4,4,1),'P',zeros(4,4,1),'l',[],'c',0,'a',[],'beta',0,'epsilon',0); 117 | trajectoryNewMBM = repmat(trajectoryNewMBM,2*m,1); 118 | 119 | % Allocate temporary memory for new tracks 120 | Sk = zeros(2,2,nu); 121 | Kk = zeros(4,2,nu); 122 | Pk = zeros(4,4,nu); 123 | ck = zeros(nu,1); 124 | sqrt_det2piSk = zeros(nu,1); 125 | yk = zeros(4,nu); 126 | 127 | % Create a new track for each measurement by updating PPP with measurement 128 | for k = 1:nu 129 | Pu = unknownPPP(k).P(:,:,end); 130 | Sk(:,:,k) = H*Pu*H' + R; 131 | sqrt_det2piSk(k) = sqrt(det(2*pi*Sk(:,:,k))); 132 | Kk(:,:,k) = Pu*H'/Sk(:,:,k); 133 | Pk(:,:,k) = Pu - Kk(:,:,k)*H*Pu; 134 | end 135 | for j = 1:m 136 | for k = 1:nu 137 | v = z(:,j) - H*unknownPPP(k).x(:,end); 138 | ck(k) = unknownPPP(k).lambda*Pd*exp(-0.5*v'/Sk(:,:,k)*v)/sqrt_det2piSk(k); 139 | yk(:,k) = unknownPPP(k).x(:,end) + Kk(:,:,k)*v; 140 | end 141 | C = sum(ck); 142 | % first single target hypothesis for measurement associated to previous 143 | % track, second for new track 144 | trajectoryNewMBM(2*j).w = 1; 145 | trajectoryNewMBM(2*j-1).c = 0; 146 | trajectoryNewMBM(2*j).c = -log(C + lambda_fa); 147 | trajectoryNewMBM(2*j).r = C/(C + lambda_fa); 148 | % only keep the most likely Gaussian component 149 | [~,maxidx] = max(ck); 150 | trajectoryNewMBM(2*j).xpre = unknownPPP(maxidx).x(:,end); 151 | trajectoryNewMBM(2*j).Ppre = unknownPPP(maxidx).P(:,:,end); 152 | ck = ck/C; 153 | trajectoryNewMBM(2*j).x = yk*ck; 154 | trajectoryNewMBM(2*j).P = zeros(4,4); 155 | for k = 1:nu 156 | v = trajectoryNewMBM(2*j).x - yk(:,k); 157 | trajectoryNewMBM(2*j).P = trajectoryNewMBM(2*j).P + ck(k)*(Pk(:,:,k) + v*v'); 158 | end 159 | % for trajectory purpose 160 | trajectoryNewMBM(2*j-1).l = [t,0]; % add 0, if there is no new target 161 | trajectoryNewMBM(2*j).l = [t,j]; % otherwise, add measurement index 162 | % update track index 163 | if nupd==0 164 | trajectoryNewMBM(2*j-1).a = j; 165 | trajectoryNewMBM(2*j).a = j; 166 | else 167 | trajectoryNewMBM(2*j-1).a = trajectoryUpdMBM(end).a+j; 168 | trajectoryNewMBM(2*j).a = trajectoryUpdMBM(end).a+j; 169 | end 170 | trajectoryNewMBM(2*j-1).beta = 0; 171 | trajectoryNewMBM(2*j-1).epsilon = 0; 172 | trajectoryNewMBM(2*j).beta = t; 173 | trajectoryNewMBM(2*j).epsilon = t; 174 | end 175 | 176 | % If a newly created trajectory has too small existence probability, 177 | % and the measurement used to create this trajectory does not fall inside 178 | % the gates of any pre-existing tracks, remove it 179 | idx_remain = true(2*m,1); 180 | for i = 1:m 181 | if trajectoryNewMBM(2*i).r < model.threshold && unusedMeas(i)==true 182 | idx_remain(2*i-1:2*i) = false; 183 | end 184 | end 185 | trajectoryNewMBM = trajectoryNewMBM(idx_remain); 186 | 187 | % Update (i.e., thin) intensity of unknown targets 188 | for i = 1:nu 189 | unknownPPP(i).lambda = (1-Pd)*unknownPPP(i).lambda; 190 | end 191 | 192 | % Truncate low weight components 193 | ss = [unknownPPP.lambda] > model.threshold; 194 | unknownPPP = unknownPPP(ss); 195 | 196 | end 197 | 198 | -------------------------------------------------------------------------------- /PMBM_Trajectory_Code/mbm01_onlineSmoothing_allTrajectory/updateStep.m: -------------------------------------------------------------------------------- 1 | function trajectoryUpdMBM = updateStep(MB_birth,trajectoryMBM,model,z,t) 2 | %UPDATE: CONSTRUCT COMPONENTS OF DISTRIBUTION UPDATED WITH MEASUREMENT 3 | 4 | % Extract parameters from model 5 | Pd = model.Pd; 6 | H = model.H; 7 | R = model.R; 8 | lambda_fa = model.lambda_fa; 9 | 10 | % Interpret sizes from inputs 11 | % If it is a hypothesis with zero existence probability, no update 12 | % e.g. n_invalid, n_valid, nupd = n_invalid + n_valid*(m+1); 13 | nb = length(MB_birth); 14 | [~,m] = size(z); 15 | n = length(trajectoryMBM); % number of single target hypotheses to be updated 16 | % number of single target hypotheses after update 17 | if n > 0 18 | nupd = length(find([trajectoryMBM.r]~=0))*m + n; 19 | else 20 | nupd = 0; 21 | end 22 | 23 | % Allocate memory for existing tracks (single target hypotheses) 24 | trajectoryUpdMBM = repmat(struct,nupd,1); 25 | 26 | % Keep record ancestor information, i.e., the index of the single target 27 | % hypothesis being updated, can be used in N-scan pruning 28 | 29 | % Update existing tracks 30 | iupd = 0; % initiate updating index 31 | insideGating = true(nupd,1); 32 | usedMeas = false(m,1); 33 | for i = 1:n 34 | % first determine whether the single target hypothesis has valid 35 | % existence probability or not 36 | if trajectoryMBM(i).r == 0 % this case corresponds to non-existence single target hypothesis 37 | iupd = iupd+1; 38 | trajectoryUpdMBM(iupd).c = trajectoryMBM(i).c; 39 | trajectoryUpdMBM(iupd).r = 0; 40 | trajectoryUpdMBM(iupd).xpre = zeros(4,1); 41 | trajectoryUpdMBM(iupd).Ppre = zeros(4,4); 42 | trajectoryUpdMBM(iupd).x = zeros(4,1); 43 | trajectoryUpdMBM(iupd).P = zeros(4,4); 44 | trajectoryUpdMBM(iupd).l = [trajectoryMBM(i).l;[t,0]]; 45 | trajectoryUpdMBM(iupd).a = trajectoryMBM(i).a; 46 | trajectoryUpdMBM(iupd).beta = 0; 47 | trajectoryUpdMBM(iupd).epsilon = 0; 48 | else 49 | % Create missed detection hypothesis 50 | iupd = iupd+1; 51 | trajectoryUpdMBM(iupd).c = trajectoryMBM(i).c-log(1-Pd*trajectoryMBM(i).w(end)); 52 | trajectoryUpdMBM(iupd).w = [trajectoryMBM(i).w(1:end-1) trajectoryMBM(i).w(end)*(1-Pd)]/(1-trajectoryMBM(i).w(end)*Pd); 53 | trajectoryUpdMBM(iupd).r = 1; 54 | trajectoryUpdMBM(iupd).xpre = trajectoryMBM(i).xpre; 55 | trajectoryUpdMBM(iupd).Ppre = trajectoryMBM(i).Ppre; 56 | trajectoryUpdMBM(iupd).x = trajectoryMBM(i).x; 57 | trajectoryUpdMBM(iupd).P = trajectoryMBM(i).P; 58 | % If it is missed detection, add 0 to measurement history 59 | trajectoryUpdMBM(iupd).l = [trajectoryMBM(i).l;[t,0]]; 60 | trajectoryUpdMBM(iupd).a = trajectoryMBM(i).a; 61 | trajectoryUpdMBM(iupd).beta = trajectoryMBM(i).beta; 62 | trajectoryUpdMBM(iupd).epsilon = trajectoryMBM(i).epsilon; 63 | 64 | % Create hypotheses with measurement updates 65 | S = H*trajectoryMBM(i).P(:,:,end)*H' + R; 66 | sqrt_det2piS = sqrt(det(2*pi*S)); 67 | K = trajectoryMBM(i).P(:,:,end)*H'/S; 68 | Pplus = trajectoryMBM(i).P(:,:,end) - K*H*trajectoryMBM(i).P(:,:,end); 69 | for j = 1:m 70 | iupd = iupd+1; 71 | % do not update hypotheses with too small existence probability 72 | % at the current time step 73 | if trajectoryMBM(i).r*trajectoryMBM(i).w(end) < model.threshold 74 | insideGating(iupd) = false; 75 | else 76 | v = z(:,j) - H*trajectoryMBM(i).x(:,end); 77 | maha = v'/S*v; 78 | temp = exp(-0.5*maha)/sqrt_det2piS; 79 | if maha > model.gamma % ellipsoidal gating 80 | insideGating(iupd) = false; 81 | else 82 | usedMeas(j) = true; 83 | trajectoryUpdMBM(iupd).c = trajectoryMBM(i).c-log(Pd*trajectoryMBM(i).w(end)*temp); 84 | trajectoryUpdMBM(iupd).w = 1; 85 | trajectoryUpdMBM(iupd).r = 1; 86 | trajectoryUpdMBM(iupd).xpre = trajectoryMBM(i).xpre; 87 | trajectoryUpdMBM(iupd).Ppre = trajectoryMBM(i).Ppre; 88 | trajectoryUpdMBM(iupd).x = trajectoryMBM(i).x; 89 | trajectoryUpdMBM(iupd).x(:,end) = trajectoryMBM(i).x(:,end) + K*v; 90 | trajectoryUpdMBM(iupd).P = trajectoryMBM(i).P; 91 | trajectoryUpdMBM(iupd).P(:,:,end) = Pplus; 92 | % Otherwise, add the index of the measurement at current scan 93 | trajectoryUpdMBM(iupd).l = [trajectoryMBM(i).l;[t,j]]; 94 | trajectoryUpdMBM(iupd).a = trajectoryMBM(i).a; 95 | trajectoryUpdMBM(iupd).beta = trajectoryMBM(i).beta; 96 | trajectoryUpdMBM(iupd).epsilon = trajectoryMBM(i).epsilon(end); 97 | end 98 | end 99 | end 100 | end 101 | end 102 | 103 | unusedMeas = ~usedMeas; 104 | 105 | % Only keep updated single target hypotheses 106 | idx_keep = insideGating; 107 | trajectoryUpdMBM = trajectoryUpdMBM(idx_keep); 108 | 109 | % Create a new track for each Bernoulli component in MB birth, each new 110 | % track contains (#measurements+2) single trajectory hypotheses, one for 111 | % not born, one for missed detection, #measurements for measurement updates 112 | trajectoryNewMBM = struct('w',1,'r',0,'xpre',zeros(4,1),'x',zeros(4,1),... 113 | 'Ppre',zeros(4,4,1),'P',zeros(4,4,1),'l',[],'c',0,'a',[],'beta',0,'epsilon',0); 114 | trajectoryNewMBM = repmat(trajectoryNewMBM,(m+2)*nb,1); 115 | 116 | % Allocate temporary working for new tracks 117 | Sk = zeros(2,2,nb); 118 | Kk = zeros(4,2,nb); 119 | Pk = zeros(4,4,nb); 120 | sqrt_det2piSk = zeros(nb,1); 121 | 122 | idx_remain = true((m+2)*nb,1); 123 | 124 | for i = 1:nb 125 | Pb = MB_birth(i).P; 126 | Sk(:,:,i) = H*Pb*H' + R; 127 | sqrt_det2piSk(i) = sqrt(det(2*pi*Sk(:,:,i))); 128 | Kk(:,:,i) = Pb*H'/Sk(:,:,i); 129 | Pk(:,:,i) = Pb - Kk(:,:,i)*H*Pb; 130 | 131 | % not born hypothesis 132 | trajectoryNewMBM((i-1)*(m+2)+1).c = -log(1-MB_birth(i).r); 133 | trajectoryNewMBM((i-1)*(m+2)+1).l = [t,0]; 134 | if nupd==0 135 | trajectoryNewMBM((i-1)*(m+2)+1).a = i; 136 | else 137 | trajectoryNewMBM((i-1)*(m+2)+1).a = trajectoryUpdMBM(end).a+i; 138 | end 139 | 140 | % missed detection hypothesis 141 | trajectoryNewMBM((i-1)*(m+2)+2).c = -log((1-Pd)*MB_birth(i).r); 142 | trajectoryNewMBM((i-1)*(m+2)+2).w = 1-Pd; 143 | trajectoryNewMBM((i-1)*(m+2)+2).r = 1; 144 | trajectoryNewMBM((i-1)*(m+2)+2).xpre = MB_birth(i).x; 145 | trajectoryNewMBM((i-1)*(m+2)+2).Ppre = MB_birth(i).P; 146 | trajectoryNewMBM((i-1)*(m+2)+2).x = MB_birth(i).x; 147 | trajectoryNewMBM((i-1)*(m+2)+2).P = MB_birth(i).P; 148 | trajectoryNewMBM((i-1)*(m+2)+2).l = [t,0]; 149 | trajectoryNewMBM((i-1)*(m+2)+2).a = trajectoryNewMBM((i-1)*(m+2)+1).a; 150 | trajectoryNewMBM((i-1)*(m+2)+2).beta = MB_birth(i).beta; 151 | trajectoryNewMBM((i-1)*(m+2)+2).epsilon = MB_birth(i).epsilon; 152 | 153 | % measurement update 154 | for j = 1:m 155 | v = z(:,j) - H*MB_birth(i).x; 156 | ck = MB_birth(i).r*Pd*exp(-0.5*v'/Sk(:,:,i)*v)/sqrt_det2piSk(i); 157 | % If a newly created trajectory with has too small existence probability, 158 | % and the measurement used to create this trajectory does not fall inside 159 | % the gates of any pre-existing tracks, remove it 160 | if ck/(ck+lambda_fa) < model.threshold && unusedMeas(j)==true 161 | idx_remain((i-1)*(m+2)+2+j) = false; 162 | else 163 | usedMeas(j) = true; 164 | trajectoryNewMBM((i-1)*(m+2)+2+j).c =-log(ck); 165 | trajectoryNewMBM((i-1)*(m+2)+2+j).r = 1; 166 | trajectoryNewMBM((i-1)*(m+2)+2+j).xpre = MB_birth(i).x; 167 | trajectoryNewMBM((i-1)*(m+2)+2+j).Ppre = MB_birth(i).P; 168 | trajectoryNewMBM((i-1)*(m+2)+2+j).x = MB_birth(i).x + Kk(:,:,i)*v; 169 | trajectoryNewMBM((i-1)*(m+2)+2+j).P = Pk(:,:,i); 170 | % Otherwise, add the index of the measurement at current scan 171 | trajectoryNewMBM((i-1)*(m+2)+2+j).l = [t,j]; 172 | trajectoryNewMBM((i-1)*(m+2)+2+j).a = trajectoryNewMBM((i-1)*(m+1)+2).a; 173 | trajectoryNewMBM((i-1)*(m+2)+2+j).beta = MB_birth(i).beta; 174 | trajectoryNewMBM((i-1)*(m+2)+2+j).epsilon = t; 175 | end 176 | end 177 | end 178 | 179 | trajectoryNewMBM = trajectoryNewMBM(idx_remain); 180 | 181 | % Create a track for each used measurement, each track contains two single 182 | % trajectory hypothesis, one representing clutter, one representing the 183 | % measurement is associated to one of the pre-existing tracks 184 | trajectoryClutterMBM = struct('w',0,'r',0,'xpre',zeros(4,1),'x',zeros(4,1),... 185 | 'Ppre',zeros(4,4,1),'P',zeros(4,4,1),'l',[],'c',0,'a',[],'beta',0,'epsilon',0); 186 | trajectoryClutterMBM = repmat(trajectoryClutterMBM,2*m,1); 187 | for j = 1:m 188 | trajectoryClutterMBM(2*j-1).l = [t,0]; 189 | trajectoryClutterMBM(2*j).l = [t,j]; 190 | trajectoryClutterMBM(2*j-1).a = trajectoryNewMBM(end).a+j; 191 | trajectoryClutterMBM(2*j).a = trajectoryNewMBM(end).a+j; 192 | trajectoryClutterMBM(2*j-1).c = 0; 193 | trajectoryClutterMBM(2*j).c = -log(lambda_fa); 194 | end 195 | trajectoryClutterMBM = trajectoryClutterMBM(logical(kron(usedMeas,[1;1]))); 196 | 197 | if nupd==0 198 | trajectoryUpdMBM = [trajectoryNewMBM;trajectoryClutterMBM]; 199 | else 200 | trajectoryUpdMBM = [trajectoryUpdMBM;trajectoryNewMBM;trajectoryClutterMBM]; 201 | end 202 | 203 | -------------------------------------------------------------------------------- /PMBM_Trajectory_Code/mbm_onlineSmoothing_allTrajectory/updateStep.m: -------------------------------------------------------------------------------- 1 | function trajectoryUpdMBM = updateStep(MB_birth,trajectoryMBM,model,z,t) 2 | %UPDATE: CONSTRUCT COMPONENTS OF DISTRIBUTION UPDATED WITH MEASUREMENT 3 | 4 | % Extract parameters from model 5 | Pd = model.Pd; 6 | H = model.H; 7 | R = model.R; 8 | lambda_fa = model.lambda_fa; 9 | 10 | % Interpret sizes from inputs 11 | % If it is a hypothesis with zero existence probability, no update 12 | % e.g. n_invalid, n_valid, nupd = n_invalid + n_valid*(m+1); 13 | nb = length(MB_birth); 14 | [~,m] = size(z); 15 | n = length(trajectoryMBM); % number of single target hypotheses to be updated 16 | % number of single target hypotheses after update 17 | if n > 0 18 | nupd = length(find([trajectoryMBM.r]~=0))*m + n; 19 | else 20 | nupd = 0; 21 | end 22 | 23 | % Allocate memory for existing tracks (single target hypotheses) 24 | trajectoryUpdMBM = repmat(struct,nupd,1); 25 | 26 | % Keep record ancestor information, i.e., the index of the single target 27 | % hypothesis being updated, need be called in N-scan pruning 28 | 29 | % Update existing tracks 30 | iupd = 0; % initiate updating index 31 | insideGating = true(nupd,1); 32 | usedMeas = false(m,1); 33 | for i = 1:n 34 | % first determine whether the single target hypothesis has valid 35 | % existence probability or not 36 | if trajectoryMBM(i).r == 0 % this case corresponds to non-existence single target hypothesis 37 | iupd = iupd+1; 38 | trajectoryUpdMBM(iupd).c = trajectoryMBM(i).c; 39 | trajectoryUpdMBM(iupd).r = 0; 40 | trajectoryUpdMBM(iupd).xpre = zeros(4,1); 41 | trajectoryUpdMBM(iupd).Ppre = zeros(4,4); 42 | trajectoryUpdMBM(iupd).x = zeros(4,1); 43 | trajectoryUpdMBM(iupd).P = zeros(4,4); 44 | trajectoryUpdMBM(iupd).l = [trajectoryMBM(i).l;[t,0]]; 45 | trajectoryUpdMBM(iupd).a = trajectoryMBM(i).a; 46 | trajectoryUpdMBM(iupd).beta = 0; 47 | trajectoryUpdMBM(iupd).epsilon = 0; 48 | else 49 | % Create missed detection hypothesis 50 | iupd = iupd+1; 51 | temp = 1-trajectoryMBM(i).r+trajectoryMBM(i).r*(1-Pd*trajectoryMBM(i).w(end)); 52 | trajectoryUpdMBM(iupd).c = trajectoryMBM(i).c-log(temp); 53 | trajectoryUpdMBM(iupd).w = [trajectoryMBM(i).w(1:end-1) trajectoryMBM(i).w(end)*(1-Pd)]/(1-trajectoryMBM(i).w(end)*Pd); 54 | trajectoryUpdMBM(iupd).r = trajectoryMBM(i).r*(1-Pd*trajectoryMBM(i).w(end))/temp; 55 | trajectoryUpdMBM(iupd).xpre = trajectoryMBM(i).xpre; 56 | trajectoryUpdMBM(iupd).Ppre = trajectoryMBM(i).Ppre; 57 | trajectoryUpdMBM(iupd).x = trajectoryMBM(i).x; 58 | trajectoryUpdMBM(iupd).P = trajectoryMBM(i).P; 59 | % If it is missed detection, add 0 to measurement history 60 | trajectoryUpdMBM(iupd).l = [trajectoryMBM(i).l;[t,0]]; 61 | trajectoryUpdMBM(iupd).a = trajectoryMBM(i).a; 62 | trajectoryUpdMBM(iupd).beta = trajectoryMBM(i).beta; 63 | trajectoryUpdMBM(iupd).epsilon = trajectoryMBM(i).epsilon; 64 | 65 | % Create hypotheses with measurement updates 66 | S = H*trajectoryMBM(i).P(:,:,end)*H' + R; 67 | sqrt_det2piS = sqrt(det(2*pi*S)); 68 | K = trajectoryMBM(i).P(:,:,end)*H'/S; 69 | Pplus = trajectoryMBM(i).P(:,:,end) - K*H*trajectoryMBM(i).P(:,:,end); 70 | for j = 1:m 71 | iupd = iupd+1; 72 | % do not update hypotheses with too small existence probability 73 | % at the current time step 74 | if trajectoryMBM(i).r*trajectoryMBM(i).w(end) < model.threshold 75 | insideGating(iupd) = false; 76 | else 77 | v = z(:,j) - H*trajectoryMBM(i).x(:,end); 78 | maha = v'/S*v; 79 | temp = exp(-0.5*maha)/sqrt_det2piS; 80 | if maha > model.gamma % ellipsoidal gating 81 | insideGating(iupd) = false; 82 | else 83 | usedMeas(j) = true; 84 | trajectoryUpdMBM(iupd).c = trajectoryMBM(i).c-log(trajectoryMBM(i).r*Pd*trajectoryMBM(i).w(end)*temp); 85 | trajectoryUpdMBM(iupd).w = 1; 86 | trajectoryUpdMBM(iupd).r = 1; 87 | trajectoryUpdMBM(iupd).xpre = trajectoryMBM(i).xpre; 88 | trajectoryUpdMBM(iupd).Ppre = trajectoryMBM(i).Ppre; 89 | trajectoryUpdMBM(iupd).x = trajectoryMBM(i).x; 90 | trajectoryUpdMBM(iupd).x(:,end) = trajectoryMBM(i).x(:,end) + K*v; 91 | trajectoryUpdMBM(iupd).P = trajectoryMBM(i).P; 92 | trajectoryUpdMBM(iupd).P(:,:,end) = Pplus; 93 | % Otherwise, add the index of the measurement at current scan 94 | trajectoryUpdMBM(iupd).l = [trajectoryMBM(i).l;[t,j]]; 95 | trajectoryUpdMBM(iupd).a = trajectoryMBM(i).a; 96 | trajectoryUpdMBM(iupd).beta = trajectoryMBM(i).beta; 97 | trajectoryUpdMBM(iupd).epsilon = trajectoryMBM(i).epsilon(end); 98 | end 99 | end 100 | end 101 | end 102 | end 103 | 104 | unusedMeas = ~usedMeas; 105 | 106 | % Only keep updated single target hypotheses 107 | idx_keep = insideGating; 108 | trajectoryUpdMBM = trajectoryUpdMBM(idx_keep); 109 | 110 | % Create a new track for each Bernoulli component in MB birth, each new 111 | % track contains (#measurements+2) single trajectory hypotheses, one for 112 | % not born, one for missed detection, #measurements for measurement updates 113 | trajectoryNewMBM = struct('w',1,'r',0,'xpre',zeros(4,1),'x',zeros(4,1),... 114 | 'Ppre',zeros(4,4,1),'P',zeros(4,4,1),'l',[],'c',0,'a',[],'beta',0,'epsilon',0); 115 | trajectoryNewMBM = repmat(trajectoryNewMBM,(m+1)*nb,1); 116 | 117 | % Allocate temporary working for new tracks 118 | Sk = zeros(2,2,nb); 119 | Kk = zeros(4,2,nb); 120 | Pk = zeros(4,4,nb); 121 | sqrt_det2piSk = zeros(nb,1); 122 | 123 | idx_remain = true((m+1)*nb,1); 124 | 125 | for i = 1:nb 126 | Pb = MB_birth(i).P; 127 | Sk(:,:,i) = H*Pb*H' + R; 128 | sqrt_det2piSk(i) = sqrt(det(2*pi*Sk(:,:,i))); 129 | Kk(:,:,i) = Pb*H'/Sk(:,:,i); 130 | Pk(:,:,i) = Pb - Kk(:,:,i)*H*Pb; 131 | 132 | % missed detection hypothesis 133 | temp = 1-MB_birth(i).r+MB_birth(i).r*(1-Pd); 134 | trajectoryNewMBM((i-1)*(m+1)+1).c = -log(temp); 135 | trajectoryNewMBM((i-1)*(m+1)+1).r = MB_birth(i).r*(1-Pd)/temp; 136 | trajectoryNewMBM((i-1)*(m+1)+1).xpre = MB_birth(i).x; 137 | trajectoryNewMBM((i-1)*(m+1)+1).Ppre = MB_birth(i).P; 138 | trajectoryNewMBM((i-1)*(m+1)+1).x = MB_birth(i).x; 139 | trajectoryNewMBM((i-1)*(m+1)+1).P = MB_birth(i).P; 140 | trajectoryNewMBM((i-1)*(m+1)+1).l = [t,0]; 141 | if nupd==0 142 | trajectoryNewMBM((i-1)*(m+1)+1).a = i; 143 | else 144 | trajectoryNewMBM((i-1)*(m+1)+1).a = trajectoryUpdMBM(end).a+i; 145 | end 146 | trajectoryNewMBM((i-1)*(m+1)+1).beta = MB_birth(i).beta; 147 | trajectoryNewMBM((i-1)*(m+1)+1).epsilon = MB_birth(i).epsilon; 148 | 149 | % measurement update 150 | for j = 1:m 151 | v = z(:,j) - H*MB_birth(i).x; 152 | ck = MB_birth(i).r*Pd*exp(-0.5*v'/Sk(:,:,i)*v)/sqrt_det2piSk(i); 153 | % If a newly created trajectory with has too small existence probability, 154 | % and the measurement used to create this trajectory does not fall inside 155 | % the gates of any pre-existing tracks, remove it 156 | if ck/(ck+lambda_fa) < model.threshold && unusedMeas(j)==true 157 | idx_remain((i-1)*(m+1)+1+j) = false; 158 | else 159 | usedMeas(j) = true; 160 | trajectoryNewMBM((i-1)*(m+1)+1+j).c =-log(ck); 161 | trajectoryNewMBM((i-1)*(m+1)+1+j).r = 1; 162 | trajectoryNewMBM((i-1)*(m+1)+1+j).xpre = MB_birth(i).x; 163 | trajectoryNewMBM((i-1)*(m+1)+1+j).Ppre = MB_birth(i).P; 164 | trajectoryNewMBM((i-1)*(m+1)+1+j).x = MB_birth(i).x + Kk(:,:,i)*v; 165 | trajectoryNewMBM((i-1)*(m+1)+1+j).P = Pk(:,:,i); 166 | % Otherwise, add the index of the measurement at current scan 167 | trajectoryNewMBM((i-1)*(m+1)+1+j).l = [t,j]; 168 | if nupd==0 169 | trajectoryNewMBM((i-1)*(m+1)+1+j).a = i; 170 | else 171 | trajectoryNewMBM((i-1)*(m+1)+1+j).a = trajectoryUpdMBM(end).a+i; 172 | end 173 | trajectoryNewMBM((i-1)*(m+1)+1+j).beta = MB_birth(i).beta; 174 | trajectoryNewMBM((i-1)*(m+1)+1+j).epsilon = t; 175 | end 176 | end 177 | end 178 | 179 | trajectoryNewMBM = trajectoryNewMBM(idx_remain); 180 | 181 | % Create a track for each used measurement, each track contains two single 182 | % trajectory hypothesis, one representing clutter, one representing the 183 | % measurement is associated to one of the pre-existing tracks 184 | trajectoryClutterMBM = struct('w',0,'r',0,'xpre',zeros(4,1),'x',zeros(4,1),... 185 | 'Ppre',zeros(4,4,1),'P',zeros(4,4,1),'l',[],'c',0,'a',[],'beta',0,'epsilon',0); 186 | trajectoryClutterMBM = repmat(trajectoryClutterMBM,2*m,1); 187 | for j = 1:m 188 | trajectoryClutterMBM(2*j-1).l = [t,0]; 189 | trajectoryClutterMBM(2*j).l = [t,j]; 190 | trajectoryClutterMBM(2*j-1).a = trajectoryNewMBM(end).a+j; 191 | trajectoryClutterMBM(2*j).a = trajectoryNewMBM(end).a+j; 192 | trajectoryClutterMBM(2*j-1).c = 0; 193 | trajectoryClutterMBM(2*j).c = -log(lambda_fa); 194 | end 195 | trajectoryClutterMBM = trajectoryClutterMBM(logical(kron(usedMeas,[1;1]))); 196 | 197 | if nupd==0 198 | trajectoryUpdMBM = [trajectoryNewMBM;trajectoryClutterMBM]; 199 | else 200 | trajectoryUpdMBM = [trajectoryUpdMBM;trajectoryNewMBM;trajectoryClutterMBM]; 201 | end 202 | 203 | end 204 | 205 | -------------------------------------------------------------------------------- /PMBM_Trajectory_Code/mbm01_onlineSmoothing_allTrajectory/dataAssoc.m: -------------------------------------------------------------------------------- 1 | function [trajectoryEst,trajectoryUpdMBM] = dataAssoc(trajectoryUpdMBM,model) 2 | 3 | % num of single target hypotheses 4 | H = length(trajectoryUpdMBM); 5 | 6 | % n: num of tracks 7 | aupd = [trajectoryUpdMBM.a]; 8 | unique_a = unique(aupd,'stable'); 9 | n = length(unique_a); 10 | 11 | % number of single target hypotheses in track i, i belongs {1,...,n} 12 | ns = zeros(n,1); 13 | for i = 1:n 14 | ns(i) = length(find(aupd==unique_a(i))); 15 | end 16 | 17 | % calculate the length of trajectory of each single target hypothesis in 18 | % tracks, used for determine the num of scans should be used 19 | tralen = zeros(H,1); 20 | for i = 1:H 21 | tralen(i) = size(trajectoryUpdMBM(i).l,1); 22 | end 23 | 24 | % cost of single target hypotheses 25 | cupd = [trajectoryUpdMBM.c]'; 26 | c = cupd; 27 | 28 | % construct binary indicator matrix for constraint (1): each track should 29 | % only be used once; this constraint is neccessary for the implementation of dual 30 | % decomposition, since sliding window is used. 31 | A0 = zeros(n,H); 32 | % for each track 33 | idx = 0; 34 | for i = 1:n 35 | A0(i,idx+1:idx+ns(i)) = 1; 36 | idx = idx+ns(i); 37 | end 38 | 39 | maxtralen = max(tralen); % maxtralen-scan data association 40 | tratemp = cell(maxtralen,1); 41 | 42 | % constriant equality 43 | slideWindow = model.slideWindow; % apply sliding window 44 | maxtralen(maxtralen>slideWindow) = slideWindow; 45 | 46 | % construct binary indicator matrix for constraint (2): each measurement in 47 | % each scan should only be used once 48 | A = cell(maxtralen,1); 49 | b = cell(maxtralen,1); 50 | 51 | At = []; 52 | bt = []; 53 | 54 | for tl = 1:maxtralen 55 | % get number of single target hypotheses existed at current-tl+1 scan 56 | Htemp = length(find(tralen>=tl)); 57 | % target trajectory of the current-tl+1 scan, tracks from left to 58 | % right, old to new 59 | tratemp{tl} = arrayfun(@(x) x.l(end-tl+1,2),trajectoryUpdMBM(tralen>=tl)); 60 | % num of measurements in current-tl+1 scan, do not count missed detection and 61 | % non-exist new track 62 | measUnique = unique(tratemp{tl}(tratemp{tl}~=0),'stable'); 63 | mtemp = length(measUnique); 64 | 65 | Atemp = false(mtemp,Htemp); % current-tl+1 scan 66 | for i = 1:mtemp 67 | Atemp(i,tratemp{tl}==measUnique(i)) = true; 68 | end 69 | Atemp = cat(2,Atemp,false(mtemp,H-Htemp)); 70 | btemp = ones(mtemp,1); 71 | 72 | A{tl} = Atemp; 73 | b{tl} = btemp; 74 | 75 | At = cat(1,At,Atemp); 76 | bt = cat(1,bt,btemp); 77 | end 78 | 79 | Amatrix = [A0;At]; 80 | % branch and bound options 81 | options = optimoptions('intlinprog','Display','off'); 82 | 83 | % pre-calculated parameters 84 | measindices = cell(maxtralen,1); 85 | idx_miss = cell(maxtralen,1); 86 | is_preCalculated = cell(maxtralen,1); 87 | tratl = cell(maxtralen,n); 88 | miss = cell(maxtralen,n); 89 | 90 | for tl = 1:maxtralen 91 | num_meas = size(A{tl},1); 92 | measindices{tl} = cell(num_meas,n); 93 | idx_miss{tl} = cell(num_meas,n); 94 | for j = 1:num_meas 95 | idx = 0; 96 | for i = 1:n 97 | % find single target hypotheses in track i that use this 98 | % measurement 99 | is_preCalculated{tl}{j,i} = find(A{tl}(j,idx+1:idx+ns(i))==true); 100 | if ~isempty(is_preCalculated{tl}{j,i}) 101 | measindices{tl}{j,i} = tratemp{tl}(idx+1:idx+ns(i)); 102 | idx_miss{tl}{j,i} = find(measindices{tl}{j,i}==0); 103 | end 104 | idx = idx+ns(i); 105 | end 106 | end 107 | 108 | idx = 0; 109 | for i = 1:n 110 | if size(trajectoryUpdMBM(idx+1).l,1) >= tl 111 | tratl{tl}{i} = tratemp{tl}(idx+1:idx+ns(i)); 112 | miss{tl}{i} = find(tratl{tl}{i}==0); 113 | end 114 | idx = idx+ns(i); 115 | end 116 | end 117 | 118 | %% 119 | % if 1 120 | % dual decomposition, solution is a binary indicator vector, decides which 121 | % single target hypotheses are included in the "best" global hypotheses. 122 | 123 | % subproblem t: min(c/t+\deltat)*u, s.t. [A0;At]*u = [b0;bt]; 124 | 125 | % Larange multiplier \delta is initialised with 0 126 | delta = zeros(H,maxtralen); 127 | % subproblem solutions 128 | u_hat = false(H,maxtralen); 129 | 130 | % initialise maximum num of iteration 131 | numIteration = 0; 132 | maxIteration = 1e1; 133 | numRepetition = false; 134 | % store the best feasible primal cost obtained so far (upper bound) 135 | bestPrimalCost = inf; 136 | uprimal = false(H,1); 137 | 138 | while (numIterationnum_meas) = 0; 172 | indicator = false(H,1); 173 | for i = 1:n 174 | if assignments(i)>0 175 | indicator(idxCost(i,assignments(i))) = true; 176 | end 177 | end 178 | 179 | % if a track has no measurement assigned to it, choose the single 180 | % target hypotheses to be non-exist or miss if the track exists 181 | % before scan N-tl, (if exists after scan N-tl, of course, no 182 | % measurment would be assigned) 183 | utemp = false(H,1); 184 | utemp(indicator) = true; 185 | 186 | idx = 0; 187 | for i = 1:n 188 | if ~any(utemp(idx+1:idx+ns(i))) 189 | if size(trajectoryUpdMBM(idx+1).l,1) >= tl+1 190 | [~,idxtratl] = min(c_hat(miss{tl}{i}+idx)); 191 | utemp(idx+miss{tl}{i}(idxtratl)) = true; 192 | else 193 | [~,idxmin] = min(c_hat(idx+1:idx+ns(i))); 194 | utemp(idx+idxmin) = true; 195 | end 196 | end 197 | idx = idx+ns(i); 198 | end 199 | u_hat(:,tl) = utemp; 200 | subDualCost(tl) = c_hat'*u_hat(:,tl); 201 | end 202 | % change data from double to binary 203 | u_hat = logical(u_hat); 204 | u_hat_mean = sum(u_hat,2)/maxtralen; 205 | 206 | % All the subproblem solutions are equal means we have found the 207 | % optimal solution 208 | if isempty(find(u_hat_mean~=1&u_hat_mean~=0,1)) 209 | uprimal = u_hat(:,1); 210 | break; 211 | end 212 | 213 | % calculate dual cost 214 | dualCosthat = sum(subDualCost); 215 | 216 | % find partial primal solution without conflicts 217 | idx_selectedHypo = u_hat_mean==1; 218 | idx_selectedHypo(tralen<=slideWindow) = false; 219 | idx_unselectedHypo = ~idx_selectedHypo; 220 | idx_uncertainMeasTracks = sum(Amatrix(:,idx_selectedHypo),2)==0; 221 | % if we are certain about the track, remove the single target 222 | % hypotheses of it 223 | for i = 1:n 224 | if idx_uncertainMeasTracks(i) == false 225 | idx_unselectedHypo(Amatrix(i,:)==1) = false; 226 | end 227 | end 228 | A_uncertain = Amatrix(idx_uncertainMeasTracks,idx_unselectedHypo); 229 | c_uncertain = c(idx_unselectedHypo); 230 | len_c_uncertain = length(c_uncertain); 231 | 232 | % implement branch and bound algorithm to find a feasible solution 233 | uprimal_uncertain = intlinprog(c_uncertain,1:len_c_uncertain,[],[],... 234 | sparse(A_uncertain),ones(size(A_uncertain,1),1),... 235 | zeros(len_c_uncertain,1),ones(len_c_uncertain,1),[],options); 236 | uprimal_uncertain = round(uprimal_uncertain); 237 | 238 | uprimalhat = u_hat_mean==1; 239 | uprimalhat(idx_unselectedHypo) = logical(uprimal_uncertain); 240 | 241 | % obtain primal cost 242 | bestPrimalCosthat = c'*uprimalhat; 243 | 244 | if bestPrimalCosthat < bestPrimalCost 245 | bestPrimalCost = bestPrimalCosthat; 246 | uprimal = uprimalhat; 247 | numRepetition = false; 248 | else 249 | % jump out the loop if the best primal cost obtained does not increase 250 | numRepetition = true; 251 | end 252 | 253 | gap = (bestPrimalCost - dualCosthat)/bestPrimalCost; 254 | if gap < 0.02 255 | % jump out the loop if the gap is too small 256 | break; 257 | end 258 | 259 | % calculate step size used in subgradient methods 260 | % calculate subgradient 261 | g = u_hat - u_hat_mean; 262 | % calculate step size used in subgradient method 263 | stepSize = (bestPrimalCost - dualCosthat)/(norm(g)^2); 264 | % update Lagrange multiplier 265 | delta = delta + stepSize*g; 266 | 267 | % increase index of iterations 268 | numIteration = numIteration+1; 269 | end 270 | 271 | u = uprimal; 272 | 273 | %% 274 | 275 | % single target hypotheses in the ML global association hypotheses updating 276 | % pre-existing tracks 277 | I = u(1:H)==1; 278 | trajectoryEst = trajectoryUpdMBM(I); 279 | 280 | % N-scan pruning 281 | idx = 0; 282 | idx_remain = false(H,1); 283 | nc = slideWindow; % prune null-hypothesis with length no less than nc 284 | for i = 1:length(trajectoryEst) 285 | trajectoryLength = size(trajectoryEst(i).l,1); 286 | if trajectoryLength>=nc && ns(i)==1 && trajectoryEst(i).r==0 287 | % prune tracks with only null-hypothesis with length no less than nc 288 | else 289 | if trajectoryLength>=slideWindow 290 | traCompared = trajectoryEst(i).l(1:end-slideWindow+1,2); 291 | for j = idx+1:idx+ns(i) 292 | % check if measurement history information matches 293 | if isequal(trajectoryUpdMBM(j).l(1:end-slideWindow+1,2),traCompared) 294 | idx_remain(j) = true; 295 | end 296 | end 297 | else 298 | idx_remain(idx+1:idx+ns(i)) = true; 299 | end 300 | end 301 | idx = idx+ns(i); 302 | end 303 | 304 | trajectoryUpdMBM = trajectoryUpdMBM(idx_remain); 305 | 306 | % remove tracks with only non-existence hypothesis 307 | if ~isempty(trajectoryUpdMBM) 308 | aupd = [trajectoryUpdMBM.a]'; 309 | Hupd = length(trajectoryUpdMBM); 310 | idx_remain = true(Hupd,1); 311 | for i = 1:Hupd 312 | if length(find(aupd==trajectoryUpdMBM(i).a))==1 && trajectoryUpdMBM(i).r==0 313 | % prune null-hypothesis 314 | idx_remain(i) = false; 315 | end 316 | end 317 | trajectoryUpdMBM = trajectoryUpdMBM(idx_remain); 318 | end 319 | 320 | end -------------------------------------------------------------------------------- /PMBM_Trajectory_Code/mbm_onlineSmoothing_allTrajectory/dataAssoc.m: -------------------------------------------------------------------------------- 1 | function [trajectoryEst,trajectoryUpdMBM] = dataAssoc(trajectoryUpdMBM,model) 2 | 3 | % num of single target hypotheses 4 | H = length(trajectoryUpdMBM); 5 | 6 | % n: num of tracks 7 | aupd = [trajectoryUpdMBM.a]; 8 | unique_a = unique(aupd,'stable'); 9 | n = length(unique_a); 10 | 11 | % number of single target hypotheses in track i, i belongs {1,...,n} 12 | ns = zeros(n,1); 13 | for i = 1:n 14 | ns(i) = length(find(aupd==unique_a(i))); 15 | end 16 | 17 | % calculate the length of trajectory of each single target hypothesis in 18 | % tracks, used for determine the num of scans should be used 19 | tralen = zeros(H,1); 20 | for i = 1:H 21 | tralen(i) = size(trajectoryUpdMBM(i).l,1); 22 | end 23 | 24 | % cost of single target hypotheses 25 | cupd = [trajectoryUpdMBM.c]'; 26 | c = cupd; 27 | 28 | % construct binary indicator matrix for constraint (1): each track should 29 | % only be used once; this constraint is neccessary for the implementation of dual 30 | % decomposition, since sliding window is used. 31 | A0 = zeros(n,H); 32 | % for each track 33 | idx = 0; 34 | for i = 1:n 35 | A0(i,idx+1:idx+ns(i)) = 1; 36 | idx = idx+ns(i); 37 | end 38 | 39 | maxtralen = max(tralen); % maxtralen-scan data association 40 | tratemp = cell(maxtralen,1); 41 | 42 | % constriant equality 43 | slideWindow = model.slideWindow; % apply sliding window 44 | maxtralen(maxtralen>slideWindow) = slideWindow; 45 | 46 | % construct binary indicator matrix for constraint (2): each measurement in 47 | % each scan should only be used once 48 | A = cell(maxtralen,1); 49 | b = cell(maxtralen,1); 50 | 51 | At = []; 52 | bt = []; 53 | 54 | for tl = 1:maxtralen 55 | % get number of single target hypotheses existed at current-tl+1 scan 56 | Htemp = length(find(tralen>=tl)); 57 | % target trajectory of the current-tl+1 scan, tracks from left to 58 | % right, old to new 59 | tratemp{tl} = arrayfun(@(x) x.l(end-tl+1,2),trajectoryUpdMBM(tralen>=tl)); 60 | % num of measurements in current-tl+1 scan, do not count missed detection and 61 | % non-exist new track 62 | measUnique = unique(tratemp{tl}(tratemp{tl}~=0),'stable'); 63 | mtemp = length(measUnique); 64 | 65 | Atemp = false(mtemp,Htemp); % current-tl+1 scan 66 | for i = 1:mtemp 67 | Atemp(i,tratemp{tl}==measUnique(i)) = true; 68 | end 69 | Atemp = cat(2,Atemp,false(mtemp,H-Htemp)); 70 | btemp = ones(mtemp,1); 71 | 72 | A{tl} = Atemp; 73 | b{tl} = btemp; 74 | 75 | At = cat(1,At,Atemp); 76 | bt = cat(1,bt,btemp); 77 | end 78 | 79 | Amatrix = [A0;At]; 80 | % branch and bound options 81 | options = optimoptions('intlinprog','Display','off'); 82 | 83 | % pre-calculated parameters 84 | measindices = cell(maxtralen,1); 85 | idx_miss = cell(maxtralen,1); 86 | is_preCalculated = cell(maxtralen,1); 87 | tratl = cell(maxtralen,n); 88 | miss = cell(maxtralen,n); 89 | 90 | for tl = 1:maxtralen 91 | num_meas = size(A{tl},1); 92 | measindices{tl} = cell(num_meas,n); 93 | idx_miss{tl} = cell(num_meas,n); 94 | for j = 1:num_meas 95 | idx = 0; 96 | for i = 1:n 97 | % find single target hypotheses in track i that use this 98 | % measurement 99 | is_preCalculated{tl}{j,i} = find(A{tl}(j,idx+1:idx+ns(i))==true); 100 | if ~isempty(is_preCalculated{tl}{j,i}) 101 | measindices{tl}{j,i} = tratemp{tl}(idx+1:idx+ns(i)); 102 | idx_miss{tl}{j,i} = find(measindices{tl}{j,i}==0); 103 | end 104 | idx = idx+ns(i); 105 | end 106 | end 107 | 108 | idx = 0; 109 | for i = 1:n 110 | if size(trajectoryUpdMBM(idx+1).l,1) >= tl 111 | tratl{tl}{i} = tratemp{tl}(idx+1:idx+ns(i)); 112 | miss{tl}{i} = find(tratl{tl}{i}==0); 113 | end 114 | idx = idx+ns(i); 115 | end 116 | end 117 | 118 | %% 119 | % if 1 120 | % dual decomposition, solution is a binary indicator vector, decides which 121 | % single target hypotheses are included in the "best" global hypotheses. 122 | 123 | % subproblem t: min(c/t+\deltat)*u, s.t. [A0;At]*u = [b0;bt]; 124 | 125 | % Larange multiplier \delta is initialised with 0 126 | delta = zeros(H,maxtralen); 127 | % subproblem solutions 128 | u_hat = false(H,maxtralen); 129 | 130 | % initialise maximum num of iteration 131 | numIteration = 0; 132 | maxIteration = 1e1; 133 | numRepetition = false; 134 | % store the best feasible primal cost obtained so far (upper bound) 135 | bestPrimalCost = inf; 136 | uprimal = false(H,1); 137 | 138 | while (numIterationnum_meas) = 0; 172 | indicator = false(H,1); 173 | for i = 1:n 174 | if assignments(i)>0 175 | indicator(idxCost(i,assignments(i))) = true; 176 | end 177 | end 178 | 179 | % if a track has no measurement assigned to it, choose the single 180 | % target hypotheses to be non-exist or miss if the track exists 181 | % before scan N-tl, (if exists after scan N-tl, of course, no 182 | % measurment would be assigned) 183 | utemp = false(H,1); 184 | utemp(indicator) = true; 185 | 186 | idx = 0; 187 | for i = 1:n 188 | if ~any(utemp(idx+1:idx+ns(i))) 189 | if size(trajectoryUpdMBM(idx+1).l,1) >= tl+1 190 | [~,idxtratl] = min(c_hat(miss{tl}{i}+idx)); 191 | utemp(idx+miss{tl}{i}(idxtratl)) = true; 192 | else 193 | [~,idxmin] = min(c_hat(idx+1:idx+ns(i))); 194 | utemp(idx+idxmin) = true; 195 | end 196 | end 197 | idx = idx+ns(i); 198 | end 199 | u_hat(:,tl) = utemp; 200 | subDualCost(tl) = c_hat'*u_hat(:,tl); 201 | end 202 | % change data from double to binary 203 | u_hat = logical(u_hat); 204 | u_hat_mean = sum(u_hat,2)/maxtralen; 205 | 206 | % All the subproblem solutions are equal means we have found the 207 | % optimal solution 208 | if isempty(find(u_hat_mean~=1&u_hat_mean~=0,1)) 209 | uprimal = u_hat(:,1); 210 | break; 211 | end 212 | 213 | % calculate dual cost 214 | dualCosthat = sum(subDualCost); 215 | 216 | % find partial primal solution without conflicts 217 | idx_selectedHypo = u_hat_mean==1; 218 | idx_selectedHypo(tralen<=slideWindow) = false; 219 | idx_unselectedHypo = ~idx_selectedHypo; 220 | idx_uncertainMeasTracks = sum(Amatrix(:,idx_selectedHypo),2)==0; 221 | % if we are certain about the track, remove the single target 222 | % hypotheses of it 223 | for i = 1:n 224 | if idx_uncertainMeasTracks(i) == false 225 | idx_unselectedHypo(Amatrix(i,:)==1) = false; 226 | end 227 | end 228 | A_uncertain = Amatrix(idx_uncertainMeasTracks,idx_unselectedHypo); 229 | c_uncertain = c(idx_unselectedHypo); 230 | len_c_uncertain = length(c_uncertain); 231 | 232 | % implement branch and bound algorithm to find a feasible solution 233 | uprimal_uncertain = intlinprog(c_uncertain,1:len_c_uncertain,[],[],... 234 | sparse(A_uncertain),ones(size(A_uncertain,1),1),... 235 | zeros(len_c_uncertain,1),ones(len_c_uncertain,1),[],options); 236 | uprimal_uncertain = round(uprimal_uncertain); 237 | 238 | uprimalhat = u_hat_mean==1; 239 | uprimalhat(idx_unselectedHypo) = logical(uprimal_uncertain); 240 | 241 | % obtain primal cost 242 | bestPrimalCosthat = c'*uprimalhat; 243 | 244 | if bestPrimalCosthat < bestPrimalCost 245 | bestPrimalCost = bestPrimalCosthat; 246 | uprimal = uprimalhat; 247 | numRepetition = false; 248 | else 249 | % jump out the loop if the best primal cost obtained does not increase 250 | numRepetition = true; 251 | end 252 | 253 | % calculate relative gap between dual cost and primal cost 254 | gap = (bestPrimalCost - dualCosthat)/bestPrimalCost; 255 | if gap < 0.02 256 | % jump out the loop if the gap is too small 257 | break; 258 | end 259 | 260 | % calculate step size used in subgradient methods 261 | % calculate subgradient 262 | g = u_hat - u_hat_mean; 263 | % calculate step size used in subgradient method 264 | stepSize = (bestPrimalCost - dualCosthat)/(norm(g)^2); 265 | % update Lagrange multiplier 266 | delta = delta + stepSize*g; 267 | 268 | % increase index of iterations 269 | numIteration = numIteration+1; 270 | end 271 | 272 | u = uprimal; 273 | 274 | %% 275 | 276 | % single target hypotheses in the ML global association hypotheses updating 277 | % pre-existing tracks 278 | I = u(1:H)==1; 279 | trajectoryEst = trajectoryUpdMBM(I); 280 | 281 | % N-scan pruning 282 | idx = 0; 283 | idx_remain = false(H,1); 284 | nc = slideWindow; 285 | for i = 1:length(trajectoryEst) 286 | trajectoryLength = size(trajectoryEst(i).l,1); 287 | if trajectoryLength>=nc && ns(i)==1 && trajectoryEst(i).r==0 288 | % prune tracks with only null-hypothesis with length no less than nc 289 | else 290 | if trajectoryLength>=slideWindow 291 | traCompared = trajectoryEst(i).l(1:end-slideWindow+1,2); 292 | for j = idx+1:idx+ns(i) 293 | % check if measurement history information matches 294 | if isequal(trajectoryUpdMBM(j).l(1:end-slideWindow+1,2),traCompared) 295 | idx_remain(j) = true; 296 | end 297 | end 298 | else 299 | idx_remain(idx+1:idx+ns(i)) = true; 300 | end 301 | end 302 | idx = idx+ns(i); 303 | end 304 | 305 | trajectoryUpdMBM = trajectoryUpdMBM(idx_remain); 306 | 307 | % find single target hypotheses with existence probability smaller than a 308 | % pre-defined threshold. Check if the pruning of such hypotheses will 309 | % affact the solvability of the multi-scan assignment, if not, prune it 310 | Aupd = At(:,1:H); 311 | Aupd = Aupd(:,idx_remain); 312 | rupd = [trajectoryUpdMBM.r]'; 313 | idx_smallExistenceProb = rupd=1; 315 | idx_highExistenceProb = rupd>=model.threshold; 316 | idx_measHighExistenceProb = sum(Aupd(:,idx_highExistenceProb),2)>=1; 317 | indicator = (idx_measSmallExistenceProb-idx_measHighExistenceProb)==1; 318 | len = size(Aupd,2); 319 | idx_remain = true(len,1); 320 | idx_remain(sum(Aupd.*indicator)>0) = false; 321 | trajectoryUpdMBM = trajectoryUpdMBM(idx_remain); 322 | 323 | % remove tracks with only non-existence or low existence hypothesis 324 | if ~isempty(trajectoryUpdMBM) 325 | aupd = [trajectoryUpdMBM.a]'; 326 | Hupd = length(trajectoryUpdMBM); 327 | idx_remain = true(Hupd,1); 328 | for i = 1:Hupd 329 | if length(find(aupd==trajectoryUpdMBM(i).a))==1 && trajectoryUpdMBM(i).r < model.threshold 330 | idx_remain(i) = false; 331 | end 332 | end 333 | trajectoryUpdMBM = trajectoryUpdMBM(idx_remain); 334 | end 335 | 336 | end -------------------------------------------------------------------------------- /PMBM_Trajectory_Code/gospaMetric/GOSPA code/assign2D.m: -------------------------------------------------------------------------------- 1 | function [col4row,row4col,gain,u,v]=assign2D(C,maximize) 2 | %%ASSIGN2D Solve the two-dimensional assignment problem with a rectangular 3 | % cost matrix C, scanning row-wise. The problem being solved can 4 | % be formulated as minimize (or maximize) 5 | % \sum_{i=1}^{numRow}\sum_{j=1}^{numCol}C_{i,j}*x_{i,j} 6 | % subject to 7 | % \sum_{j=1}^{numCol}x_{i,j}<=1 for all i 8 | % \sum_{i=1}^{numRow}x_{i,j}=1 for all j 9 | % x_{i,j}=0 or 1. 10 | % Assuming that numCol<=numRow. If numCol>numRow, then the 11 | % inequality and inequality conditions are switched. A modified 12 | % Jonker-Volgenant algorithm is used. 13 | % 14 | %INPUTS: C A numRowXnumCol cost matrix that does not contain any NaNs and 15 | % where the largest finite element minus the smallest element is a 16 | % finite quantity (does not overflow) when performing minimization 17 | % and where the smallest finite element minus the largest 18 | % element is finite when performing maximization. Forbidden 19 | % assignments can be given costs of +Inf for minimization and -Inf 20 | % for maximization. 21 | % maximize If true, the minimization problem is transformed into a 22 | % maximization problem. The default if this parameter is omitted 23 | % or an empty matrix is passed is false. 24 | % 25 | %OUTPUTS: col4row A numRowX1 vector where the entry in each element is an 26 | % assignment of the element in that row to a column. 0 27 | % entries signify unassigned rows. If the problem is 28 | % infeasible, this is an empty matrix. 29 | % row4col A numColX1 vector where the entry in each element is an 30 | % assignment of the element in that column to a row. 0 31 | % entries signify unassigned columns. If the problem is 32 | % infeasible, this is an empty matrix. 33 | % gain The sum of the values of the assigned elements in C. If 34 | % the problem is infeasible, this is -1. 35 | % u,v The dual variable for the columns and for the rows. See 36 | % the example below demonstrating how they relate to 37 | % complementary slackness of a transformed problem for 38 | % minimization or maximization. 39 | % 40 | %DEPENDENCIES: None 41 | % 42 | %If the number of rows is <= the number of columns, then every row is 43 | %assigned to one column; otherwise every column is assigned to one row. The 44 | %assignment minimizes the sum of the assigned elements (the gain). 45 | %During minimization, assignments can be forbidden by placing Inf in 46 | %elements. During maximization, assignment can be forbidden by placing -Inf 47 | %in elements. The cost matrix can not contain any -Inf elements during 48 | %minimization nor any +Inf elements during maximization to try to force an 49 | %assignment. If no complete assignment can be made with finite cost, 50 | %then col4row and row4col are empty and gain is set to -1. 51 | % 52 | %Note that the dual variables produced by a shortest path assignment 53 | %algorithm that scans by row are not interchangeable with those of a 54 | %shortest path assignment algorithm that scans by column. Matlab stores 55 | %matrices row-wise. Additionally, the dual variables are only valid for the 56 | %transformed cost matrix on which optimization is actually performed, which 57 | %is not necessarily the original cost matrix provided. 58 | % 59 | %When performing minimization with all positive array elements, the initial 60 | %preprocessing step only changes the returned dual variables by offsetting 61 | %the u values by -min(min(C)). Adding min(min(C)) to the returned u values, 62 | %one can get what the dual variables would have been without the 63 | %preprocessing step. 64 | % 65 | %The algorithm is described in detail in [1] and [2]. 66 | % 67 | %EXAMPLE 1: 68 | % C=[Inf, 2, Inf,Inf,3; 69 | % 7, Inf, 23, Inf,Inf; 70 | % 17, 24, Inf,Inf,Inf; 71 | % Inf, 6, 13, 20, Inf]; 72 | % maximize=false; 73 | % [col4row,row4col,gain,u,v]=assign2D(C,maximize) 74 | %One will get an optimal assignment having a gain of 47. 75 | % 76 | %EXAMPLE 2: 77 | %Here we demonstrate that the dual variables satisfy the complementary 78 | %slackness condition for transformed versions of C. If minimization is 79 | %performed, one subtracts minC=min(0,min(C(:))) from C to get the 80 | %transformed problem for complementary slackness; when performing 81 | %maximization, subtract maxC=max(0,max(C(:))). The complementary slackness 82 | %condition could be forced to hold on C by adding minC or maxC to v. The 83 | %complementary slackness condition is described, for example, in [1]. 84 | % numRows=50; 85 | % numCols=80; 86 | % C=1000*randn(numRows,numCols); 87 | % maximize=false; 88 | % [col4row,~,gain,u,v]=assign2D(C,maximize); 89 | % minC=min(0,min(C(:))); 90 | % slackVal=0; 91 | % for curRow=1:numRows 92 | % %Note that we are using C(curRow,col4row(curRow))-minC during 93 | % %minimization instead of C(curRow,col4row(curRow)). 94 | % slackVal=slackVal+abs((C(curRow,col4row(curRow))-minC)*(C(curRow,col4row(curRow))-minC-v(curRow)-u(col4row(curRow)))); 95 | % end 96 | % slackVal 97 | % %One will see that slackVal is zero (within finite precision limits), 98 | % %which is what the complementary slackness condition says. 99 | % %Switching to maximization with the same matrix: 100 | % maximize=true; 101 | % [col4row,~,gain,u,v]=assign2D(C,maximize); 102 | % slackVal=0; 103 | % maxC=max(0,max(C(:))); 104 | % for curRow=1:numRows 105 | % %Note that we are using C(curRow,col4row(curRow))+maxC during 106 | % %minimization instead of C(curRow,col4row(curRow)). 107 | % slackVal=slackVal+abs((C(curRow,col4row(curRow))-maxC)*(C(curRow,col4row(curRow))-maxC-v(curRow)-u(col4row(curRow)))); 108 | % end 109 | % slackVal 110 | % %slackVal is again zero, within finite precision limits. 111 | % 112 | %EXAMPLE 3: 113 | %This is the example used in [3]. Here, we demonstrate how to form 114 | %assignment tuples from col4row. 115 | % C=[7, 51, 52, 87, 38, 60, 74, 66, 0, 20; 116 | % 50, 12, 0, 64, 8, 53, 0, 46, 76, 42; 117 | % 27, 77, 0, 18, 22, 48, 44, 13, 0, 57; 118 | % 62, 0, 3, 8, 5, 6, 14, 0, 26, 39; 119 | % 0, 97, 0, 5, 13, 0, 41, 31, 62, 48; 120 | % 79, 68, 0, 0, 15, 12, 17, 47, 35, 43; 121 | % 76, 99, 48, 27, 34, 0, 0, 0, 28, 0; 122 | % 0, 20, 9, 27, 46, 15, 84, 19, 3, 24; 123 | % 56, 10, 45, 39, 0, 93, 67, 79, 19, 38; 124 | % 27, 0, 39, 53, 46, 24, 69, 46, 23, 1]; 125 | % [col4row, row4col, gain]=assign2D(C); 126 | % N=size(C,1);%It is a square matrix. 127 | % tuples=zeros(2,N); 128 | % for curRow=1:N 129 | % tuples(1,curRow)=curRow; 130 | % tuples(2,curRow)=col4row(curRow); 131 | % end 132 | % 133 | % gain1=0; 134 | % gain2=0; 135 | % gain3=0; 136 | % for k=1:N 137 | % gain1=gain1+C(k,col4row(k)); 138 | % gain2=gain2+C(row4col(k),k); 139 | % gain3=gain3+C(tuples(1,k),tuples(2,k)); 140 | % end 141 | % [gain,gain1,gain2,gain3] 142 | % tuples 143 | %One will see that all of the gains are the same (0) and the assigned 144 | %tuples match what is in [3]. However, the assigned tuples is NOT obtained 145 | %by attaching col4row to row4col. 146 | % 147 | %REFERENCES: 148 | %[1] D. F. Crouse, "On Implementing 2D Rectangular Assignment Algorithms," 149 | % IEEE Transactions on Aerospace and Electronic Systems, vol. 52, no. 4, 150 | % pp. 1679-1696, Aug. 2016. 151 | %[2] D. F. Crouse, "Advances in displaying uncertain estimates of multiple 152 | % targets," in Proceedings of SPIE: Signal Processing, Sensor Fusion, 153 | % and Target Recognition XXII, vol. 8745, Baltimore, MD, Apr. 2013. 154 | %[3] Murty, K. G. "An algorithm for ranking all the assignments in order of 155 | % increasing cost," Operations Research, vol. 16, no. 3, pp. 682-687, 156 | % May-Jun. 1968. 157 | % 158 | %October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C. 159 | %(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release. 160 | 161 | if(nargin<2||isempty(maximize)) 162 | maximize=false; 163 | end 164 | 165 | numRow=size(C,1); 166 | numCol=size(C,2); 167 | 168 | didFlip=false; 169 | if(numCol>numRow) 170 | C=C'; 171 | temp=numRow; 172 | numRow=numCol; 173 | numCol=temp; 174 | didFlip=true; 175 | end 176 | 177 | %The cost matrix must have all non-negative elements for the assignment 178 | %algorithm to work. This forces all of the elements to be positive. The 179 | %delta is added back in when computing the gain in the end. 180 | if(maximize==true) 181 | CDelta=max(C(:)); 182 | 183 | %If C is all negative, do not shift. 184 | if(CDelta<0) 185 | CDelta=0; 186 | end 187 | 188 | C=-C+CDelta; 189 | else 190 | CDelta=min(C(:)); 191 | 192 | %If C is all positive, do not shift. 193 | if(CDelta>0) 194 | CDelta=0; 195 | end 196 | 197 | C=C-CDelta; 198 | end 199 | 200 | %These store the assignment as it is made. 201 | col4row=zeros(numRow,1); 202 | row4col=zeros(numCol,1); 203 | u=zeros(numCol,1);%The dual variable for the columns 204 | v=zeros(numRow,1);%The dual variable for the rows. 205 | 206 | %Initially, none of the columns are assigned. 207 | for curUnassCol=1:numCol 208 | %This finds the shortest augmenting path starting at k and returns 209 | %the last node in the path. 210 | [sink,pred,u,v]=ShortestPath(curUnassCol,u,v,C,col4row,row4col); 211 | 212 | %If the problem is infeasible, mark it as such and return. 213 | if(sink==0) 214 | col4row=[]; 215 | row4col=[]; 216 | gain=-1; 217 | return; 218 | end 219 | 220 | %We have to remove node k from those that must be assigned. 221 | j=sink; 222 | while(1) 223 | i=pred(j); 224 | col4row(j)=i; 225 | h=row4col(i); 226 | row4col(i)=j; 227 | j=h; 228 | 229 | if(i==curUnassCol) 230 | break; 231 | end 232 | end 233 | end 234 | 235 | %Calculate the gain that should be returned. 236 | if(nargout>2) 237 | gain=0; 238 | for curCol=1:numCol 239 | gain=gain+C(row4col(curCol),curCol); 240 | end 241 | 242 | %Adjust the gain for the initial offset of the cost matrix. 243 | if(maximize) 244 | gain=-gain+CDelta*numCol; 245 | u=-u; 246 | v=-v; 247 | else 248 | gain=gain+CDelta*numCol; 249 | end 250 | end 251 | 252 | if(didFlip==true) 253 | temp=row4col; 254 | row4col=col4row; 255 | col4row=temp; 256 | 257 | temp=u; 258 | u=v; 259 | v=temp; 260 | end 261 | end 262 | 263 | function [sink, pred, u, v]=ShortestPath(curUnassCol,u,v,C,col4row,row4col) 264 | numRow=size(C,1); 265 | numCol=size(C,2); 266 | pred=zeros(numCol,1); 267 | 268 | %Initially, none of the rows and columns have been scanned. 269 | %This will store a 1 in every column that has been scanned. 270 | ScannedCols=zeros(numCol,1); 271 | 272 | %This will store a 1 in every row that has been scanned. 273 | ScannedRow=zeros(numRow,1); 274 | Row2Scan=1:numRow; 275 | numRow2Scan=numRow; 276 | 277 | sink=0; 278 | delta=0; 279 | curCol=curUnassCol; 280 | shortestPathCost=Inf(numRow,1); 281 | 282 | while(sink==0) 283 | %Mark the current column as having been visited. 284 | ScannedCols(curCol)=1; 285 | 286 | %Scan all of the rows that have not already been scanned. 287 | minVal=Inf; 288 | for curRowScan=1:numRow2Scan 289 | curRow=Row2Scan(curRowScan); 290 | 291 | reducedCost=delta+C(curRow,curCol)-u(curCol)-v(curRow); 292 | if(reducedCost 0; 14 | trajectoryEst = trajectoryNewMBM(idx_r_new); 15 | trajectoryOutMBM = trajectoryNewMBM; 16 | return; 17 | end 18 | 19 | % extract single target hypothesis index 20 | aupd = [trajectoryUpdMBM.a]; 21 | % find uniques ones 22 | unique_aupd = unique(aupd,'stable'); 23 | 24 | % npre: num of pre-existing tracks; num of new tracks = num of meas at current scan 25 | npre = length(unique_aupd); 26 | % number of single target hypotheses in pre-exist track i, i belongs {1,...,npre} 27 | ns = zeros(npre,1); 28 | for i = 1:npre 29 | ns(i) = length(find(aupd==unique_aupd(i))); 30 | end 31 | 32 | % calculate the length of trajectory of each single target hypothesis in 33 | % pre-existing tracks, used for determine the num of scans should be used 34 | tralen = zeros(Hpre,1); 35 | for i = 1:Hpre 36 | tralen(i) = size(trajectoryUpdMBM(i).l,1); 37 | end 38 | 39 | % cost of single target hypotheses 40 | c = [trajectoryUpdMBM.c trajectoryNewMBM.c]'; 41 | 42 | % construct binary indicator matrix for constraint (1): each track should 43 | % only be used once, each measurement received in current scan creates a 44 | % new track; this constraint is neccessary for the implementation of dual 45 | % decomposition, since sliding window is used. 46 | A0 = zeros(npre+mcur,H); 47 | % for each pre-existing track 48 | idx = 0; 49 | for i = 1:npre 50 | A0(i,idx+1:idx+ns(i)) = 1; 51 | idx = idx+ns(i); 52 | end 53 | % for each new track 54 | for i = 1:mcur 55 | A0(npre+i,idx+1:idx+2) = 1; 56 | idx = idx+2; 57 | end 58 | 59 | maxtralen = max(tralen); % maxtralen-scan data association 60 | tratemp = cell(maxtralen,1); 61 | 62 | % construct binary indicator matrix for constraint (2): each measurement in 63 | % each scan should only be used once 64 | % target trajectory of the current scan 65 | tratemp{1} = arrayfun(@(x) x.l(end,2),trajectoryUpdMBM); 66 | tcur = [tratemp{1};arrayfun(@(x) x.l(2),trajectoryNewMBM)]; 67 | 68 | At = false(mcur,H); 69 | for i = 1:mcur 70 | At(i,tcur==i) = true; 71 | end 72 | 73 | % constriant equality 74 | bt = ones(mcur,1); 75 | 76 | slideWindow = model.slideWindow; % apply sliding window 77 | maxtralen(maxtralen>slideWindow) = slideWindow; 78 | 79 | A = cell(maxtralen,1); 80 | b = cell(maxtralen,1); 81 | 82 | A{1} = At; 83 | b{1} = bt; 84 | 85 | for tl = 2:maxtralen 86 | % get number of single target hypotheses existed at current-tl+1 scan 87 | Htemp = length(find(tralen>=tl)); 88 | % target trajectory of the current-tl+1 scan, tracks from left to right, old to new 89 | tratemp{tl} = arrayfun(@(x) x.l(end-tl+1,2),trajectoryUpdMBM(tralen>=tl)); 90 | % num of measurements in current-tl+1 scan, do not count missed detection and non-exist new track 91 | measUnique = unique(tratemp{tl}(tratemp{tl}~=0),'stable'); 92 | mtemp = length(measUnique); 93 | 94 | Atemp = false(mtemp,Htemp); % current-tl+1 scan 95 | for i = 1:mtemp 96 | Atemp(i,tratemp{tl}==measUnique(i)) = true; 97 | end 98 | Atemp = cat(2,Atemp,false(mtemp,H-Htemp)); 99 | btemp = ones(mtemp,1); 100 | 101 | A{tl} = Atemp; 102 | b{tl} = btemp; 103 | 104 | At = cat(1,At,Atemp); 105 | bt = cat(1,bt,btemp); 106 | end 107 | 108 | Amatrix = [A0;At]; 109 | % branch and bound options, disable displaying result information 110 | options = optimoptions('intlinprog','Display','off'); 111 | 112 | % pre-calculate some parameters to speed up implementation 113 | measindices = cell(maxtralen,1); 114 | idx_miss = cell(maxtralen,1); 115 | is_preCalculated = cell(maxtralen,1); 116 | tratl = cell(maxtralen,npre); 117 | missornull = cell(maxtralen,npre); 118 | 119 | for tl = 1:maxtralen 120 | num_meas = size(A{tl},1); 121 | measindices{tl} = cell(num_meas,npre); 122 | idx_miss{tl} = cell(num_meas,npre); 123 | for j = 1:num_meas 124 | idx = 0; 125 | for i = 1:npre 126 | % find single target hypotheses in track i that use this 127 | % measurement 128 | is_preCalculated{tl}{j,i} = find(A{tl}(j,idx+1:idx+ns(i))==true); 129 | if ~isempty(is_preCalculated{tl}{j,i}) 130 | measindices{tl}{j,i} = tratemp{tl}(idx+1:idx+ns(i)); 131 | idx_miss{tl}{j,i} = find(measindices{tl}{j,i}==0); 132 | end 133 | idx = idx+ns(i); 134 | end 135 | end 136 | idx = 0; 137 | for i = 1:npre 138 | if size(trajectoryUpdMBM(idx+1).l,1) >= tl+1 139 | tratl{tl}{i} = tratemp{tl}(idx+1:idx+ns(i)); 140 | missornull{tl}{i} = find(tratl{tl}{i}==0); 141 | end 142 | idx = idx+ns(i); 143 | end 144 | end 145 | 146 | %% 147 | 148 | % dual decomposition, solution is a binary indicator vector, decides which 149 | % single target hypotheses are included in the "best" global hypotheses. 150 | 151 | % subproblem t: min(c/t+\deltat)*u, s.t. [A0;At]*u = [b0;bt]; 152 | 153 | % Larange multiplier \delta is initialised with 0 154 | delta = zeros(H,maxtralen); 155 | % subproblem solutions 156 | u_hat = false(H,maxtralen); 157 | 158 | % initialise maximum num of iteration 159 | numIteration = 0; 160 | maxIteration = 1e1; 161 | numRepetition = false; % indicate whether to jump out the loop or not 162 | % store the best feasible primal cost obtained so far (upper bound) 163 | bestPrimalCost = inf; 164 | uprimal = false(H,1); 165 | 166 | while (numIterationnum_meas) = 0; 213 | indicator = false(H,1); 214 | for i = 1:npre+mcur 215 | if assignments(i)>0 216 | indicator(idxCost(i,assignments(i))) = true; 217 | end 218 | end 219 | 220 | % if a track has no measurement assigned to it, choose the single 221 | % target hypotheses to be non-exist or miss if the track exists 222 | % before scan N-tl, (if exists after scan N-tl, of course, no 223 | % measurment would be assigned) 224 | utemp = false(H,1); 225 | utemp(indicator) = true; 226 | 227 | idx = 0; 228 | % process pre-existing tracks 229 | for i = 1:npre 230 | if ~any(utemp(idx+1:idx+ns(i))) 231 | if size(trajectoryUpdMBM(idx+1).l,1) >= tl+1 232 | [~,idxtratl] = min(c_hat(missornull{tl}{i}+idx)); 233 | utemp(idx+missornull{tl}{i}(idxtratl)) = true; 234 | else 235 | [~,idxmin] = min(c_hat(idx+1:idx+ns(i))); 236 | utemp(idx+idxmin) = true; 237 | end 238 | end 239 | idx = idx+ns(i); 240 | end 241 | % process new tracks 242 | for i = 1:mcur 243 | if ~any(utemp(idx+1:idx+2)) 244 | [~,idxmin] = min(c_hat(idx+1:idx+2)); 245 | utemp(idx+idxmin) = true; 246 | end 247 | idx = idx+2; 248 | end 249 | u_hat(:,tl) = utemp; 250 | subDualCost(tl) = c_hat'*u_hat(:,tl); 251 | end 252 | % change data from double to binary 253 | u_hat = logical(u_hat); 254 | u_hat_mean = sum(u_hat,2)/maxtralen; 255 | 256 | % All the subproblem solutions are equal means we have found the 257 | % optimal solution 258 | if isempty(find(u_hat_mean~=1&u_hat_mean~=0,1)) 259 | uprimal = u_hat(:,1); 260 | break; 261 | end 262 | 263 | % calculate dual cost 264 | dualCosthat = sum(subDualCost); 265 | 266 | % find partial primal solution without conflicts 267 | idx_selectedHypo = u_hat_mean==1; 268 | idx_selectedHypo(tralen<=slideWindow) = false; 269 | idx_unselectedHypo = ~idx_selectedHypo; 270 | idx_uncertainMeasTracks = sum(Amatrix(:,idx_selectedHypo),2)==0; 271 | % if we are certain about the track, remove the single target 272 | % hypotheses of it 273 | for i = 1:npre 274 | if idx_uncertainMeasTracks(i) == false 275 | idx_unselectedHypo(Amatrix(i,:)==1) = false; 276 | end 277 | end 278 | A_uncertain = Amatrix(idx_uncertainMeasTracks,idx_unselectedHypo); 279 | c_uncertain = c(idx_unselectedHypo); 280 | len_c_uncertain = length(c_uncertain); 281 | 282 | % implement branch and bound algorithm to find a feasible solution 283 | uprimal_uncertain = intlinprog(c_uncertain,1:len_c_uncertain,[],[],... 284 | sparse(A_uncertain),ones(size(A_uncertain,1),1),... 285 | zeros(len_c_uncertain,1),ones(len_c_uncertain,1),[],options); 286 | uprimal_uncertain = round(uprimal_uncertain); 287 | 288 | uprimalhat = u_hat_mean==1; 289 | uprimalhat(idx_unselectedHypo) = logical(uprimal_uncertain); 290 | 291 | % obtain primal cost 292 | bestPrimalCosthat = c'*uprimalhat; 293 | 294 | if bestPrimalCosthat < bestPrimalCost 295 | bestPrimalCost = bestPrimalCosthat; 296 | uprimal = uprimalhat; 297 | numRepetition = false; 298 | else 299 | % jump out the loop if the best primal cost obtained does not increase 300 | numRepetition = true; 301 | end 302 | 303 | % calculate relative gap between dual cost and primal cost 304 | gap = (bestPrimalCost - dualCosthat)/bestPrimalCost; 305 | if gap < 0.02 306 | % jump out the loop if the gap is too small 307 | uprimal = uprimalhat; 308 | break; 309 | end 310 | 311 | % calculate step size used in subgradient methods 312 | % calculate subgradient 313 | g = u_hat - u_hat_mean; 314 | % calculate step size used in subgradient method 315 | stepSize = (bestPrimalCost - dualCosthat)/(norm(g)^2); 316 | % update Lagrange multiplier 317 | delta = delta + stepSize*g; 318 | 319 | % increase index of iterations 320 | numIteration = numIteration+1; 321 | end 322 | 323 | u = uprimal; 324 | 325 | %% 326 | 327 | % single target hypotheses in the ML global association hypotheses updating 328 | % pre-existing tracks 329 | I = u(1:Hpre)==1; 330 | trajectoryEst = trajectoryUpdMBM(I); 331 | 332 | % N-scan pruning 333 | idx_remain = false(Hpre,1); 334 | idx = 0; 335 | nc = slideWindow; 336 | for i = 1:length(trajectoryEst) 337 | trajectoryLength = size(trajectoryEst(i).l,1); 338 | if trajectoryLength>=nc && ns(i)==1 && trajectoryEst(i).r==0 339 | % prune tracks with only null-hypothesis with length no less than nc 340 | else 341 | if trajectoryLength>=slideWindow 342 | traCompared = trajectoryEst(i).l(1:end-slideWindow+1,2); 343 | for j = idx+1:idx+ns(i) 344 | % check if measurement history information matches 345 | if isequal(trajectoryUpdMBM(j).l(1:end-slideWindow+1,2),traCompared) 346 | idx_remain(j) = true; 347 | end 348 | end 349 | else 350 | idx_remain(idx+1:idx+ns(i)) = true; 351 | end 352 | end 353 | idx = idx+ns(i); 354 | end 355 | 356 | trajectoryUpdMBM = trajectoryUpdMBM(idx_remain); 357 | 358 | % find single target hypotheses with existence probability smaller than a 359 | % pre-defined threshold. Check if the pruning of such hypotheses will 360 | % affect the solvability of the multi-scan assignment, if not, prune it 361 | Aupd = At(:,1:Hpre); 362 | Aupd = Aupd(:,idx_remain); 363 | rupd = [trajectoryUpdMBM.r]'; 364 | idx_smallExistenceProb = rupd=1; 366 | idx_highExistenceProb = rupd>=model.threshold; 367 | idx_measHighExistenceProb = sum(Aupd(:,idx_highExistenceProb),2)>=1; 368 | indicator = (idx_measSmallExistenceProb-idx_measHighExistenceProb)==1; 369 | len = size(Aupd,2); 370 | idx_remain = true(len,1); 371 | idx_remain(sum(Aupd.*indicator)>0) = false; 372 | trajectoryUpdMBM = trajectoryUpdMBM(idx_remain); 373 | 374 | % remove tracks with only non-existence or low existence hypothesis 375 | if ~isempty(trajectoryUpdMBM) 376 | aupd = [trajectoryUpdMBM.a]'; 377 | Hupd = length(trajectoryUpdMBM); 378 | idx_remain = true(Hupd,1); 379 | for i = 1:Hupd 380 | if length(find(aupd==trajectoryUpdMBM(i).a))==1 && trajectoryUpdMBM(i).r < model.threshold 381 | idx_remain(i) = false; 382 | end 383 | end 384 | trajectoryUpdMBM = trajectoryUpdMBM(idx_remain); 385 | end 386 | 387 | % single target hypotheses in the ML global association hypotheses 388 | % updating new tracks 389 | Inew = u(Hpre+1:H)==1; 390 | trajectoryEst = [trajectoryEst;trajectoryNewMBM(Inew)]; 391 | idx_r = [trajectoryEst.r]' > 0; 392 | trajectoryEst = trajectoryEst(idx_r); 393 | trajectoryOutMBM = [trajectoryUpdMBM;trajectoryNewMBM]; 394 | 395 | end -------------------------------------------------------------------------------- /PMBM_Trajectory_Code/trajectoryMetric/LPTrajMetric_sparse.m: -------------------------------------------------------------------------------- 1 | function [dxy, loc_cost, miss_cost, fa_cost, switch_cost]=LPTrajMetric_sparse(X, Y, c, p, gamma) 2 | 3 | 4 | %% function [dxy, loc_cost, miss_cost, fa_cost, switch_cost = LPTrajMetric(X, Y, c, p, gamma) 5 | % This function computes the LP metric between sets of trajectories defined 6 | % in https://arxiv.org/abs/1605.01177. 7 | % ------------------------------------------------------------------------- 8 | % Input: 9 | % X, Y: sets of trajctories which are structs as follows: 10 | % X.tVec: 'nx x 1' dimensional vector that has start times of the 'nx' 11 | % trajectories in 'X'. 12 | % X.iVec: 'nx x 1' dimensional vector that has the duration of the 'nx' 13 | % trajectories in 'X'. 14 | % X.xState: 'stDim x T x nx' dimensional matrix, where 'stDim' is the 15 | % state dimension, 'T' is the max length of the trajectories. The 16 | % states of trajectory 'ind', 'X.xState(:, :, ind)' has '0' values 17 | % outisde '[X.tVec(ind), X.tVec(ind)+X.iVec(ind)-1]'. Note that within the 18 | % window where X.xState is valid can have 'holes', with 'nan' values. 19 | % c: >0, cut-off parameter 20 | % p: >= 1, exponent parameter 21 | % gamma: >0, track switch penalty 22 | % ------------------------------------------------------------------------- 23 | % Output: 24 | % dxy: Metric value 25 | % loc_cost: localisation cost (to the p-th power) for properly detected targets over time of dimension 'T x 1' 26 | % miss_cost: cost (to the p-th power) for missed targets over time, dimension 'Tx1' 27 | % fa_cost: cost (to the p-th power) for false targets over time, dimension 'Tx1' 28 | % switch_cost: cost (to the p-th power) for switches over time, dimension '(T-1)x1' 29 | % ------------------------------------------------------------------------- 30 | % Authors: Abu S. Rahmatullah and Ángel F. García-Fernández 31 | % This code uses clustering to compute the solution 32 | 33 | 34 | %%%%%%%%%% Parameters of use %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 35 | nx = size(X.xState, 3); 36 | ny = size(Y.xState, 3); 37 | T = size(X.xState, 2); 38 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 39 | if(nx==0 && ny==0) 40 | dxy=0; 41 | loc_cost=zeros(T,1); 42 | miss_cost=zeros(T,1); 43 | fa_cost=zeros(T,1); 44 | switch_cost=zeros(T-1,1); 45 | return; 46 | end 47 | 48 | 49 | 50 | %%%%%%%%%% localisation cost computation %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 51 | DAB = locCostComp_v2(X, Y, c, p); 52 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 53 | 54 | %Clustering 55 | G=zeros(nx,ny); 56 | T_min_a=zeros(nx,ny); %Matrices with minimum and maximum times in which trajectories i and j can be associated 57 | T_max_a=zeros(nx,ny); 58 | for i=1:nx 59 | t_i_x=X.tVec(i); 60 | t_f_x=X.tVec(i)+X.iVec(i)-1; 61 | for j=1:ny 62 | t_i_y=Y.tVec(j); 63 | t_f_y=Y.tVec(j)+Y.iVec(j)-1; 64 | t_i_max=max(t_i_x,t_i_y); 65 | t_f_min=min(t_f_x,t_f_y); 66 | %Option 1 67 | G_ij=squeeze(DAB(i,j,t_i_max:t_f_min))>=c^p; 68 | isnan_ij=or(isnan(X.xState(1,t_i_max:t_f_min,i)),isnan(Y.xState(1,t_i_max:t_f_min,j)))'; %We need to check when any of the trajectories isnan in this interval 69 | sum_isnan_ij=sum(isnan_ij); 70 | G(i,j)=sum(G_ij)==max(t_f_min-t_i_max+1-sum_isnan_ij,0); 71 | t_min_ij=find(and(G_ij==0,isnan_ij==0)); %They can be associated if this value is zero 72 | if(~isempty(t_min_ij)) 73 | T_min_a(i,j)= t_min_ij(1)+t_i_max-1; 74 | T_max_a(i,j)= t_min_ij(end)+t_i_max-1; 75 | else 76 | T_min_a(i,j)= T+1; % We put infeasible values 77 | T_max_a(i,j)= 0; 78 | 79 | end 80 | end 81 | end 82 | 83 | 84 | %Graph connectivity 85 | G_con=not(G(1:nx,1:ny)); 86 | Adj_matrix=[eye(nx),G_con;G_con',eye(ny)]; 87 | %Clustering 88 | r = fliplr(symrcm(Adj_matrix)); 89 | Clusters = {r(1)}; 90 | 91 | max_length_c=1; 92 | for i = 2:length(r) 93 | if any(Adj_matrix(Clusters{end},r(i))) 94 | Clusters{end}(end+1) = r(i); 95 | else 96 | Clusters{end+1} = r(i); 97 | end 98 | if(length(Clusters{end})>max_length_c) 99 | max_length_c=length(Clusters{end}); 100 | end 101 | end 102 | 103 | 104 | 105 | loc_cost=zeros(T,1); 106 | miss_cost=zeros(T,1); 107 | fa_cost=zeros(T,1); 108 | switch_cost=zeros(T-1,1); 109 | dxy=0; 110 | for i=1:length(Clusters) 111 | Cluster_i=Clusters{i}; 112 | if(length(Cluster_i)==1) 113 | %The calculations simplify if there is only one trajectory in the 114 | %cluster 115 | i_x=find(Cluster_i<=nx); 116 | if(isempty(i_x)) 117 | %Then it is a trajectory in Y 118 | i_y=find(Cluster_i>nx); 119 | list_y=Cluster_i(i_y)-nx; 120 | t_axis=Y.tVec(list_y):Y.tVec(list_y)+ Y.iVec(list_y)-1; 121 | isrealY_i=~isnan(Y.xState(1, t_axis,list_y))'; 122 | dxy_i=c^p/2*sum(isrealY_i); 123 | fa_cost(t_axis)=fa_cost(t_axis)+c^p/2*isrealY_i; 124 | else 125 | list_x=Cluster_i(i_x); 126 | t_axis=X.tVec(list_x):X.tVec(list_x)+ X.iVec(list_x)-1; 127 | isrealX_i=~isnan(X.xState(1,t_axis,list_x))'; 128 | dxy_i=c^p/2*sum(isrealX_i); 129 | miss_cost(t_axis)=miss_cost(t_axis)+c^p/2*isrealX_i; 130 | end 131 | elseif(length(Cluster_i)==2) 132 | i_x=find(Cluster_i<=nx); 133 | list_x=Cluster_i(i_x); 134 | i_y=find(Cluster_i>nx); 135 | list_y=Cluster_i(i_y)-nx; 136 | dxy_i=sum(DAB(list_x,list_y,:)); 137 | 138 | t_axis=X.tVec(list_x):X.tVec(list_x)+ X.iVec(list_x)-1; 139 | isnan_X=isnan(X.xState(1,:,list_x))'; 140 | no_exist_X=ones(T,1); 141 | no_exist_X(t_axis)=0; 142 | 143 | t_axis=Y.tVec(list_y):Y.tVec(list_y)+ Y.iVec(list_y)-1; 144 | isnan_Y=isnan(Y.xState(1,:,list_y))'; 145 | no_exist_Y=ones(T,1); 146 | no_exist_Y(t_axis)=0; 147 | 148 | DAB_i=squeeze(DAB(list_x,list_y,:)); 149 | 150 | %Errors equal to c^p correspond to false and missed target costs 151 | index1=DAB_i==c^p; 152 | fa_cost(index1)=fa_cost(index1)+c^p/2; 153 | miss_cost(index1)=miss_cost(index1)+c^p/2; 154 | 155 | %Missed targets 156 | index2=and(DAB_i==c^p/2,or(isnan_X,no_exist_X)); 157 | fa_cost(index2)=fa_cost(index2)+c^p/2; 158 | 159 | %False targets 160 | index3=and(DAB_i==c^p/2,or(isnan_Y,no_exist_Y)); 161 | miss_cost(index3)=miss_cost(index3)+c^p/2; 162 | 163 | %Localisation cost 164 | index4=and(DAB_i>0,not(or(index1, or(index2,index3)))); 165 | 166 | loc_cost(index4)=loc_cost(index4)+DAB_i(index4); 167 | 168 | else 169 | 170 | 171 | i_x=find(Cluster_i<=nx); 172 | list_x=Cluster_i(i_x); 173 | i_y=find(Cluster_i>nx); 174 | list_y=Cluster_i(i_y)-nx; 175 | 176 | 177 | X_i.tVec=X.tVec(list_x); 178 | X_i.iVec=X.iVec(list_x); 179 | 180 | Y_i.tVec=Y.tVec(list_y); 181 | Y_i.iVec=Y.iVec(list_y); 182 | 183 | %Calculate minimum and maximum times when we need to consider the 184 | %assignments 185 | t_min=min(min(T_min_a(list_x,list_y))); 186 | t_max=max(max(T_max_a(list_x,list_y))); 187 | 188 | 189 | tf_X=X_i.tVec+X_i.iVec-1; 190 | tf_Y=Y_i.tVec+Y_i.iVec-1; 191 | 192 | 193 | %We sum the costs outside the considered window 194 | miss_cost_i=zeros(size(miss_cost)); 195 | fa_cost_i=zeros(size(fa_cost)); 196 | isrealX_i=~isnan(X.xState(1,:,list_x)); 197 | isrealY_i=~isnan(Y.xState(1,:,list_y)); 198 | 199 | for j=1:length(X_i.tVec) 200 | t_axis=[X_i.tVec(j):t_min-1, t_max+1:tf_X(j)]; 201 | miss_cost_i(t_axis)=miss_cost_i(t_axis)+c^p/2*squeeze(isrealX_i(1,t_axis,j))'; 202 | end 203 | 204 | for j=1:length(Y_i.tVec) 205 | t_axis=[Y_i.tVec(j):t_min-1, t_max+1:tf_Y(j)]; 206 | fa_cost_i(t_axis)=fa_cost_i(t_axis)+c^p/2*squeeze(isrealY_i(1,t_axis,j))'; 207 | end 208 | 209 | X_i.xState=X.xState(:,t_min:t_max,list_x); 210 | Y_i.xState=Y.xState(:,t_min:t_max,list_y); 211 | 212 | ti_X=X_i.tVec; 213 | ti_Y=Y_i.tVec; 214 | 215 | 216 | X_i.tVec=max(ti_X-t_min+1,1); 217 | Y_i.tVec=max(ti_Y-t_min+1,1); 218 | 219 | X_i.iVec=X_i.iVec-max(t_min-ti_X,0)-max(tf_X-t_max,0); 220 | Y_i.iVec=Y_i.iVec-max(t_min-ti_Y,0)-max(tf_Y-t_max,0); 221 | 222 | T_i=t_max-t_min+1; 223 | DAB_i=DAB([list_x,nx+1],[list_y,ny+1],t_min:t_max); 224 | 225 | nx_i=length(list_x); 226 | ny_i=length(list_y); 227 | nxny_i=nx_i*ny_i; 228 | nxny2_i=(nx_i+1)*(ny_i+1); 229 | 230 | 231 | 232 | [dxy_i,loc_cost_i2, miss_cost_i2, fa_cost_i2, switch_cost_i2]=LP_metric_cluster(X_i,Y_i,DAB_i,nx_i,ny_i,nxny_i,nxny2_i,T_i,c,p,gamma); 233 | dxy_i=dxy_i+sum(miss_cost_i)+sum(fa_cost_i); 234 | 235 | 236 | %We add the false and missed target costs of the tails 237 | 238 | miss_cost_i(t_min:t_max)=miss_cost_i(t_min:t_max)+miss_cost_i2; 239 | fa_cost_i(t_min:t_max)=fa_cost_i(t_min:t_max)+fa_cost_i2; 240 | 241 | loc_cost(t_min:t_max)=loc_cost(t_min:t_max)+loc_cost_i2; 242 | switch_cost(t_min:t_max-1)=switch_cost(t_min:t_max-1)+switch_cost_i2; 243 | miss_cost=miss_cost+miss_cost_i; 244 | fa_cost=fa_cost+fa_cost_i; 245 | 246 | end 247 | dxy=dxy+dxy_i; 248 | 249 | end 250 | dxy = dxy.^(1/p); 251 | 252 | end 253 | 254 | 255 | 256 | 257 | function [dxy,loc_cost, miss_cost, fa_cost, switch_cost]=LP_metric_cluster(X,Y,DAB,nx,ny,nxny,nxny2,T,c,p,gamma) 258 | %%%%%%%%%% variables to be estimated in LP %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 259 | % x = [W_1,1(1) W_2,1(1) .. W_nx+1,1(1), .. W_1,ny+1(1) W_2,ny+1(1) ... 260 | % W_nx+1,ny+1(1), W_1,1(T) W_2,1(T) .. W_nx+1,1(T), .. W_1,ny+1(T) 261 | % W_2,ny+1(T) ... W_nx+1,ny+1(T) e(1) .. e(T-1) h_11(1) .. h_nx,ny(1) ... 262 | % h_1,1(T-1) ... h_nx,ny(T-1)]' 263 | 264 | %%% Length of the variable components in x 265 | WtLen = nxny2*T; etLen = T-1; htLen = nxny * (T-1); 266 | nParam = WtLen + etLen + htLen; % total number of variables 267 | 268 | %%% Position of the variable components in x 269 | WtPos = (1:WtLen); etPos = WtLen+(1:etLen); 270 | htPos = WtLen + etLen + (1:htLen); 271 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 272 | 273 | 274 | %%%%%%%%%% objective function f %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 275 | f = zeros(nParam, 1); 276 | f(WtPos) = reshape(DAB, [WtLen,1]); % for vec(W(1)) to vec(W(T)), loc cost 277 | f(etPos) = 0.5 * gamma^p * ones(T-1,1); %for e(1) to e(T-1), switch cost 278 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 279 | 280 | 281 | %%%%%%%%%% equality constraints %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 282 | %Constraint 1 283 | index_x=repmat(1:T*ny,nx+1,1); 284 | index_x=index_x(:); 285 | index_y=zeros(T*ny*(nx+1),1); 286 | index_rep=1:ny*(nx+1); 287 | for i=1:T 288 | index_y(index_rep+(i-1)*length(index_rep))=(ny+1)*(nx+1)*(i-1)+index_rep; 289 | end 290 | Aeq1=sparse(index_x,index_y,1,ny*T,nParam); 291 | beq1 = ones(ny*T, 1); 292 | 293 | 294 | 295 | 296 | 297 | %%%% Constraint 2 %%%% 298 | index_x=repmat(reshape(1:T*nx,nx,T),ny+1,1); 299 | index_x=index_x(:); 300 | index_y=repmat(1:nx+1:(nx+1)/nx*length(index_x),nx,1); 301 | index_y2=repmat((0:nx-1)',1,size(index_y,2)); 302 | index_y=index_y2+index_y; 303 | index_y=index_y(:); 304 | Aeq2=sparse(index_x,index_y,1,nx*T,nParam); 305 | beq2 = ones(nx*T, 1); 306 | Aeq = [Aeq1; Aeq2]; beq = [beq1; beq2]; 307 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 308 | 309 | 310 | %%%%%%%%%% upper and lower bound constraints %%%%%%%%%%%%%%%%%%%%%%%%%%%%% 311 | % 0 <= W < inf, 0 <= e < inf, 0 <= h < inf 312 | lb = zeros(nParam, 1); 313 | ub = inf(nParam, 1); 314 | 315 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 316 | 317 | 318 | %%%%%%%%%% inequality constraints %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 319 | %%%% Constraint 1 %%%% 320 | index_minus_x=1:T-1; 321 | index_minus_y=WtLen+index_minus_x; 322 | value_minus=-1*ones(1,T-1); 323 | index_one_x=repmat(1:T-1,nxny,1); 324 | index_one_x=index_one_x(:); 325 | index_one_y=WtLen + etLen +1:WtLen + etLen + (T-2)*nxny +nxny; 326 | value_one=ones(1,length(index_one_y)); 327 | A1=sparse([index_minus_x';index_one_x],[index_minus_y';index_one_y'],[value_minus';value_one']); 328 | 329 | 330 | %%%% Constraint 2 %%%% 331 | index_m1_x=1: nxny*(T-1); 332 | index_m1_y=htPos; 333 | index_1_x=index_m1_x; 334 | index_y=repmat(1:nx+1:(nx+1)*(ny+1)*(T-1),nx,1); 335 | index_y(:,ny+1:ny+1:end)=[]; 336 | index_1_y=index_y+repmat((0:nx-1)',1,size(index_y,2)); 337 | index_1_y=index_1_y(:); 338 | index_2_x=index_1_x; 339 | index_2_y=index_1_y+(nx+1)*(ny+1); 340 | A3=sparse([index_1_x';index_m1_x';index_2_x'],[index_1_y;index_m1_y';index_2_y],[ones(length(index_1_y),1);-ones(length(index_m1_y),1);-ones(length(index_2_y),1)]); 341 | 342 | 343 | 344 | %%%% Constraint 3 %%%% 345 | A4=sparse([index_1_x';index_m1_x';index_2_x'],[index_1_y;index_m1_y';index_2_y],[-ones(length(index_1_y),1);-ones(length(index_m1_y),1);ones(length(index_2_y),1)]); 346 | 347 | 348 | A = [A1; A3; A4]; 349 | b = sparse((T-1)+nxny*(T-1)+nxny*(T-1),1); 350 | 351 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 352 | 353 | 354 | %%%%%%%%%% optimisation %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 355 | linProgOptions = optimoptions('linprog', 'Display','off'); 356 | [x, dxy] = linprog(f, A, b, Aeq, beq, lb, ub, [], linProgOptions); 357 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 358 | 359 | 360 | %%%%%%%%%% Metric and the assignment values to be returned %%%%%%%%%%%%%%% 361 | wMat = reshape(x(1:nxny2*T), [nx+1, ny+1, T]); 362 | [loc_cost, miss_cost, fa_cost, switch_cost] ... 363 | = computeLocFalseMissedSwitchCosts(wMat, DAB, X, Y, c, p, gamma); 364 | 365 | 366 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 367 | end 368 | 369 | function locCostMat = locCostComp_v2(X, Y, c, p) 370 | % function locCostMat = locCostComp(stMat, X, Y, c, p) 371 | % computing the localisation cost at each time 't' for every (i,j) 372 | 373 | tmpCost = c^p / 2; % cost for being unassigned 374 | T = size(X.xState, 2); nx = size(X.xState, 3); ny = size(Y.xState, 3); 375 | locCostMat = zeros(nx+1, ny+1, T); 376 | for t = 1:T 377 | for xind = 1:nx+1 378 | if (xind <= nx) % xind not dummy 379 | if (t>=X.tVec(xind)) && (t<=(X.tVec(xind)+X.iVec(xind)-1)) 380 | % if X_xind exists at t 381 | for yind = 1:ny+1 382 | if (yind <= ny) && (t >= Y.tVec(yind)) && ... 383 | (t <= (Y.iVec(yind) + Y.tVec(yind) - 1)) 384 | % if Y_yind exists at t 385 | locCostMat(xind,yind,t) = computeLocCostPerTime( ... 386 | X.xState(:,t,xind), Y.xState(:,t,yind), c, p); 387 | 388 | else % yind does not exist or yind is dummy 389 | locCostMat(xind,yind,t) = tmpCost; 390 | end 391 | end 392 | else % if X_xind does not exist at t 393 | for yind = 1:ny 394 | if (t >= Y.tVec(yind)) && ... 395 | (t <= (Y.iVec(yind) + Y.tVec(yind) - 1)) 396 | % if Y_yind exists at t 397 | locCostMat(xind,yind,t) = tmpCost; 398 | end 399 | end 400 | end 401 | else % xind is dummy 402 | for yind = 1:ny 403 | if (t >= Y.tVec(yind)) && ... 404 | (t <= (Y.iVec(yind) + Y.tVec(yind) - 1)) 405 | % if Y_yind exists at t 406 | locCostMat(xind,yind,t) = tmpCost; 407 | end 408 | end 409 | end 410 | end 411 | end 412 | end 413 | 414 | function d = computeLocCostPerTime(x, y, c, p) 415 | if all(~isnan(x)) && all(~isnan(y)) 416 | % neither x nor y has hole 417 | d = min(norm(x-y, p)^p,c^p); 418 | elseif any(isnan(x) & ~isnan(y)) || any(~isnan(x) & isnan(y)) 419 | % exactly one of x and y has hole 420 | d = c^p/2; 421 | else 422 | d = 0; 423 | end 424 | end 425 | 426 | function [loc_cost, miss_cost, fa_cost, switch_cost] ... 427 | = computeLocFalseMissedSwitchCosts(w_mat, locCostMat, X, Y, c, p, gamma) 428 | % computing the localisation cost, swtiching cost and cost for missed and false 429 | % targets. 430 | 431 | tmp_cost = c^p / 2; % cost for being unassigned 432 | T = size(X.xState, 2); nx = size(X.xState, 3); ny = size(Y.xState, 3); 433 | 434 | 435 | if(nx>1 && ny>1) 436 | switch_cost = 0.5 * gamma^p * ... 437 | squeeze(sum(sum(abs(diff(w_mat(1:nx, 1:ny, :), 1, 3))))); 438 | elseif(nx==0 || ny==0) 439 | switch_cost=zeros(T-1,1); 440 | elseif(nx==1 && ny==1) 441 | switch_cost = 0.5 * gamma^p * ... 442 | squeeze(abs(diff(w_mat(1:nx, 1:ny, :), 1, 3))); 443 | 444 | else 445 | switch_cost = 0.5 * gamma^p * ... 446 | squeeze(sum(abs(diff(w_mat(1:nx, 1:ny, :), 1, 3)))); 447 | end 448 | 449 | loc_mask = zeros(size(w_mat)); 450 | miss_mask = zeros(size(w_mat)); 451 | fa_mask = zeros(size(w_mat)); 452 | 453 | fa_miss_mask = zeros(size(w_mat)); %Accounts for false and missed target costs that arise for a localisation cost of c^p 454 | 455 | for t = 1:T 456 | % localisation and miss cost calculations 457 | for xind = 1:nx 458 | if (t>=X.tVec(xind)) && (t<=(X.tVec(xind)+X.iVec(xind)-1)) 459 | % if X_xind exists at t 460 | for yind = 1:ny 461 | if (yind <= ny) && (t >= Y.tVec(yind)) && ... 462 | (t <= (Y.iVec(yind) + Y.tVec(yind) - 1)) 463 | % if Y_yind exists at t 464 | if all(~isnan(X.xState(:,t,xind))) && ... 465 | all(~isnan(Y.xState(:,t,yind))) 466 | % there is no hole in x or y at this time 467 | %%% add to localisation cost at the time based on 468 | %%% weight (unless the weight is c^p 469 | 470 | if(locCostMat(xind, yind, t)<2*tmp_cost) 471 | loc_mask(xind, yind, t) = 1; 472 | else 473 | fa_miss_mask(xind, yind, t) = 1; 474 | end 475 | elseif any(isnan(X.xState(:,t,xind))) && ... 476 | all(~isnan(Y.xState(:,t,yind))) 477 | % there is a hole in x but no hole in y 478 | fa_mask(xind, yind, t) = 1; 479 | elseif all(~isnan(X.xState(:,t,xind))) && ... 480 | any(isnan(Y.xState(:,t,yind))) 481 | % there is no hole in x but a hole in y 482 | miss_mask(xind, yind, t) = 1; 483 | end 484 | else % yind does not exist 485 | 486 | %%% add to miss cost at the time 487 | miss_mask(xind, yind, t) = 1; 488 | end 489 | end 490 | %%% add to miss cost at the time for yind = ny+1 491 | yind = ny+1; 492 | miss_mask(xind, yind, t) = 1; 493 | end 494 | end 495 | 496 | for yind = 1:ny 497 | if (t>=Y.tVec(yind)) && (t<=(Y.tVec(yind)+Y.iVec(yind)-1)) 498 | % if Y_yind exists at t 499 | if all(~isnan(Y.xState(:, t, yind))) % no hole in y 500 | for xind = 1:nx 501 | if ~((xind <= nx) && (t >= X.tVec(xind)) && ... 502 | (t <= (X.iVec(xind) + X.tVec(xind) - 1))) 503 | % if X_xind does not exist at t 504 | 505 | %%% add to fa cost at the time 506 | fa_mask(xind, yind, t) = 1; 507 | end 508 | end 509 | end 510 | 511 | %%% add to fa cost at the time for xind = nx+1 512 | xind = nx+1; 513 | fa_mask(xind, yind, t) = 1; 514 | end 515 | end 516 | end 517 | 518 | loc_cost = squeeze(sum(sum(locCostMat .* w_mat .* loc_mask, 1), 2)); 519 | miss_cost = tmp_cost * squeeze(sum(sum(w_mat .* miss_mask, 1), 2))+ tmp_cost * squeeze(sum(sum(w_mat .* fa_miss_mask, 1), 2)); 520 | fa_cost = tmp_cost * squeeze(sum(sum(w_mat .* fa_mask, 1), 2))+ tmp_cost * squeeze(sum(sum(w_mat .* fa_miss_mask, 1), 2)); 521 | end 522 | --------------------------------------------------------------------------------