initiation GM-PHD filter

CalculateOSPAMetric.m

@@ -0,0 +1,99 @@
+%CalculatePerformanceMetric
+%Last modified 21 November 2013
+%Matlab code by Bryan Clarke b.clarke@acfr.usyd.edu.au 
+
+%This is an implementation of the Optimal Subpattern Assignment
+%(OSPA) metric proposed by Schuhmacher et al in 
+%Schuhmacher, D.; Ba-Tuong Vo; Ba-Ngu Vo, "A Consistent Metric for Performance Evaluation of Multi-Object Filters," Signal Processing, IEEE Transactions on , vol.56, no.8, pp.3447,3457, Aug. 2008
+%X is the estimated state in the form [ [x1; y1; vx1; vy1], [x2; y2; vx2;
+%vy2] , ...]
+%Y is the ground truth in the form [ [x1; y1; vx1; vy1], [x2; y2; vx2;
+%vy2] , ...]
+%This isn't actually important, as we will swap the labels so that X
+%is the label of the shorter vector and Y is the label of the longer.
+%cutoff_c and order_p are parameters that control the metric calculation;
+%cutoff is a saturation threshold, order controls how punishing it is to
+%larger errors versus smaller ones. See the paper by Schuhmacher et al to
+%get a handle on these in more detail.
+%NOTE: This implementation is still a work in progress and is a bit buggy. Use with caution.
+%NOTE: We only use 2D OSPA, for position; we don't use the velocities.
+function ospa = CalculateOSPAMetric(X, Y, cutoff_c, order_p)
+
+    m = size(X, 2);%Length of vector X
+    n = size(Y, 2);%Length of vector Y
+
+    alphas = cutoff_c * ones(1, n);%Initialise to cutoff, overwrite if there is a shorter value
+    bestOMATCost = -1;
+    bestOMATDataAssoc_i = [];
+
+    %m (i.e. the length of X) needs to be less than or equal to n (the length of Y)
+    if(m > n)%Swap them if this is not the case. X and Y are just labels that can be applied to either vector; whichever one is estimate or truth is not important.
+        tmpX = X;
+        tmpm = m;
+        X = Y;
+        m = n;
+        Y = tmpX;
+        n = tmpm;
+    end
+    if(m > 0)%If there are other values, we need to find the best data association.
+        %We calculate all potential combinations (ie. subsampling without
+        %replacement)
+        comboSize = m;
+        valuesSampled = 1:n;
+        allCombos = combnk(valuesSampled, comboSize);
+        nCombos = size(allCombos, 1);
+
+        %Then for each combination, we calculate every permutation (i.e.
+        %different ways to order the numbers) to find all possible data
+        %associations
+        for i = 1:nCombos
+            thisCombo = allCombos(i,:);%The combination we are using
+            allDataAssocs = perms(thisCombo);
+            nDataAssocs = size(allDataAssocs, 1);
+            %We check all the data associations for this combination
+            for j = 1:nDataAssocs
+                thisDataAssoc = allDataAssocs(j,:);%An ordered list of the indices of Y to match them against the values in X                
+                thisY = Y(:,thisDataAssoc);
+                thisOMATCost = CalculateOMATMetric(X, thisY, order_p);
+
+                if(bestOMATCost < 0) || (thisOMATCost < bestOMATCost) %If this is the first iteration, or the best so far, save
+                    bestOMATCost = thisOMATCost;
+                    bestOMATDataAssoc_i = thisDataAssoc;
+                end
+            end
+        end
+
+        %Now that we have the best data association, we use it to calculate
+        %alphas for the matched points
+        for i = 1:m
+           thisX = X(:,i);
+           thisY = Y(:,bestOMATDataAssoc_i(i));%Use the data association array to pull out the corresponding value in Y
+           alphas(i) = min(cutoff_c, norm(thisX - thisY) ^ order_p);%New alpha is either the distance or the cutoff, whichever is lower.                
+        end
+        %Add cutoff for unmatched targets
+        for i = 1:(n - m)
+            alphas(m+i) = cutoff_c;
+        end
+
+    end
+    alphasSum = sum(alphas) / n;
+    ospa = alphasSum ^ (1/order_p);
+end
+
+%We assume that the length of X = the length of Y, since we only use this
+%for finding the best data association for the OSPA metric.
+%X and Y are 2d arrays [[x1; y1], [x2; y2] ...[xN; yN]]
+%order_p is an integer greater than zero
+function omat = CalculateOMATMetric(X, Y, order_p)
+    m = size(X, 2);
+    omat = 0;
+    for i = 1:m
+        thisX = X(:,i);
+        thisY = Y(:,i);
+        dx = thisX(1) - thisY(1);
+        dy = thisX(2) - thisY(2);
+        omat = omat + hypot(dx, dy) ^ order_p;
+    end
+    omat = omat / m;
+    omat = omat ^ (1/order_p);%Since length of X = length of Y, the OMAT distance simplifies to this
+end

Calculate_Jacobian_H.m

@@ -0,0 +1,57 @@
+%% Calculate_Jacobian_H
+%Calculates the Jacobian of a range-bearing measurement model
+%xS and yS are the position of the sensor
+%xL, yL, vxL, vyL are the state of the landmark (position and velocities)
+%Velocities are not used.
+%This function is re-implemented as a one-line anonymous function in
+%GM_EKF_PHD_Initialise_Jacobians but I kept this here in case you want more
+%info on how it was calculated, or want to change it.
+function H = Calculate_Jacobian_H(xS, yS, xL, yL, vxL, vyL)
+%H = [d(f_r)/dX; d(f_theta)/dX ]
+%H = [d(f_r) / dx         d(f_r) / dy       d(f_r) / dvx      d(f_r) / dvy]
+%    [d(f_theta) / dx     d(f_theta) / dy   d(f_theta)/ dvx   d(f_theta) / dvy]
+%f_r = (delta_x^2 + delta_y^2)^1/2  = ((xL - xR)^2 + (yL - yR)^2)^1/2
+%f_theta = atan(delta_y/delta_x)
+%d(f_r) / dx = (xL - xR) / range (where range = hypot(xL - xR, yL - yR))
+%d(f_r) / dy = (yL - yR) / range
+%d(f_theta) / dx = (yR - yL) / range^2
+%d(f_theta) / dy = (xL - xR) / range^2
+
+% We use wolfram alpha to differentiate things.
+% "Derivative of ((x - a)^2 + (y - b)^2)^1/2 with respect to x" gives
+% (x - a) / ((x - a)^2 + (y - b)^2)^1/2
+% "Derivative of ((x - a)^2 + (y - b)^2)^1/2 with respect to y" gives
+% (y - b) / ((x - a)^2 + (y - b)^2)^1/2
+%"Derivative of atan2((y-a),(x-b)) with respect to x" gives
+%(a - y) / ((x - a)^2 + (y - b)^2) <--------- NOTE that this is (a - y) on
+%numerator, not (y - a)
+%"Derivative of atan2((y-a),(x-b)) with respect to y" gives
+%(x - b) / ((x - a)^2 + (y - b)^2)
+
+range = hypot(xL - xS, yL - yS);
+delta_x = xL - xS;
+delta_y = yL - yS;
+
+dfr_dx = delta_x / range;
+dfr_dy = delta_y / range;
+dfr_dvx = 0;
+dfr_dvy = 0;
+
+dftheta_dx = -delta_y / range^2;
+dftheta_dy = delta_x / range^2;
+dftheta_dvx = 0;
+dftheta_dvy = 0;
+
+dfvx_dx = 0;
+dfvx_dy = 0;
+dfvx_dvx = 1;
+dfvx_dvy = 0;
+
+dfvy_dx = 0;
+dfvy_dy = 0;
+dfvy_dvx = 0;
+dfvy_dvy = 1;
+
+H = [ [dfr_dx, dfr_dy, dfr_dvx, dfr_dvy]; [dftheta_dx, dftheta_dy, dftheta_dvx, dftheta_dvy]; [dfvx_dx, dfvx_dy, dfvx_dvx, dfvx_dvy ]; [dfvy_dx, dfvy_dy, dfvy_dvx, dfvy_dvy] ];
+
+end

ConvertPlusMinusPi.m

@@ -0,0 +1,8 @@
+function theta = ConvertPlusMinusPi(theta)
+    while(theta > pi)
+        theta = theta - 2 * pi;
+    end
+    while(theta < -pi)
+        theta = theta + 2 * pi;
+    end
+end

GM_EKF_PHD_Construct_Update_Components.m

@@ -0,0 +1,62 @@
+%% GM_EKF_PHD_Construct_Update_Components
+%Matlab code by Bryan Clarke b.clarke@acfr.usyd.edu.au 
+
+%This file creates the components needed for performing an extended Kalman filter update on the
+%targets using the measurement.
+s = sprintf('Step 3: Constructing update components for all targets, new and existing.');
+disp(s);
+
+%We need to clear the data structures each iteration
+eta = [];
+S = [];
+K = [];
+P_k_k = [];
+
+xR = x_sensor(1);
+yR = x_sensor(2);
+hR = x_sensor(3);
+R_polar = calculate_R_polar(sigma_r, sigma_theta);%Sensor covariance, polar
+
+for j = 1:numTargets_Jk_k_minus_1
+
+    m_j = mk_k_minus_1(:,j);
+    eta_j = zeros(4,1);%Expected observation
+    eta_j(1:2) = h(xR,yR,hR,m_j(1), m_j(2));%Observation model.
+    eta_j(3:4) = [m_j(3); m_j(4)];%Assume constant velocity.
+    P_range = calculateDataRange4(j); %4x4 array
+
+    H = calculate_Jacobian_H(xR, yR, m_j(1), m_j(2));
+
+    PHt = Pk_k_minus_1(:,P_range) * H'; %Taken from Tim Bailey's EKF code. 4x2 array
+
+    r = eta_j(1);
+    theta = eta_j(2);
+    %We need to extend the sensor covariance R to include the covariance of
+    %the speed estimation. See GM_PHD_Initialise_Jacobians for details on
+    %this calculation.
+    R_cartesian = calculate_R_cartesian(R_polar, r, theta, hR);%sensor covariance, cartesian
+    R_velocity = calculate_R_velocity_cartesian(Pk_k_minus_1(1:2,P_range(1:2)), R_cartesian, dt);%We need the landmark position uncertainty, the sensor uncertainty (in cartesian space) and the time delta
+    R = [ [R_polar, zeros(2,2)]; [zeros(2,2), R_velocity] ];
+
+    %Calculate K via Tim Bailey's method.
+    S_j = U * R * U' + H * PHt;    %U is set in GM_PHD_Initialise_Jacobians.
+
+    %At this point, Tim Bailey's code makes S_j symmetric. In this case, it leads to the matrix being non-positive definite a lot of the time and chol crashes.
+    %So we won't do that. 
+    SChol= chol(S_j);
+
+    SCholInv =  SChol \ eye(size(SChol)); % triangular matrix, invert via left division
+    W1 = PHt * SCholInv;
+
+    HPHt = H * PHt;
+
+    K_j = W1 * SCholInv';
+
+    P_j = Pk_k_minus_1(:,P_range) - W1*W1';%4x4 array
+    %End Tim Bailey's code.
+
+    eta = [eta, eta_j];
+    S = [S, S_j];
+    K = [K, K_j];
+    P_k_k = [P_k_k, P_j]; 
+end

GM_EKF_PHD_Create_Birth.m

@@ -0,0 +1,156 @@
+%GM_EKF_PHD_Create_Birth
+%Matlab code by Bryan Clarke b.clarke@acfr.usyd.edu.au 
+
+%This file performs the processing on measurements to extract new targets
+%and populates the birth lists (mean, weight, covariance) needed to instantiate them next iteration.
+
+%This is not formally spelt out in Vo&Ma so I have used my best judgement to
+%come up with a method to initialise targets. Targets are initialised as
+%static (using the position of current measurements) and/or dynamic (using
+%two generations of measurements and calculating the movement between).
+%If only static targets are added, the number of new targets = the number
+%of measurements. 
+%If only dynamic targets are added, the number of new targets = the number of
+%measurements this teration * the number of measurements last iteration
+%If both static and dynamic targets are added, the number of new targets is
+%equal to the sum of these.
+disp('Step 7: Creating new targets from measurements, for birthing next iteration');
+w_birth = [];
+m_birth = [];
+P_birth = [];
+w_spawn = [];
+m_spawn = [];
+P_spawn = [];
+numBirthedTargets = 0;
+numSpawnedTargets = 0;
+
+%We create targets using two generations of measurements.
+%The first generation is used to calculate the velocity (dx/dt) to the
+%second generation.
+%The second generation gives the position.
+%We also add a set of targets from the second generation but with velocity
+%zero, since some may be unlinked to the first generation, but we have no
+%velocity information for them.
+if((addVelocityForNewTargets == true) && (k >= 2))%If we want to add targets with initial velocities.If only one iteration complete, cannot calculate velocity
+    %Each measurement consists of 2 rows
+    thisMeasRowRange = k;
+    prevMeasRowRange = k-1;
+
+    thisMeas = simMeasurementHistory{thisMeasRowRange};
+    prevMeas = simMeasurementHistory{prevMeasRowRange};
+
+    for j_this = 1:size(thisMeas,2)
+        for j_prev = 1:1:size(prevMeas,2)%Match every pair from previous to current
+            z_this = thisMeas(:,j_this);
+            z_prev = prevMeas(:, j_prev);
+            %Calculate and add the velocity.
+            m_i = inv_h(z_this(1), z_this(2), x_sensor(1), x_sensor(2), x_sensor(3));
+            thisV = (m_i(1:2) - inv_h(z_prev(1), z_prev(2), x_sensor(1), x_sensor(2), x_sensor(3))) / dt;
+            if(abs(thisV(1)) > MAX_V) || (abs(thisV(2)) > MAX_V)
+                continue;%To reduce the number of targets added, we filter out the targets with unacceptable velocities.
+            end
+            %Augment with velocity
+            m_i = [m_i; thisV];
+
+            %Decide if the target is birthed (from birth position)
+            %or spawned (from an existing target)
+            %Initialise the weight to birth
+            birthWeight = birth_intensity(m_i);
+            %Targets can also spawn from existing targets. We will
+            %take whichever is a higher weight - birthing or
+            %spawning
+            nTargets = size(X_k, 2);
+            maxSpawnWeight = -1;
+            for targetI = 1:nTargets
+                thisWeight = spawn_intensity(m_i, X_k(:,targetI)) * X_k_w(targetI);%Spawn weight is a function of proximity to the existing target, and the weight of the existing target.
+                if(thisWeight > maxSpawnWeight)
+                    maxSpawnWeight = thisWeight;
+                end
+            end
+            %Check if birthing had higher weight.
+            if(birthWeight > maxSpawnWeight)
+                %Birth the target
+                w_i = birthWeight;
+                %Initialise the covariance
+                P_i = covariance_birth;
+                w_birth = [w_birth, w_i];
+                m_birth = [m_birth m_i];
+                P_birth = [P_birth, P_i];
+                numBirthedTargets = numBirthedTargets + 1;
+            else
+                %Spawn the target
+                w_i = maxSpawnWeight;
+                %Initialise the covariance
+                P_i = covariance_spawn;
+                w_spawn = [w_spawn, w_i];
+                m_spawn = [m_spawn, m_i];
+                P_spawn = [P_spawn, P_i];
+                numSpawnedTargets = numSpawnedTargets + 1;
+            end                
+        end
+    end
+end
+%If we want to add targets, treating them as if they are
+%static.
+if (addStaticNewTargets == true)
+     thisMeasRowRange = k;
+    thisMeas = simMeasurementHistory{thisMeasRowRange};
+    for j_this = 1:size(thisMeas,2)    %Each measurement consists of 2 rows
+
+        %Add a static target
+        z_this = thisMeas(:,j_this);
+        m_i = inv_h(z_this(1), z_this(2), x_sensor(1), x_sensor(2), x_sensor(3));
+
+        m_i(3:4) = [0; 0];
+
+        %Decide if the target is birthed (from birth position)
+        %or spawned (from an existing target)
+        %Initialise the weight to birth
+        birthWeight = birth_intensity(m_i);
+        %Targets can also spawn from existing targets. We will
+        %take whichever is a higher weight - birthing or
+        %spawning
+        nTargets = size(X_k, 2);
+        maxSpawnWeight = -1;
+        for targetI = 1:nTargets
+            thisWeight = spawn_intensity(m_i, X_k(:,targetI)) * X_k_w(targetI);%Spawn weight is a function of proximity to the existing target, and the weight of the existing target.
+            if(thisWeight > maxSpawnWeight)
+                maxSpawnWeight = thisWeight;
+            end
+        end
+        %Check if birthing had higher weight.
+        if(birthWeight > maxSpawnWeight)
+            %Birth the target
+            w_i = birthWeight;
+            %Initialise the covariance
+            P_i = covariance_birth;
+            w_birth = [w_birth, w_i];
+            m_birth = [m_birth m_i];
+            P_birth = [P_birth, P_i];
+            numBirthedTargets = numBirthedTargets + 1;
+        else
+            %Spawn the target
+            w_i = maxSpawnWeight;
+            %Initialise the covariance
+            P_i = covariance_spawn;
+            w_spawn = [w_spawn, w_i];
+            m_spawn = [m_spawn m_i];
+            P_spawn= [P_spawn, P_i];
+            numSpawnedTargets = numSpawnedTargets + 1;
+        end
+    end
+end
+
+
+if VERBOSE == 1
+    for j = 1:numBirthedTargets
+        thisM = m_birth(:,j);
+        s = sprintf('Target to birth %d: %3.4f %3.4f %3.4f %3.4f Weight %3.9f', j, thisM(1), thisM(2), thisM(3), thisM(4), w_birth(j));
+        disp(s);
+    end
+    for j = 1:numSpawnedTargets
+        thisM = m_spawn(:,j);
+        s = sprintf('Target to spawn %d: %3.4f %3.4f %3.4f %3.4f Weight %3.9f', j, thisM(1), thisM(2), thisM(3), thisM(4), w_spawn(j));
+        disp(s);
+    end
+end