How to decrease correction of kalman filter while traking cars on a road with their real positions at 1hz?

Hello,

I am trying to track cars on a road send out CAM (cooperative awareness messages) each second, which I collect or rather simulate with SUMO.

I used the trackerTOMHT for track maintanace and the initekfimm as the initialisation filter. You can see my configuration in the following code snippet.

tracker = trackerTOMHT( ...

'FilterInitializationFcn',@initekfimm,... % Function for filter initialization

'MaxNumTracks',9, ... % Maximum number of tracks to maintain

'MaxNumSensors', 1, ... % Maximum number of sensors

'MaxNumHypotheses', 1, ... % Maximum number of hypotheses per track

'MaxNumTrackBranches', 2, ... % Maximum number of track branches

'MaxNumHistoryScans', 4, ... % Maximum number of history scans to consider

'NScanPruning', 'None', ... Pruning method for scan hypothesis

'HasCostMatrixInput', false, ... % Flag indicating whether a cost matrix is provided as input

'DetectionProbability', 0.99999, ... % Detection probability

'FalseAlarmRate', 0.00000001, ... % False alarm rate

'AssignmentThreshold',30* ... % Assignment thresholds

[0.4 ... % C1

0.7 ... % C2

1 ... % C3

Inf], ... % C4

'DeletionThreshold', -12 ... % Deletion threshold for tracks

);

The problem with this is that when i visualize the tracks you see that they are diverging apart from the truths (truths in the picture are with dotted lines and the tracks from TOMHT with continous lines).

My approach now would be to cancel out the correction of the kalman filter and just use the points that I receive as tracking steps. But actually I did not found any way to achieve that.

Or is there any setting that allows me to set the importance of the received points higher so that the correction can go on but has high importance on the truth reeived values?

Thank you for any tipps!

Answer 1

[Elad Kivelevitch]

Hannes,

I am not sure that I understand your use case and whether you're using the right tracking algorithms. A few questions:

1. What is reported by the CAM? Is it a raw position measurement or is it a filtered position? Filtered positions that are reported can have correlation in time, which means that you should not use a tracker that assumes that the inputs are uncorrelated.
2. Why did you choose trackerTOMHT? Is there an ambiguity in assigning the right report to the right object that you want to track? If not, I would recommend using starting with trackerGNN or trackerJPDA as they are simpler to use and configure.
3. Similarly, why did you choose the trackingIMM? Do you expect the object to change its motion model / maneuver over time?
4. When you feed the position reports, what is the measurement noise that you report?
5. From the plot, it is unclear where the measurements are.
6. Can you share the entire code to see if everything is defined as expected?

[Hannes]

Hello Elad,

thank you for your answer! To your questions:

1. From the CAM message I take the time, position (x,y,z) and the velocity in each direction (vx, vy, vz) and give that to the tracker. The messages are correlated in a way that they have to been sent within 0.1 to 1 second after the last. In our simulatioin we assume the worst case and take the positions every second from every car. Is there a better tracker that is able to incorperate that 10 to 1 Hz restriciton?
2. Because I am going to use biger scenarios with multiple cars going next to each other there will be ambiguity in the assigning process. But JPDA suits for that aswell I think. I just don't care about computational ressources and thought it could be more accurate.
3. Yeah the cars will drive randomly in a city and therefore change their velocity, direction and acceleration. So from what I read IMM suits the best. Or do you have a different proposal?
4. At the moment I use the identiy matrix because I assume that the measurements have very little Error. Last week I played with them and found out that if I reduce the values the tracker will create lots of new tracks and if I increase it the tracker will be more accurate until it cant even track anything. On the site:https://de.mathworks.com/help/fusion/ug/tuning-kalman-filter-to-improve-state-estimation.html you say the following: " Measurement Noise

Kalman filters also model "measurement noise" which helps inform the filter how much it should weight the new measurements versus the current motion model. Specifying a high measurement noise indicates that the measurements are inaccurate and causes the filter to favor the existing motion model and react more slowly to deviations from the motion model.

The ratio between the process noise and the measurement noise determines whether the filter follows closer to the motion model, if the process noise is smaller, or closer to the measurements, if the measurement noise is smaller."

If I assume that my measurements have very little noise and the positions of the simulation should be weigthed higher than the positinons of the motion model according to this section I should use a smaller measurement noise, but that isn't working for me.

5. The blue and the red dotted lines that go straigth at the top are the real positions and the blue and the red straight lines at the botton are the positions that I receive from the tracker.

6. yeah I can share my code with you here is my dataset first, which I created using Traci4Matlab. This is the simplest scenario where two cars go straigth till a junction, wait at a stop sign and then go off by 45° to the top and the second car overtakes the first.

Here is the code I used for tracking:

% Time to track : we can choose an arbitrary one or load it from the data
%trackingTime = 136; % choosed one 
trackingTime = numel(truthlog); % loaded from the data

% number of cars
%numCars = 2;
shape = size(detections.Detections);
numCars = shape(1);

% Create AssignmentError & Errormetrics functionality
tam = trackAssignmentMetrics; % Object for tracking assignment metrics
tem = trackErrorMetrics; % Object for tracking error metrics

tracker = trackerTOMHT( ...
    'FilterInitializationFcn',@initekfimm,... % Function for filter initialization
    'MaxNumTracks',10, ... % Maximum number of tracks to maintain
    'MaxNumSensors', 1, ... % Maximum number of sensors
    'MaxNumHypotheses', 1, ... % Maximum number of hypotheses per track
    'MaxNumTrackBranches', 2, ... % Maximum number of track branches
    'MaxNumHistoryScans', 4, ... % Maximum number of history scans to consider
    'NScanPruning', 'None', ...  Pruning method for scan hypothesis
    'HasCostMatrixInput', false, ... % Flag indicating whether a cost matrix is provided as input
    'DetectionProbability', 0.99999, ... % Detection probability
    'FalseAlarmRate', 0.00000001, ... % False alarm rate
    'AssignmentThreshold',30* ... % Assignment thresholds
                        [0.4 ... % C1
                        0.7 ... % C2
                       1 ... % C3
                           Inf], ... % C4
    'DeletionThreshold', -12 ... % Deletion threshold for tracks
    );

% Storing values
detlog = {}; % Keeps the detections over time
tracklog = {}; % Keeps the confirmed tracks over time
infolog = {}; % Keeps the analysis information over time
tentlog = {}; % Keeps the tentative tracks over time

warning('off', 'MATLAB:nearlySingularMatrix');
warning('off', 'MATLAB:singularMatrix');

% % Create a structure to store branch, hypothesis, and track information
% trackingInfo = struct('Branches', [], 'Hypotheses', [], 'Tracks', []);

for time = 1:trackingTime
   
    dettime = double(detections.Time(time));
    car = 1;
    detection = {};

    % Extract all detections at one time up to a maximum of the numbers of
    % cars
    while car <= numCars && ~isempty(detections.Detections{car,time})
        detection{car} = detections.Detections{car,time};
        car = car + 1;
    end

    % Perform tracking using the tracker object
    [confirmedTracks, tentativeTracks, allTracks, analysisInfo] = tracker(detection, dettime);
    
    detlog{end+1} = detection; % Store the detections for this iteration
    tentlog{end+1} = tentativeTracks; % Store the tentative tracks for this iteration
    tracklog{end+1} = confirmedTracks'; % Store the confirmed tracks for this iteration
    infolog{end+1} = analysisInfo; % Store the analysis information for this iteration
    
end

To evaluate the tracker I use the trackassignmentError:

% Initialization
trackAM = [];
truthAM = [];

% Looping over the tracks for each time sample. 
for i=1:numel(tracklog) 
    tracks = tracklog{i}; %OBJECT track for each time
    truths = truthlog{i}; %STRUCT truth for each time 
    [trackAM, truthAM] = tam(tracks,truths); % computes assignments metrics details 
    [trackIDs,truthIDs] = currentAssignment(tam); %compute current assignement of trac
end

% getting the last implementation of the metrics
tr = trackMetricsTable(tam)
tu = truthMetricsTable(tam)

and the GOSPA metrics:

% Initilization
tgm = trackGOSPAMetric('SwitchingPenalty',5); % Initialisation of the object

lgospa = zeros(numel(tracklog),1); % GOSPA metric including switching component, returned as a nonnegative real scalar.
gospa = zeros(numel(tracklog),1); % GOSPA metric, returned as a nonnegative real scalar.
switching = zeros(numel(tracklog),1); % Switching component, returned as a nonnegative real scalar.
localization = zeros(numel(tracklog),1); % Localization component, returned as a nonnegative real scalar.
missTarget = zeros(numel(tracklog),1); % Missed target component, returned as a nonnegative real scalar.
falseTracks = zeros(numel(tracklog),1); % False track component, returned as a nonnegative real scalar.

% Extraction of the information
for i = 1:numel(tracklog)
    tracks = tracklog{i};
    truths = truthlog{i};
    [lgospa(i),gospa(i),switching(i),localization(i),missTarget(i),falseTracks(i)] = tgm(tracks,truths);
end

% Plot the result
plot([lgospa gospa switching localization missTarget falseTracks])
legend('Labeled GOSPA','GOSPA','Switching Component',...
    'Localization Component','Missed Target Component','False Tracks Component')
xlabel('Time (s)')
ylabel('')

I hope now it is clear what my goal and my problem is.

Thank you already!