JPDA now works with dynamic targets for traffic estimation (in file t…

…est.m)

JPDA Filter/associate.asv

@@ -0,0 +1,78 @@
+function [nu_bar, beta, beta_auxillary, z_nc] = associate(z, mu_bar,sigma_bar)
+    % This function performs the JPDA association
+    % Inputs:
+    %       z               dimMeasurementsXn_measurements      | measurements for each target
+    %       mu_bar          dimStatesXtau                       | predicted state for each target
+    %       sigma_bar       dimStatesXdimStatesXtau             | predicted covariance for each target
+    % Outputs:
+    %       nu_bar          dimMeasurementsXn_measurementsXtau  | innovation for each separate association
+    %       beta            tauXn_measurements                  | marginalised probability for each seperate association
+    %       beta_auxillary  tauX1                               | probability of target being misdetected
+
+    global tau
+    global Q
+    global H
+    global P_D
+    global P_FA
+
+    % number of measurements available
+    n_measurements = size(z, 2);
+
+    % First, compute individual association probabilities
+    p_association = zeros(tau, n_measurements);
+    nu_bar = zeros(size(z,1),n_measurements,tau);
+    for t = 1:tau
+        z_hat = H*mu_bar(:,t);
+        for j = 1:n_measurements
+            nu_bar(:,j,t) = z(:,j) - z_hat;
+            S_bar = H*sigma_bar(:,:,t)*H' + Q;
+            d = nu_bar(:,j,t)'/S_bar*nu_bar(:,j,t);
+            % Mea
+            if d < 50
+            p_association(t,j) = 1/((2*pi)^(size(z,1)/2)*sqrt(det(S_bar)))*exp(-0.5*d);
+        end
+        % normalise the likelihoods to detection probability
+        if (sum(p_association(t,:)) > 0)
+            p_association(t,:) = P_D*p_association(t,:)/sum(p_association(t,:));
+        end
+    end
+
+    % Now find legal association events
+    [theta] = find_legal_association_events(n_measurements,tau);
+
+    % Evaluate joint probability of event given measurements: p(theta|Z)
+    event_probability = ones(size(theta,2),1);
+    for event = 1:size(theta,2)
+        % Keep track of amount of clutter measurements in event
+        n_misdetected = 0;
+        for t = 1:tau
+            if theta(t,event) ~= 0
+                event_probability(event) = event_probability(event)*p_association(t,theta(t,event));
+            else
+                n_misdetected = n_misdetected + 1;
+            end
+        end
+        event_probability(event) = event_probability(event)*P_D^(tau-n_misdetected)*(1-P_D)^n_misdetected*P_FA^(n_measurements-(tau-n_misdetected));
+    end
+    % Normalize the joint probabilities
+    event_probability = event_probability/sum(event_probability); 
+
+    % Compute marginalised probabilities
+    beta = zeros(tau,n_measurements);
+    % Auxillary is to keep track of misdetection
+    beta_auxillary = zeros(tau,1);
+    for t = 1:tau
+        for j = 1:n_measurements
+            % Marginalize over association events that contain a t,j association
+            beta(t,j) = sum(event_probability(theta(t,:) == j));
+        end
+        % Probability that object t was not detected
+        beta_auxillary(t) = 1 - sum(beta(t,:));
+    end
+
+    % Heuristic to find measurements that did not reach association treshold
+    % When sum of probability of all individual associations of a
+    % measurement is arbitrarily unlikely
+    z_nc = z(:,sum(beta,1) < 0.1);
+end
+

JPDA Filter/associate.m

@@ -1,4 +1,4 @@
-function [nu_bar, beta, beta_auxillary] = associate(z, mu_bar,sigma_bar)
+function [nu_bar, beta, beta_auxillary, z_nc] = associate(z, mu_bar,sigma_bar)
     % This function performs the JPDA association
     % Inputs:
     %       z               dimMeasurementsXn_measurements      | measurements for each target
@@ -27,7 +27,14 @@
             nu_bar(:,j,t) = z(:,j) - z_hat;
             S_bar = H*sigma_bar(:,:,t)*H' + Q;
             d = nu_bar(:,j,t)'/S_bar*nu_bar(:,j,t);
-            p_association(t,j) = 1/((2*pi)^(size(z,1)/2)*sqrt(det(S_bar)))*exp(-0.5*d);
+            % Measurement validation
+            if d < 40
+                p_association(t,j) = 1/((2*pi)^(size(z,1)/2)*sqrt(det(S_bar)))*exp(-0.5*d);
+            end
+        end
+        % normalise the likelihoods to detection probability
+        if (sum(p_association(t,:)) > 0)
+            p_association(t,:) = P_D*p_association(t,:)/sum(p_association(t,:));
         end
     end
 
@@ -63,5 +70,10 @@
         % Probability that object t was not detected
         beta_auxillary(t) = 1 - sum(beta(t,:));
     end
+
+    % Heuristic to find measurements that did not reach association treshold
+    % When sum of probability of all individual associations of a
+    % measurement is arbitrarily unlikely
+    z_nc = z(:,sum(beta,1) < 0.1);
 end
 

JPDA Filter/check_promotion.m

@@ -0,0 +1,25 @@
+function [promote, purge, tracker] = check_promotion(beta_auxillary, tracker)
+    % This function checks for M/N heuristics.
+    % Inputs:
+    %       beta_auxillary          1xtau     | Probability of misdetection
+    %       tracker                 (n+1)xtau | M/N heuristic for each target
+    % Outputs:
+    %       tracker                 (n+1)xtau | updated M/N heuristic for each target
+    %       promote                 1xtau     | 1 if promoted, 0 otherwise
+    %       purge                   1xtau     | 1 if purged, 0 otherwise
+
+    global M_P
+    global M_D
+
+    tracker(1:(end-1),:) = tracker(2:end,:);
+    % Give a hit if probability of misdetection is bigger than probability of detection
+    tracker(end,:) = beta_auxillary > 0.9; 
+    % Total hits in past n times
+    M = sum(tracker(2:end,:),1);
+
+    purge = M > M_D;
+    % after N steps, -1 that was initialised at end position will have moved to first position
+    promote = find((tracker(1,:) == -1).*(M < M_P));
+    purge = find(purge + ((tracker(1,:) == -1) .* (~(M < M_P))));
+end
+

JPDA Filter/check_promotion_new.m

@@ -0,0 +1,29 @@
+function [mu_promoted, z_nn] = check_promotion_new(mu_new, z_nt)
+    % This function checks if a new track can be promoted to a tentative track
+    % This happens if a second measurement is made that is close enough
+
+    global DeltaMax
+
+    if isempty(z_nt)
+        mu_promoted = zeros(size(mu_new,1),0);
+        z_nn = zeros(size(z_nt));
+    else
+        % To account for dimensionality
+        treshold = DeltaMax^(size(mu_new,1));
+
+        distance = zeros(size(mu_new,2),size(z_nt,2));
+        for t = 1:size(mu_new,2)
+            distance(t,:) = sum(abs(mu_new(:,t)-z_nt).^2,1);
+        end
+        [closest, ind] = min(distance,[],2);
+        promoted = ind(closest < treshold);
+
+        mu_promoted = z_nt(:,promoted);
+        if isempty(promoted)
+            z_nn = z_nt;
+        else
+            z_nn = z_nt(:,~promoted);
+        end
+    end
+end
+

JPDA Filter/find_legal_association_events.m

@@ -19,7 +19,10 @@
         permutations = reshape(combinations(:,perms(1:tau)),[],tau);
         theta = [theta, permutations'];
     end
-
+
+    if n_measurements == 1
+        theta = [theta, zeros(tau,1)];
+    end
     % Remove duplicates
     theta = unique(theta',"rows")';
 end

JPDA Filter/iterate.m

@@ -1,4 +1,4 @@
-function [mu, sigma, beta] = iterate(mu, sigma, u, z)
+function [mu, sigma, beta, beta_auxillary, z_unassociated] = iterate(mu, sigma, u, z)
     % This function performs one iteration of the JPDA filter.
     % Inputs:
     %           mu(t-1)                   dimStates X tau
@@ -13,7 +13,7 @@
     [mu_bar, sigma_bar] = predict(mu, sigma, u);
 
     % associate step
-    [nu_bar, beta, beta_auxillary] = associate(z, mu_bar,sigma_bar);
+    [nu_bar, beta, beta_auxillary, z_unassociated] = associate(z, mu_bar,sigma_bar);
 
     % update step
     [mu, sigma] = update(mu_bar, sigma_bar, beta, beta_auxillary, nu_bar);

car_tracking_testing/car_detection_visualization.m

@@ -0,0 +1,37 @@
+clear, clc
+% Load video
+video = VideoReader('/Data/car2.mp4');
+
+foregroundDetector = vision.ForegroundDetector('NumGaussians', 3, ...
+    'NumTrainingFrames', 100);
+blobAnalysis = vision.BlobAnalysis('BoundingBoxOutputPort', true, ...
+    'AreaOutputPort', false, 'CentroidOutputPort', true, ...
+    'MinimumBlobArea', 250);
+
+figure(1),
+% subplot(3,1,1)
+% title('Foreground')
+% subplot(3,1,2)
+% title("Filtered Foreground")
+% subplot(3,1,3)
+% title("Results")
+for i = 0:video.NumFrames
+    % Read the next video frame
+    frame = readFrame(video); 
+    % Retrieve Foreground
+    foreground = step(foregroundDetector, frame);
+    % Remove Clutter
+    se = strel('disk', 3);
+    filteredForeground = imopen(foreground, se);
+    % Find Morphological objects
+    [centroid, bbox] = step(blobAnalysis, filteredForeground);
+
+    % Results
+    result = insertShape(frame, 'Rectangle', bbox, 'Color', 'green');
+
+    imshow(result)
+%     subplot(3,1,1), imshow(foreground)
+%     subplot(3,1,2), imshow(filteredForeground)
+%     subplot(3,1,3), imshow(result)
+
+end

car_tracking_testing/car_detector.m

@@ -7,7 +7,7 @@
     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
     %Morphological Operation to Remove Noise
-    Structure = strel('square', 3);
+    Structure = strel('disk', 3);
     Noise_Free_Object = imopen(Object, Structure);
 
 

car_tracking_testing/car_measurements.m

@@ -0,0 +1,33 @@
+function [z, delta_t, bboxes] = car_measurements(video, frame_range)
+
+    foregroundDetector = vision.ForegroundDetector('NumGaussians', 3, ...
+        'NumTrainingFrames', 100);
+    blobAnalysis = vision.BlobAnalysis('BoundingBoxOutputPort', true, ...
+        'AreaOutputPort', false, 'CentroidOutputPort', true, ...
+        'MinimumBlobArea', 200);
+
+    % Pre-allocate
+    z = cell(video.NumFrames,1);
+    bboxes = cell(video.NumFrames,1);
+
+    if nargin < 2
+        frame_range = [1, video.NumFrames];
+    else
+        video.CurrentTime = frame_range/video.FrameRate;
+    end
+
+    for i = frame_range(1):frame_range(2)
+        % Read the next video frame
+        frame = readFrame(video);
+        % Retrieve Foreground
+        foreground = step(foregroundDetector, frame);
+        % Remove Clutter
+        se = strel('disk', 3);
+        filteredForeground = imopen(foreground, se);
+        % Find Morphological objects
+        [centroid, bboxes{i}] = step(blobAnalysis, filteredForeground);    
+        z{i} = centroid';
+    end
+
+    delta_t = 1/video.FrameRate;
+end

car_tracking_testing/main.m

@@ -5,10 +5,10 @@
 
 % Load video
 Video = VideoReader('videoplayback_short.mp4');
-% Video = VideoReader('car2.mp4');
+%Video = VideoReader('car2.mp4');
 
 Object_Detector = vision.ForegroundDetector(...
-       'NumTrainingFrames', 7, ... 
+       'NumTrainingFrames', 50, ... 
        'InitialVariance', 30*30);
 
 % Read the first 25 frames to get the background