Skip to content

Commit

Permalink
Clean-up and radar tests update
Browse files Browse the repository at this point in the history
  • Loading branch information
@sglvladi
sglvladi committed May 29, 2018
1 parent ddd9590 commit 8f2a17a
Show file tree
Hide file tree
Showing 5 changed files with 48 additions and 121 deletions.
28 changes: 22 additions & 6 deletions Hypothesisers/Efficient Hypothesis Management/EfficientHypothesisManagementX.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@
LikelihoodMatrix
AssocWeightsMatrix
Mode = 'Track-Oriented'
timeout = 5
end

methods
Expand Down Expand Up @@ -105,14 +106,20 @@
this.LikelihoodMatrix = LikelihoodMatrix;
end
ValidationMatrix = this.LikelihoodMatrix>0;
this.NetObj = this.buildNetTO(ValidationMatrix);
[AssocWeightsMatrix, this.NetObj] = this.computeAssocWeights(this.NetObj,this.LikelihoodMatrix);
this.AssocWeightsMatrix = AssocWeightsMatrix;
[this.NetObj, timedOut] = this.buildNetTO(ValidationMatrix, this.timeout);
if(timedOut)
[NumTracks, NumMeasp1] = size(this.LikelihoodMatrix);
AssocWeightsMatrix = [ones(NumTracks,1); zeros(NumTracks,NumMeasp1-1)];
this.AssocWeightsMatrix = AssocWeightsMatrix;
else
[AssocWeightsMatrix, this.NetObj] = this.computeAssocWeights(this.NetObj,this.LikelihoodMatrix);
this.AssocWeightsMatrix = AssocWeightsMatrix;
end
end
end

methods (Static)
function NetObj = buildNetTO(ValidationMatrix)
function [NetObj,timedOut] = buildNetTO(ValidationMatrix,timeOut)
% BUILDNETTO Build Track-Oriented (TO) EHM net and compute respective
% association probabilities (betta).
%
Expand Down Expand Up @@ -145,7 +152,11 @@
% between these and any entries in the node's MeasIndList.
%
% Author: Lyudmil Vladimirov


% Set timedout to false
timedOut = 0;
tic;

% Get number of tracks/layers
TrackNum = size(ValidationMatrix,1);
LayerNum = TrackNum; % Layer 1 is root layer
Expand Down Expand Up @@ -191,7 +202,12 @@

% For every measurement in M_jm1
for i=1:size(M_jm1,2)


if(toc > timeOut)
timedOut = 1;
return;
end

% Get the track index
MeasInd = M_jm1(i);

Expand Down
103 changes: 2 additions & 101 deletions TrackInitiators/PhdExistProbTrackInitiatorX/PhdExistProbTrackInitiatorX.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -86,13 +86,8 @@
% 3) Compute Existence Probabilities
for trackInd = 1:numTracks
TrackList{trackInd}.ProbOfExist = (1 - this.PHDFilter.ProbOfDeath)*TrackList{trackInd}.ProbOfExist;
%Lj_total = (1-this.PHDFilter.ProbOfDetection*this.ProbOfGating) ...
% + this.PHDFilter.ProbOfDetection*this.ProbOfGating/TrackList{trackInd}.ClutterDensity*sum(TrackList{trackInd}.Filter.MeasLikelihood,2)';
%Lj = this.AssocWeightsMatrix(trackInd,1)*Lj_total...
% + Lj_total*sum(this.AssocWeightsMatrix(trackInd,find(TrackList{trackInd}.ValidationMatrix)));
denom = sum(this.AssocWeightsMatrix(trackInd,:))*TrackList{trackInd}.ProbOfExist + this.AssocWeightsMatrix(trackInd,1)*(1-TrackList{trackInd}.ProbOfExist)/((1-this.PHDFilter.ProbOfDetection*this.ProbOfGating));
TrackList{trackInd}.ProbOfExist = (sum(this.AssocWeightsMatrix(trackInd,:))*TrackList{trackInd}.ProbOfExist)/denom;
% TrackList{trackInd}.ProbOfExist = TrackList{trackInd}.ProbOfExist*Lj/(1-(1-Lj)*TrackList{trackInd}.ProbOfExist);
end

% Predict the PHD filter
Expand Down Expand Up @@ -149,8 +144,7 @@
% around the measurement
[NewTrack.Filter.Particles, NewTrack.Filter.Weights] = this.PHDFilter.Resampler.resample(NewTrack.Filter.Particles, NewTrack.Filter.Weights,NewTrack.Filter.NumParticles);
NewTrack.ProbOfExist = p_i(measInd);
TrackList{end+1} = NewTrack;
%this.Params.NewTracks{end+1} = NewTrack;
TrackList{end+1} = NewTrack;
end

%
Expand All @@ -163,103 +157,10 @@
if(TrackList{trackInd}.ProbOfExist<0.1)
TrackList{trackInd} = [];
end
% TrackList{trackInd}.ProbOfExist = (1 - this.PHDFilter.ProbOfDeath)*TrackList{trackInd}.ProbOfExist;
% %Lj_total = (1-this.PHDFilter.ProbOfDetection*this.ProbOfGating) ...
% % + this.PHDFilter.ProbOfDetection*this.ProbOfGating/TrackList{trackInd}.ClutterDensity*sum(TrackList{trackInd}.Filter.MeasLikelihood,2)';
% %Lj = this.AssocWeightsMatrix(trackInd,1)*Lj_total...
% % + Lj_total*sum(this.AssocWeightsMatrix(trackInd,find(TrackList{trackInd}.ValidationMatrix)));
% denom = sum(this.AssocWeightsMatrix(trackInd,:))*TrackList{trackInd}.ProbOfExist + this.AssocWeightsMatrix(trackInd,1)*(1-TrackList{trackInd}.ProbOfExist)/((1-this.PHDFilter.ProbOfDetection*this.ProbOfGating));
% TrackList{trackInd}.ProbOfExist = (sum(this.AssocWeightsMatrix(trackInd,:))*TrackList{trackInd}.ProbOfExist)/denom;
% TrackList{trackInd}.ProbOfExist = TrackList{trackInd}.ProbOfExist*Lj/(1-(1-Lj)*TrackList{trackInd}.ProbOfExist);
end
if(numel(TrackList))
TrackList = TrackList(~cellfun('isempty',TrackList));
end
%TrackList( :, ~any(TrackList,1) ) = [];

% % 7) Initiate tracks
% for j = 1:length(phd.Params.NewTracks)
% disp("Initating new track");
% if(isa(filter,'KalmanFilterX'))
% filter.Params.x = sum(phd.Params.NewTracks{j}.particles.*phd.Params.NewTracks{j}.w,2);
% filter.Params.P = weightedcov(phd.Params.NewTracks{j}.particles, phd.Params.NewTracks{j}.w);
% else
% filter.Params.particles = phd.Params.NewTracks{j}.particles;
% filter.Params.w = phd.Params.NewTracks{j}.w;
% filter.Params.x = sum(phd.Params.NewTracks{j}.particles.*phd.Params.NewTracks{j}.w,2);
% end
% jpdaf.Params.TrackList{end+1}.TrackObj = copy(filter);
% jpdaf.Params.TrackList{end}.ExistProb = phd.Params.NewTracks{j}.ExistProb;
% newTrackId = 1;
% while(ismember(newTrackId,TrackIds))
% newTrackId = newTrackId + 1;
% end
% jpdaf.Params.TrackList{end}.trackId = newTrackId;
% TrackIds = [TrackIds, newTrackId];
% jpdaf.Params.nTracks = jpdaf.Params.nTracks + 1;
end
end
end

% function [jpdaf, phd, TrackIds] = ExistProbPHDSearchX(jpdaf, phd, filter, TrackIds)
% %%ExistProbPHDSearchX Perform track management using existence
% % probabilities and a PHD filter search track.
%
% % 3) Compute Existence Probabilities
% for t = 1:jpdaf.Params.nTracks
% jpdaf.Params.TrackList{t}.ExistProb = (1-phd.Params.pDeath)*jpdaf.Params.TrackList{t}.ExistProb;
% c = sum(jpdaf.Params.AssocWeightsMatrix(t,:))*jpdaf.Params.TrackList{t}.ExistProb + jpdaf.Params.AssocWeightsMatrix(t,1)*(1-jpdaf.Params.TrackList{t}.ExistProb)/((1-jpdaf.Params.pDetect*jpdaf.Params.pGate));
% jpdaf.Params.TrackList{t}.ExistProb = (sum(jpdaf.Params.AssocWeightsMatrix(t,:))*jpdaf.Params.TrackList{t}.ExistProb)/c;% 1-(pf.betta(1)*(1-pf.ExistProb))/c;
% end
% % 4) Predict the PHD filter
% phd.Predict();
% % 5) Compute rhi as given by Eq. (16) in [2]
% rhi = zeros(1, size(jpdaf.Params.DataList,2));
% for m = 1:size(jpdaf.Params.DataList,2)
% rhi_tmp = 1;
% if(jpdaf.Params.AssocWeightsMatrix>-1) % Check if beta exists
% for t = 1:jpdaf.Params.nTracks
% rhi_tmp = rhi_tmp*(1-jpdaf.Params.AssocWeightsMatrix(t,m+1));
% end
% end
% rhi(m) = rhi_tmp;
% end
% phd.Params.rhi = rhi;%==1;
% % 6) Update PHD filter
% phd.Update();
% % 7) Initiate tracks
% for j = 1:length(phd.Params.NewTracks)
% disp("Initating new track");
% if(isa(filter,'KalmanFilterX'))
% filter.Params.x = sum(phd.Params.NewTracks{j}.particles.*phd.Params.NewTracks{j}.w,2);
% filter.Params.P = weightedcov(phd.Params.NewTracks{j}.particles, phd.Params.NewTracks{j}.w);
% else
% filter.Params.particles = phd.Params.NewTracks{j}.particles;
% filter.Params.w = phd.Params.NewTracks{j}.w;
% filter.Params.x = sum(phd.Params.NewTracks{j}.particles.*phd.Params.NewTracks{j}.w,2);
% end
% jpdaf.Params.TrackList{end+1}.TrackObj = copy(filter);
% jpdaf.Params.TrackList{end}.ExistProb = phd.Params.NewTracks{j}.ExistProb;
% newTrackId = 1;
% while(ismember(newTrackId,TrackIds))
% newTrackId = newTrackId + 1;
% end
% jpdaf.Params.TrackList{end}.trackId = newTrackId;
% TrackIds = [TrackIds, newTrackId];
% jpdaf.Params.nTracks = jpdaf.Params.nTracks + 1;
% end
% % 8) Delete tracks
% del_tracks = 0;
% del_flag = 0;
% for t = 1:jpdaf.Params.nTracks
% if(jpdaf.Params.TrackList{t}.ExistProb<0.1)
% jpdaf.Params.TrackList{t} = [];
% del_tracks = del_tracks + 1;
% del_flag = 1;
% end
% end
% if(del_flag)
% jpdaf.Params.TrackList = jpdaf.Params.TrackList(~cellfun('isempty',jpdaf.Params.TrackList));
% jpdaf.Params.nTracks = jpdaf.Params.nTracks - del_tracks;
% end
% end
end
34 changes: 20 additions & 14 deletions TrackInitiators/PhdTrackInitiatorX/Example/example_radar.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,8 +9,8 @@
% - PHD.BirthScheme = {'Expansion', 5000}
%
%% Clear all data and import dataset
clear all;
load('radar_ned.mat');
%clear all;
%load('cabodegata.mat');
N = size(DataList,2); % Simulation length

%% Plot & Recording settings
Expand All @@ -19,26 +19,26 @@
ShowUpdate = 1; % Set to 0 to skip showing update plots

% Recording settings
Record = 1; % Set to (0|1) to turn video recording (off|on)
Record = 0; % Set to (0|1) to turn video recording (off|on)
FrameRate = 0.5; % Number of frames per second
VideoQuality = 100; % Set to desired quality percentage
VideoPathName = "./example_radar.avi"; % Set to the desired path and name of produced recording

%% Instantiation of necessary components

% Instantiate a Dynamic model (CV model with q = 1 m/s^2)
dyn = ConstantVelocityModelX_2D('VelocityErrVariance',2);
dyn = ConstantVelocityModelX_2D('VelocityErrVariance',1);
dyn.TimeVariant = 2;

% Instantiate an Observation model (Variance of 50m^2 on each coordinate)
obs = LinGaussObsModelX_2D('NumStateDims',4,'ObsErrVariance',50,'Mapping',[1 3]);
obs = LinGaussObsModelX_2D('NumStateDims',4,'ObsErrVariance',25,'Mapping',[1 3]);

% Compile the State-Space model
ssm = StateSpaceModelX(dyn,obs);

% Surveillance region parameters
V_bounds = [-8154.72944624983;... % X-min |
-212.289393440959;... % X-max | Surveillance region bounding
3000;... % X-max | Surveillance region bounding
-7548.44272179096;... % Y-min | box coordinates (m)
4355.32645897434]'; % Y-max |
V = (abs(V_bounds(2)-V_bounds(1))*abs(V_bounds(4)-V_bounds(3))); % Total area of surveillance region
Expand All @@ -53,7 +53,7 @@
config.PriorDistFcn = @ (Np) deal(config.BirthIntFcn(Np), repmat(1/Np, Np, 1)'); % Uniform position and weights.
config.BirthScheme = {'Expansion', 5000};
config.ProbOfDeath = 0.005; % Probability of death = 0.5%
config.ProbOfDetection = 0.8; % Probability of detection = 70%
config.ProbOfDetection = 0.7; % Probability of detection = 70%
config.ResamplingScheme = 'Multinomial'; % Use Multinomial Resampling

% Instantiate PHD filter
Expand All @@ -65,14 +65,14 @@
% Initiate PDAF parameters
Params_pdaf.Clusterer = NaiveClustererX();
Params_pdaf.Gater = EllipsoidalGaterX(2,'ProbOfGating',0.99)';
Params_pdaf.ProbOfDetect = 0.8;
Params_pdaf.ProbOfDetect = 0.9;
mypdaf = JointProbabilisticDataAssocX(Params_pdaf);

% Initiate Track Initiator
config_ti.Filter = mypf;
config_ti.PHDFilter = myphd;
config_ti.ProbOfGating = 0.99;
config_ti.ProbOfConfirm = 0.9;
config_ti.ProbOfConfirm = 0.97;
myti = PhdExistProbTrackInitiatorX(config_ti);

TrackList = [];
Expand All @@ -99,7 +99,7 @@
fprintf('Iteration = %d/%d\n================>\n',k,N);

% Extract DataList at time k
tempDataList = DataList{k}(:,:);
tempDataList = DataList{k}(1:2,:);
tempDataList( :, ~any(tempDataList,1) ) = [];

% Process JPDAF
Expand Down Expand Up @@ -141,20 +141,26 @@
% Convert measurements to LLA and plot them
[lat,lon,~] = ned2geodetic(DataList{k}(2,:),...
DataList{k}(1,:),...
0,50.346069,...
-4.113670,...
0,50.36286670714617,...
-4.156833300366998,...
0,...
referenceEllipsoid('wgs84'));
% lat = DataList{k}(3,:);
% lon = DataList{k}(4,:);
plots(end+1) = plot(ax(1), lon,lat,'k*','MarkerSize', 10);
plot(ax(1), -4.156833300366998,50.36286670714617,...
'-s','MarkerSize',20,...
'MarkerEdgeColor','red',...
'MarkerFaceColor',[1 .6 .6]);

% Plot all existing tracks
for j=1:numel(TrackList)

% Convert track trajectory to LLA and plot it
[lat,lon,~] = ned2geodetic(TrackList{j}.Trajectory(3,:),...
TrackList{j}.Trajectory(1,:),...
0,50.346069,...
-4.113670,...
0,50.36286670714617,...
-4.156833300366998,...
0,...
referenceEllipsoid('wgs84'));
plots(end+1) = plot(ax(1), lon,lat,'r','LineWidth',2);
Expand Down
Binary file modified _common/api_key.mat
View file
Binary file not shown.
4 changes: 4 additions & 0 deletions _datasets/.gitignore
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
/almeria.mat
/almeria_mini.mat
/cabodegata.mat
/longroom.mat

0 comments on commit 8f2a17a

Please sign in to comment.