Sensor fusion with Kalman filter

(1) I was wondering how to perform object tracking with the linear Kalman filter “trackingKF” using more than one measurement of the tracked object. In other words, I would like to perform sensor fusion with Kalman filter of a moving object using two position measurements (calculated from two processed sensor data). The code below illustrates how I do it with one set of position measurement.

XY_pos1 = [ 33.544      -214.86
            99.551      -218.83
            140.1       -213.7
            179.69      -206.96
            218.29      -200.07
            236.45      -215.53
            270.55      -200.66
            311.27      -193.14
            347.55      -176.33
            378.1      -167.25
            405.9       -145.6
            422.43      -132.86
            452.4      -92.974
            462.61      -70.885
            470.82      -54.077
            470.59      -42.721
            467.96      -24.212
            459.13        4.495
            452.39       40.051
            458.39       69.918
            440.54        104.4 ];

initialState = [XY_pos1(1); 0; XY_pos1(2); 0];
KF = trackingKF('MotionModel','2D Constant Velocity','State',initialState);
timeStep=0.5;

for k=1:size(XY_pos1,1)
    predStates(k,:) = predict(KF,timeStep);
    corrStates(k,:) = correct(KF,XY_pos1(k,:));
end

(2) Besides, I was wondering if the linear Kalman filter can ignore the outlier or noisy position measurements.

Thank you for any idea you could provide.

Answer 1

[Elad Kivelevitch]

To answer your first question: Yes, the Kalman filter does not care if the measurements arrive from the same sensor, two sensors, or multiple sensors. As long as you know when each sensor made a detection and you use the correct time difference between each consecutive measurements, you can use it in the same way that you wrote above. It will look something along the lines of:

XY_pos1 = [ 33.544      -214.86
            99.551      -218.83
            140.1       -213.7
            179.69      -206.96
            218.29      -200.07
            236.45      -215.53
            270.55      -200.66
            311.27      -193.14
            347.55      -176.33
            378.1      -167.25
            405.9       -145.6
            422.43      -132.86
            452.4      -92.974
            462.61      -70.885
            470.82      -54.077
            470.59      -42.721
            467.96      -24.212
            459.13        4.495
            452.39       40.051
            458.39       69.918
            440.54        104.4 ];
t1 = 0.5*(1:size(XY_pos1,1)); % Assume dt for sensor 1 is 0.5 second

% Create an array with both time and positions for sensor 1 measurements
XYT1 = [t1',XY_pos1];

% Repeat for sensor 2 assuming it has a dt of 1 seconds. I used the same
% data but with every other time-measurement pair
XYT2 = XYT1(1:2:end,:);

% Sort the measurements by time
XYT = sortrows([XYT1;XYT2]);

% From here on the code is essentially the same as yours, but note the use
% of XYT variable
initialState = [XYT(1,2); 0; XYT(1,3); 0];
KF = trackingKF('MotionModel','2D Constant Velocity','State',initialState);

for k = 1:size(XYT,1)-1
    timeStep = XYT(k+1,1)-XYT(k,1);
    predStates(k,:) = predict(KF,timeStep);
    corrStates(k,:) = correct(KF,XYT(k+1,2:3)');    
end

% Plotting the results
plot(XYT(:,2),XYT(:,3),'x', DisplayName = 'measurements');
hold on
plot(predStates(:,1),predStates(:,3), DisplayName = 'predicted');
plot(corrStates(:,1),corrStates(:,3), DisplayName = 'corrected');
legend(Location = "Northwest")

Can the Kalman filter ignore "outliers" or "noisy" position measurements?

The short answer is no. The main use of a Kalman filter is to filter noisy measurements, so it should not ignore noisy measurements. However, if you know that there are certain outliers that are completely out of reason (e.g., you get a measurement at [1000, 1000] in the set above, then you - as a person familiar with the problem - should remove that measurement from the dataset. The filter cannot guess that this measurement is any more outlier than another measurement.

[AEW]

Thank you for your comprehensive reply.

[AEW]

I was wondering if there exists an estimation filter that performs sensor fusion and object tracking while rejecting outliers that are far away from a previous state. MATLAB function rmoutliers was not able to remove all outliers which degraded the Kalman filter performance.

[Elad Kivelevitch]

Your question is about association of measurements to tracks. To be able to use that, you can do it in one of two ways:

Option 1: Recommended if you plan to scale the tracking to multiple targets:

Use one of the multi-target trackers that are built into the Sensor Fusion and Tracking Toolbox, for example trackerGNN or trackerJPDA and set the AssignmentThreshold property to a value that will reject the anomalies - or false measurements.You will have to convert your measurements to objectDetection format.

Option 2: Recommended if you just want to track a single target:

If you only track one object, you can use the distance method to compute the distance between the measurement and the predicted measurement. Note: The distance is a augmented Mahalonobis distance proposed by Blackman and Popoli:

If the distance exceeds the threshold, simply do not pass the measurement to the filter. Modifying the for-loop in the code above:

assignmentThreshold = 50; % Tweak this value 
for k = 1:size(XYT,1)-1
    timeStep = XYT(k+1,1)-XYT(k,1);
    predStates(k,:) = predict(KF,timeStep);
    % correct only if the measurement falls within the assignment threshold
    if distance(KF,XYT(k+1,2:3)') <= assignmentThreshold
       corrStates(k,:) = correct(KF,XYT(k+1,2:3)'); 
    end
end

[AEW]

Thank you very much.

[Christian Rupido]

Good day! asking what type of toolbox you used to use kalman filter? and so taht i can use the function of trackingkf

[AEW]

Sensor Fusion and Tracking Toolbox

Sensor Fusion and Tracking Toolbox — Functions (mathworks.com)