trackOSPAMetric for my data

8 Ansichten (letzte 30 Tage)
alex
alex am 5 Jan. 2025
Kommentiert: alex am 13 Jan. 2025 um 6:30
Hello. I want to use trackOSPAMetric to evaluate tracking result with artificial data. but it does not match my data. I attached the code and the error. any ideas? thank you very much for your time and attention.
clc; clear; close;
% 2DPF+3DOSPA
% Parameters
num_trajectories = 4;
num_paths = 2;
points_per_trajectory = 100;
num_clusters = 60;
% Define base paths as nearly horizontal lines
base_paths = { [linspace(0, 10, points_per_trajectory)', 2 * ones(points_per_trajectory, 1)];
[linspace(0, 10, points_per_trajectory)', 4 * ones(points_per_trajectory, 1)];
[linspace(0, 10, points_per_trajectory)', 6 * ones(points_per_trajectory, 1)];
[linspace(0, 10, points_per_trajectory)', 8 * ones(points_per_trajectory, 1)] };
% Initialize trajectories
trajectories = cell(1, num_trajectories);
% Generate trajectories with random deviations from base paths
for ii = 1:num_trajectories
path = base_paths{mod(ii, num_paths) + 1};
deviation = [0.01 * randn(points_per_trajectory, 1), ...
0.01 * randn(points_per_trajectory, 1)];
trajectories{ii} = path + deviation;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Select trajectory for tracking
selected_traj = trajectories{1};
numParticles = 1000;
stateDimension = 4;
init_state = [selected_traj(1, 1); 0;...
selected_traj(1, 2); 0];
pf = trackingPF(@constvel, @cvmeas, init_state, ...
'StateCovariance', 0.5 * eye(stateDimension), ...
'NumParticles', numParticles, ...
'HasAdditiveProcessNoise', true);
% Particle filter tracking
estimated_positions_pf = ...
zeros(size(selected_traj, 1), stateDimension);
pf_tot = cell(size(selected_traj, 1),1);
estimated_positions_pf(1, :) = init_state';
pf_no_km = clone(pf);
for t = 2:size(selected_traj, 1)
[xPred_pf, PPred_pf] = predict(pf_no_km, 1);
observation = ...
[selected_traj(t, 1); selected_traj(t, 2); 0];
[xCorr_km, Pcorr_km] = correct(pf, observation);
[xCorr_pf, Pcorr_pf] = correct(pf_no_km, observation);
estimated_positions_pf(t, :) = xCorr_pf;
pf_tot{t} = Pcorr_pf;
end
% Plot tracking results
figure (101);
plot(selected_traj(:, 1), selected_traj(:, 2), 'g', ...
'DisplayName', 'True Trajectory');
hold on;
plot(estimated_positions_pf(:, 1), ...
estimated_positions_pf(:, 3), 'bo-.', ...
'DisplayName', 'Estimated (No KM)');
title('Tracking Map');
legend;
xlabel('X');
ylabel('Y');
hold off;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% OSPA metric computation
truths_mine = ...
[selected_traj zeros(size(selected_traj,1),1)];
%%
% Construct the truth and tracks structs
num_timesteps = size(selected_traj, 1);
truths_cell = cell(1,num_timesteps);
tracks_PF_cell = cell(1,num_timesteps);
% Compute velocities for truth data
velocities_truth = diff(truths_mine); % Approximate velocity (forward difference)
velocities_truth = [0 0 0;velocities_truth];
for t = 1:num_timesteps
% Truth struct
truth_struct = struct();
truth_struct.PlatformID = 1;
truth_struct.Time = t; % Time step
truth_struct.Position = ...
[selected_traj(t, 1); selected_traj(t, 2);0]'; % Position [x; y]
% if t < num_timesteps
truth_struct.Velocity = velocities_truth(t, :); % Velocity [vx; vy]
truths_cell{t} = truth_struct;
% Tracks struct
tracks_struct_PF = struct();
tracks_struct_PF.UpdateTime = t; % Time step
tracks_struct_PF.State = ...
[estimated_positions_pf(t, :),0,0]';%; estimated_positions_pf(t, 3);0]; % Estimated position [x; y]
tracks_struct_PF.TrackID = 1;
if t==1
tracks_struct_PF.StateCovariance = [1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 1 0 0 0; 0 0 0 1 0 0; 0 0 0 0 0 0 ; 0 0 0 0 0 0 ];
else
pf_cov = [pf_tot{t};zeros(2,4)];
pf_cov2 = [pf_cov, zeros(6,2)];
tracks_struct_KM.StateCovariance = pf_cov2;
end
tracks_PF_cell{t} = tracks_struct_PF;
end
%%
tom = trackOSPAMetric;
ospa = zeros(numel(tracks_PF_cell),1);
cardOspa = zeros(numel(tracks_PF_cell),1);
locOspa = zeros(numel(tracks_PF_cell),1);
for ik = 1:numel(tracks_PF_cell)
ik
tracks_mi = tracks_PF_cell{ik}
truths_mi = truths_cell{ik}
[ospa(ik), locOspa(ik), cardOspa(ik)] = tom(tracks_mi, truths_mi);
end
ik = 1
tracks_mi = struct with fields:
UpdateTime: 1 State: [6x1 double] TrackID: 1 StateCovariance: [6x6 double]
truths_mi = struct with fields:
PlatformID: 1 Time: 1 Position: [-0.0072 3.9938 0] Velocity: [0 0 0]
Warning: Matrix is singular to working precision.
Warning: Matrix is singular to working precision.
Error using assignjv
Expected COSTMATRIX to be non-NaN.

Error in fusion.internal.assignment.lapCheckCostMatrix (line 10)
validateattributes(costMatrix, {'single','double'}, ...

Error in assignjv (line 110)
fusion.internal.assignment.lapCheckCostMatrix(costMatrix, mfilename);

Error in fusion.internal.metrics.OSPABase/stepImpl (line 219)
optimalAssignment = assignjv(dMatrix, 2*cN);

Error in trackOSPAMetric/stepImpl (line 305)
[locOspa, cardOspa, optimalAssignment] = stepImpl@fusion.internal.metrics.OSPABase(obj, tracks, truths);
%%
% Compute square error between truth and track locations
error_pf = (selected_traj(:, 1) - estimated_positions_pf(:, 1)).^2 + ...
(selected_traj(:, 2) - estimated_positions_pf(:, 3)).^2;
Here is the error:
Error using assignjv
Expected COSTMATRIX to be non-NaN.
Error in fusion.internal.assignment.lapCheckCostMatrix (line 10)
validateattributes(costMatrix, {'single','double'}, ...
Error in assignjv (line 110)
fusion.internal.assignment.lapCheckCostMatrix(costMatrix, mfilename);
Error in fusion.internal.metrics.OSPABase/stepImpl (line 219)
optimalAssignment = assignjv(dMatrix, 2*cN);
Error in trackOSPAMetric/stepImpl (line 305)
[locOspa, cardOspa, optimalAssignment] = stepImpl@fusion.internal.metrics.OSPABase(obj, tracks, truths);
Error in OnlyPF (line 139)
[ospa(ik), locOspa(ik), cardOspa(ik)] = tom(tracks_mi, truths_mi);
>>

Antworten (1)

Gayathri
Gayathri am 6 Jan. 2025
Hi @alex,
The error you're encountering is due to the covariance matrix being singular. This issue arises because the last two rows of the covariance matrix consist entirely of zeros, causing the determinant to become zero.
Please change the code as shown below to resolve the error.
if t == 1
tracks_struct_PF.StateCovariance = eye(6); % Initial covariance
else
pf_cov = [pf_tot{t}; ones(2, 4)];
pf_cov2 = [pf_cov, ones(6, 2)];
tracks_struct_PF.StateCovariance = pf_cov2;
end
To address the singularity issue, I've replaced "zeros(2,4)" with "ones(2,4)". Additionally, when t is 1, I have set the covariance matrix to be an identity matrix.
In the else condition, I corrected the variable name from "tracks_struct_KM.StateCovariance" to "tracks_struct_PF.StateCovariance". This change has been implemented in the code mentioned above.
For more information on "trackOSPAMetric", please refer to the documentation link given below.
Hope you find this information helpful!
  1 Kommentar
alex
alex am 13 Jan. 2025 um 6:30
thank you very much for the accurate answer. I have a question: does replacing "zeros(2,4)" with "ones(2,4)" change the state covariance theory? becuse I do not have third dimension and corresponding third dimeension velocity. my data is 2D (x,y). because of that I put zeros(2,4) in my covariance matrix.

Melden Sie sich an, um zu kommentieren.

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by