Sensor fusion with Kalman filter
39 views (last 30 days)
Show older comments
(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.
0 Comments
Accepted Answer
Elad Kivelevitch
on 3 Jan 2023
Edited: Elad Kivelevitch
on 3 Jan 2023
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.
6 Comments
Christian Rupido
on 1 Feb 2023
Good day! asking what type of toolbox you used to use kalman filter? and so taht i can use the function of trackingkf
More Answers (0)
See Also
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!