Initial commit

JPDA_associate.m

@@ -0,0 +1,47 @@
+% This function performs the ML data association
+%           S_bar(t)                 3XM | (x,y,weight for each particle)
+%           z(t)                     2Xn | n measurements of x,y
+%           association_ground_truth 1Xn | ground truth target ID for every measurement  
+% Outputs: 
+%           outlier                  1Xn   | (1 means outlier, 0 means not outlier) 
+%           Psi(t)                   1XnXM
+%           c                        1xnxM
+function [outlier, Psi, c] = JPDA_associate(S_bar, z, association_ground_truth)
+    if nargin < 3
+        association_ground_truth = [];
+    end
+
+    global lambda_psi % threshold on average likelihood for outlier detection
+    global Q % covariance matrix of the measurement model
+    global M % number of particles
+    global tau % number of targets
+
+    % TODO: Use JPDA. Now LAB2 association is still used
+
+    nObs = size(z,2);
+
+    % Initiate Storage Variables
+    z_hat = zeros(2, M, tau);
+    nu = zeros(size(z_hat)); 
+    psi = zeros(M, tau);
+    Psi = zeros(1,nObs,M);
+    outlier = zeros(1,nObs);
+    c = zeros(1,nObs, M);
+
+    % Loop through all targets. More efficient to do out of main loop
+    for k = 1:tau
+        z_hat(:,:,k) = observation_model(S_bar, k);
+    end
+
+    % Loop through each observation
+    for i = 1:nObs
+        nu(:,:,:) = z(:,i) - z_hat;
+        nu(2,:,:) = mod(nu(2,:,:) + pi, 2*pi) - pi;
+
+        psi(:,:) = 1/(2*pi*det(Q)^(1/2)) * exp(-1/2 * sum(nu.^2 ./ repmat(diag(Q),[1,M,tau])));
+        [Psi(1,i,:), c(1,i,:)] = max(psi,[],2);
+
+    end
+    outlier = mean(Psi,3) <= lambda_psi;
+
+end

observation_model.m

@@ -0,0 +1,13 @@
+% This function is the implementation of the measurement model.
+% The bearing should be in the interval [-pi,pi)
+% Inputs:
+%           S(t)                           3XM | Particle set (x,y,weight for each particle)
+%           j                              1X1
+% Outputs:  
+%           z_j                            2XM | Expected measurement (x,y)
+function z_j = observation_model(S, j)
+
+    % TODO: Implement observation model
+    z_j = S(1:2,:);
+
+end

predict_states.m

@@ -0,0 +1,21 @@
+% This function performs the prediction step.
+% Inputs:
+%           S(t-1)            3XM | Particle set (x,y,weight for each particle)
+%           u                 3X1 | inputs, 3rd input always 0
+% Outputs:   
+%           S_bar(t)          3XM | Particle set (x,y,weight for each particle)
+function [S_bar] = predict_states(S, u, delta_t)
+
+    global R % covariance matrix of motion model | shape 2x2
+    global M % number of particles
+
+    % TODO: define the dynamical model. 
+    F = [1, 0; 0, 1]; 
+    G = [1, 0; 0, 1];
+
+    % Appending zeros for the extra weight state
+    F = [F, [0; 0]; 0, 0, 1];
+    G = [G, [0; 0]; 0, 0, 0];
+
+    S_bar = F*S + G*repmat(u,M) + [R * randn(2,M); zeros(1,M)];
+end

systematic_resample.m

@@ -0,0 +1,18 @@
+% This function performs systematic re-sampling
+% Inputs:   
+%           S_bar(t):       3XM | Particle set (x,y,weight for each particle)
+% Outputs:
+%           S(t):           3XM | Particle set (x,y,weight for each particle)
+function S = systematic_resample(S_bar)
+
+    global M % number of particles 
+
+    S = zeros(3,M);
+    CDF = cumsum(S_bar(3,:));
+    r = rand/M;
+
+    for m = 1:M
+        index = find(CDF >= (r + (m-1)/M), 1, 'first');
+        S(:,m) = [S_bar(1:2,index); 1/M];
+    end
+end

weight_particles.m

@@ -0,0 +1,17 @@
+% This function calcultes the weights for each particle based on the
+% observation likelihood
+%           S_bar(t)            3XM | Particle set (x,y,weight for each particle)
+%           outlier             1Xn
+%           Psi(t)              1XnXM
+% Outputs: 
+%           S_bar(t)            3XM | Particle set (x,y,weight for each particle)
+function S_bar = weight_particles(S_bar, Psi, outlier)
+    % Remove Outliers
+    Psi_filtered = Psi(1,~outlier,:);
+
+    % Get weights
+    weights = prod(Psi_filtered,2);
+    weights = (weights/sum(weights));
+
+    S_bar(3,:) = weights;
+end