Robotics.particle filters does not work

Hy everybody! I'm on a serious trouble and i need your help. I'm trying to implement a particle filters for real time object tracking using the robotics. particle filters methods in matlab. This is the code:

clear all
close all
clc

%% Video initialization
video = ipcam('http://192.168.178.38:4747/video');
frame=snapshot(video);

%% Compute the background image
temp = zeros(size(frame));
[M,N] = size(temp(:,:,1));
diffimg = zeros(M,N); 
numFrame=50;
for i = 1:numFrame   
    frame = snapshot(video);
    temp = double(frame) + temp;
end
imbkg = temp/numFrame;
imshow(imbkg);

th = 50; %threshold
thBox = 8000; % min box area

%% particle filters initialization
pf = robotics.ParticleFilter;

initialize(pf, 5000, [0, 0, 0, 0, 0], 10*eye(5));
pf.StateEstimationMethod = 'mean';
pf.ResamplingMethod = 'systematic';

robotPred=zeros();
robotCorrected=zeros();
robotCov = zeros();
while true
    frame = snapshot(video);
    imcurrent = double(frame);
    
    diffimg = (abs(imcurrent(:,:,1)-imbkg(:,:,1))>th) | (abs(imcurrent(:,:,2)-imbkg(:,:,2))>th) | (abs(imcurrent(:,:,3)-imbkg(:,:,3))>th);
 
    labelimg = bwlabel(diffimg,4);
    markimg = regionprops(labelimg,['basic']);
    
    %select object area >=thBox
    markimg = selectBox(markimg,thBox);
    
    if ~isempty(markimg)
        measurement = [markimg.Area; markimg.Centroid(1); markimg.Centroid(2); markimg.BoundingBox(1);markimg.BoundingBox(2)];
    else
        measurement = [0;0;0;0;0];
    end
    [robotPred,robotCov] = predict(pf);
    
    [robotCorrected,robotCov] = correct(pf,measurement);
    
    if ~isempty(markimg)
        for i=1:size(markimg,2)
            frame = insertObjectAnnotation(frame,'rectangle',markimg(i).BoundingBox,'object');
            frame = insertMarker(frame,markimg(i).Centroid);
        end
    end
    imshow(frame);
end

The simulation without the particle filters works great. Using the particle filters to predict and correct the state given by the measure of Area, Center of Box and coordinates of the top left of the box the state does not change and its measure is almost 0. So my question is: i have to implement the filter in another way or there is a method to upgrade this one in order to achieve the results.

Thanks for your help.

Matteo

Answer 1

[Cam Salzberger]

Hey Matteo,

Just looking at the code has a few things that call out to me. I'm not sure what the selectBox call is, but can you check how often marking is empty when you are using the particle filter? If the measurement is always the same, and the same as the expected initial position, I wouldn't expect the particles to change position very much at all.

-Cam